1 files changed, 106 insertions, 19 deletions

diff --git a/drivers/pci/pcie/aspm.c b/drivers/pci/pcie/aspm.c
index e361c7dc726f..4d8e2c7b2ad1 100644
--- a/drivers/pci/pcie/aspm.c
+++ b/drivers/pci/pcie/aspm.c
@@ -33,6 +33,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;
@@ -125,7 +130,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 +154,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;
 }
 
 /*
@@ -419,9 +440,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 +483,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 +517,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 +626,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 +637,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 +721,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);