2 files changed, 31 insertions, 28 deletions

diff --git a/src/southbridge/intel/common/smihandler.c b/src/southbridge/intel/common/smihandler.c
index 18f171dd1f..4279eb88d7 100644
--- a/src/southbridge/intel/common/smihandler.c
+++ b/src/southbridge/intel/common/smihandler.c
@@ -93,14 +93,11 @@ __weak void southbridge_smm_xhci_sleep(u8 slp_type)
 {
 }
 
-static void southbridge_smi_sleep(void)
+static int power_on_after_fail(void)
 {
-	u8 reg8;
-	u32 reg32;
-	u8 slp_typ;
 	u8 s5pwr = CONFIG_MAINBOARD_POWER_FAILURE_STATE;
 
-	// save and recover RTC port values
+	/* save and recover RTC port values */
 	u8 tmp70, tmp72;
 	tmp70 = inb(0x70);
 	tmp72 = inb(0x72);
@@ -108,6 +105,15 @@ static void southbridge_smi_sleep(void)
 	outb(tmp70, 0x70);
 	outb(tmp72, 0x72);
 
+	/* For "KEEP", switch to "OFF" - KEEP is software emulated. */
+	return (s5pwr == MAINBOARD_POWER_ON);
+}
+
+static void southbridge_smi_sleep(void)
+{
+	u32 reg32;
+	u8 slp_typ;
+
 	/* First, disable further SMIs */
 	write_pmbase8(SMI_EN, read_pmbase8(SMI_EN) & ~SLP_SMI_EN);
 
@@ -153,16 +159,11 @@ static void southbridge_smi_sleep(void)
 
 		write_pmbase32(GPE0_EN, 0);
 
-		/* Always set the flag in case CMOS was changed on runtime. For
-		 * "KEEP", switch to "OFF" - KEEP is software emulated
-		 */
-		reg8 = pci_read_config8(PCI_DEV(0, 0x1f, 0), D31F0_GEN_PMCON_3);
-		if (s5pwr == MAINBOARD_POWER_ON) {
-			reg8 &= ~1;
-		} else {
-			reg8 |= 1;
-		}
-		pci_write_config8(PCI_DEV(0, 0x1f, 0), D31F0_GEN_PMCON_3, reg8);
+		/* Always set the flag in case CMOS was changed on runtime. */
+		if (power_on_after_fail())
+			pci_and_config8(PCI_DEV(0, 0x1f, 0), D31F0_GEN_PMCON_3, ~1);
+		else
+			pci_or_config8(PCI_DEV(0, 0x1f, 0), D31F0_GEN_PMCON_3, 1);
 
 		/* also iterates over all bridges on bus 0 */
 		busmaster_disable_on_bus(0);
diff --git a/src/southbridge/intel/lynxpoint/smihandler.c b/src/southbridge/intel/lynxpoint/smihandler.c
index e3b0f3c25c..aebc038189 100644
--- a/src/southbridge/intel/lynxpoint/smihandler.c
+++ b/src/southbridge/intel/lynxpoint/smihandler.c
@@ -76,13 +76,9 @@ static void busmaster_disable_on_bus(int bus)
 	}
 }
 
-static void southbridge_smi_sleep(void)
+static int power_on_after_fail(void)
 {
-	u8 reg8;
-	u32 reg32;
-	u8 slp_typ;
 	u8 s5pwr = CONFIG_MAINBOARD_POWER_FAILURE_STATE;
-	u16 pmbase = get_pmbase();
 
 	/* save and recover RTC port values */
 	u8 tmp70, tmp72;
@@ -92,6 +88,16 @@ static void southbridge_smi_sleep(void)
 	outb(tmp70, 0x70);
 	outb(tmp72, 0x72);
 
+	/* For "KEEP", switch to "OFF" - KEEP is software emulated. */
+	return (s5pwr == MAINBOARD_POWER_ON);
+}
+
+static void southbridge_smi_sleep(void)
+{
+	u32 reg32;
+	u8 slp_typ;
+	u16 pmbase = get_pmbase();
+
 	/* First, disable further SMIs */
 	disable_smi(SLP_SMI_EN);
 
@@ -139,15 +145,11 @@ static void southbridge_smi_sleep(void)
 		/* Disable all GPE */
 		disable_all_gpe();
 
-		/* Always set the flag in case CMOS was changed on runtime. For
-		 * "KEEP", switch to "OFF" - KEEP is software emulated
-		 */
-		reg8 = pci_read_config8(PCH_LPC_DEV, GEN_PMCON_3);
-		if (s5pwr == MAINBOARD_POWER_ON)
-			reg8 &= ~1;
+		/* Always set the flag in case CMOS was changed on runtime. */
+		if (power_on_after_fail())
+			pci_and_config8(PCH_LPC_DEV, GEN_PMCON_3, ~1);
 		else
-			reg8 |= 1;
-		pci_write_config8(PCH_LPC_DEV, GEN_PMCON_3, reg8);
+			pci_or_config8(PCH_LPC_DEV, GEN_PMCON_3, 1);
 
 		/* also iterates over all bridges on bus 0 */
 		busmaster_disable_on_bus(0);