3 files changed, 52 insertions, 14 deletions

diff --git a/src/soc/amd/stoneyridge/include/soc/iomap.h b/src/soc/amd/stoneyridge/include/soc/iomap.h
index 341d97d687..beb2bc83d3 100644
--- a/src/soc/amd/stoneyridge/include/soc/iomap.h
+++ b/src/soc/amd/stoneyridge/include/soc/iomap.h
@@ -52,9 +52,13 @@
 #define ACPI_SMI_CTL_PORT		0xb2
 #define STONEYRIDGE_ACPI_IO_BASE	CONFIG_STONEYRIDGE_ACPI_IO_BASE
 #define  ACPI_PM_EVT_BLK	(STONEYRIDGE_ACPI_IO_BASE + 0x00) /* 4 bytes */
+#define  ACPI_PM1_STS		(ACPI_PM_EVT_BLK + 0x00)	  /* 2 bytes */
+#define  ACPI_PM1_EN		(ACPI_PM_EVT_BLK + 0x02)	  /* 2 bytes */
 #define  ACPI_PM1_CNT_BLK	(STONEYRIDGE_ACPI_IO_BASE + 0x04) /* 2 bytes */
 #define  ACPI_CPU_CONTROL	(STONEYRIDGE_ACPI_IO_BASE + 0x08) /* 6 bytes */
 #define  ACPI_GPE0_BLK		(STONEYRIDGE_ACPI_IO_BASE + 0x10) /* 8 bytes */
+#define  ACPI_GPE0_STS		(ACPI_GPE0_BLK + 0x00)		  /* 4 bytes */
+#define  ACPI_GPE0_EN		(ACPI_GPE0_BLK + 0x04)		  /* 4 bytes */
 #define  ACPI_PM_TMR_BLK	(STONEYRIDGE_ACPI_IO_BASE + 0x18) /* 4 bytes */
 #define SMB_BASE_ADDR			0xb00
 #define PM2_INDEX			0xcd0
diff --git a/src/soc/amd/stoneyridge/include/soc/southbridge.h b/src/soc/amd/stoneyridge/include/soc/southbridge.h
index 10b1f4328b..1750547dea 100644
--- a/src/soc/amd/stoneyridge/include/soc/southbridge.h
+++ b/src/soc/amd/stoneyridge/include/soc/southbridge.h
@@ -353,6 +353,9 @@
 #define   FCH_AOAC_STAT0		BIT(6)
 #define   FCH_AOAC_STAT1		BIT(7)
 
+#define PM1_LIMIT			16
+#define GPE0_LIMIT			28
+
 struct stoneyridge_aoac {
 	int enable;
 	int status;
diff --git a/src/soc/amd/stoneyridge/southbridge.c b/src/soc/amd/stoneyridge/southbridge.c
index cc21601c3e..02daa26046 100644
--- a/src/soc/amd/stoneyridge/southbridge.c
+++ b/src/soc/amd/stoneyridge/southbridge.c
@@ -616,8 +616,8 @@ static void print_num_status_bits(int num_bits, uint32_t status,
 
 static uint16_t reset_pm1_status(void)
 {
-	uint16_t pm1_sts = inw(ACPI_PM_EVT_BLK);
-	outw(pm1_sts, ACPI_PM_EVT_BLK);
+	uint16_t pm1_sts = inw(ACPI_PM1_STS);
+	outw(pm1_sts, ACPI_PM1_STS);
 	return pm1_sts;
 }
 
@@ -663,21 +663,24 @@ static void sb_log_pm1_status(uint16_t pm1_sts)
 }
 
 struct soc_amd_sws {
-	uint32_t pm1i;
-	uint32_t gevent;
+	uint16_t pm1_sts;
+	uint16_t pm1_en;
+	uint32_t gpe0_sts;
+	uint32_t gpe0_en;
 };
 
 static struct soc_amd_sws sws;
 
-static void sb_save_sws(uint32_t pm1_status)
+static void sb_save_sws(uint16_t pm1_status)
 {
 	uint32_t reg32;
 
-	sws.pm1i = pm1_status;
-	reg32 = inl(ACPI_GPE0_BLK);
-	outl(ACPI_GPE0_BLK, reg32);
-	reg32 &= inl(ACPI_GPE0_BLK + sizeof(uint32_t));
-	sws.gevent = reg32;
+	sws.pm1_sts = pm1_status;
+	sws.pm1_en = inw(ACPI_PM1_EN);
+	reg32 = inl(ACPI_GPE0_STS);
+	outl(ACPI_GPE0_STS, reg32);
+	sws.gpe0_sts = reg32;
+	sws.gpe0_en = inl(ACPI_GPE0_EN);
 }
 
 static void sb_clear_pm1_status(void)
@@ -689,19 +692,47 @@ static void sb_clear_pm1_status(void)
 	print_pm1_status(pm1_sts);
 }
 
+static int get_index_bit(uint32_t value, uint16_t limit)
+{
+	uint16_t i;
+	uint32_t t;
+
+	if (limit >= sizeof(uint32_t))
+		return -1;
+
+	/* get a mask of valid bits. Ex limit = 3, set bits 0-2 */
+	t = (1 << limit) - 1;
+	if ((value & t) == 0)
+		return -1;
+	t = 1;
+	for (i = 0; i < limit; i++) {
+		if (value & t)
+			break;
+		t <<= 1;
+	}
+	return i;
+}
+
 static void set_nvs_sws(void *unused)
 {
 	struct global_nvs_t *gnvs;
+	int index;
 
 	gnvs = cbmem_find(CBMEM_ID_ACPI_GNVS);
 	if (gnvs == NULL)
 		return;
 
-	gnvs->pm1i = sws.pm1i;
-	gnvs->gpei = sws.gevent;
+	index = get_index_bit(sws.pm1_sts & sws.pm1_en, PM1_LIMIT);
+	if (index < 0)
+		gnvs->pm1i = ~0ULL;
+	else
+		gnvs->pm1i = index;
 
-	printk(BIOS_DEBUG, "Loaded _SWS parameters PM1 0x%08x, EVENT 0x%08x into nvs\n",
-				sws.pm1i, sws.gevent);
+	index = get_index_bit(sws.gpe0_sts & sws.gpe0_en, GPE0_LIMIT);
+	if (index < 0)
+		gnvs->gpei = ~0ULL;
+	else
+		gnvs->gpei = index;
 }
 
 BOOT_STATE_INIT_ENTRY(BS_OS_RESUME, BS_ON_ENTRY, set_nvs_sws, NULL);