1 files changed, 42 insertions, 26 deletions

diff --git a/src/southbridge/intel/bd82x6x/smihandler.c b/src/southbridge/intel/bd82x6x/smihandler.c
index 1f8b44846e..250035c4c3 100644
--- a/src/southbridge/intel/bd82x6x/smihandler.c
+++ b/src/southbridge/intel/bd82x6x/smihandler.c
@@ -419,38 +419,54 @@ static void southbridge_smi_sleep(unsigned int node, smm_state_save_area_t *stat
 }
 
 #if CONFIG_ELOG_GSMI
-static void southbridge_smi_gsmi(void)
+/*
+ * Look for Synchronous IO SMI and use save state from that
+ * core in case we are not running on the same core that
+ * initiated the IO transaction.
+ */
+static em64t101_smm_state_save_area_t *smi_apmc_find_state_save(u64 cmd)
 {
-	em64t101_smm_state_save_area_t *io_smi;
-	u32 base = smi_get_tseg_base() + 0x8000;
-	u32 *ret, *param;
-	u8 sub_command, node;
+	em64t101_smm_state_save_area_t *state;
+	u32 base = smi_get_tseg_base() + 0x8000 + 0x7d00;
+	int node;
 
-	/*
-	 * Check for Synchronous IO SMI and use save state from that
-	 * core in case we are not running on the same core that
-	 * initiated the IO transaction.
-	 */
+	/* Check all nodes looking for the one that issued the IO */
 	for (node = 0; node < CONFIG_MAX_CPUS; node++) {
-		/*
-		 * Look for IO Misc Info:
-		 *  Synchronous bit[0]=1
-		 *  Byte bit[3:1]=1
-		 *  Output bit[7:4]=0
-		 *  APMC port bit[31:16]=0xb2
-		 * RAX[7:0] == 0xEF
-		 */
-		u32 io_want_info = (APM_CNT << 16) | 0x3;
-		io_smi = (em64t101_smm_state_save_area_t *)
-			(base + 0x7d00 - (node * 0x400));
+		state = (em64t101_smm_state_save_area_t *)
+			(base - (node * 0x400));
+
+		/* Check for Synchronous IO (bit0==1) */
+		if (!(state->io_misc_info & (1 << 0)))
+			continue;
+
+		/* Make sure it was a write (bit4==0) */
+		if (state->io_misc_info & (1 << 4))
+			continue;
+
+		/* Check for APMC IO port */
+		if (((state->io_misc_info >> 16) & 0xff) != APM_CNT)
+			continue;
 
-		if (io_smi->io_misc_info == io_want_info &&
-		    ((u8)io_smi->rax == ELOG_GSMI_APM_CNT))
-			break;
+		/* Check AX against the requested command */
+		if (state->rax != cmd)
+			continue;
+
+		return state;
 	}
 
-	/* Did not find matching CPU Save State */
-	if (node == CONFIG_MAX_CPUS)
+	return NULL;
+}
+#endif
+
+#if CONFIG_ELOG_GSMI
+static void southbridge_smi_gsmi(void)
+{
+	u32 *ret, *param;
+	u8 sub_command;
+	em64t101_smm_state_save_area_t *io_smi =
+		smi_apmc_find_state_save(ELOG_GSMI_APM_CNT);
+
+	if (!io_smi)
 		return;
 
 	/* Command and return value in EAX */