[PATCH 03/17] PCI/ASPM: Use pcie_lnkctl_clear_and_set()

[Ilpo Järvinen <ilpo.jarvinen@xxxxxxxxxxxxxxx>]

Don't assume that the device is fully under the control of ASPM and
use pcie_lnkctl_clear_and_set() which does proper locking to avoid
losing concurrent updates to the register values.

If configuration fails in pcie_aspm_configure_common_clock(), the
function attempts to restore the old PCI_EXP_LNKCTL_CCC settings. Store
only the old PCI_EXP_LNKCTL_CCC bit for the relevant devices rather
than the content of the whole LNKCTL registers. It aligns better with
how pcie_lnkctl_clear_and_set() expects its parameter and makes the
code more obvious to understand.

Suggested-by: Lukas Wunner <lukas@xxxxxxxxx>
Signed-off-by: Ilpo Järvinen <ilpo.jarvinen@xxxxxxxxxxxxxxx>
---
 drivers/pci/pcie/aspm.c | 48 ++++++++++++++---------------------------
 1 file changed, 16 insertions(+), 32 deletions(-)

diff --git a/drivers/pci/pcie/aspm.c b/drivers/pci/pcie/aspm.c
index dde1ef13d0d1..6dbef0332c2b 100644
--- a/drivers/pci/pcie/aspm.c
+++ b/drivers/pci/pcie/aspm.c
@@ -147,9 +147,7 @@ static void pcie_set_clkpm_nocheck(struct pcie_link_state *link, int enable)
 	u32 val = enable ? PCI_EXP_LNKCTL_CLKREQ_EN : 0;
 
 	list_for_each_entry(child, &linkbus->devices, bus_list)
-		pcie_capability_clear_and_set_word(child, PCI_EXP_LNKCTL,
-						   PCI_EXP_LNKCTL_CLKREQ_EN,
-						   val);
+		pcie_lnkctl_clear_and_set(child, PCI_EXP_LNKCTL_CLKREQ_EN, val);
 	link->clkpm_enabled = !!enable;
 }
 
@@ -213,7 +211,6 @@ static bool pcie_wait_for_retrain(struct pci_dev *pdev)
 static bool pcie_retrain_link(struct pcie_link_state *link)
 {
 	struct pci_dev *parent = link->pdev;
-	u16 reg16;
 
 	/*
 	 * Ensure the updated LNKCTL parameters are used during link
@@ -224,17 +221,14 @@ static bool pcie_retrain_link(struct pcie_link_state *link)
 	if (!pcie_wait_for_retrain(parent))
 		return false;
 
-	pcie_capability_read_word(parent, PCI_EXP_LNKCTL, &reg16);
-	reg16 |= PCI_EXP_LNKCTL_RL;
-	pcie_capability_write_word(parent, PCI_EXP_LNKCTL, reg16);
+	pcie_lnkctl_clear_and_set(parent, 0, PCI_EXP_LNKCTL_RL);
 	if (parent->clear_retrain_link) {
 		/*
 		 * Due to an erratum in some devices the Retrain Link bit
 		 * needs to be cleared again manually to allow the link
 		 * training to succeed.
 		 */
-		reg16 &= ~PCI_EXP_LNKCTL_RL;
-		pcie_capability_write_word(parent, PCI_EXP_LNKCTL, reg16);
+		pcie_lnkctl_clear_and_set(parent, PCI_EXP_LNKCTL_RL, 0);
 	}
 
 	return pcie_wait_for_retrain(parent);
@@ -248,7 +242,7 @@ static bool pcie_retrain_link(struct pcie_link_state *link)
 static void pcie_aspm_configure_common_clock(struct pcie_link_state *link)
 {
 	int same_clock = 1;
-	u16 reg16, parent_reg, child_reg[8];
+	u16 reg16, parent_old_ccc, child_old_ccc[8];
 	struct pci_dev *child, *parent = link->pdev;
 	struct pci_bus *linkbus = parent->subordinate;
 	/*
@@ -270,6 +264,7 @@ static void pcie_aspm_configure_common_clock(struct pcie_link_state *link)
 
 	/* Port might be already in common clock mode */
 	pcie_capability_read_word(parent, PCI_EXP_LNKCTL, &reg16);
+	parent_old_ccc = reg16 & PCI_EXP_LNKCTL_CCC;
 	if (same_clock && (reg16 & PCI_EXP_LNKCTL_CCC)) {
 		bool consistent = true;
 
@@ -289,22 +284,14 @@ static void pcie_aspm_configure_common_clock(struct pcie_link_state *link)
 	/* Configure downstream component, all functions */
 	list_for_each_entry(child, &linkbus->devices, bus_list) {
 		pcie_capability_read_word(child, PCI_EXP_LNKCTL, &reg16);
-		child_reg[PCI_FUNC(child->devfn)] = reg16;
-		if (same_clock)
-			reg16 |= PCI_EXP_LNKCTL_CCC;
-		else
-			reg16 &= ~PCI_EXP_LNKCTL_CCC;
-		pcie_capability_write_word(child, PCI_EXP_LNKCTL, reg16);
+		child_old_ccc[PCI_FUNC(child->devfn)] = reg16 & PCI_EXP_LNKCTL_CCC;
+		pcie_lnkctl_clear_and_set(child, PCI_EXP_LNKCTL_CCC,
+					  same_clock ? PCI_EXP_LNKCTL_CCC : 0);
 	}
 
 	/* Configure upstream component */
-	pcie_capability_read_word(parent, PCI_EXP_LNKCTL, &reg16);
-	parent_reg = reg16;
-	if (same_clock)
-		reg16 |= PCI_EXP_LNKCTL_CCC;
-	else
-		reg16 &= ~PCI_EXP_LNKCTL_CCC;
-	pcie_capability_write_word(parent, PCI_EXP_LNKCTL, reg16);
+	pcie_lnkctl_clear_and_set(parent, PCI_EXP_LNKCTL_CCC,
+				  same_clock ? PCI_EXP_LNKCTL_CCC : 0);
 
 	if (pcie_retrain_link(link))
 		return;
@@ -312,9 +299,9 @@ static void pcie_aspm_configure_common_clock(struct pcie_link_state *link)
 	/* Training failed. Restore common clock configurations */
 	pci_err(parent, "ASPM: Could not configure common clock\n");
 	list_for_each_entry(child, &linkbus->devices, bus_list)
-		pcie_capability_write_word(child, PCI_EXP_LNKCTL,
-					   child_reg[PCI_FUNC(child->devfn)]);
-	pcie_capability_write_word(parent, PCI_EXP_LNKCTL, parent_reg);
+		pcie_lnkctl_clear_and_set(child, PCI_EXP_LNKCTL_CCC,
+					  child_old_ccc[PCI_FUNC(child->devfn)]);
+	pcie_lnkctl_clear_and_set(parent, PCI_EXP_LNKCTL_CCC, parent_old_ccc);
 }
 
 /* Convert L0s latency encoding to ns */
@@ -745,10 +732,8 @@ static void pcie_config_aspm_l1ss(struct pcie_link_state *link, u32 state)
 	 * in pcie_config_aspm_link().
 	 */
 	if (enable_req & (ASPM_STATE_L1_1 | ASPM_STATE_L1_2)) {
-		pcie_capability_clear_and_set_word(child, PCI_EXP_LNKCTL,
-						   PCI_EXP_LNKCTL_ASPM_L1, 0);
-		pcie_capability_clear_and_set_word(parent, PCI_EXP_LNKCTL,
-						   PCI_EXP_LNKCTL_ASPM_L1, 0);
+		pcie_lnkctl_clear_and_set(child, PCI_EXP_LNKCTL_ASPM_L1, 0);
+		pcie_lnkctl_clear_and_set(parent, PCI_EXP_LNKCTL_ASPM_L1, 0);
 	}
 
 	val = 0;
@@ -770,8 +755,7 @@ static void pcie_config_aspm_l1ss(struct pcie_link_state *link, u32 state)
 
 static void pcie_config_aspm_dev(struct pci_dev *pdev, u32 val)
 {
-	pcie_capability_clear_and_set_word(pdev, PCI_EXP_LNKCTL,
-					   PCI_EXP_LNKCTL_ASPMC, val);
+	pcie_lnkctl_clear_and_set(pdev, PCI_EXP_LNKCTL_ASPMC, val);
 }
 
 static void pcie_config_aspm_link(struct pcie_link_state *link, u32 state)
-- 
2.30.2