From 54fdb1e44845be75570918311cdb39c1d32244a7 Mon Sep 17 00:00:00 2001
From: Hemant Kumar <hemantk@codeaurora.org>
Date: Wed, 31 Mar 2021 23:36:40 -0700
Subject: [PATCH] PCIe: ASPM: Save/restore ASPM-L1SS controls for
 suspend/resume

Previously ASPM L1-Sub-States control registers (CTL1 and CTL2)
weren't saved and restored during suspend/resume leading to ASPM-L1SS
configuration being lost post resume. Save the ASPM-L1SS control
registers so that the configuration is retained post resume.

Change-Id: Iaf8e873aca798557eb38c2ea105dcaadff998e86
Signed-off-by: Hemant Kumar <hemantk@codeaurora.org>
---
 drivers/pci/pci.c       | 16 +++++++++++++++
 drivers/pci/pci.h       | 12 ++++++++++++
 drivers/pci/pcie/aspm.c | 43 +++++++++++++++++++++++++++++++++++++++++
 3 files changed, 71 insertions(+)

diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 35c720cdb466..ff745f13e14e 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -1359,6 +1359,11 @@ int pci_save_state(struct pci_dev *dev)
 		return i;
 
 	pci_save_ltr_state(dev);
+
+#ifdef CONFIG_PCI_QTI
+	pci_save_aspm_l1ss_state(dev);
+#endif
+
 	pci_save_dpc_state(dev);
 	return pci_save_vc_state(dev);
 }
@@ -1464,6 +1469,10 @@ void pci_restore_state(struct pci_dev *dev)
 	 */
 	pci_restore_ltr_state(dev);
 
+#ifdef CONFIG_PCI_QTI
+	pci_restore_aspm_l1ss_state(dev);
+#endif
+
 	pci_restore_pcie_state(dev);
 	pci_restore_pasid_state(dev);
 	pci_restore_pri_state(dev);
@@ -3117,6 +3126,13 @@ void pci_allocate_cap_save_buffers(struct pci_dev *dev)
 	if (error)
 		pci_err(dev, "unable to allocate suspend buffer for LTR\n");
 
+#ifdef CONFIG_PCI_QTI
+	error = pci_add_ext_cap_save_buffer(dev, PCI_EXT_CAP_ID_L1SS,
+					    2 * sizeof(u32));
+	if (error)
+		pci_err(dev, "unable to allocate suspend buffer for ASPM-L1SS\n");
+#endif
+
 	pci_allocate_vc_save_buffers(dev);
 }
 
diff --git a/drivers/pci/pci.h b/drivers/pci/pci.h
index ea692e5ceba4..cca8b8e97357 100644
--- a/drivers/pci/pci.h
+++ b/drivers/pci/pci.h
@@ -544,11 +544,23 @@ void pcie_aspm_init_link_state(struct pci_dev *pdev);
 void pcie_aspm_exit_link_state(struct pci_dev *pdev);
 void pcie_aspm_pm_state_change(struct pci_dev *pdev);
 void pcie_aspm_powersave_config_link(struct pci_dev *pdev);
+
+#ifdef CONFIG_PCI_QTI
+void pci_save_aspm_l1ss_state(struct pci_dev *dev);
+void pci_restore_aspm_l1ss_state(struct pci_dev *dev);
+#endif
+
 #else
 static inline void pcie_aspm_init_link_state(struct pci_dev *pdev) { }
 static inline void pcie_aspm_exit_link_state(struct pci_dev *pdev) { }
 static inline void pcie_aspm_pm_state_change(struct pci_dev *pdev) { }
 static inline void pcie_aspm_powersave_config_link(struct pci_dev *pdev) { }
+
+#ifdef CONFIG_PCI_QTI
+static inline void pci_save_aspm_l1ss_state(struct pci_dev *dev) { }
+static inline void pci_restore_aspm_l1ss_state(struct pci_dev *dev) { }
+#endif
+
 #endif
 
 #ifdef CONFIG_PCIEASPM_DEBUG
diff --git a/drivers/pci/pcie/aspm.c b/drivers/pci/pcie/aspm.c
index 7624c71011c6..15bf2f310df4 100644
--- a/drivers/pci/pcie/aspm.c
+++ b/drivers/pci/pcie/aspm.c
@@ -742,6 +742,49 @@ static void pcie_config_aspm_l1ss(struct pcie_link_state *link, u32 state)
 				PCI_L1SS_CTL1_L1SS_MASK, val);
 }
 
+#ifdef CONFIG_PCI_QTI
+void pci_save_aspm_l1ss_state(struct pci_dev *dev)
+{
+	struct pci_cap_saved_state *save_state;
+	int aspm_l1ss;
+	u32 *cap;
+
+	if (!pci_is_pcie(dev))
+		return;
+
+	aspm_l1ss = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_L1SS);
+	if (!aspm_l1ss)
+		return;
+
+	save_state = pci_find_saved_ext_cap(dev, PCI_EXT_CAP_ID_L1SS);
+	if (!save_state)
+		return;
+
+	cap = (u32 *)&save_state->cap.data[0];
+	pci_read_config_dword(dev, aspm_l1ss + PCI_L1SS_CTL1, cap++);
+	pci_read_config_dword(dev, aspm_l1ss + PCI_L1SS_CTL2, cap++);
+}
+
+void pci_restore_aspm_l1ss_state(struct pci_dev *dev)
+{
+	struct pci_cap_saved_state *save_state;
+	int aspm_l1ss;
+	u32 *cap;
+
+	if (!pci_is_pcie(dev))
+		return;
+
+	save_state = pci_find_saved_ext_cap(dev, PCI_EXT_CAP_ID_L1SS);
+	aspm_l1ss = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_L1SS);
+	if (!save_state || !aspm_l1ss)
+		return;
+
+	cap = (u32 *)&save_state->cap.data[0];
+	pci_write_config_dword(dev, aspm_l1ss + PCI_L1SS_CTL1, *cap++);
+	pci_write_config_dword(dev, aspm_l1ss + PCI_L1SS_CTL2, *cap++);
+}
+#endif
+
 static void pcie_config_aspm_dev(struct pci_dev *pdev, u32 val)
 {
 	pcie_capability_clear_and_set_word(pdev, PCI_EXP_LNKCTL,