[PATCH] PCI: Save/restore L1 PM Substate extended capability registers

["David E. Box" <david.e.box@xxxxxxxxxxxxxxx>]

On Intel systems that support ACPI Low Power Idle it has been observed
that the L1 Substate capability can return disabled after a s2idle
cycle. This causes the loss of L1 Substate support during runtime
leading to higher power consumption. Add save/restore of the L1SS
control registers.

Signed-off-by: David E. Box <david.e.box@xxxxxxxxxxxxxxx>
---
 drivers/pci/pci.c | 49 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 49 insertions(+)

diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index e578d34095e9..beee3d9952a6 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -1539,6 +1539,48 @@ static void pci_restore_ltr_state(struct pci_dev *dev)
 	pci_write_config_word(dev, ltr + PCI_LTR_MAX_NOSNOOP_LAT, *cap++);
 }
 
+static void pci_save_l1ss_state(struct pci_dev *dev)
+{
+	int l1ss;
+	struct pci_cap_saved_state *save_state;
+	u16 *cap;
+
+	if (!pci_is_pcie(dev))
+		return;
+
+	l1ss = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_L1SS);
+	if (!l1ss)
+		return;
+
+	save_state = pci_find_saved_ext_cap(dev, PCI_EXT_CAP_ID_L1SS);
+	if (!save_state) {
+		pci_err(dev, "no suspend buffer for L1 Substates\n");
+		return;
+	}
+
+	cap = (u16 *)&save_state->cap.data[0];
+	pci_read_config_word(dev, l1ss + PCI_L1SS_CTL1, cap++);
+	pci_read_config_word(dev, l1ss + PCI_L1SS_CTL1 + 2, cap++);
+	pci_read_config_word(dev, l1ss + PCI_L1SS_CTL2, cap++);
+}
+
+static void pci_restore_l1ss_state(struct pci_dev *dev)
+{
+	struct pci_cap_saved_state *save_state;
+	int l1ss;
+	u16 *cap;
+
+	save_state = pci_find_saved_ext_cap(dev, PCI_EXT_CAP_ID_L1SS);
+	l1ss = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_L1SS);
+	if (!save_state || !l1ss)
+		return;
+
+	cap = (u16 *)&save_state->cap.data[0];
+	pci_write_config_word(dev, l1ss + PCI_L1SS_CTL1, *cap++);
+	pci_write_config_word(dev, l1ss + PCI_L1SS_CTL1 + 2, *cap++);
+	pci_write_config_word(dev, l1ss + PCI_L1SS_CTL2, *cap++);
+}
+
 /**
  * pci_save_state - save the PCI configuration space of a device before
  *		    suspending
@@ -1563,6 +1605,7 @@ int pci_save_state(struct pci_dev *dev)
 	if (i != 0)
 		return i;
 
+	pci_save_l1ss_state(dev);
 	pci_save_ltr_state(dev);
 	pci_save_dpc_state(dev);
 	pci_save_aer_state(dev);
@@ -1670,6 +1713,7 @@ void pci_restore_state(struct pci_dev *dev)
 	 */
 	pci_restore_ltr_state(dev);
 
+	pci_restore_l1ss_state(dev);
 	pci_restore_pcie_state(dev);
 	pci_restore_pasid_state(dev);
 	pci_restore_pri_state(dev);
@@ -3332,6 +3376,11 @@ void pci_allocate_cap_save_buffers(struct pci_dev *dev)
 	if (error)
 		pci_err(dev, "unable to allocate suspend buffer for LTR\n");
 
+	error = pci_add_ext_cap_save_buffer(dev, PCI_EXT_CAP_ID_L1SS,
+					    3 * sizeof(u16));
+	if (error)
+		pci_err(dev, "unable to allocate suspend buffer for L1 Substates\n");
+
 	pci_allocate_vc_save_buffers(dev);
 }
 
-- 
2.20.1