diff --git a/drivers/pci/controller/dwc/pcie-dw-rockchip.c b/drivers/pci/controller/dwc/pcie-dw-rockchip.c index 8b9b13e812a2..8d6720bd07db 100644 --- a/drivers/pci/controller/dwc/pcie-dw-rockchip.c +++ b/drivers/pci/controller/dwc/pcie-dw-rockchip.c @@ -143,6 +143,7 @@ struct rk_pcie { raw_spinlock_t intx_lock; u16 aspm; u32 l1ss_ctl1; + u32 l1ss_ctl2; struct dentry *debugfs; u32 msi_vector_num; struct workqueue_struct *hot_rst_wq; @@ -1434,46 +1435,69 @@ static void rk_pcie_downstream_dev_to_d0(struct rk_pcie *rk_pcie, bool enable) } /* Save and restore root bus ASPM */ - if (enable) { - if (rk_pcie->l1ss_ctl1) - dw_pcie_writel_dbi(rk_pcie->pci, bridge->l1ss + PCI_L1SS_CTL1, rk_pcie->l1ss_ctl1); - - /* rk_pcie->aspm woule be saved in advance when enable is false */ - dw_pcie_writel_dbi(rk_pcie->pci, bridge->pcie_cap + PCI_EXP_LNKCTL, rk_pcie->aspm); - } else { + if (!enable) { val = dw_pcie_readl_dbi(rk_pcie->pci, bridge->l1ss + PCI_L1SS_CTL1); - if (val & PCI_L1SS_CTL1_L1SS_MASK) + if (val & PCI_L1SS_CTL1_L1SS_MASK) { rk_pcie->l1ss_ctl1 = val; - else + rk_pcie->l1ss_ctl2 = dw_pcie_readl_dbi(rk_pcie->pci, + bridge->l1ss + PCI_L1SS_CTL2); + } else { rk_pcie->l1ss_ctl1 = 0; + rk_pcie->l1ss_ctl2 = 0; + } val = dw_pcie_readl_dbi(rk_pcie->pci, bridge->pcie_cap + PCI_EXP_LNKCTL); rk_pcie->aspm = val & PCI_EXP_LNKCTL_ASPMC; - val &= ~(PCI_EXP_LNKCAP_ASPM_L1 | PCI_EXP_LNKCAP_ASPM_L0S); - dw_pcie_writel_dbi(rk_pcie->pci, bridge->pcie_cap + PCI_EXP_LNKCTL, val); } + if (enable) { + if (rk_pcie->l1ss_ctl1) { + dw_pcie_writel_dbi(rk_pcie->pci, bridge->l1ss + PCI_L1SS_CTL2, + rk_pcie->l1ss_ctl2); + dw_pcie_writel_dbi(rk_pcie->pci, bridge->l1ss + PCI_L1SS_CTL1, + rk_pcie->l1ss_ctl1); + if (bridge->ltr_path) + pcie_capability_set_word(bridge, PCI_EXP_DEVCTL2, 0x0400); + } + } + + if (enable) { + list_for_each_entry(pdev, &root_bus->devices, bus_list) { + if (PCI_SLOT(pdev->devfn) == 0) { + if (rk_pcie->l1ss_ctl1) { + pci_write_config_dword(pdev, pdev->l1ss + PCI_L1SS_CTL2, + rk_pcie->l1ss_ctl2); + pci_write_config_dword(pdev, pdev->l1ss + PCI_L1SS_CTL1, + rk_pcie->l1ss_ctl1); + } + } + } + } + + if (enable) + dw_pcie_writel_dbi(rk_pcie->pci, bridge->pcie_cap + PCI_EXP_LNKCTL, rk_pcie->aspm); + list_for_each_entry(pdev, &root_bus->devices, bus_list) { if (PCI_SLOT(pdev->devfn) == 0) { if (pci_set_power_state(pdev, PCI_D0)) dev_err(rk_pcie->pci->dev, - "Failed to transition %s to D3hot state\n", + "Failed to transition %s to D0 state\n", dev_name(&pdev->dev)); - if (enable) { - if (rk_pcie->l1ss_ctl1) { - pci_read_config_dword(pdev, pdev->l1ss + PCI_L1SS_CTL1, &val); - val &= ~PCI_L1SS_CTL1_L1SS_MASK; - val |= (rk_pcie->l1ss_ctl1 & PCI_L1SS_CTL1_L1SS_MASK); - pci_write_config_dword(pdev, pdev->l1ss + PCI_L1SS_CTL1, val); - } - + if (enable) pcie_capability_clear_and_set_word(pdev, PCI_EXP_LNKCTL, - PCI_EXP_LNKCTL_ASPMC, rk_pcie->aspm); - } else { - pci_disable_link_state(pdev, PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1); - } + PCI_EXP_LNKCTL_ASPMC, + rk_pcie->aspm); + else + pci_disable_link_state(pdev, + PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1); } } + + if (!enable) { + val = dw_pcie_readl_dbi(rk_pcie->pci, bridge->pcie_cap + PCI_EXP_LNKCTL); + val &= ~(PCI_EXP_LNKCTL_ASPM_L1 | PCI_EXP_LNKCTL_ASPM_L0S); + dw_pcie_writel_dbi(rk_pcie->pci, bridge->pcie_cap + PCI_EXP_LNKCTL, val); + } } #endif