6 files changed, 290 insertions, 171 deletions

diff --git a/drivers/pci/pcie/aer/aerdrv_acpi.c b/drivers/pci/pcie/aer/aerdrv_acpi.c
index 6dd7b13e9808..ebce26c37049 100644
--- a/drivers/pci/pcie/aer/aerdrv_acpi.c
+++ b/drivers/pci/pcie/aer/aerdrv_acpi.c
@@ -38,7 +38,6 @@ int aer_osc_setup(struct pcie_device *pciedev)
 
         handle = acpi_find_root_bridge_handle(pdev);
         if (handle) {
-                pcie_osc_support_set(OSC_EXT_PCI_CONFIG_SUPPORT);
                 status = pci_osc_control_set(handle,
                                         OSC_PCI_EXPRESS_AER_CONTROL |
                                         OSC_PCI_EXPRESS_CAP_STRUCTURE_CONTROL);
diff --git a/drivers/pci/pcie/aer/aerdrv_errprint.c b/drivers/pci/pcie/aer/aerdrv_errprint.c
index 3933d4f30e8c..0fc29ae80df8 100644
--- a/drivers/pci/pcie/aer/aerdrv_errprint.c
+++ b/drivers/pci/pcie/aer/aerdrv_errprint.c
@@ -233,7 +233,7 @@ void aer_print_error(struct pci_dev *dev, struct aer_err_info *info)
 
                 if (info->flags & AER_TLP_HEADER_VALID_FLAG) {
                         unsigned char *tlp = (unsigned char *) &info->tlp;
-                        printk("%sTLB Header:\n", loglevel);
+                        printk("%sTLP Header:\n", loglevel);
                         printk("%s%02x%02x%02x%02x %02x%02x%02x%02x"
                                 " %02x%02x%02x%02x %02x%02x%02x%02x\n",
                                 loglevel,
diff --git a/drivers/pci/pcie/aspm.c b/drivers/pci/pcie/aspm.c
index 9aad608bcf3f..586b6f75910d 100644
--- a/drivers/pci/pcie/aspm.c
+++ b/drivers/pci/pcie/aspm.c
@@ -17,6 +17,7 @@
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/jiffies.h>
+#include <linux/delay.h>
 #include <linux/pci-aspm.h>
 #include "../pci.h"
 
@@ -33,6 +34,11 @@ struct endpoint_state {
 struct pcie_link_state {
         struct list_head sibiling;
         struct pci_dev *pdev;
+        bool downstream_has_switch;
+
+        struct pcie_link_state *parent;
+        struct list_head children;
+        struct list_head link;
 
         /* ASPM state */
         unsigned int support_state;
@@ -70,6 +76,8 @@ static const char *policy_str[] = {
         [POLICY_POWERSAVE] = "powersave"
 };
 
+#define LINK_RETRAIN_TIMEOUT HZ
+
 static int policy_to_aspm_state(struct pci_dev *pdev)
 {
         struct pcie_link_state *link_state = pdev->link_state;
@@ -125,7 +133,7 @@ static void pcie_set_clock_pm(struct pci_dev *pdev, int enable)
         link_state->clk_pm_enabled = !!enable;
 }
 
-static void pcie_check_clock_pm(struct pci_dev *pdev)
+static void pcie_check_clock_pm(struct pci_dev *pdev, int blacklist)
 {
         int pos;
         u32 reg32;
@@ -149,10 +157,26 @@ static void pcie_check_clock_pm(struct pci_dev *pdev)
                 if (!(reg16 & PCI_EXP_LNKCTL_CLKREQ_EN))
                         enabled = 0;
         }
-        link_state->clk_pm_capable = capable;
         link_state->clk_pm_enabled = enabled;
         link_state->bios_clk_state = enabled;
-        pcie_set_clock_pm(pdev, policy_to_clkpm_state(pdev));
+        if (!blacklist) {
+                link_state->clk_pm_capable = capable;
+                pcie_set_clock_pm(pdev, policy_to_clkpm_state(pdev));
+        } else {
+                link_state->clk_pm_capable = 0;
+                pcie_set_clock_pm(pdev, 0);
+        }
+}
+
+static bool pcie_aspm_downstream_has_switch(struct pci_dev *pdev)
+{
+        struct pci_dev *child_dev;
+
+        list_for_each_entry(child_dev, &pdev->subordinate->devices, bus_list) {
+                if (child_dev->pcie_type == PCI_EXP_TYPE_UPSTREAM)
+                        return true;
+        }
+        return false;
 }
 
 /*
@@ -217,16 +241,18 @@ static void pcie_aspm_configure_common_clock(struct pci_dev *pdev)
         pci_write_config_word(pdev, pos + PCI_EXP_LNKCTL, reg16);
 
         /* Wait for link training end */
-        /* break out after waiting for 1 second */
+        /* break out after waiting for timeout */
         start_jiffies = jiffies;
-        while ((jiffies - start_jiffies) < HZ) {
+        for (;;) {
                 pci_read_config_word(pdev, pos + PCI_EXP_LNKSTA, &reg16);
                 if (!(reg16 & PCI_EXP_LNKSTA_LT))
                         break;
-                cpu_relax();
+                if (time_after(jiffies, start_jiffies + LINK_RETRAIN_TIMEOUT))
+                        break;
+                msleep(1);
         }
         /* training failed -> recover */
-        if ((jiffies - start_jiffies) >= HZ) {
+        if (reg16 & PCI_EXP_LNKSTA_LT) {
                 dev_printk (KERN_ERR, &pdev->dev, "ASPM: Could not configure"
                             " common clock\n");
                 i = 0;
@@ -419,9 +445,9 @@ static unsigned int pcie_aspm_check_state(struct pci_dev *pdev,
 {
         struct pci_dev *child_dev;
 
-        /* If no child, disable the link */
+        /* If no child, ignore the link */
         if (list_empty(&pdev->subordinate->devices))
-                return 0;
+                return state;
         list_for_each_entry(child_dev, &pdev->subordinate->devices, bus_list) {
                 if (child_dev->pcie_type == PCI_EXP_TYPE_PCI_BRIDGE) {
                         /*
@@ -462,6 +488,9 @@ static void __pcie_aspm_config_link(struct pci_dev *pdev, unsigned int state)
         int valid = 1;
         struct pcie_link_state *link_state = pdev->link_state;
 
+        /* If no child, disable the link */
+        if (list_empty(&pdev->subordinate->devices))
+                state = 0;
         /*
          * if the downstream component has pci bridge function, don't do ASPM
          * now
@@ -493,20 +522,52 @@ static void __pcie_aspm_config_link(struct pci_dev *pdev, unsigned int state)
         link_state->enabled_state = state;
 }
 
+static struct pcie_link_state *get_root_port_link(struct pcie_link_state *link)
+{
+        struct pcie_link_state *root_port_link = link;
+        while (root_port_link->parent)
+                root_port_link = root_port_link->parent;
+        return root_port_link;
+}
+
+/* check the whole hierarchy, and configure each link in the hierarchy */
 static void __pcie_aspm_configure_link_state(struct pci_dev *pdev,
         unsigned int state)
 {
         struct pcie_link_state *link_state = pdev->link_state;
+        struct pcie_link_state *root_port_link = get_root_port_link(link_state);
+        struct pcie_link_state *leaf;
 
-        if (link_state->support_state == 0)
-                return;
         state &= PCIE_LINK_STATE_L0S|PCIE_LINK_STATE_L1;
 
-        /* state 0 means disabling aspm */
-        state = pcie_aspm_check_state(pdev, state);
+        /* check all links who have specific root port link */
+        list_for_each_entry(leaf, &link_list, sibiling) {
+                if (!list_empty(&leaf->children) ||
+                        get_root_port_link(leaf) != root_port_link)
+                        continue;
+                state = pcie_aspm_check_state(leaf->pdev, state);
+        }
+        /* check root port link too in case it hasn't children */
+        state = pcie_aspm_check_state(root_port_link->pdev, state);
+
         if (link_state->enabled_state == state)
                 return;
-        __pcie_aspm_config_link(pdev, state);
+
+        /*
+         * we must change the hierarchy. See comments in
+         * __pcie_aspm_config_link for the order
+         **/
+        if (state & PCIE_LINK_STATE_L1) {
+                list_for_each_entry(leaf, &link_list, sibiling) {
+                        if (get_root_port_link(leaf) == root_port_link)
+                                __pcie_aspm_config_link(leaf->pdev, state);
+                }
+        } else {
+                list_for_each_entry_reverse(leaf, &link_list, sibiling) {
+                        if (get_root_port_link(leaf) == root_port_link)
+                                __pcie_aspm_config_link(leaf->pdev, state);
+                }
+        }
 }
 
 /*
@@ -570,6 +631,7 @@ void pcie_aspm_init_link_state(struct pci_dev *pdev)
         unsigned int state;
         struct pcie_link_state *link_state;
         int error = 0;
+        int blacklist;
 
         if (aspm_disabled || !pdev->is_pcie || pdev->link_state)
                 return;
@@ -580,29 +642,58 @@ void pcie_aspm_init_link_state(struct pci_dev *pdev)
         if (list_empty(&pdev->subordinate->devices))
                 goto out;
 
-        if (pcie_aspm_sanity_check(pdev))
-                goto out;
+        blacklist = !!pcie_aspm_sanity_check(pdev);
 
         mutex_lock(&aspm_lock);
 
         link_state = kzalloc(sizeof(*link_state), GFP_KERNEL);
         if (!link_state)
                 goto unlock_out;
-        pdev->link_state = link_state;
 
-        pcie_aspm_configure_common_clock(pdev);
+        link_state->downstream_has_switch = pcie_aspm_downstream_has_switch(pdev);
+        INIT_LIST_HEAD(&link_state->children);
+        INIT_LIST_HEAD(&link_state->link);
+        if (pdev->bus->self) {/* this is a switch */
+                struct pcie_link_state *parent_link_state;
 
-        pcie_aspm_cap_init(pdev);
+                parent_link_state = pdev->bus->parent->self->link_state;
+                if (!parent_link_state) {
+                        kfree(link_state);
+                        goto unlock_out;
+                }
+                list_add(&link_state->link, &parent_link_state->children);
+                link_state->parent = parent_link_state;
+        }
 
-        /* config link state to avoid BIOS error */
-        state = pcie_aspm_check_state(pdev, policy_to_aspm_state(pdev));
-        __pcie_aspm_config_link(pdev, state);
+        pdev->link_state = link_state;
 
-        pcie_check_clock_pm(pdev);
+        if (!blacklist) {
+                pcie_aspm_configure_common_clock(pdev);
+                pcie_aspm_cap_init(pdev);
+        } else {
+                link_state->enabled_state = PCIE_LINK_STATE_L0S|PCIE_LINK_STATE_L1;
+                link_state->bios_aspm_state = 0;
+                /* Set support state to 0, so we will disable ASPM later */
+                link_state->support_state = 0;
+        }
 
         link_state->pdev = pdev;
         list_add(&link_state->sibiling, &link_list);
 
+        if (link_state->downstream_has_switch) {
+                /*
+                 * If link has switch, delay the link config. The leaf link
+                 * initialization will config the whole hierarchy. but we must
+                 * make sure BIOS doesn't set unsupported link state
+                 **/
+                state = pcie_aspm_check_state(pdev, link_state->bios_aspm_state);
+                __pcie_aspm_config_link(pdev, state);
+        } else
+                __pcie_aspm_configure_link_state(pdev,
+                        policy_to_aspm_state(pdev));
+
+        pcie_check_clock_pm(pdev, blacklist);
+
 unlock_out:
         if (error)
                 free_link_state(pdev);
@@ -635,6 +726,7 @@ void pcie_aspm_exit_link_state(struct pci_dev *pdev)
         /* All functions are removed, so just disable ASPM for the link */
         __pcie_aspm_config_one_dev(parent, 0);
         list_del(&link_state->sibiling);
+        list_del(&link_state->link);
         /* Clock PM is for endpoint device */
 
         free_link_state(parent);
@@ -857,24 +949,15 @@ void pcie_no_aspm(void)
                 aspm_disabled = 1;
 }
 
-#ifdef CONFIG_ACPI
-#include <acpi/acpi_bus.h>
-#include <linux/pci-acpi.h>
-static void pcie_aspm_platform_init(void)
-{
-        pcie_osc_support_set(OSC_ACTIVE_STATE_PWR_SUPPORT|
-                OSC_CLOCK_PWR_CAPABILITY_SUPPORT);
-}
-#else
-static inline void pcie_aspm_platform_init(void) { }
-#endif
-
-static int __init pcie_aspm_init(void)
+/**
+ * pcie_aspm_enabled - is PCIe ASPM enabled?
+ *
+ * Returns true if ASPM has not been disabled by the command-line option
+ * pcie_aspm=off.
+ **/
+int pcie_aspm_enabled(void)
 {
-        if (aspm_disabled)
-                return 0;
-        pcie_aspm_platform_init();
-        return 0;
+       return !aspm_disabled;
 }
+EXPORT_SYMBOL(pcie_aspm_enabled);
 
-fs_initcall(pcie_aspm_init);
diff --git a/drivers/pci/pcie/portdrv_bus.c b/drivers/pci/pcie/portdrv_bus.c
index 359fe5568df1..eec89b767f9f 100644
--- a/drivers/pci/pcie/portdrv_bus.c
+++ b/drivers/pci/pcie/portdrv_bus.c
@@ -16,14 +16,10 @@
 #include "portdrv.h"
 
 static int pcie_port_bus_match(struct device *dev, struct device_driver *drv);
-static int pcie_port_bus_suspend(struct device *dev, pm_message_t state);
-static int pcie_port_bus_resume(struct device *dev);
 
 struct bus_type pcie_port_bus_type = {
         .name           = "pci_express",
         .match          = pcie_port_bus_match,
-        .suspend        = pcie_port_bus_suspend,
-        .resume         = pcie_port_bus_resume, 
 };
 EXPORT_SYMBOL_GPL(pcie_port_bus_type);
 
@@ -49,32 +45,12 @@ static int pcie_port_bus_match(struct device *dev, struct device_driver *drv)
         return 1;
 }
 
-static int pcie_port_bus_suspend(struct device *dev, pm_message_t state)
+int pcie_port_bus_register(void)
 {
-        struct pcie_device *pciedev;
-        struct pcie_port_service_driver *driver;
-
-        if (!dev || !dev->driver)
-                return 0;
-
-        pciedev = to_pcie_device(dev);
-        driver = to_service_driver(dev->driver);
-        if (driver && driver->suspend)
-                driver->suspend(pciedev, state);
-        return 0;
+        return bus_register(&pcie_port_bus_type);
 }
 
-static int pcie_port_bus_resume(struct device *dev)
+void pcie_port_bus_unregister(void)
 {
-        struct pcie_device *pciedev;
-        struct pcie_port_service_driver *driver;
-
-        if (!dev || !dev->driver)
-                return 0;
-
-        pciedev = to_pcie_device(dev);
-        driver = to_service_driver(dev->driver);
-        if (driver && driver->resume)
-                driver->resume(pciedev);
-        return 0;
+        bus_unregister(&pcie_port_bus_type);
 }
diff --git a/drivers/pci/pcie/portdrv_core.c b/drivers/pci/pcie/portdrv_core.c
index 2e091e014829..8b3f8c18032f 100644
--- a/drivers/pci/pcie/portdrv_core.c
+++ b/drivers/pci/pcie/portdrv_core.c
@@ -19,91 +19,15 @@
 
 extern int pcie_mch_quirk;      /* MSI-quirk Indicator */
 
-static int pcie_port_probe_service(struct device *dev)
-{
-        struct pcie_device *pciedev;
-        struct pcie_port_service_driver *driver;
-        int status;
-
-        if (!dev || !dev->driver)
-                return -ENODEV;
-
-        driver = to_service_driver(dev->driver);
-        if (!driver || !driver->probe)
-                return -ENODEV;
-
-        pciedev = to_pcie_device(dev);
-        status = driver->probe(pciedev, driver->id_table);
-        if (!status) {
-                dev_printk(KERN_DEBUG, dev, "service driver %s loaded\n",
-                        driver->name);
-                get_device(dev);
-        }
-        return status;
-}
-
-static int pcie_port_remove_service(struct device *dev)
-{
-        struct pcie_device *pciedev;
-        struct pcie_port_service_driver *driver;
-
-        if (!dev || !dev->driver)
-                return 0;
-
-        pciedev = to_pcie_device(dev);
-        driver = to_service_driver(dev->driver);
-        if (driver && driver->remove) { 
-                dev_printk(KERN_DEBUG, dev, "unloading service driver %s\n",
-                        driver->name);
-                driver->remove(pciedev);
-                put_device(dev);
-        }
-        return 0;
-}
-
-static void pcie_port_shutdown_service(struct device *dev) {}
-
-static int pcie_port_suspend_service(struct device *dev, pm_message_t state)
-{
-        struct pcie_device *pciedev;
-        struct pcie_port_service_driver *driver;
-
-        if (!dev || !dev->driver)
-                return 0;
-
-        pciedev = to_pcie_device(dev);
-        driver = to_service_driver(dev->driver);
-        if (driver && driver->suspend)
-                driver->suspend(pciedev, state);
-        return 0;
-}
-
-static int pcie_port_resume_service(struct device *dev)
-{
-        struct pcie_device *pciedev;
-        struct pcie_port_service_driver *driver;
-
-        if (!dev || !dev->driver)
-                return 0;
-
-        pciedev = to_pcie_device(dev);
-        driver = to_service_driver(dev->driver);
-
-        if (driver && driver->resume)
-                driver->resume(pciedev);
-        return 0;
-}
-
-/*
- * release_pcie_device
- *      
- *      Being invoked automatically when device is being removed 
- *      in response to device_unregister(dev) call.
- *      Release all resources being claimed.
+/**
+ * release_pcie_device - free PCI Express port service device structure
+ * @dev: Port service device to release
+ *
+ * Invoked automatically when device is being removed in response to
+ * device_unregister(dev).  Release all resources being claimed.
  */
 static void release_pcie_device(struct device *dev)
 {
-        dev_printk(KERN_DEBUG, dev, "free port service\n");
         kfree(to_pcie_device(dev));                     
 }
 
@@ -128,7 +52,16 @@ static int is_msi_quirked(struct pci_dev *dev)
         }
         return quirk;
 }
-        
+
+/**
+ * assign_interrupt_mode - choose interrupt mode for PCI Express port services
+ *                         (INTx, MSI-X, MSI) and set up vectors
+ * @dev: PCI Express port to handle
+ * @vectors: Array of interrupt vectors to populate
+ * @mask: Bitmask of port capabilities returned by get_port_device_capability()
+ *
+ * Return value: Interrupt mode associated with the port
+ */
 static int assign_interrupt_mode(struct pci_dev *dev, int *vectors, int mask)
 {
         int i, pos, nvec, status = -EINVAL;
@@ -150,7 +83,6 @@ static int assign_interrupt_mode(struct pci_dev *dev, int *vectors, int mask)
         if (pos) {
                 struct msix_entry msix_entries[PCIE_PORT_DEVICE_MAXSERVICES] = 
                         {{0, 0}, {0, 1}, {0, 2}, {0, 3}};
-                dev_info(&dev->dev, "found MSI-X capability\n");
                 status = pci_enable_msix(dev, msix_entries, nvec);
                 if (!status) {
                         int j = 0;
@@ -165,7 +97,6 @@ static int assign_interrupt_mode(struct pci_dev *dev, int *vectors, int mask)
         if (status) {
                 pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
                 if (pos) {
-                        dev_info(&dev->dev, "found MSI capability\n");
                         status = pci_enable_msi(dev);
                         if (!status) {
                                 interrupt_mode = PCIE_PORT_MSI_MODE;
@@ -177,6 +108,16 @@ static int assign_interrupt_mode(struct pci_dev *dev, int *vectors, int mask)
         return interrupt_mode;
 }
 
+/**
+ * get_port_device_capability - discover capabilities of a PCI Express port
+ * @dev: PCI Express port to examine
+ *
+ * The capabilities are read from the port's PCI Express configuration registers
+ * as described in PCI Express Base Specification 1.0a sections 7.8.2, 7.8.9 and
+ * 7.9 - 7.11.
+ *
+ * Return value: Bitmask of discovered port capabilities
+ */
 static int get_port_device_capability(struct pci_dev *dev)
 {
         int services = 0, pos;
@@ -204,6 +145,15 @@ static int get_port_device_capability(struct pci_dev *dev)
         return services;
 }
 
+/**
+ * pcie_device_init - initialize PCI Express port service device
+ * @dev: Port service device to initialize
+ * @parent: PCI Express port to associate the service device with
+ * @port_type: Type of the port
+ * @service_type: Type of service to associate with the service device
+ * @irq: Interrupt vector to associate with the service device
+ * @irq_mode: Interrupt mode of the service (INTx, MSI-X, MSI)
+ */
 static void pcie_device_init(struct pci_dev *parent, struct pcie_device *dev, 
         int port_type, int service_type, int irq, int irq_mode)
 {
@@ -224,11 +174,19 @@ static void pcie_device_init(struct pci_dev *parent, struct pcie_device *dev,
         device->driver = NULL;
         device->driver_data = NULL;
         device->release = release_pcie_device;  /* callback to free pcie dev */
-        snprintf(device->bus_id, sizeof(device->bus_id), "%s:pcie%02x",
+        dev_set_name(device, "%s:pcie%02x",
                  pci_name(parent), get_descriptor_id(port_type, service_type));
         device->parent = &parent->dev;
 }
 
+/**
+ * alloc_pcie_device - allocate PCI Express port service device structure
+ * @parent: PCI Express port to associate the service device with
+ * @port_type: Type of the port
+ * @service_type: Type of service to associate with the service device
+ * @irq: Interrupt vector to associate with the service device
+ * @irq_mode: Interrupt mode of the service (INTx, MSI-X, MSI)
+ */
 static struct pcie_device* alloc_pcie_device(struct pci_dev *parent,
         int port_type, int service_type, int irq, int irq_mode)
 {
@@ -239,10 +197,13 @@ static struct pcie_device* alloc_pcie_device(struct pci_dev *parent,
                 return NULL;
 
         pcie_device_init(parent, device, port_type, service_type, irq,irq_mode);
-        dev_printk(KERN_DEBUG, &device->device, "allocate port service\n");
         return device;
 }
 
+/**
+ * pcie_port_device_probe - check if device is a PCI Express port
+ * @dev: Device to check
+ */
 int pcie_port_device_probe(struct pci_dev *dev)
 {
         int pos, type;
@@ -260,6 +221,13 @@ int pcie_port_device_probe(struct pci_dev *dev)
         return -ENODEV;
 }
 
+/**
+ * pcie_port_device_register - register PCI Express port
+ * @dev: PCI Express port to register
+ *
+ * Allocate the port extension structure and register services associated with
+ * the port.
+ */
 int pcie_port_device_register(struct pci_dev *dev)
 {
         struct pcie_port_device_ext *p_ext;
@@ -323,6 +291,11 @@ static int suspend_iter(struct device *dev, void *data)
         return 0;
 }
 
+/**
+ * pcie_port_device_suspend - suspend port services associated with a PCIe port
+ * @dev: PCI Express port to handle
+ * @state: Representation of system power management transition in progress
+ */
 int pcie_port_device_suspend(struct pci_dev *dev, pm_message_t state)
 {
         return device_for_each_child(&dev->dev, &state, suspend_iter);
@@ -341,6 +314,10 @@ static int resume_iter(struct device *dev, void *data)
         return 0;
 }
 
+/**
+ * pcie_port_device_suspend - resume port services associated with a PCIe port
+ * @dev: PCI Express port to handle
+ */
 int pcie_port_device_resume(struct pci_dev *dev)
 {
         return device_for_each_child(&dev->dev, NULL, resume_iter);
@@ -363,6 +340,13 @@ static int remove_iter(struct device *dev, void *data)
         return 0;
 }
 
+/**
+ * pcie_port_device_remove - unregister PCI Express port service devices
+ * @dev: PCI Express port the service devices to unregister are associated with
+ *
+ * Remove PCI Express port service devices associated with given port and
+ * disable MSI-X or MSI for the port.
+ */
 void pcie_port_device_remove(struct pci_dev *dev)
 {
         struct device *device;
@@ -386,16 +370,80 @@ void pcie_port_device_remove(struct pci_dev *dev)
                 pci_disable_msi(dev);
 }
 
-int pcie_port_bus_register(void)
+/**
+ * pcie_port_probe_service - probe driver for given PCI Express port service
+ * @dev: PCI Express port service device to probe against
+ *
+ * If PCI Express port service driver is registered with
+ * pcie_port_service_register(), this function will be called by the driver core
+ * whenever match is found between the driver and a port service device.
+ */
+static int pcie_port_probe_service(struct device *dev)
 {
-        return bus_register(&pcie_port_bus_type);
+        struct pcie_device *pciedev;
+        struct pcie_port_service_driver *driver;
+        int status;
+
+        if (!dev || !dev->driver)
+                return -ENODEV;
+
+        driver = to_service_driver(dev->driver);
+        if (!driver || !driver->probe)
+                return -ENODEV;
+
+        pciedev = to_pcie_device(dev);
+        status = driver->probe(pciedev, driver->id_table);
+        if (!status) {
+                dev_printk(KERN_DEBUG, dev, "service driver %s loaded\n",
+                        driver->name);
+                get_device(dev);
+        }
+        return status;
 }
 
-void pcie_port_bus_unregister(void)
+/**
+ * pcie_port_remove_service - detach driver from given PCI Express port service
+ * @dev: PCI Express port service device to handle
+ *
+ * If PCI Express port service driver is registered with
+ * pcie_port_service_register(), this function will be called by the driver core
+ * when device_unregister() is called for the port service device associated
+ * with the driver.
+ */
+static int pcie_port_remove_service(struct device *dev)
 {
-        bus_unregister(&pcie_port_bus_type);
+        struct pcie_device *pciedev;
+        struct pcie_port_service_driver *driver;
+
+        if (!dev || !dev->driver)
+                return 0;
+
+        pciedev = to_pcie_device(dev);
+        driver = to_service_driver(dev->driver);
+        if (driver && driver->remove) {
+                dev_printk(KERN_DEBUG, dev, "unloading service driver %s\n",
+                        driver->name);
+                driver->remove(pciedev);
+                put_device(dev);
+        }
+        return 0;
 }
 
+/**
+ * pcie_port_shutdown_service - shut down given PCI Express port service
+ * @dev: PCI Express port service device to handle
+ *
+ * If PCI Express port service driver is registered with
+ * pcie_port_service_register(), this function will be called by the driver core
+ * when device_shutdown() is called for the port service device associated
+ * with the driver.
+ */
+static void pcie_port_shutdown_service(struct device *dev) {}
+
+/**
+ * pcie_port_service_register - register PCI Express port service driver
+ * @new: PCI Express port service driver to register
+ */
 int pcie_port_service_register(struct pcie_port_service_driver *new)
 {
         new->driver.name = (char *)new->name;
@@ -403,15 +451,17 @@ int pcie_port_service_register(struct pcie_port_service_driver *new)
         new->driver.probe = pcie_port_probe_service;
         new->driver.remove = pcie_port_remove_service;
         new->driver.shutdown = pcie_port_shutdown_service;
-        new->driver.suspend = pcie_port_suspend_service;
-        new->driver.resume = pcie_port_resume_service;
 
         return driver_register(&new->driver);
 }
 
-void pcie_port_service_unregister(struct pcie_port_service_driver *new)
+/**
+ * pcie_port_service_unregister - unregister PCI Express port service driver
+ * @drv: PCI Express port service driver to unregister
+ */
+void pcie_port_service_unregister(struct pcie_port_service_driver *drv)
 {
-        driver_unregister(&new->driver);
+        driver_unregister(&drv->driver);
 }
 
 EXPORT_SYMBOL(pcie_port_service_register);
diff --git a/drivers/pci/pcie/portdrv_pci.c b/drivers/pci/pcie/portdrv_pci.c
index 584422da8d8b..99a914a027f8 100644
--- a/drivers/pci/pcie/portdrv_pci.c
+++ b/drivers/pci/pcie/portdrv_pci.c
@@ -41,7 +41,6 @@ static int pcie_portdrv_restore_config(struct pci_dev *dev)
 {
         int retval;
 
-        pci_restore_state(dev);
         retval = pci_enable_device(dev);
         if (retval)
                 return retval;
@@ -52,11 +51,18 @@ static int pcie_portdrv_restore_config(struct pci_dev *dev)
 #ifdef CONFIG_PM
 static int pcie_portdrv_suspend(struct pci_dev *dev, pm_message_t state)
 {
-        int ret = pcie_port_device_suspend(dev, state);
+        return pcie_port_device_suspend(dev, state);
 
-        if (!ret)
-                ret = pcie_portdrv_save_config(dev);
-        return ret;
+}
+
+static int pcie_portdrv_suspend_late(struct pci_dev *dev, pm_message_t state)
+{
+        return pci_save_state(dev);
+}
+
+static int pcie_portdrv_resume_early(struct pci_dev *dev)
+{
+        return pci_restore_state(dev);
 }
 
 static int pcie_portdrv_resume(struct pci_dev *dev)
@@ -66,6 +72,8 @@ static int pcie_portdrv_resume(struct pci_dev *dev)
 }
 #else
 #define pcie_portdrv_suspend NULL
+#define pcie_portdrv_suspend_late NULL
+#define pcie_portdrv_resume_early NULL
 #define pcie_portdrv_resume NULL
 #endif
 
@@ -221,6 +229,7 @@ static pci_ers_result_t pcie_portdrv_slot_reset(struct pci_dev *dev)
 
         /* If fatal, restore cfg space for possible link reset at upstream */
         if (dev->error_state == pci_channel_io_frozen) {
+                pci_restore_state(dev);
                 pcie_portdrv_restore_config(dev);
                 pci_enable_pcie_error_reporting(dev);
         }
@@ -283,6 +292,8 @@ static struct pci_driver pcie_portdriver = {
         .remove         = pcie_portdrv_remove,
 
         .suspend        = pcie_portdrv_suspend,
+        .suspend_late   = pcie_portdrv_suspend_late,
+        .resume_early   = pcie_portdrv_resume_early,
         .resume         = pcie_portdrv_resume,
 
         .err_handler    = &pcie_portdrv_err_handler,