PCI: pciehp: Add Disable/enable link functions

Will use it during power off/on of slots

Signed-off-by: Yinghai Lu <yinghai.lu@oracle.com>
Signed-off-by: Jesse Barnes <jbarnes@virtuousgeek.org>

diff --git a/drivers/pci/hotplug/pciehp_hpc.c b/drivers/pci/hotplug/pciehp_hpc.c
index 4ebdc1de2cb4dfe10902ff9f1ce6061b1071226c..db33688af5888c133e010e9d3a5847135d52e30a 100644 (file)
--- a/drivers/pci/hotplug/pciehp_hpc.c
+++ b/drivers/pci/hotplug/pciehp_hpc.c
@@ -352,6 +352,42 @@ int pciehp_check_link_status(struct controller *ctrl)
        return retval;
 }
 
+static int __pciehp_link_set(struct controller *ctrl, bool enable)
+{
+       u16 lnk_ctrl;
+       int retval = 0;
+
+       retval = pciehp_readw(ctrl, PCI_EXP_LNKCTL, &lnk_ctrl);
+       if (retval) {
+               ctrl_err(ctrl, "Cannot read LNKCTRL register\n");
+               return retval;
+       }
+
+       if (enable)
+               lnk_ctrl &= ~PCI_EXP_LNKCTL_LD;
+       else
+               lnk_ctrl |= PCI_EXP_LNKCTL_LD;
+
+       retval = pciehp_writew(ctrl, PCI_EXP_LNKCTL, lnk_ctrl);
+       if (retval) {
+               ctrl_err(ctrl, "Cannot write LNKCTRL register\n");
+               return retval;
+       }
+       ctrl_dbg(ctrl, "%s: lnk_ctrl = %x\n", __func__, lnk_ctrl);
+
+       return retval;
+}
+
+static int pciehp_link_enable(struct controller *ctrl)
+{
+       return __pciehp_link_set(ctrl, true);
+}
+
+static int pciehp_link_disable(struct controller *ctrl)
+{
+       return __pciehp_link_set(ctrl, false);
+}
+
 int pciehp_get_attention_status(struct slot *slot, u8 *status)
 {
        struct controller *ctrl = slot->ctrl;