Merge sandy/ivybridge romstage flow for MRC and non-MRC.

Change-Id: I11e09804ed1d8a7ae8b8d4502bd18f6be933f9fa
Signed-off-by: Vladimir Serbinenko <phcoder@gmail.com>
Reviewed-on: https://review.coreboot.org/13656
Tested-by: build bot (Jenkins)
Reviewed-by: Alexandru Gagniuc <mr.nuke.me@gmail.com>
Reviewed-by: Martin Roth <martinroth@google.com>

31 files changed, 270 insertions, 702 deletions

diff --git a/src/mainboard/apple/macbookair4_2/early_southbridge.c b/src/mainboard/apple/macbookair4_2/early_southbridge.c
index 67f89db0e3..e2a785cb7e 100644
--- a/src/mainboard/apple/macbookair4_2/early_southbridge.c
+++ b/src/mainboard/apple/macbookair4_2/early_southbridge.c
@@ -55,6 +55,10 @@ const struct southbridge_usb_port mainboard_usb_ports[] = {
 void mainboard_early_init(int s3resume) {
 }
 
+void mainboard_config_superio(void)
+{
+}
+
 void mainboard_get_spd(spd_raw_data *spd)
 {
 	void *spd_file;
diff --git a/src/mainboard/gigabyte/ga-b75m-d3h/romstage.c b/src/mainboard/gigabyte/ga-b75m-d3h/romstage.c
index ff85ce1795..43dc6dddc8 100644
--- a/src/mainboard/gigabyte/ga-b75m-d3h/romstage.c
+++ b/src/mainboard/gigabyte/ga-b75m-d3h/romstage.c
@@ -208,3 +208,7 @@ static void dmi_config(void)
 
 void mainboard_early_init(int s3resume) {
 }
+
+void mainboard_config_superio(void)
+{
+}
diff --git a/src/mainboard/gigabyte/ga-b75m-d3v/romstage.c b/src/mainboard/gigabyte/ga-b75m-d3v/romstage.c
index 436f82e625..afb3f9f5ff 100644
--- a/src/mainboard/gigabyte/ga-b75m-d3v/romstage.c
+++ b/src/mainboard/gigabyte/ga-b75m-d3v/romstage.c
@@ -118,3 +118,7 @@ void mainboard_get_spd(spd_raw_data *spd) {
 
 void mainboard_early_init(int s3resume) {
 }
+
+void mainboard_config_superio(void)
+{
+}
diff --git a/src/mainboard/google/butterfly/romstage.c b/src/mainboard/google/butterfly/romstage.c
index 40b5e762dc..65a294c555 100644
--- a/src/mainboard/google/butterfly/romstage.c
+++ b/src/mainboard/google/butterfly/romstage.c
@@ -131,3 +131,7 @@ void mainboard_get_spd(spd_raw_data *spd) {
 
 void mainboard_early_init(int s3resume) {
 }
+
+void mainboard_config_superio(void)
+{
+}
diff --git a/src/mainboard/google/link/gpio.h b/src/mainboard/google/link/gpio.h
index 1dab97e75e..ea6110e8f9 100644
--- a/src/mainboard/google/link/gpio.h
+++ b/src/mainboard/google/link/gpio.h
@@ -98,7 +98,7 @@ const struct pch_gpio_set3 pch_gpio_set3_direction = {
 const struct pch_gpio_set3 pch_gpio_set3_level = {
 };
 
-const struct pch_gpio_map link_gpio_map = {
+const struct pch_gpio_map mainboard_gpio_map = {
 	.set1 = {
 		.mode      = &pch_gpio_set1_mode,
 		.direction = &pch_gpio_set1_direction,
diff --git a/src/mainboard/google/link/romstage.c b/src/mainboard/google/link/romstage.c
index a0970df562..2f40ba5186 100644
--- a/src/mainboard/google/link/romstage.c
+++ b/src/mainboard/google/link/romstage.c
@@ -33,7 +33,6 @@
 #include <southbridge/intel/bd82x6x/gpio.h>
 #include "ec/google/chromeec/ec.h"
 #include <arch/cpu.h>
-#include <cpu/x86/bist.h>
 #include <cpu/x86/msr.h>
 #include <halt.h>
 #include "gpio.h"
@@ -42,7 +41,7 @@
 
 #include <southbridge/intel/bd82x6x/chip.h>
 
-static void pch_enable_lpc(void)
+void pch_enable_lpc(void)
 {
 	const struct device *lpc;
 	const struct southbridge_intel_bd82x6x_config *config = NULL;
@@ -68,7 +67,7 @@ static void pch_enable_lpc(void)
 	pci_write_config32(PCH_LPC_DEV, LPC_GEN4_DEC, config->gen4_dec);
 }
 
-static void rcba_config(void)
+void rcba_config(void)
 {
 	u32 reg32;
 
@@ -144,13 +143,9 @@ static void copy_spd(struct pei_data *peid)
 	       sizeof(peid->spd_data[0]));
 }
 
-#include <cpu/intel/romstage.h>
-void main(unsigned long bist)
+void mainboard_fill_pei_data(struct pei_data *pei_data)
 {
-	int boot_mode = 0;
-	int cbmem_was_initted;
-
-	struct pei_data pei_data = {
+	struct pei_data pei_data_template = {
 		.pei_version = PEI_VERSION,
 		.mchbar = (uintptr_t)DEFAULT_MCHBAR,
 		.dmibar = (uintptr_t)DEFAULT_DMIBAR,
@@ -195,89 +190,25 @@ void main(unsigned long bist)
 			{ 0, 4, 0x0000 }, /* P13: Empty */
 		},
 	};
+	*pei_data = pei_data_template;
+	copy_spd(pei_data);
+}
 
-	timestamp_init(get_initial_timestamp());
-	timestamp_add_now(TS_START_ROMSTAGE);
-
-	if (bist == 0)
-		enable_lapic();
-
-	pch_enable_lpc();
-
-	/* Enable GPIOs */
-	pci_write_config32(PCH_LPC_DEV, GPIO_BASE, DEFAULT_GPIOBASE|1);
-	pci_write_config8(PCH_LPC_DEV, GPIO_CNTL, 0x10);
-	setup_pch_gpios(&link_gpio_map);
-
-	/* Initialize console device(s) */
-	console_init();
-
-	/* Halt if there was a built in self test failure */
-	report_bist_failure(bist);
-
-	if (MCHBAR16(SSKPD) == 0xCAFE) {
-		printk(BIOS_DEBUG, "soft reset detected\n");
-		boot_mode = 1;
-
-		/* System is not happy after keyboard reset... */
-		printk(BIOS_DEBUG, "Issuing CF9 warm reset\n");
-		outb(0x6, 0xcf9);
-		halt();
-	}
-
-	/* Perform some early chipset initialization required
-	 * before RAM initialization can work
-	 */
-	sandybridge_early_initialization(SANDYBRIDGE_MOBILE);
-	printk(BIOS_DEBUG, "Back from sandybridge_early_initialization()\n");
-
-	boot_mode = southbridge_detect_s3_resume() ? 2 : 0;
-	if (boot_mode == 0) {
+void mainboard_early_init(int s3resume)
+{
+	if (!s3resume) {
 		/* This is the fastest way to let users know
 		 * the Intel CPU is now alive.
 		 */
 		google_chromeec_kbbacklight(100);
 	}
+}
 
-	post_code(0x38);
-	/* Enable SPD ROMs and DDR-III DRAM */
-	enable_smbus();
-
-	/* Prepare USB controller early in S3 resume */
-	if (boot_mode == 2)
-		enable_usb_bar();
-
-	post_code(0x39);
-
-	copy_spd(&pei_data);
-
-	post_code(0x3a);
-	pei_data.boot_mode = boot_mode;
-	timestamp_add_now(TS_BEFORE_INITRAM);
-	sdram_initialize(&pei_data);
-
-	timestamp_add_now(TS_AFTER_INITRAM);
-	post_code(0x3c);
-
-	rcba_config();
-	post_code(0x3d);
-
-	quick_ram_check();
-	post_code(0x3e);
-
-	cbmem_was_initted = !cbmem_recovery(boot_mode==2);
-	if (boot_mode!=2)
-		save_mrc_data(&pei_data);
-
-	if (boot_mode==2 && !cbmem_was_initted) {
-		/* Failed S3 resume, reset to come up cleanly */
-		outb(0x6, 0xcf9);
-		halt();
-	}
-	northbridge_romstage_finalize(boot_mode==2);
+int mainboard_should_reset_usb(int s3resume)
+{
+	return !s3resume;
+}
 
-	post_code(0x3f);
-	if (CONFIG_LPC_TPM) {
-		init_tpm(boot_mode == 2);
-	}
+void mainboard_config_superio(void)
+{
 }
diff --git a/src/mainboard/google/parrot/gpio.h b/src/mainboard/google/parrot/gpio.h
index 1cfd487f09..c3e3e2fbe7 100644
--- a/src/mainboard/google/parrot/gpio.h
+++ b/src/mainboard/google/parrot/gpio.h
@@ -254,7 +254,7 @@ const struct pch_gpio_set3 pch_gpio_set3_level = {
 	.gpio75 = GPIO_LEVEL_LOW,
 };
 
-const struct pch_gpio_map parrot_gpio_map = {
+const struct pch_gpio_map mainboard_gpio_map = {
 	.set1 = {
 		.mode      = &pch_gpio_set1_mode,
 		.direction = &pch_gpio_set1_direction,
diff --git a/src/mainboard/google/parrot/romstage.c b/src/mainboard/google/parrot/romstage.c
index 029805b045..60c3f010f8 100644
--- a/src/mainboard/google/parrot/romstage.c
+++ b/src/mainboard/google/parrot/romstage.c
@@ -31,7 +31,6 @@
 #include <southbridge/intel/bd82x6x/pch.h>
 #include <southbridge/intel/bd82x6x/gpio.h>
 #include <arch/cpu.h>
-#include <cpu/x86/bist.h>
 #include <cpu/x86/msr.h>
 #include <halt.h>
 #include "gpio.h"
@@ -39,7 +38,7 @@
 #include <tpm.h>
 #include "ec/compal/ene932/ec.h"
 
-static void pch_enable_lpc(void)
+void pch_enable_lpc(void)
 {
 	/* Parrot EC Decode Range Port60/64, Port62/66 */
 	/* Enable EC, PS/2 Keyboard/Mouse */
@@ -52,7 +51,7 @@ static void pch_enable_lpc(void)
 	pci_write_config32(PCH_LPC_DEV, LPC_GEN2_DEC, (68 & ~3) | 0x00040001);
 }
 
-static void rcba_config(void)
+void rcba_config(void)
 {
 	u32 reg32;
 
@@ -107,13 +106,13 @@ static void rcba_config(void)
 	RCBA32(FD) = reg32;
 }
 
-#include <cpu/intel/romstage.h>
-void main(unsigned long bist)
+void mainboard_early_init(int s3resume)
 {
-	int boot_mode = 0;
-	int cbmem_was_initted;
+}
 
-	struct pei_data pei_data = {
+void mainboard_fill_pei_data(struct pei_data *pei_data)
+{
+	struct pei_data pei_data_template = {
 		.pei_version = PEI_VERSION,
 		.mchbar = (uintptr_t)DEFAULT_MCHBAR,
 		.dmibar = (uintptr_t)DEFAULT_DMIBAR,
@@ -158,81 +157,14 @@ void main(unsigned long bist)
 			{ 0, 4, 0x0000 }, /* P13: Empty */
 		},
 	};
+	*pei_data = pei_data_template;
+}
 
-	timestamp_init(get_initial_timestamp());
-	timestamp_add_now(TS_START_ROMSTAGE);
-
-	if (bist == 0)
-		enable_lapic();
-
-	pch_enable_lpc();
-
-	/* Enable GPIOs */
-	pci_write_config32(PCH_LPC_DEV, GPIO_BASE, DEFAULT_GPIOBASE|1);
-	pci_write_config8(PCH_LPC_DEV, GPIO_CNTL, 0x10);
-	setup_pch_gpios(&parrot_gpio_map);
-
-	/* Initialize console device(s) */
-	console_init();
-
-	/* Halt if there was a built in self test failure */
-	report_bist_failure(bist);
-
-	if (MCHBAR16(SSKPD) == 0xCAFE) {
-		printk(BIOS_DEBUG, "soft reset detected\n");
-		boot_mode = 1;
-
-		/* System is not happy after keyboard reset... */
-		printk(BIOS_DEBUG, "Issuing CF9 warm reset\n");
-		outb(0x6, 0xcf9);
-		halt();
-	}
-
-	/* Perform some early chipset initialization required
-	 * before RAM initialization can work
-	 */
-	sandybridge_early_initialization(SANDYBRIDGE_MOBILE);
-	printk(BIOS_DEBUG, "Back from sandybridge_early_initialization()\n");
-
-	boot_mode = southbridge_detect_s3_resume() ? 2 : 0;
-
-	post_code(0x38);
-	/* Enable SPD ROMs and DDR-III DRAM */
-	enable_smbus();
-
-	/* Prepare USB controller early in S3 resume */
-	if (boot_mode == 2)
-		enable_usb_bar();
-
-	post_code(0x39);
-
-	post_code(0x3a);
-	pei_data.boot_mode = boot_mode;
-	timestamp_add_now(TS_BEFORE_INITRAM);
-	sdram_initialize(&pei_data);
-
-	timestamp_add_now(TS_AFTER_INITRAM);
-	post_code(0x3c);
-
-	rcba_config();
-	post_code(0x3d);
-
-	quick_ram_check();
-	post_code(0x3e);
-
-	cbmem_was_initted = !cbmem_recovery(boot_mode==2);
-	if (boot_mode!=2)
-		save_mrc_data(&pei_data);
-
-	if (boot_mode==2 && !cbmem_was_initted) {
-		/* Failed S3 resume, reset to come up cleanly */
-		outb(0x6, 0xcf9);
-		halt();
-	}
-	northbridge_romstage_finalize(boot_mode==2);
+void mainboard_config_superio(void)
+{
+}
 
-	post_code(0x3f);
-	if (CONFIG_LPC_TPM) {
-		init_tpm(boot_mode == 2);
-	}
+int mainboard_should_reset_usb(int s3resume)
+{
+	return !s3resume;
 }
diff --git a/src/mainboard/google/stout/gpio.h b/src/mainboard/google/stout/gpio.h
index f992013519..7fffe8b48b 100644
--- a/src/mainboard/google/stout/gpio.h
+++ b/src/mainboard/google/stout/gpio.h
@@ -267,7 +267,7 @@ const struct pch_gpio_set3 pch_gpio_set3_level = {
 	.gpio72 = GPIO_LEVEL_HIGH,
 };
 
-const struct pch_gpio_map stout_gpio_map = {
+const struct pch_gpio_map mainboard_gpio_map = {
 	.set1 = {
 		.mode      = &pch_gpio_set1_mode,
 		.direction = &pch_gpio_set1_direction,
diff --git a/src/mainboard/google/stout/romstage.c b/src/mainboard/google/stout/romstage.c
index 030dee75ff..99979e9e90 100644
--- a/src/mainboard/google/stout/romstage.c
+++ b/src/mainboard/google/stout/romstage.c
@@ -31,7 +31,6 @@
 #include <southbridge/intel/bd82x6x/pch.h>
 #include <southbridge/intel/bd82x6x/gpio.h>
 #include <arch/cpu.h>
-#include <cpu/x86/bist.h>
 #include <cpu/x86/msr.h>
 #include <halt.h>
 #include "gpio.h"
@@ -42,7 +41,7 @@
 #include "ec.h"
 #include "onboard.h"
 
-static void pch_enable_lpc(void)
+void pch_enable_lpc(void)
 {
 	/*
 	 * Enable:
@@ -58,7 +57,7 @@ static void pch_enable_lpc(void)
 	pci_write_config32(PCH_LPC_DEV, LPC_GEN1_DEC, (0x68 | 0x40001));
 }
 
-static void rcba_config(void)
+void rcba_config(void)
 {
 	u32 reg32;
 
@@ -113,13 +112,6 @@ static void rcba_config(void)
 	RCBA32(FD) = reg32;
 }
 
-// FIXME, this function is generic code that should go to sb/... or
-// nb/../early_init.c
-static void early_pch_init(void)
-{
-	// Nothing to do for stout
-}
-
 	/*
 	 * The Stout EC needs to be reset to RW mode. It is important that
 	 * the RTC_PWR_STS is not set until ramstage EC init.
@@ -148,13 +140,9 @@ static void early_ec_init(void)
 	}
 }
 
-#include <cpu/intel/romstage.h>
-void main(unsigned long bist)
+void mainboard_fill_pei_data(struct pei_data *pei_data)
 {
-	int boot_mode = 0;
-	int cbmem_was_initted;
-
-	struct pei_data pei_data = {
+	struct pei_data pei_data_template = {
 		.pei_version = PEI_VERSION,
 		.mchbar = (uintptr_t)DEFAULT_MCHBAR,
 		.dmibar = (uintptr_t)DEFAULT_DMIBAR,
@@ -204,89 +192,22 @@ void main(unsigned long bist)
 			.xhci_streams = XHCI_STREAMS,
 		},
 	};
+	*pei_data = pei_data_template;
+}
 
-	timestamp_init(get_initial_timestamp());
-	timestamp_add_now(TS_START_ROMSTAGE);
-
-	if (bist == 0)
-		enable_lapic();
-
-	pch_enable_lpc();
-
-	/* Enable GPIOs */
-	pci_write_config32(PCH_LPC_DEV, GPIO_BASE, DEFAULT_GPIOBASE|1);
-	pci_write_config8(PCH_LPC_DEV, GPIO_CNTL, 0x10);
-	setup_pch_gpios(&stout_gpio_map);
-
-	/* Initialize console device(s) */
-	console_init();
-
-	/* Halt if there was a built in self test failure */
-	report_bist_failure(bist);
-
-	if (MCHBAR16(SSKPD) == 0xCAFE) {
-		printk(BIOS_DEBUG, "soft reset detected\n");
-		boot_mode = 1;
-
-		/* System is not happy after keyboard reset... */
-		printk(BIOS_DEBUG, "Issuing CF9 warm reset\n");
-		outb(0x6, 0xcf9);
-		halt();
-	}
-
-
-	/* Perform some early chipset initialization required
-	 * before RAM initialization can work
-	 */
-	sandybridge_early_initialization(SANDYBRIDGE_MOBILE);
-	printk(BIOS_DEBUG, "Back from sandybridge_early_initialization()\n");
-
-	boot_mode = southbridge_detect_s3_resume() ? 2 : 0;
-
+void mainboard_early_init(int s3resume)
+{
 	/* Do ec reset as early as possible, but skip it on S3 resume */
-	if (boot_mode < 2)
+	if (!s3resume) {
 		early_ec_init();
-
-	post_code(0x38);
-	/* Enable SPD ROMs and DDR-III DRAM */
-	enable_smbus();
-
-	/* Prepare USB controller early in S3 resume */
-	if (boot_mode == 2)
-		enable_usb_bar();
-
-	post_code(0x39);
-
-	post_code(0x3a);
-	pei_data.boot_mode = boot_mode;
-	timestamp_add_now(TS_BEFORE_INITRAM);
-	sdram_initialize(&pei_data);
-
-	timestamp_add_now(TS_AFTER_INITRAM);
-	post_code(0x3b);
-	/* Perform some initialization that must run before stage2 */
-	early_pch_init();
-	post_code(0x3c);
-
-	rcba_config();
-	post_code(0x3d);
-
-	quick_ram_check();
-	post_code(0x3e);
-
-	cbmem_was_initted = !cbmem_recovery(boot_mode==2);
-	if (boot_mode!=2)
-		save_mrc_data(&pei_data);
-
-	if (boot_mode==2 && !cbmem_was_initted) {
-		/* Failed S3 resume, reset to come up cleanly */
-		outb(0x6, 0xcf9);
-		halt();
 	}
-	northbridge_romstage_finalize(boot_mode==2);
+}
 
-	post_code(0x3f);
-	if (CONFIG_LPC_TPM) {
-		init_tpm(boot_mode == 2);
-	}
+int mainboard_should_reset_usb(int s3resume)
+{
+	return !s3resume;
+}
+
+void mainboard_config_superio(void)
+{
 }
diff --git a/src/mainboard/intel/emeraldlake2/gpio.h b/src/mainboard/intel/emeraldlake2/gpio.h
index 81bccdfb03..37b2430aea 100644
--- a/src/mainboard/intel/emeraldlake2/gpio.h
+++ b/src/mainboard/intel/emeraldlake2/gpio.h
@@ -80,7 +80,7 @@ const struct pch_gpio_set3 pch_gpio_set3_direction = {
 const struct pch_gpio_set3 pch_gpio_set3_level = {
 };
 
-const struct pch_gpio_map emeraldlake2_gpio_map = {
+const struct pch_gpio_map mainboard_gpio_map = {
 	.set1 = {
 		.mode      = &pch_gpio_set1_mode,
 		.direction = &pch_gpio_set1_direction,
diff --git a/src/mainboard/intel/emeraldlake2/romstage.c b/src/mainboard/intel/emeraldlake2/romstage.c
index 5cf24b2030..145526a060 100644
--- a/src/mainboard/intel/emeraldlake2/romstage.c
+++ b/src/mainboard/intel/emeraldlake2/romstage.c
@@ -32,7 +32,6 @@
 #include <southbridge/intel/bd82x6x/pch.h>
 #include <southbridge/intel/bd82x6x/gpio.h>
 #include <arch/cpu.h>
-#include <cpu/x86/bist.h>
 #include <cpu/x86/msr.h>
 #include <halt.h>
 #include <tpm.h>
@@ -40,7 +39,7 @@
 
 #define SIO_PORT 0x164e
 
-static void pch_enable_lpc(void)
+void pch_enable_lpc(void)
 {
 	device_t dev = PCH_LPC_DEV;
 
@@ -64,7 +63,7 @@ static void pch_enable_lpc(void)
 	}
 }
 
-static void rcba_config(void)
+void rcba_config(void)
 {
 	u32 reg32;
 
@@ -76,19 +75,7 @@ static void rcba_config(void)
 	RCBA32(FD) = reg32;
 }
 
-// FIXME, this function is generic code that should go to sb/... or
-// nb/../early_init.c
-static void early_pch_init(void)
-{
-	u8 reg8;
-
-	// reset rtc power status
-	reg8 = pci_read_config8(PCH_LPC_DEV, 0xa4);
-	reg8 &= ~(1 << 2);
-	pci_write_config8(PCH_LPC_DEV, 0xa4, reg8);
-}
-
-static void setup_sio_gpios(void)
+void mainboard_config_superio(void)
 {
 	const u16 port = SIO_PORT;
 	const u16 runtime_port = 0x180;
@@ -121,13 +108,9 @@ static void setup_sio_gpios(void)
 	outb(0xaa, port);
 }
 
-#include <cpu/intel/romstage.h>
-void main(unsigned long bist)
+void mainboard_fill_pei_data(struct pei_data *pei_data)
 {
-	int boot_mode = 0;
-	int cbmem_was_initted;
-
-	struct pei_data pei_data = {
+	struct pei_data pei_data_template = {
 		.pei_version = PEI_VERSION,
 		.mchbar = (uintptr_t)DEFAULT_MCHBAR,
 		.dmibar = (uintptr_t)DEFAULT_DMIBAR,
@@ -170,86 +153,14 @@ void main(unsigned long bist)
 			{ 1, 5, 0x0040 }, /* P13: Back port  (OC5) */
 		},
 	};
+	*pei_data = pei_data_template;
+}
 
-	timestamp_init(get_initial_timestamp());
-	timestamp_add_now(TS_START_ROMSTAGE);
-
-	if (bist == 0)
-		enable_lapic();
-
-	pch_enable_lpc();
-
-	/* Enable GPIOs */
-	pci_write_config32(PCH_LPC_DEV, GPIO_BASE, DEFAULT_GPIOBASE|1);
-	pci_write_config8(PCH_LPC_DEV, GPIO_CNTL, 0x10);
-	setup_pch_gpios(&emeraldlake2_gpio_map);
-	setup_sio_gpios();
-
-	/* Early SuperIO setup */
-	console_init();
-
-	/* Halt if there was a built in self test failure */
-	report_bist_failure(bist);
-
-	if (MCHBAR16(SSKPD) == 0xCAFE) {
-		printk(BIOS_DEBUG, "soft reset detected\n");
-		boot_mode = 1;
-
-		/* System is not happy after keyboard reset... */
-		printk(BIOS_DEBUG, "Issuing CF9 warm reset\n");
-		outb(0x6, 0xcf9);
-		halt();
-	}
-
-	/* Perform some early chipset initialization required
-	 * before RAM initialization can work
-	 */
-	sandybridge_early_initialization(SANDYBRIDGE_MOBILE);
-	printk(BIOS_DEBUG, "Back from sandybridge_early_initialization()\n");
-
-	boot_mode = southbridge_detect_s3_resume() ? 2 : 0;
-
-	post_code(0x38);
-	/* Enable SPD ROMs and DDR-III DRAM */
-	enable_smbus();
-
-	/* Prepare USB controller early in S3 resume */
-	if (boot_mode == 2)
-		enable_usb_bar();
-
-	post_code(0x3a);
-	pei_data.boot_mode = boot_mode;
-	timestamp_add_now(TS_BEFORE_INITRAM);
-	sdram_initialize(&pei_data);
-
-	timestamp_add_now(TS_AFTER_INITRAM);
-	post_code(0x3b);
-	/* Perform some initialization that must run before stage2 */
-	early_pch_init();
-	post_code(0x3c);
-
-	/* This should probably go away. Until now it is required
-	 * and mainboard specific
-	 */
-	rcba_config();
-	post_code(0x3d);
-
-	quick_ram_check();
-	post_code(0x3e);
-
-	cbmem_was_initted = !cbmem_recovery(boot_mode==2);
-	if (boot_mode!=2)
-		save_mrc_data(&pei_data);
-
-	if (boot_mode==2 && !cbmem_was_initted) {
-		/* Failed S3 resume, reset to come up cleanly */
-		outb(0x6, 0xcf9);
-		halt();
-	}
-	northbridge_romstage_finalize(boot_mode==2);
+void mainboard_early_init(int s3resume)
+{
+}
 
-	post_code(0x3f);
-	if (CONFIG_LPC_TPM) {
-		init_tpm(boot_mode == 2);
-	}
+int mainboard_should_reset_usb(int s3resume)
+{
+	return !s3resume;
 }
diff --git a/src/mainboard/kontron/ktqm77/gpio.h b/src/mainboard/kontron/ktqm77/gpio.h
index 23139f77bd..a6c39606e5 100644
--- a/src/mainboard/kontron/ktqm77/gpio.h
+++ b/src/mainboard/kontron/ktqm77/gpio.h
@@ -279,7 +279,7 @@ const struct pch_gpio_set3 pch_gpio_set3_level = {
 	.gpio75 = GPIO_LEVEL_LOW,  /* Native */
 };
 
-const struct pch_gpio_map ktqm77_gpio_map = {
+const struct pch_gpio_map mainboard_gpio_map = {
 	.set1 = {
 		.mode		= &pch_gpio_set1_mode,
 		.direction	= &pch_gpio_set1_direction,
diff --git a/src/mainboard/kontron/ktqm77/romstage.c b/src/mainboard/kontron/ktqm77/romstage.c
index 4a9efa656f..197b460e49 100644
--- a/src/mainboard/kontron/ktqm77/romstage.c
+++ b/src/mainboard/kontron/ktqm77/romstage.c
@@ -31,12 +31,11 @@
 #include <southbridge/intel/bd82x6x/pch.h>
 #include <southbridge/intel/bd82x6x/gpio.h>
 #include <arch/cpu.h>
-#include <cpu/x86/bist.h>
 #include <cpu/x86/msr.h>
 #include <halt.h>
 #include "gpio.h"
 
-static void pch_enable_lpc(void)
+void pch_enable_lpc(void)
 {
 	/* Set COM3/COM1 decode ranges: 0x3e8/0x3f8 */
 	pci_write_config16(PCH_LPC_DEV, LPC_IO_DEC, 0x0070);
@@ -52,12 +51,10 @@ static void pch_enable_lpc(void)
 			   COMA_LPC_EN | COMB_LPC_EN);
 }
 
-static void rcba_config(void)
+void rcba_config(void)
 {
 	u32 reg32;
 
-	southbridge_configure_default_intmap();
-
 	/* Disable unused devices (board specific) */
 	reg32 = RCBA32(FD);
 	reg32 |= PCH_DISABLE_ALWAYS;
@@ -79,7 +76,7 @@ static void pnp_exit_ext_func_mode(device_t dev)
 	outb(0xaa, port);
 }
 
-static void superio_gpio_config(void)
+void mainboard_config_superio(void)
 {
 	int lvds_3v = 0; // 0 (5V) or 1 (3V3)
 	int dis_bl_inv = 1; // backlight inversion: 1 = disabled, 0 = enabled
@@ -100,13 +97,9 @@ static void superio_gpio_config(void)
 	pnp_exit_ext_func_mode(dev);
 }
 
-#include <cpu/intel/romstage.h>
-void main(unsigned long bist)
+void mainboard_fill_pei_data(struct pei_data *pei_data)
 {
-	int boot_mode = 0;
-	int cbmem_was_initted;
-
-	struct pei_data pei_data = {
+	struct pei_data pei_data_template = {
 		.pei_version = PEI_VERSION,
 		.mchbar = (uintptr_t)DEFAULT_MCHBAR,
 		.dmibar = (uintptr_t)DEFAULT_DMIBAR,
@@ -159,84 +152,18 @@ void main(unsigned long bist)
 		},
 		.pcie_init = 1,
 	};
+	*pei_data = pei_data_template;
+}
 
-	timestamp_init(get_initial_timestamp());
-	timestamp_add_now(TS_START_ROMSTAGE);
-
-	if (bist == 0)
-		enable_lapic();
-
-	pch_enable_lpc();
-
-	/* Enable GPIOs */
-	pci_write_config32(PCH_LPC_DEV, GPIO_BASE, DEFAULT_GPIOBASE|1);
-	pci_write_config8(PCH_LPC_DEV, GPIO_CNTL, 0x10);
-	setup_pch_gpios(&ktqm77_gpio_map);
-	superio_gpio_config();
-
-	/* Initialize console device(s) */
-	console_init();
-
-	/* Halt if there was a built in self test failure */
-	report_bist_failure(bist);
-
-	if (MCHBAR16(SSKPD) == 0xCAFE) {
-		printk(BIOS_DEBUG, "soft reset detected\n");
-		boot_mode = 1;
-
-		/* System is not happy after keyboard reset... */
-		printk(BIOS_DEBUG, "Issuing CF9 warm reset\n");
-		outb(0x6, 0xcf9);
-		halt();
-	}
-
-	/* Perform some early chipset initialization required
-	 * before RAM initialization can work
-	 */
-	sandybridge_early_initialization(SANDYBRIDGE_MOBILE);
-	printk(BIOS_DEBUG, "Back from sandybridge_early_initialization()\n");
-
+void mainboard_early_init(int s3resume)
+{
 	/* Enable PEG10 (1x16) */
 	pci_write_config32(PCI_DEV(0, 0, 0), DEVEN,
 			   pci_read_config32(PCI_DEV(0, 0, 0), DEVEN) |
 			   DEVEN_PEG10);
+}
 
-	boot_mode = southbridge_detect_s3_resume() ? 2 : 0;
-
-	post_code(0x38);
-	/* Enable SPD ROMs and DDR-III DRAM */
-	enable_smbus();
-
-	/* Prepare USB controller early in S3 resume */
-	if (boot_mode == 2)
-		enable_usb_bar();
-
-	post_code(0x39);
-
-	post_code(0x3a);
-	pei_data.boot_mode = boot_mode;
-	timestamp_add_now(TS_BEFORE_INITRAM);
-	sdram_initialize(&pei_data);
-
-	timestamp_add_now(TS_AFTER_INITRAM);
-	post_code(0x3c);
-
-	rcba_config();
-	post_code(0x3d);
-
-	quick_ram_check();
-	post_code(0x3e);
-
-	cbmem_was_initted = !cbmem_recovery(boot_mode==2);
-	if (boot_mode!=2)
-		save_mrc_data(&pei_data);
-
-	if (boot_mode==2 && !cbmem_was_initted) {
-		/* Failed S3 resume, reset to come up cleanly */
-		outb(0x6, 0xcf9);
-		halt();
-	}
-	northbridge_romstage_finalize(boot_mode==2);
-
-	post_code(0x3f);
+int mainboard_should_reset_usb(int s3resume)
+{
+	return !s3resume;
 }
diff --git a/src/mainboard/lenovo/t420s/romstage.c b/src/mainboard/lenovo/t420s/romstage.c
index c0204582a7..e5e14164e8 100644
--- a/src/mainboard/lenovo/t420s/romstage.c
+++ b/src/mainboard/lenovo/t420s/romstage.c
@@ -70,3 +70,7 @@ void mainboard_get_spd(spd_raw_data *spd) {
 
 void mainboard_early_init(int s3resume) {
 }
+
+void mainboard_config_superio(void)
+{
+}
diff --git a/src/mainboard/lenovo/t430s/romstage.c b/src/mainboard/lenovo/t430s/romstage.c
index 4a99b6d7d6..d1bcc3bf1d 100644
--- a/src/mainboard/lenovo/t430s/romstage.c
+++ b/src/mainboard/lenovo/t430s/romstage.c
@@ -70,3 +70,7 @@ void mainboard_get_spd(spd_raw_data *spd) {
 
 void mainboard_early_init(int s3resume) {
 }
+
+void mainboard_config_superio(void)
+{
+}
diff --git a/src/mainboard/lenovo/t520/romstage.c b/src/mainboard/lenovo/t520/romstage.c
index 59bad9a1bf..4c41665333 100644
--- a/src/mainboard/lenovo/t520/romstage.c
+++ b/src/mainboard/lenovo/t520/romstage.c
@@ -85,3 +85,7 @@ void mainboard_get_spd(spd_raw_data *spd) {
 
 void mainboard_early_init(int s3resume) {
 }
+
+void mainboard_config_superio(void)
+{
+}
diff --git a/src/mainboard/lenovo/t530/romstage.c b/src/mainboard/lenovo/t530/romstage.c
index 23f2704391..7bbb2a8b15 100644
--- a/src/mainboard/lenovo/t530/romstage.c
+++ b/src/mainboard/lenovo/t530/romstage.c
@@ -72,3 +72,7 @@ void mainboard_get_spd(spd_raw_data *spd) {
 
 void mainboard_early_init(int s3resume) {
 }
+
+void mainboard_config_superio(void)
+{
+}
diff --git a/src/mainboard/lenovo/x220/romstage.c b/src/mainboard/lenovo/x220/romstage.c
index 59b3728665..db227829e1 100644
--- a/src/mainboard/lenovo/x220/romstage.c
+++ b/src/mainboard/lenovo/x220/romstage.c
@@ -82,3 +82,7 @@ void mainboard_get_spd(spd_raw_data *spd) {
 
 void mainboard_early_init(int s3resume) {
 }
+
+void mainboard_config_superio(void)
+{
+}
diff --git a/src/mainboard/lenovo/x230/romstage.c b/src/mainboard/lenovo/x230/romstage.c
index cf3eb5a76b..9aa37e330c 100644
--- a/src/mainboard/lenovo/x230/romstage.c
+++ b/src/mainboard/lenovo/x230/romstage.c
@@ -85,3 +85,7 @@ void mainboard_get_spd(spd_raw_data *spd) {
 
 void mainboard_early_init(int s3resume) {
 }
+
+void mainboard_config_superio(void)
+{
+}
diff --git a/src/mainboard/samsung/lumpy/gpio.h b/src/mainboard/samsung/lumpy/gpio.h
index 58b61b6fe1..e5737bb0f8 100644
--- a/src/mainboard/samsung/lumpy/gpio.h
+++ b/src/mainboard/samsung/lumpy/gpio.h
@@ -309,7 +309,7 @@ const struct pch_gpio_set2 pch_gpio_set2_reset = {
 	.gpio43 = GPIO_RESET_RSMRST,
 };
 
-const struct pch_gpio_map lumpy_gpio_map = {
+const struct pch_gpio_map mainboard_gpio_map = {
 	.set1 = {
 		.mode      = &pch_gpio_set1_mode,
 		.direction = &pch_gpio_set1_direction,
diff --git a/src/mainboard/samsung/lumpy/romstage.c b/src/mainboard/samsung/lumpy/romstage.c
index 5f37583fc7..b91573bb42 100644
--- a/src/mainboard/samsung/lumpy/romstage.c
+++ b/src/mainboard/samsung/lumpy/romstage.c
@@ -34,7 +34,6 @@
 #include <southbridge/intel/bd82x6x/pch.h>
 #include <southbridge/intel/bd82x6x/gpio.h>
 #include <arch/cpu.h>
-#include <cpu/x86/bist.h>
 #include <cpu/x86/msr.h>
 #include <halt.h>
 #include "option_table.h"
@@ -43,7 +42,7 @@
 #include <superio/smsc/lpc47n207/lpc47n207.h>
 #endif
 
-static void pch_enable_lpc(void)
+void pch_enable_lpc(void)
 {
 	/* Set COM1/COM2 decode range */
 	pci_write_config16(PCH_LPC_DEV, LPC_IO_DEC, 0x0010);
@@ -64,7 +63,7 @@ static void pch_enable_lpc(void)
 #endif
 }
 
-static void rcba_config(void)
+void rcba_config(void)
 {
 	u32 reg32;
 
@@ -115,23 +114,9 @@ static void rcba_config(void)
 	RCBA32(FD) = reg32;
 }
 
-static void early_pch_init(void)
+void mainboard_fill_pei_data(struct pei_data *pei_data)
 {
-	u8 reg8;
-
-	// reset rtc power status
-	reg8 = pci_read_config8(PCH_LPC_DEV, 0xa4);
-	reg8 &= ~(1 << 2);
-	pci_write_config8(PCH_LPC_DEV, 0xa4, reg8);
-}
-
-#include <cpu/intel/romstage.h>
-void main(unsigned long bist)
-{
-	int boot_mode = 0;
-	int cbmem_was_initted;
-
-	struct pei_data pei_data = {
+	struct pei_data pei_data_template = {
 		.pei_version = PEI_VERSION,
 		.mchbar = (uintptr_t)DEFAULT_MCHBAR,
 		.dmibar = (uintptr_t)DEFAULT_DMIBAR,
@@ -174,58 +159,11 @@ void main(unsigned long bist)
 			{ 0, 4, 0x0000 }, /* P13: Empty */
 		},
 	};
-
+	*pei_data = pei_data_template;
 	typedef const uint8_t spd_blob[256];
 	spd_blob *spd_data;
 	size_t spd_file_len;
 
-
-	timestamp_init(get_initial_timestamp());
-	timestamp_add_now(TS_START_ROMSTAGE);
-
-	if (bist == 0)
-		enable_lapic();
-
-	pch_enable_lpc();
-
-	/* Enable GPIOs */
-	pci_write_config32(PCH_LPC_DEV, GPIO_BASE, DEFAULT_GPIOBASE|1);
-	pci_write_config8(PCH_LPC_DEV, GPIO_CNTL, 0x10);
-	setup_pch_gpios(&lumpy_gpio_map);
-
-	console_init();
-
-	init_bootmode_straps();
-
-	/* Halt if there was a built in self test failure */
-	report_bist_failure(bist);
-
-	if (MCHBAR16(SSKPD) == 0xCAFE) {
-		printk(BIOS_DEBUG, "soft reset detected\n");
-		boot_mode = 1;
-
-		/* System is not happy after keyboard reset... */
-		printk(BIOS_DEBUG, "Issuing CF9 warm reset\n");
-		outb(0x6, 0xcf9);
-		halt();
-	}
-
-	/* Perform some early chipset initialization required
-	 * before RAM initialization can work
-	 */
-	sandybridge_early_initialization(SANDYBRIDGE_MOBILE);
-	printk(BIOS_DEBUG, "Back from sandybridge_early_initialization()\n");
-
-	boot_mode = southbridge_detect_s3_resume() ? 2 : 0;
-
-	post_code(0x38);
-	/* Enable SPD ROMs and DDR-III DRAM */
-	enable_smbus();
-
-	/* Prepare USB controller early in S3 resume */
-	if (boot_mode == 2)
-		enable_usb_bar();
-
 	u32 gp_lvl2 = inl(DEFAULT_GPIOBASE + 0x38);
 	u8 gpio33, gpio41, gpio49;
 	gpio33 = (gp_lvl2 >> (33-32)) & 1;
@@ -271,37 +209,19 @@ void main(unsigned long bist)
 	if (spd_file_len < (spd_index + 1) * 256)
 		die("Missing SPD data.");
 	// leave onboard dimm address at f0, and copy spd data there.
-	memcpy(pei_data.spd_data[0], spd_data[spd_index], 256);
-
-	post_code(0x39);
-	pei_data.boot_mode = boot_mode;
-	timestamp_add_now(TS_BEFORE_INITRAM);
-	sdram_initialize(&pei_data);
-
-	timestamp_add_now(TS_AFTER_INITRAM);
-	post_code(0x3a);
-	/* Perform some initialization that must run before stage2 */
-	early_pch_init();
-	post_code(0x3b);
-
-	rcba_config();
-	post_code(0x3c);
+	memcpy(pei_data->spd_data[0], spd_data[spd_index], 256);
+}
 
-	quick_ram_check();
-	post_code(0x3e);
+void mainboard_early_init(int s3resume)
+{
+	init_bootmode_straps();
+}
 
-	cbmem_was_initted = !cbmem_recovery(boot_mode==2);
-	if (boot_mode!=2)
-		save_mrc_data(&pei_data);
+int mainboard_should_reset_usb(int s3resume)
+{
+	return !s3resume;
+}
 
-	if (boot_mode == 2 && !cbmem_was_initted) {
-		/* Failed S3 resume, reset to come up cleanly */
-		outb(0x6, 0xcf9);
-		halt();
-	}
-	northbridge_romstage_finalize(boot_mode==2);
-	post_code(0x3f);
-	if (CONFIG_LPC_TPM) {
-		init_tpm(boot_mode == 2);
-	}
+void mainboard_config_superio(void)
+{
 }
diff --git a/src/mainboard/samsung/stumpy/gpio.h b/src/mainboard/samsung/stumpy/gpio.h
index 74d095b0da..13711558ef 100644
--- a/src/mainboard/samsung/stumpy/gpio.h
+++ b/src/mainboard/samsung/stumpy/gpio.h
@@ -285,7 +285,7 @@ const struct pch_gpio_set3 pch_gpio_set3_level = {
 	.gpio75 = GPIO_LEVEL_LOW,
 };
 
-const struct pch_gpio_map stumpy_gpio_map = {
+const struct pch_gpio_map mainboard_gpio_map = {
 	.set1 = {
 		.mode      = &pch_gpio_set1_mode,
 		.direction = &pch_gpio_set1_direction,
diff --git a/src/mainboard/samsung/stumpy/romstage.c b/src/mainboard/samsung/stumpy/romstage.c
index bf1ddb37be..2caf23e86e 100644
--- a/src/mainboard/samsung/stumpy/romstage.c
+++ b/src/mainboard/samsung/stumpy/romstage.c
@@ -34,7 +34,6 @@
 #include <southbridge/intel/bd82x6x/pch.h>
 #include <southbridge/intel/bd82x6x/gpio.h>
 #include <arch/cpu.h>
-#include <cpu/x86/bist.h>
 #include <cpu/x86/msr.h>
 #include <halt.h>
 #include <tpm.h>
@@ -56,7 +55,7 @@
 #define SERIAL_DEV PNP_DEV(0x2e, IT8772F_SP1)
 #define GPIO_DEV PNP_DEV(0x2e, IT8772F_GPIO)
 
-static void pch_enable_lpc(void)
+void pch_enable_lpc(void)
 {
 	/* Set COM1/COM2 decode range */
 	pci_write_config16(PCH_LPC_DEV, LPC_IO_DEC, 0x0010);
@@ -76,7 +75,7 @@ static void pch_enable_lpc(void)
 #endif
 }
 
-static void rcba_config(void)
+void rcba_config(void)
 {
 	u32 reg32;
 
@@ -124,16 +123,6 @@ static void rcba_config(void)
 	RCBA32(FD) = reg32;
 }
 
-static void early_pch_init(void)
-{
-	u8 reg8;
-
-	// reset rtc power status
-	reg8 = pci_read_config8(PCH_LPC_DEV, 0xa4);
-	reg8 &= ~(1 << 2);
-	pci_write_config8(PCH_LPC_DEV, 0xa4, reg8);
-}
-
 static void setup_sio_gpios(void)
 {
 	/*
@@ -168,13 +157,9 @@ static void setup_sio_gpios(void)
 	it8772f_gpio_setup(DUMMY_DEV, 6, 0x00, 0x00, 0x00, 0x00, 0x00);
 }
 
-#include <cpu/intel/romstage.h>
-void main(unsigned long bist)
+void mainboard_fill_pei_data(struct pei_data *pei_data)
 {
-	int boot_mode = 0;
-	int cbmem_was_initted;
-
-	struct pei_data pei_data = {
+	struct pei_data pei_data_template = {
 		.pei_version = PEI_VERSION,
 		.mchbar = (uintptr_t)DEFAULT_MCHBAR,
 		.dmibar = (uintptr_t)DEFAULT_DMIBAR,
@@ -217,56 +202,17 @@ void main(unsigned long bist)
 			{ 1, 5, 0x0040 }, /* P13: Back port  (OC5) */
 		},
 	};
+	*pei_data = pei_data_template;
+}
 
-	timestamp_init(get_initial_timestamp());
-	timestamp_add_now(TS_START_ROMSTAGE);
-
-	if (bist == 0)
-		enable_lapic();
-
-	pch_enable_lpc();
-
-	/* Enable GPIOs */
-	pci_write_config32(PCH_LPC_DEV, GPIO_BASE, DEFAULT_GPIOBASE|1);
-	pci_write_config8(PCH_LPC_DEV, GPIO_CNTL, 0x10);
-	setup_pch_gpios(&stumpy_gpio_map);
-	setup_sio_gpios();
-
-	/* Early SuperIO setup */
-	it8772f_ac_resume_southbridge(DUMMY_DEV);
-	ite_kill_watchdog(GPIO_DEV);
-	ite_enable_serial(SERIAL_DEV, CONFIG_TTYS0_BASE);
-	console_init();
-
+void mainboard_early_init(int s3resume)
+{
 	init_bootmode_straps();
+}
 
-	/* Halt if there was a built in self test failure */
-	report_bist_failure(bist);
-
-	if (MCHBAR16(SSKPD) == 0xCAFE) {
-		printk(BIOS_DEBUG, "soft reset detected\n");
-		boot_mode = 1;
-
-		/* System is not happy after keyboard reset... */
-		printk(BIOS_DEBUG, "Issuing CF9 warm reset\n");
-		outb(0x6, 0xcf9);
-		halt();
-	}
-
-	/* Perform some early chipset initialization required
-	 * before RAM initialization can work
-	 */
-	sandybridge_early_initialization(SANDYBRIDGE_MOBILE);
-	printk(BIOS_DEBUG, "Back from sandybridge_early_initialization()\n");
-
-	boot_mode = southbridge_detect_s3_resume() ? 2 : 0;
-
-	post_code(0x38);
-	/* Enable SPD ROMs and DDR-III DRAM */
-	enable_smbus();
-
-	/* Prepare USB controller early in S3 resume */
-	if (boot_mode == 2) {
+int mainboard_should_reset_usb(int s3resume)
+{
+	if (s3resume) {
 		/*
 		 * For Stumpy the back USB ports are reset on resume
 		 * so default to resetting the controller to make the
@@ -275,48 +221,26 @@ void main(unsigned long bist)
 		 * the device power loss better in the future.
 		 */
 		u8 magic = cmos_read(CMOS_USB_RESET_DISABLE);
-
 		if (magic == USB_RESET_DISABLE_MAGIC) {
 			printk(BIOS_DEBUG, "USB Controller Reset Disabled\n");
-			enable_usb_bar();
+			return 0;
 		} else {
 			printk(BIOS_DEBUG, "USB Controller Reset Enabled\n");
+			return 1;
 		}
 	} else {
 		/* Ensure USB reset on resume is enabled at boot */
 		cmos_write(0, CMOS_USB_RESET_DISABLE);
+		return 1;
 	}
+}
 
-	post_code(0x39);
-	pei_data.boot_mode = boot_mode;
-	timestamp_add_now(TS_BEFORE_INITRAM);
-	sdram_initialize(&pei_data);
-
-	timestamp_add_now(TS_AFTER_INITRAM);
-	post_code(0x3a);
-	/* Perform some initialization that must run before stage2 */
-	early_pch_init();
-	post_code(0x3b);
-
-	rcba_config();
-	post_code(0x3c);
-
-	quick_ram_check();
-	post_code(0x3e);
-
-	cbmem_was_initted = !cbmem_recovery(boot_mode==2);
-	if (boot_mode!=2)
-		save_mrc_data(&pei_data);
-
-	if (boot_mode==2 && !cbmem_was_initted) {
-		/* Failed S3 resume, reset to come up cleanly */
-		outb(0x6, 0xcf9);
-		halt();
-	}
-	northbridge_romstage_finalize(boot_mode==2);
+void mainboard_config_superio(void)
+{
+	setup_sio_gpios();
 
-	post_code(0x3f);
-	if (CONFIG_LPC_TPM) {
-		init_tpm(boot_mode == 2);
-	}
+	/* Early SuperIO setup */
+	it8772f_ac_resume_southbridge(DUMMY_DEV);
+	ite_kill_watchdog(GPIO_DEV);
+	ite_enable_serial(SERIAL_DEV, CONFIG_TTYS0_BASE);
 }
diff --git a/src/northbridge/intel/sandybridge/Makefile.inc b/src/northbridge/intel/sandybridge/Makefile.inc
index 2d68c03d87..90abe4dab6 100644
--- a/src/northbridge/intel/sandybridge/Makefile.inc
+++ b/src/northbridge/intel/sandybridge/Makefile.inc
@@ -28,11 +28,10 @@ romstage-y += ram_calc.c
 romstage-$(CONFIG_NORTHBRIDGE_INTEL_IVYBRIDGE_MRC) += raminit_mrc.c
 romstage-$(CONFIG_NORTHBRIDGE_INTEL_SANDYBRIDGE_MRC) += raminit_mrc.c
 romstage-$(CONFIG_NORTHBRIDGE_INTEL_IVYBRIDGE) += raminit.c
-romstage-$(CONFIG_NORTHBRIDGE_INTEL_IVYBRIDGE) += romstage.c
 romstage-$(CONFIG_NORTHBRIDGE_INTEL_IVYBRIDGE) += ../../../device/dram/ddr3.c
 romstage-$(CONFIG_NORTHBRIDGE_INTEL_SANDYBRIDGE) += raminit.c
-romstage-$(CONFIG_NORTHBRIDGE_INTEL_SANDYBRIDGE) += romstage.c
 romstage-$(CONFIG_NORTHBRIDGE_INTEL_SANDYBRIDGE) += ../../../device/dram/ddr3.c
+romstage-y += romstage.c
 romstage-y += mrccache.c
 romstage-y += iommu.c
 romstage-y += early_init.c
diff --git a/src/northbridge/intel/sandybridge/raminit.c b/src/northbridge/intel/sandybridge/raminit.c
index aae963e239..7490ff7f29 100644
--- a/src/northbridge/intel/sandybridge/raminit.c
+++ b/src/northbridge/intel/sandybridge/raminit.c
@@ -24,12 +24,14 @@
 #include <cbfs.h>
 #include <halt.h>
 #include <ip_checksum.h>
+#include <timestamp.h>
 #include <pc80/mc146818rtc.h>
 #include <device/pci_def.h>
 #include "raminit_native.h"
 #include "sandybridge.h"
 #include <delay.h>
 #include <lib.h>
+#include <device/device.h>
 
 /* Management Engine is in the southbridge */
 #include "southbridge/intel/bd82x6x/me.h"
@@ -37,6 +39,7 @@
 #include "southbridge/intel/bd82x6x/smbus.h"
 #include "arch/cpu.h"
 #include "cpu/x86/msr.h"
+#include <northbridge/intel/sandybridge/chip.h>
 
 /* FIXME: no ECC support.  */
 /* FIXME: no support for 3-channel chipsets.  */
@@ -4034,3 +4037,44 @@ void init_dram_ddr3(spd_raw_data * spds, int mobile, int min_tck,
 		halt();
 	}
 }
+
+#define HOST_BRIDGE	PCI_DEVFN(0, 0)
+#define DEFAULT_TCK	TCK_800MHZ
+
+static unsigned int get_mem_min_tck(void)
+{
+	const struct device *dev;
+	const struct northbridge_intel_sandybridge_config *cfg;
+
+	dev = dev_find_slot(0, HOST_BRIDGE);
+	if (!(dev && dev->chip_info))
+		return DEFAULT_TCK;
+
+	cfg = dev->chip_info;
+
+	/* If this is zero, it just means devicetree.cb didn't set it */
+	if (cfg->max_mem_clock_mhz == 0)
+		return DEFAULT_TCK;
+
+	if (cfg->max_mem_clock_mhz >= 800)
+		return TCK_800MHZ;
+	else if (cfg->max_mem_clock_mhz >= 666)
+		return TCK_666MHZ;
+	else if (cfg->max_mem_clock_mhz >= 533)
+		return TCK_533MHZ;
+	return TCK_400MHZ;
+}
+
+void perform_raminit(int s3resume)
+{
+	spd_raw_data spd[4];
+
+	post_code(0x3a);
+
+	memset (spd, 0, sizeof (spd));
+	mainboard_get_spd(spd);
+
+	timestamp_add_now(TS_BEFORE_INITRAM);
+
+	init_dram_ddr3(spd, 1, get_mem_min_tck(), s3resume);
+}
diff --git a/src/northbridge/intel/sandybridge/raminit.h b/src/northbridge/intel/sandybridge/raminit.h
index 5bda2ef2b6..4e684ec409 100644
--- a/src/northbridge/intel/sandybridge/raminit.h
+++ b/src/northbridge/intel/sandybridge/raminit.h
@@ -27,6 +27,7 @@ struct sys_info {
 
 void sdram_initialize(struct pei_data *pei_data);
 void save_mrc_data(struct pei_data *pei_data);
+void mainboard_fill_pei_data(struct pei_data *pei_data);
 int fixup_sandybridge_errata(void);
 
 #endif				/* RAMINIT_H */
diff --git a/src/northbridge/intel/sandybridge/raminit_mrc.c b/src/northbridge/intel/sandybridge/raminit_mrc.c
index a370eccd31..162caf6107 100644
--- a/src/northbridge/intel/sandybridge/raminit_mrc.c
+++ b/src/northbridge/intel/sandybridge/raminit_mrc.c
@@ -25,6 +25,7 @@
 #include <pc80/mc146818rtc.h>
 #include <device/pci_def.h>
 #include <halt.h>
+#include <timestamp.h>
 #include "raminit.h"
 #include "pei_data.h"
 #include "sandybridge.h"
@@ -279,3 +280,29 @@ void sdram_initialize(struct pei_data *pei_data)
 
 	report_memory_config();
 }
+
+void perform_raminit(int s3resume)
+{
+	int cbmem_was_initted;
+	struct pei_data pei_data;
+
+	/* Prepare USB controller early in S3 resume */
+	if (!mainboard_should_reset_usb(s3resume))
+		enable_usb_bar();
+
+	mainboard_fill_pei_data(&pei_data);
+
+	post_code(0x3a);
+	pei_data.boot_mode = s3resume ? 2 : 0;
+	timestamp_add_now(TS_BEFORE_INITRAM);
+	sdram_initialize(&pei_data);
+	cbmem_was_initted = !cbmem_recovery(s3resume);
+	if (!s3resume)
+		save_mrc_data(&pei_data);
+
+	if (s3resume && !cbmem_was_initted) {
+		/* Failed S3 resume, reset to come up cleanly */
+		outb(0x6, 0xcf9);
+		halt();
+	}
+}
diff --git a/src/northbridge/intel/sandybridge/raminit_native.h b/src/northbridge/intel/sandybridge/raminit_native.h
index b41aa855aa..8f8d057313 100644
--- a/src/northbridge/intel/sandybridge/raminit_native.h
+++ b/src/northbridge/intel/sandybridge/raminit_native.h
@@ -13,17 +13,15 @@
  * GNU General Public License for more details.
  */
 
-#ifndef RAMINIT_H
-#define RAMINIT_H
+#ifndef RAMINIT_NATIVE_H
+#define RAMINIT_NATIVE_H
 
+#include "sandybridge.h"
 #include <device/dram/ddr3.h>
 
 /* The order is ch0dimmA, ch0dimmB, ch1dimmA, ch1dimmB.  */
 void init_dram_ddr3(spd_raw_data *spds, int mobile, int min_tck, int s3resume);
 void read_spd(spd_raw_data *spd, u8 addr);
 void mainboard_get_spd(spd_raw_data *spd);
-void rcba_config(void);
-void pch_enable_lpc(void);
-void mainboard_early_init(int s3resume);
 
 #endif				/* RAMINIT_H */
diff --git a/src/northbridge/intel/sandybridge/romstage.c b/src/northbridge/intel/sandybridge/romstage.c
index 3d05f8e3fa..04db608f83 100644
--- a/src/northbridge/intel/sandybridge/romstage.c
+++ b/src/northbridge/intel/sandybridge/romstage.c
@@ -29,43 +29,23 @@
 #include <device/device.h>
 #include <halt.h>
 #include <tpm.h>
-#include "raminit_native.h"
 #include <northbridge/intel/sandybridge/chip.h>
 #include "southbridge/intel/bd82x6x/pch.h"
 #include "southbridge/intel/bd82x6x/gpio.h"
 
-#define HOST_BRIDGE	PCI_DEVFN(0, 0)
-#define DEFAULT_TCK	TCK_800MHZ
-
-static unsigned int get_mem_min_tck(void)
+static void early_pch_init(void)
 {
-	const struct device *dev;
-	const struct northbridge_intel_sandybridge_config *cfg;
-
-	dev = dev_find_slot(0, HOST_BRIDGE);
-	if (!(dev && dev->chip_info))
-		return DEFAULT_TCK;
-
-	cfg = dev->chip_info;
-
-	/* If this is zero, it just means devicetree.cb didn't set it */
-	if (cfg->max_mem_clock_mhz == 0)
-		return DEFAULT_TCK;
-
-	if (cfg->max_mem_clock_mhz >= 800)
-		return TCK_800MHZ;
-	else if (cfg->max_mem_clock_mhz >= 666)
-		return TCK_666MHZ;
-	else if (cfg->max_mem_clock_mhz >= 533)
-		return TCK_533MHZ;
-	else
-		return TCK_400MHZ;
+	u8 reg8;
+
+	// reset rtc power status
+	reg8 = pci_read_config8(PCH_LPC_DEV, 0xa4);
+	reg8 &= ~(1 << 2);
+	pci_write_config8(PCH_LPC_DEV, 0xa4, reg8);
 }
 
 void main(unsigned long bist)
 {
 	int s3resume = 0;
-	spd_raw_data spd[4];
 
 	if (MCHBAR16(SSKPD) == 0xCAFE) {
 		outb(0x6, 0xcf9);
@@ -86,7 +66,14 @@ void main(unsigned long bist)
 
 	setup_pch_gpios(&mainboard_gpio_map);
 
-	early_usb_init(mainboard_usb_ports);
+	/* Initialize superio */
+	mainboard_config_superio();
+
+	/* USB is inited in MRC if MRC is used.  */
+	if (!(CONFIG_NORTHBRIDGE_INTEL_SANDYBRIDGE_MRC
+	      || CONFIG_NORTHBRIDGE_INTEL_IVYBRIDGE_MRC)) {
+		early_usb_init(mainboard_usb_ports);
+	}
 
 	/* Initialize console device(s) */
 	console_init();
@@ -111,27 +98,25 @@ void main(unsigned long bist)
 
 	post_code(0x39);
 
-	post_code(0x3a);
-
-	memset (spd, 0, sizeof (spd));
-	mainboard_get_spd(spd);
-
-	timestamp_add_now(TS_BEFORE_INITRAM);
-
-	init_dram_ddr3(spd, 1, get_mem_min_tck(), s3resume);
+	perform_raminit(s3resume);
 
 	timestamp_add_now(TS_AFTER_INITRAM);
+
+	post_code(0x3b);
+	/* Perform some initialization that must run before stage2 */
+	early_pch_init();
 	post_code(0x3c);
 
 	southbridge_configure_default_intmap();
 	rcba_config();
+
 	post_code(0x3d);
 
 	northbridge_romstage_finalize(s3resume);
 
-#if CONFIG_LPC_TPM
-       init_tpm(s3resume);
-#endif
+	if (CONFIG_LPC_TPM) {
+		init_tpm(s3resume);
+	}
 
 	post_code(0x3f);
 }
diff --git a/src/northbridge/intel/sandybridge/sandybridge.h b/src/northbridge/intel/sandybridge/sandybridge.h
index 0a1f20a06f..570e1f7864 100644
--- a/src/northbridge/intel/sandybridge/sandybridge.h
+++ b/src/northbridge/intel/sandybridge/sandybridge.h
@@ -215,8 +215,16 @@ void dump_pci_devices(void);
 void dump_spd_registers(void);
 void dump_mem(unsigned start, unsigned end);
 void report_platform_info(void);
+
 #endif /* !__SMM__ */
 
+void rcba_config(void);
+void pch_enable_lpc(void);
+void mainboard_early_init(int s3resume);
+void mainboard_config_superio(void);
+int mainboard_should_reset_usb(int s3resume);
+void perform_raminit(int s3resume);
+
 #if ENV_RAMSTAGE
 #include <device/device.h>