AGESA: Introduce AGESA_LEGACY_WRAPPER and its counterpart

We define AGESA_LEGACY_WRAPPER a method of calling AGESA
via functions in agesawrapper.c file. The approach implemented
there makes it very inconvenient to do board-specific
customisation or present common platform-specific features.
Seems like it also causes assertion errors on AGESA side.
The flag is applied here to all boards and then individually
removed one at a time, as things get tested.

New method is not to call AGESA internal functions directly,
but via the dispatcher. AGESA call parameters are routed to
hooks in both platform and board -directories, to allow for
easy capture or modification as needed.

For each AGESA dispatcher call made, eventlog entries are
replayed to the console log. Also relocations of AGESA heap
that took place are recorded.

New method is expected to be compatible with binaryPI.

Change-Id: Iac3d7f8b0354e9f02c2625576f36fe06b05eb4ce
Signed-off-by: Kyösti Mälkki <kyosti.malkki@gmail.com>
Reviewed-on: https://review.coreboot.org/18628
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>
Reviewed-by: Aaron Durbin <adurbin@chromium.org>
diff --git a/src/cpu/amd/agesa/Kconfig b/src/cpu/amd/agesa/Kconfig
index cd14975..e43d4b0 100644
--- a/src/cpu/amd/agesa/Kconfig
+++ b/src/cpu/amd/agesa/Kconfig
@@ -37,6 +37,10 @@
 config AGESA_LEGACY
 	def_bool n
 
+config AGESA_LEGACY_WRAPPER
+	bool
+	default AGESA_LEGACY
+
 config AGESA_NO_LEGACY
 	bool
 	default !AGESA_LEGACY
diff --git a/src/cpu/amd/agesa/Makefile.inc b/src/cpu/amd/agesa/Makefile.inc
index 19f7975..b5bb8d9 100644
--- a/src/cpu/amd/agesa/Makefile.inc
+++ b/src/cpu/amd/agesa/Makefile.inc
@@ -32,7 +32,7 @@
 
 romstage-y += heapmanager.c
 ramstage-y += heapmanager.c
-ramstage-y += amd_late_init.c
+ramstage-$(CONFIG_AGESA_LEGACY_WRAPPER) += amd_late_init.c
 
 ifeq ($(CONFIG_HAVE_ACPI_RESUME), y)
 
diff --git a/src/cpu/amd/agesa/romstage.c b/src/cpu/amd/agesa/romstage.c
index 11a62ad..4422b66 100644
--- a/src/cpu/amd/agesa/romstage.c
+++ b/src/cpu/amd/agesa/romstage.c
@@ -32,6 +32,9 @@
 {
 	memset(cb, 0, sizeof(*cb));
 	cb->s3resume = acpi_is_wakeup_s3();
+
+	if (!HAS_LEGACY_WRAPPER)
+		agesa_set_interface(cb);
 }
 
 void * asmlinkage romstage_main(unsigned long bist)
@@ -55,7 +58,22 @@
 	/* Halt if there was a built in self test failure */
 	report_bist_failure(bist);
 
-	agesa_main(cb);
+	if (!HAS_LEGACY_WRAPPER) {
+
+		agesa_execute_state(cb, AMD_INIT_RESET);
+
+		agesa_execute_state(cb, AMD_INIT_EARLY);
+
+		if (!cb->s3resume)
+			agesa_execute_state(cb, AMD_INIT_POST);
+		else
+			agesa_execute_state(cb, AMD_INIT_RESUME);
+
+	} else {
+
+		agesa_main(cb);
+
+	}
 
 	uintptr_t stack_top = CACHE_TMP_RAMTOP;
 	if (cb->s3resume) {
@@ -80,7 +98,16 @@
 	printk(BIOS_DEBUG, "CAR disabled.\n");
 
 	fill_sysinfo(cb);
-	agesa_postcar(cb);
+
+	if (!HAS_LEGACY_WRAPPER) {
+		if (!cb->s3resume)
+			agesa_execute_state(cb, AMD_INIT_ENV);
+		else
+			agesa_execute_state(cb, AMD_S3LATE_RESTORE);
+	} else {
+
+		agesa_postcar(cb);
+	}
 
 	if (cb->s3resume)
 		set_resume_cache();
diff --git a/src/mainboard/amd/olivehill/Kconfig b/src/mainboard/amd/olivehill/Kconfig
index 37fb43f..462d221 100644
--- a/src/mainboard/amd/olivehill/Kconfig
+++ b/src/mainboard/amd/olivehill/Kconfig
@@ -17,6 +17,7 @@
 
 config BOARD_SPECIFIC_OPTIONS # dummy
 	def_bool y
+	select AGESA_LEGACY_WRAPPER
 	select CPU_AMD_AGESA_FAMILY16_KB
 	select NORTHBRIDGE_AMD_AGESA_FAMILY16_KB
 	select SOUTHBRIDGE_AMD_AGESA_YANGTZE
diff --git a/src/mainboard/amd/torpedo/Kconfig b/src/mainboard/amd/torpedo/Kconfig
index 5c85c4b..348f995 100644
--- a/src/mainboard/amd/torpedo/Kconfig
+++ b/src/mainboard/amd/torpedo/Kconfig
@@ -17,6 +17,7 @@
 
 config BOARD_SPECIFIC_OPTIONS # dummy
 	def_bool y
+	select AGESA_LEGACY_WRAPPER
 	select CPU_AMD_AGESA_FAMILY12
 	select NORTHBRIDGE_AMD_AGESA_FAMILY12
 	select SOUTHBRIDGE_AMD_CIMX_SB900
diff --git a/src/mainboard/asrock/e350m1/Kconfig b/src/mainboard/asrock/e350m1/Kconfig
index 1c53212..8dc53ae 100644
--- a/src/mainboard/asrock/e350m1/Kconfig
+++ b/src/mainboard/asrock/e350m1/Kconfig
@@ -17,6 +17,7 @@
 
 config BOARD_SPECIFIC_OPTIONS # dummy
 	def_bool y
+	select AGESA_LEGACY_WRAPPER
 	select CPU_AMD_AGESA_FAMILY14
 	select NORTHBRIDGE_AMD_AGESA_FAMILY14
 	select SOUTHBRIDGE_AMD_CIMX_SB800
diff --git a/src/mainboard/asrock/imb-a180/Kconfig b/src/mainboard/asrock/imb-a180/Kconfig
index e118bfb..c50ea46 100644
--- a/src/mainboard/asrock/imb-a180/Kconfig
+++ b/src/mainboard/asrock/imb-a180/Kconfig
@@ -17,6 +17,7 @@
 
 config BOARD_SPECIFIC_OPTIONS # dummy
 	def_bool y
+	select AGESA_LEGACY_WRAPPER
 	select CPU_AMD_AGESA_FAMILY16_KB
 	select NORTHBRIDGE_AMD_AGESA_FAMILY16_KB
 	select SOUTHBRIDGE_AMD_AGESA_YANGTZE
diff --git a/src/mainboard/asus/f2a85-m/Kconfig b/src/mainboard/asus/f2a85-m/Kconfig
index f3b2d7d..651c107 100644
--- a/src/mainboard/asus/f2a85-m/Kconfig
+++ b/src/mainboard/asus/f2a85-m/Kconfig
@@ -18,6 +18,7 @@
 
 config BOARD_SPECIFIC_OPTIONS # dummy
 	def_bool y
+	select AGESA_LEGACY_WRAPPER
 	select CPU_AMD_AGESA_FAMILY15_TN
 	select NORTHBRIDGE_AMD_AGESA_FAMILY15_TN
 	select SOUTHBRIDGE_AMD_AGESA_HUDSON
diff --git a/src/mainboard/bap/ode_e20XX/Kconfig b/src/mainboard/bap/ode_e20XX/Kconfig
index 6631ac8..ea9fec5 100644
--- a/src/mainboard/bap/ode_e20XX/Kconfig
+++ b/src/mainboard/bap/ode_e20XX/Kconfig
@@ -18,6 +18,7 @@
 
 config BOARD_SPECIFIC_OPTIONS # dummy
 	def_bool y
+	select AGESA_LEGACY_WRAPPER
 	select CPU_AMD_AGESA_FAMILY16_KB
 	select NORTHBRIDGE_AMD_AGESA_FAMILY16_KB
 	select SOUTHBRIDGE_AMD_AGESA_YANGTZE
diff --git a/src/mainboard/elmex/pcm205400/Kconfig b/src/mainboard/elmex/pcm205400/Kconfig
index a901301..9da780c 100644
--- a/src/mainboard/elmex/pcm205400/Kconfig
+++ b/src/mainboard/elmex/pcm205400/Kconfig
@@ -29,6 +29,7 @@
 
 config BOARD_SPECIFIC_OPTIONS # dummy
 	def_bool y
+	select AGESA_LEGACY_WRAPPER
 	select CPU_AMD_AGESA_FAMILY14
 	select NORTHBRIDGE_AMD_AGESA_FAMILY14
 	select SOUTHBRIDGE_AMD_CIMX_SB800
diff --git a/src/mainboard/gizmosphere/gizmo/Kconfig b/src/mainboard/gizmosphere/gizmo/Kconfig
index 4177b1d..766b3b9 100644
--- a/src/mainboard/gizmosphere/gizmo/Kconfig
+++ b/src/mainboard/gizmosphere/gizmo/Kconfig
@@ -18,6 +18,7 @@
 
 config BOARD_SPECIFIC_OPTIONS # dummy
 	def_bool y
+	select AGESA_LEGACY_WRAPPER
 	select CPU_AMD_AGESA_FAMILY14
 	select NORTHBRIDGE_AMD_AGESA_FAMILY14
 	select SOUTHBRIDGE_AMD_CIMX_SB800
diff --git a/src/mainboard/lenovo/g505s/Kconfig b/src/mainboard/lenovo/g505s/Kconfig
index aac3a84..07dbd1f 100644
--- a/src/mainboard/lenovo/g505s/Kconfig
+++ b/src/mainboard/lenovo/g505s/Kconfig
@@ -18,6 +18,7 @@
 config BOARD_SPECIFIC_OPTIONS # dummy
 	def_bool y
 	select SYSTEM_TYPE_LAPTOP
+	select AGESA_LEGACY_WRAPPER
 	select CPU_AMD_AGESA_FAMILY15_RL
 	select NORTHBRIDGE_AMD_AGESA_FAMILY15_RL
 	select SOUTHBRIDGE_AMD_AGESA_HUDSON
diff --git a/src/mainboard/msi/ms7721/Kconfig b/src/mainboard/msi/ms7721/Kconfig
index 7a2623b..8187dbe 100644
--- a/src/mainboard/msi/ms7721/Kconfig
+++ b/src/mainboard/msi/ms7721/Kconfig
@@ -20,6 +20,7 @@
 
 config BOARD_SPECIFIC_OPTIONS # dummy
 	def_bool y
+	select AGESA_LEGACY_WRAPPER
 	select CPU_AMD_AGESA_FAMILY15_TN
 	select NORTHBRIDGE_AMD_AGESA_FAMILY15_TN
 	select SOUTHBRIDGE_AMD_AGESA_HUDSON
diff --git a/src/mainboard/pcengines/apu1/Kconfig b/src/mainboard/pcengines/apu1/Kconfig
index 5e927cc..8edb36c 100644
--- a/src/mainboard/pcengines/apu1/Kconfig
+++ b/src/mainboard/pcengines/apu1/Kconfig
@@ -18,6 +18,7 @@
 
 config BOARD_SPECIFIC_OPTIONS # dummy
 	def_bool y
+	select AGESA_LEGACY_WRAPPER
 	select CPU_AMD_AGESA_FAMILY14
 	select NORTHBRIDGE_AMD_AGESA_FAMILY14
 	select SOUTHBRIDGE_AMD_CIMX_SB800
diff --git a/src/northbridge/amd/agesa/Makefile.inc b/src/northbridge/amd/agesa/Makefile.inc
index fff6524..d735673 100644
--- a/src/northbridge/amd/agesa/Makefile.inc
+++ b/src/northbridge/amd/agesa/Makefile.inc
@@ -23,8 +23,16 @@
 subdirs-$(CONFIG_NORTHBRIDGE_AMD_AGESA_FAMILY15_RL) += family15rl
 subdirs-$(CONFIG_NORTHBRIDGE_AMD_AGESA_FAMILY16_KB) += family16kb
 
-romstage-y += def_callouts.c agesawrapper.c eventlog.c
-ramstage-y += def_callouts.c agesawrapper.c eventlog.c acpi_tables.c
+romstage-y += def_callouts.c eventlog.c
+ramstage-y += def_callouts.c eventlog.c acpi_tables.c
+
+ifeq ($(CONFIG_AGESA_LEGACY_WRAPPER), y)
+romstage-y += agesawrapper.c
+ramstage-y += agesawrapper.c
+else
+romstage-y += state_machine.c
+ramstage-y += state_machine.c
+endif
 
 romstage-y += oem_s3.c
 ramstage-y += oem_s3.c
diff --git a/src/northbridge/amd/agesa/acpi_tables.c b/src/northbridge/amd/agesa/acpi_tables.c
index 7edf43b..5335c02 100644
--- a/src/northbridge/amd/agesa/acpi_tables.c
+++ b/src/northbridge/amd/agesa/acpi_tables.c
@@ -14,6 +14,7 @@
  * GNU General Public License for more details.
  */
 
+#include <northbridge/amd/agesa/state_machine.h>
 #include <northbridge/amd/agesa/agesa_helper.h>
 
 #include "AGESA.h"
@@ -40,6 +41,11 @@
 	AmdLateParams = Late;
 }
 
+void completion_InitLate(struct sysinfo *cb, AMD_LATE_PARAMS *Late)
+{
+	AmdLateParams = Late;
+}
+
 void *agesawrapper_getlateinitptr(int pick)
 {
 	ASSERT(AmdLateParams != NULL);
diff --git a/src/northbridge/amd/agesa/agesawrapper.h b/src/northbridge/amd/agesa/agesawrapper.h
index ab7db33..40b5f22 100644
--- a/src/northbridge/amd/agesa/agesawrapper.h
+++ b/src/northbridge/amd/agesa/agesawrapper.h
@@ -16,6 +16,8 @@
 #ifndef _AGESAWRAPPER_H_
 #define _AGESAWRAPPER_H_
 
+#if IS_ENABLED(CONFIG_AGESA_LEGACY_WRAPPER)
+
 #include <stdint.h>
 #include "Porting.h"
 #include "AGESA.h"
@@ -49,12 +51,16 @@
 
 extern const struct OEM_HOOK OemCustomize;
 
-/* For suspend-to-ram support. */
-#if !IS_ENABLED(CONFIG_CPU_AMD_PI)
-/* TODO: With binaryPI we need different interface. */
-AGESA_STATUS OemInitResume(AMD_S3_PARAMS *dataBlock);
-AGESA_STATUS OemS3LateRestore(AMD_S3_PARAMS *dataBlock);
-AGESA_STATUS OemS3Save(AMD_S3_PARAMS *dataBlock);
-#endif
+#else
+
+/* Defined to make unused agesa_main() build. */
+static inline int agesawrapper_amdinitreset(void) { return -1; }
+static inline int agesawrapper_amdinitearly(void) { return -1; }
+static inline int agesawrapper_amdinitenv(void) { return -1; }
+static inline int agesawrapper_amdinitpost(void) { return -1; }
+static inline int agesawrapper_amdinitresume(void) { return -1; }
+static inline int agesawrapper_amds3laterestore(void) { return -1; }
+
+#endif /* IS_ENABLED(CONFIG_AGESA_LEGACY_WRAPPER) */
 
 #endif /* _AGESAWRAPPER_H_ */
diff --git a/src/northbridge/amd/agesa/def_callouts.c b/src/northbridge/amd/agesa/def_callouts.c
index 4a5f693..f786a11 100644
--- a/src/northbridge/amd/agesa/def_callouts.c
+++ b/src/northbridge/amd/agesa/def_callouts.c
@@ -108,21 +108,31 @@
 
 AGESA_STATUS agesa_RunFuncOnAp (UINT32 Func, UINTN Data, VOID *ConfigPtr)
 {
+	AMD_CONFIG_PARAMS *StdHeader = ConfigPtr;
 	AGESA_STATUS status;
 	AP_EXE_PARAMS ApExeParams;
 
 	memset(&ApExeParams, 0, sizeof(AP_EXE_PARAMS));
 
-	ApExeParams.StdHeader.AltImageBasePtr = 0;
-	ApExeParams.StdHeader.CalloutPtr = &GetBiosCallout;
-	ApExeParams.StdHeader.Func = 0;
-	ApExeParams.StdHeader.ImageBasePtr = 0;
+	if (HAS_LEGACY_WRAPPER) {
+		ApExeParams.StdHeader.AltImageBasePtr = 0;
+		ApExeParams.StdHeader.CalloutPtr = &GetBiosCallout;
+		ApExeParams.StdHeader.Func = 0;
+		ApExeParams.StdHeader.ImageBasePtr = 0;
+	} else {
+		memcpy(&ApExeParams.StdHeader, StdHeader, sizeof(*StdHeader));
+	}
+
 	ApExeParams.FunctionNumber = Func;
 	ApExeParams.RelatedDataBlock = ConfigPtr;
 
+#if HAS_LEGACY_WRAPPER
 	status = AmdLateRunApTask(&ApExeParams);
-	ASSERT(status == AGESA_SUCCESS);
+#else
+	status = module_dispatch(AMD_LATE_RUN_AP_TASK, &ApExeParams.StdHeader);
+#endif
 
+	ASSERT(status == AGESA_SUCCESS);
 	return status;
 }
 
diff --git a/src/northbridge/amd/agesa/eventlog.c b/src/northbridge/amd/agesa/eventlog.c
index 3291333..55e75b1 100644
--- a/src/northbridge/amd/agesa/eventlog.c
+++ b/src/northbridge/amd/agesa/eventlog.c
@@ -759,7 +759,12 @@
 	int i = 0;
 
 	do {
-		AGESA_STATUS status = AmdReadEventLog(Event);
+		AGESA_STATUS status;
+#if HAS_LEGACY_WRAPPER
+		status = AmdReadEventLog(Event);
+#else
+		status = module_dispatch(AMD_READ_EVENT_LOG, &Event->StdHeader);
+#endif
 		if (status != AGESA_SUCCESS)
 			return;
 		if (Event->EventClass == 0)
@@ -779,11 +784,15 @@
 
 	memset(&AmdEventParams, 0, sizeof(EVENT_PARAMS));
 
-	AmdEventParams.StdHeader.AltImageBasePtr = 0;
-	AmdEventParams.StdHeader.CalloutPtr = &GetBiosCallout;
-	AmdEventParams.StdHeader.Func = 0;
-	AmdEventParams.StdHeader.ImageBasePtr = 0;
-	AmdEventParams.StdHeader.HeapStatus = StdHeader->HeapStatus;
+	if (HAS_LEGACY_WRAPPER) {
+		AmdEventParams.StdHeader.AltImageBasePtr = 0;
+		AmdEventParams.StdHeader.CalloutPtr = &GetBiosCallout;
+		AmdEventParams.StdHeader.Func = 0;
+		AmdEventParams.StdHeader.ImageBasePtr = 0;
+		AmdEventParams.StdHeader.HeapStatus = StdHeader->HeapStatus;
+	} else {
+		memcpy(&AmdEventParams.StdHeader, StdHeader, sizeof(*StdHeader));
+	}
 
 	amd_flush_eventlog(&AmdEventParams);
 }
diff --git a/src/northbridge/amd/agesa/family12/Makefile.inc b/src/northbridge/amd/agesa/family12/Makefile.inc
index 6b2ca78..41c40c3 100644
--- a/src/northbridge/amd/agesa/family12/Makefile.inc
+++ b/src/northbridge/amd/agesa/family12/Makefile.inc
@@ -16,3 +16,8 @@
 romstage-y += dimmSpd.c
 
 ramstage-y += northbridge.c
+
+ifneq ($(CONFIG_AGESA_LEGACY_WRAPPER), y)
+romstage-y += state_machine.c
+ramstage-y += state_machine.c
+endif
diff --git a/src/northbridge/amd/agesa/family12/northbridge.c b/src/northbridge/amd/agesa/family12/northbridge.c
index 3bf4607..4995e64 100644
--- a/src/northbridge/amd/agesa/family12/northbridge.c
+++ b/src/northbridge/amd/agesa/family12/northbridge.c
@@ -32,7 +32,9 @@
 #include <cpu/amd/mtrr.h>
 
 #include "sb_cimx.h"
+
 #include <northbridge/amd/agesa/agesawrapper.h>
+#include <northbridge/amd/agesa/state_machine.h>
 #include <northbridge/amd/agesa/agesa_helper.h>
 
 #define FX_DEVS 1
@@ -597,6 +599,7 @@
 
 static void domain_enable_resources(device_t dev)
 {
+#if IS_ENABLED(CONFIG_AGESA_LEGACY_WRAPPER)
 	printk(BIOS_DEBUG, "\nFam12h - northbridge.c - %s - Start.\n",__func__);
 
 	/* Must be called after PCI enumeration and resource allocation */
@@ -610,6 +613,7 @@
 
 	agesawrapper_amdinitmid();
 	printk(BIOS_DEBUG, "Fam12h - northbridge.c - %s - End.\n",__func__);
+#endif
 }
 
 
diff --git a/src/northbridge/amd/agesa/family12/state_machine.c b/src/northbridge/amd/agesa/family12/state_machine.c
new file mode 100644
index 0000000..ed216c7
--- /dev/null
+++ b/src/northbridge/amd/agesa/family12/state_machine.c
@@ -0,0 +1,89 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2016 Kyösti Mälkki
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "Porting.h"
+#include "AGESA.h"
+
+#include <cbmem.h>
+#include <northbridge/amd/agesa/state_machine.h>
+#include <northbridge/amd/agesa/agesa_helper.h>
+
+#include <cpu/amd/agesa/s3_resume.h>
+
+#include <sb_cimx.h>
+
+void platform_BeforeInitReset(struct sysinfo *cb, AMD_RESET_PARAMS *Reset)
+{
+}
+
+void platform_BeforeInitEarly(struct sysinfo *cb, AMD_EARLY_PARAMS *Early)
+{
+}
+
+void platform_BeforeInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post)
+{
+}
+
+void platform_AfterInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post)
+{
+	backup_top_of_low_cacheable(Post->MemConfig.Sub4GCacheTop);
+
+	sb_before_pci_init();
+}
+
+void platform_BeforeInitResume(struct sysinfo *cb, AMD_RESUME_PARAMS *Resume)
+{
+	OemInitResume(&Resume->S3DataBlock);
+}
+
+void platform_AfterInitResume(struct sysinfo *cb, AMD_RESUME_PARAMS *Resume)
+{
+}
+
+void platform_BeforeInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env)
+{
+	EmptyHeap();
+}
+
+void platform_AfterInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env)
+{
+	sb_After_Pci_Init();
+}
+
+void platform_BeforeS3LateRestore(struct sysinfo *cb, AMD_S3LATE_PARAMS *S3Late)
+{
+	OemS3LateRestore(&S3Late->S3DataBlock);
+}
+
+void platform_AfterS3LateRestore(struct sysinfo *cb, AMD_S3LATE_PARAMS *S3Late)
+{
+}
+
+void platform_BeforeInitMid(struct sysinfo *cb, AMD_MID_PARAMS *Mid)
+{
+	sb_Mid_Post_Init();
+
+	amd_initcpuio();
+}
+
+void platform_AfterInitLate(struct sysinfo *cb, AMD_LATE_PARAMS *Late)
+{
+	sb_Late_Post();
+}
+
+void platform_AfterS3Save(struct sysinfo *cb, AMD_S3SAVE_PARAMS *S3Save)
+{
+	OemS3Save(&S3Save->S3DataBlock);
+}
diff --git a/src/northbridge/amd/agesa/family14/Makefile.inc b/src/northbridge/amd/agesa/family14/Makefile.inc
index 6b2ca78..41c40c3 100644
--- a/src/northbridge/amd/agesa/family14/Makefile.inc
+++ b/src/northbridge/amd/agesa/family14/Makefile.inc
@@ -16,3 +16,8 @@
 romstage-y += dimmSpd.c
 
 ramstage-y += northbridge.c
+
+ifneq ($(CONFIG_AGESA_LEGACY_WRAPPER), y)
+romstage-y += state_machine.c
+ramstage-y += state_machine.c
+endif
diff --git a/src/northbridge/amd/agesa/family14/northbridge.c b/src/northbridge/amd/agesa/family14/northbridge.c
index d5bf730..34e7ce6 100644
--- a/src/northbridge/amd/agesa/family14/northbridge.c
+++ b/src/northbridge/amd/agesa/family14/northbridge.c
@@ -32,6 +32,7 @@
 #include <cpu/amd/mtrr.h>
 
 #include <northbridge/amd/agesa/agesawrapper.h>
+#include <northbridge/amd/agesa/state_machine.h>
 #include <northbridge/amd/agesa/agesa_helper.h>
 
 #include <sb_cimx.h>
@@ -581,6 +582,10 @@
 
 static void domain_enable_resources(device_t dev)
 {
+#if IS_ENABLED(CONFIG_AGESA_LEGACY_WRAPPER)
+	/* Must be called after PCI enumeration and resource allocation */
+	printk(BIOS_DEBUG, "\nFam14h - %s\n", __func__);
+
 #if IS_ENABLED(CONFIG_AMD_SB_CIMX)
 	if (!acpi_is_wakeup_s3()) {
 		sb_After_Pci_Init();
@@ -590,9 +595,6 @@
 	}
 #endif
 
-	/* Must be called after PCI enumeration and resource allocation */
-	printk(BIOS_DEBUG, "\nFam14h - %s\n", __func__);
-
 	if (!acpi_is_wakeup_s3()) {
 		/* Enable MMIO on AMD CPU Address Map Controller */
 		amd_initcpuio();
@@ -601,6 +603,7 @@
 	}
 
 	printk(BIOS_DEBUG, "  ader - leaving domain_enable_resources.\n");
+#endif
 }
 
 static const char *domain_acpi_name(struct device *dev)
diff --git a/src/northbridge/amd/agesa/family14/state_machine.c b/src/northbridge/amd/agesa/family14/state_machine.c
new file mode 100644
index 0000000..fbc2d7e
--- /dev/null
+++ b/src/northbridge/amd/agesa/family14/state_machine.c
@@ -0,0 +1,94 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2016 Kyösti Mälkki
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "Porting.h"
+#include "AGESA.h"
+
+#include <cbmem.h>
+#include <northbridge/amd/agesa/state_machine.h>
+#include <northbridge/amd/agesa/agesa_helper.h>
+
+#include <sb_cimx.h>
+
+void platform_BeforeInitReset(struct sysinfo *cb, AMD_RESET_PARAMS *Reset)
+{
+}
+
+void platform_BeforeInitEarly(struct sysinfo *cb, AMD_EARLY_PARAMS *Early)
+{
+}
+
+void platform_BeforeInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post)
+{
+}
+
+void platform_AfterInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post)
+{
+	backup_top_of_low_cacheable(Post->MemConfig.Sub4GCacheTop);
+}
+
+void platform_BeforeInitResume(struct sysinfo *cb, AMD_RESUME_PARAMS *Resume)
+{
+	OemInitResume(&Resume->S3DataBlock);
+}
+
+void platform_AfterInitResume(struct sysinfo *cb, AMD_RESUME_PARAMS *Resume)
+{
+}
+
+void platform_BeforeInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env)
+{
+	EmptyHeap();
+}
+
+void platform_AfterInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env)
+{
+	amd_initenv();
+#if 0
+	/* FIXME: It's only in ramstage. */
+	sb_Before_Pci_Init();
+#endif
+}
+
+void platform_BeforeS3LateRestore(struct sysinfo *cb, AMD_S3LATE_PARAMS *S3Late)
+{
+	OemS3LateRestore(&S3Late->S3DataBlock);
+}
+
+void platform_AfterS3LateRestore(struct sysinfo *cb, AMD_S3LATE_PARAMS *S3Late)
+{
+#if 0
+	/* FIXME: It's only in ramstage. */
+	sb_Before_Pci_Restore_Init();
+#endif
+}
+
+void platform_BeforeInitMid(struct sysinfo *cb, AMD_MID_PARAMS *Mid)
+{
+	sb_After_Pci_Init();
+	sb_Mid_Post_Init();
+
+	amd_initcpuio();
+}
+
+void platform_AfterInitLate(struct sysinfo *cb, AMD_LATE_PARAMS *Late)
+{
+	sb_Late_Post();
+}
+
+void platform_AfterS3Save(struct sysinfo *cb, AMD_S3SAVE_PARAMS *S3Save)
+{
+	OemS3Save(&S3Save->S3DataBlock);
+}
diff --git a/src/northbridge/amd/agesa/family15/Makefile.inc b/src/northbridge/amd/agesa/family15/Makefile.inc
index 3cf4ed1..25e3cc8 100644
--- a/src/northbridge/amd/agesa/family15/Makefile.inc
+++ b/src/northbridge/amd/agesa/family15/Makefile.inc
@@ -16,3 +16,8 @@
 romstage-y += dimmSpd.c
 
 ramstage-y += northbridge.c
+
+ifneq ($(CONFIG_AGESA_LEGACY_WRAPPER), y)
+romstage-y += state_machine.c
+ramstage-y += state_machine.c
+endif
diff --git a/src/northbridge/amd/agesa/family15/northbridge.c b/src/northbridge/amd/agesa/family15/northbridge.c
index cef6674..16ccf3f 100644
--- a/src/northbridge/amd/agesa/family15/northbridge.c
+++ b/src/northbridge/amd/agesa/family15/northbridge.c
@@ -39,8 +39,8 @@
 #include <cpuRegisters.h>
 
 #include <northbridge/amd/agesa/agesawrapper.h>
+#include <northbridge/amd/agesa/state_machine.h>
 #include <northbridge/amd/agesa/agesa_helper.h>
-
 #include "sb_cimx.h"
 
 #define MAX_NODE_NUMS (MAX_NODES * MAX_DIES)
@@ -637,6 +637,7 @@
 
 static void domain_enable_resources(device_t dev)
 {
+#if IS_ENABLED(CONFIG_AGESA_LEGACY_WRAPPER)
 	/* Must be called after PCI enumeration and resource allocation */
 	printk(BIOS_DEBUG, "\nFam15 - %s: AmdInitMid.\n", __func__);
 
@@ -648,6 +649,7 @@
 
 	agesawrapper_amdinitmid();
 	printk(BIOS_DEBUG, "  Fam15 - leaving %s.\n", __func__);
+#endif
 }
 
 #if CONFIG_HW_MEM_HOLE_SIZEK != 0
diff --git a/src/northbridge/amd/agesa/family15/state_machine.c b/src/northbridge/amd/agesa/family15/state_machine.c
new file mode 100644
index 0000000..83eadb0
--- /dev/null
+++ b/src/northbridge/amd/agesa/family15/state_machine.c
@@ -0,0 +1,78 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2016 Kyösti Mälkki
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "Porting.h"
+#include "AGESA.h"
+
+#include <cbmem.h>
+#include <northbridge/amd/agesa/state_machine.h>
+#include <northbridge/amd/agesa/agesa_helper.h>
+
+void platform_BeforeInitReset(struct sysinfo *cb, AMD_RESET_PARAMS *Reset)
+{
+}
+
+void platform_BeforeInitEarly(struct sysinfo *cb, AMD_EARLY_PARAMS *Early)
+{
+}
+
+void platform_BeforeInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post)
+{
+}
+
+void platform_AfterInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post)
+{
+	backup_top_of_low_cacheable(Post->MemConfig.Sub4GCacheTop);
+}
+
+void platform_BeforeInitResume(struct sysinfo *cb, AMD_RESUME_PARAMS *Resume)
+{
+	OemInitResume(&Resume->S3DataBlock);
+}
+
+void platform_AfterInitResume(struct sysinfo *cb, AMD_RESUME_PARAMS *Resume)
+{
+}
+
+void platform_BeforeInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env)
+{
+	EmptyHeap();
+}
+
+void platform_AfterInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env)
+{
+}
+
+void platform_BeforeS3LateRestore(struct sysinfo *cb, AMD_S3LATE_PARAMS *S3Late)
+{
+	OemS3LateRestore(&S3Late->S3DataBlock);
+}
+
+void platform_AfterS3LateRestore(struct sysinfo *cb, AMD_S3LATE_PARAMS *S3Late)
+{
+}
+
+void platform_BeforeInitMid(struct sysinfo *cb, AMD_MID_PARAMS *Mid)
+{
+}
+
+void platform_AfterInitLate(struct sysinfo *cb, AMD_LATE_PARAMS *Late)
+{
+}
+
+void platform_AfterS3Save(struct sysinfo *cb, AMD_S3SAVE_PARAMS *S3Save)
+{
+	OemS3Save(&S3Save->S3DataBlock);
+}
diff --git a/src/northbridge/amd/agesa/family15rl/Makefile.inc b/src/northbridge/amd/agesa/family15rl/Makefile.inc
index a188a91..a84bfc0 100644
--- a/src/northbridge/amd/agesa/family15rl/Makefile.inc
+++ b/src/northbridge/amd/agesa/family15rl/Makefile.inc
@@ -17,3 +17,8 @@
 
 ramstage-y += iommu.c
 ramstage-y += northbridge.c
+
+ifneq ($(CONFIG_AGESA_LEGACY_WRAPPER), y)
+romstage-y += ../family15tn/state_machine.c
+ramstage-y += ../family15tn/state_machine.c
+endif
diff --git a/src/northbridge/amd/agesa/family15rl/northbridge.c b/src/northbridge/amd/agesa/family15rl/northbridge.c
index 800d6b1..28b9923 100644
--- a/src/northbridge/amd/agesa/family15rl/northbridge.c
+++ b/src/northbridge/amd/agesa/family15rl/northbridge.c
@@ -40,6 +40,7 @@
 #include <cpuRegisters.h>
 
 #include <northbridge/amd/agesa/agesawrapper.h>
+#include <northbridge/amd/agesa/state_machine.h>
 #include <northbridge/amd/agesa/agesa_helper.h>
 
 #define MAX_NODE_NUMS (MAX_NODES * MAX_DIES)
@@ -633,6 +634,7 @@
 
 static void domain_enable_resources(struct device *dev)
 {
+#if IS_ENABLED(CONFIG_AGESA_LEGACY_WRAPPER)
 	if (acpi_is_wakeup_s3())
 		agesawrapper_fchs3laterestore();
 
@@ -644,6 +646,7 @@
 		agesawrapper_amdinitmid();
 	}
 	printk(BIOS_DEBUG, "  ader - leaving %s.\n", __func__);
+#endif
 }
 
 #if CONFIG_HW_MEM_HOLE_SIZEK != 0
diff --git a/src/northbridge/amd/agesa/family15tn/Makefile.inc b/src/northbridge/amd/agesa/family15tn/Makefile.inc
index a188a91..d6cbc1f 100644
--- a/src/northbridge/amd/agesa/family15tn/Makefile.inc
+++ b/src/northbridge/amd/agesa/family15tn/Makefile.inc
@@ -17,3 +17,8 @@
 
 ramstage-y += iommu.c
 ramstage-y += northbridge.c
+
+ifneq ($(CONFIG_AGESA_LEGACY_WRAPPER), y)
+romstage-y += state_machine.c
+ramstage-y += state_machine.c
+endif
diff --git a/src/northbridge/amd/agesa/family15tn/northbridge.c b/src/northbridge/amd/agesa/family15tn/northbridge.c
index 374c74f..eb705b8 100644
--- a/src/northbridge/amd/agesa/family15tn/northbridge.c
+++ b/src/northbridge/amd/agesa/family15tn/northbridge.c
@@ -39,6 +39,7 @@
 #include <cpuRegisters.h>
 
 #include <northbridge/amd/agesa/agesawrapper.h>
+#include <northbridge/amd/agesa/state_machine.h>
 #include <northbridge/amd/agesa/agesa_helper.h>
 
 #define MAX_NODE_NUMS (MAX_NODES * MAX_DIES)
@@ -632,6 +633,7 @@
 
 static void domain_enable_resources(device_t dev)
 {
+#if IS_ENABLED(CONFIG_AGESA_LEGACY_WRAPPER)
 	if (acpi_is_wakeup_s3())
 		agesawrapper_fchs3laterestore();
 
@@ -643,6 +645,7 @@
 		agesawrapper_amdinitmid();
 	}
 	printk(BIOS_DEBUG, "  ader - leaving %s.\n", __func__);
+#endif
 }
 
 #if CONFIG_HW_MEM_HOLE_SIZEK != 0
diff --git a/src/northbridge/amd/agesa/family15tn/state_machine.c b/src/northbridge/amd/agesa/family15tn/state_machine.c
new file mode 100644
index 0000000..ad5a14c
--- /dev/null
+++ b/src/northbridge/amd/agesa/family15tn/state_machine.c
@@ -0,0 +1,85 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2016 Kyösti Mälkki
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "Porting.h"
+#include "AGESA.h"
+
+#include <cbmem.h>
+#include <northbridge/amd/agesa/state_machine.h>
+#include <northbridge/amd/agesa/agesa_helper.h>
+
+void platform_BeforeInitReset(struct sysinfo *cb, AMD_RESET_PARAMS *Reset)
+{
+}
+
+void platform_BeforeInitEarly(struct sysinfo *cb, AMD_EARLY_PARAMS *Early)
+{
+}
+
+void platform_BeforeInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post)
+{
+}
+
+void platform_AfterInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post)
+{
+	backup_top_of_low_cacheable(Post->MemConfig.Sub4GCacheTop);
+}
+
+void platform_BeforeInitResume(struct sysinfo *cb, AMD_RESUME_PARAMS *Resume)
+{
+	OemInitResume(&Resume->S3DataBlock);
+}
+
+void platform_AfterInitResume(struct sysinfo *cb, AMD_RESUME_PARAMS *Resume)
+{
+}
+
+void platform_BeforeInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env)
+{
+	EmptyHeap();
+}
+
+void platform_AfterInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env)
+{
+}
+
+void platform_BeforeS3LateRestore(struct sysinfo *cb, AMD_S3LATE_PARAMS *S3Late)
+{
+	OemS3LateRestore(&S3Late->S3DataBlock);
+}
+
+void platform_AfterS3LateRestore(struct sysinfo *cb, AMD_S3LATE_PARAMS *S3Late)
+{
+	amd_initcpuio();
+
+	fchs3earlyrestore();
+}
+
+void platform_BeforeInitMid(struct sysinfo *cb, AMD_MID_PARAMS *Mid)
+{
+	amd_initcpuio();
+}
+
+void platform_AfterInitLate(struct sysinfo *cb, AMD_LATE_PARAMS *Late)
+{
+	/* FIXME: not reached S3 path */
+	if (cb->s3resume)
+		fchs3laterestore();
+}
+
+void platform_AfterS3Save(struct sysinfo *cb, AMD_S3SAVE_PARAMS *S3Save)
+{
+	OemS3Save(&S3Save->S3DataBlock);
+}
diff --git a/src/northbridge/amd/agesa/family16kb/Makefile.inc b/src/northbridge/amd/agesa/family16kb/Makefile.inc
index 3cf4ed1..25e3cc8 100644
--- a/src/northbridge/amd/agesa/family16kb/Makefile.inc
+++ b/src/northbridge/amd/agesa/family16kb/Makefile.inc
@@ -16,3 +16,8 @@
 romstage-y += dimmSpd.c
 
 ramstage-y += northbridge.c
+
+ifneq ($(CONFIG_AGESA_LEGACY_WRAPPER), y)
+romstage-y += state_machine.c
+ramstage-y += state_machine.c
+endif
diff --git a/src/northbridge/amd/agesa/family16kb/northbridge.c b/src/northbridge/amd/agesa/family16kb/northbridge.c
index d586e7a..ab2e8e8 100644
--- a/src/northbridge/amd/agesa/family16kb/northbridge.c
+++ b/src/northbridge/amd/agesa/family16kb/northbridge.c
@@ -39,6 +39,7 @@
 #include <cpuRegisters.h>
 
 #include <northbridge/amd/agesa/agesawrapper.h>
+#include <northbridge/amd/agesa/state_machine.h>
 #include <northbridge/amd/agesa/agesa_helper.h>
 
 #define MAX_NODE_NUMS (MAX_NODES * MAX_DIES)
@@ -647,6 +648,7 @@
 
 static void domain_enable_resources(device_t dev)
 {
+#if IS_ENABLED(CONFIG_AGESA_LEGACY_WRAPPER)
 	if (acpi_is_wakeup_s3())
 		agesawrapper_fchs3laterestore();
 
@@ -658,6 +660,7 @@
 		agesawrapper_amdinitmid();
 	}
 	printk(BIOS_DEBUG, "  ader - leaving domain_enable_resources.\n");
+#endif
 }
 
 #if CONFIG_HW_MEM_HOLE_SIZEK != 0
diff --git a/src/northbridge/amd/agesa/family16kb/state_machine.c b/src/northbridge/amd/agesa/family16kb/state_machine.c
new file mode 100644
index 0000000..ad5a14c
--- /dev/null
+++ b/src/northbridge/amd/agesa/family16kb/state_machine.c
@@ -0,0 +1,85 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2016 Kyösti Mälkki
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "Porting.h"
+#include "AGESA.h"
+
+#include <cbmem.h>
+#include <northbridge/amd/agesa/state_machine.h>
+#include <northbridge/amd/agesa/agesa_helper.h>
+
+void platform_BeforeInitReset(struct sysinfo *cb, AMD_RESET_PARAMS *Reset)
+{
+}
+
+void platform_BeforeInitEarly(struct sysinfo *cb, AMD_EARLY_PARAMS *Early)
+{
+}
+
+void platform_BeforeInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post)
+{
+}
+
+void platform_AfterInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post)
+{
+	backup_top_of_low_cacheable(Post->MemConfig.Sub4GCacheTop);
+}
+
+void platform_BeforeInitResume(struct sysinfo *cb, AMD_RESUME_PARAMS *Resume)
+{
+	OemInitResume(&Resume->S3DataBlock);
+}
+
+void platform_AfterInitResume(struct sysinfo *cb, AMD_RESUME_PARAMS *Resume)
+{
+}
+
+void platform_BeforeInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env)
+{
+	EmptyHeap();
+}
+
+void platform_AfterInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env)
+{
+}
+
+void platform_BeforeS3LateRestore(struct sysinfo *cb, AMD_S3LATE_PARAMS *S3Late)
+{
+	OemS3LateRestore(&S3Late->S3DataBlock);
+}
+
+void platform_AfterS3LateRestore(struct sysinfo *cb, AMD_S3LATE_PARAMS *S3Late)
+{
+	amd_initcpuio();
+
+	fchs3earlyrestore();
+}
+
+void platform_BeforeInitMid(struct sysinfo *cb, AMD_MID_PARAMS *Mid)
+{
+	amd_initcpuio();
+}
+
+void platform_AfterInitLate(struct sysinfo *cb, AMD_LATE_PARAMS *Late)
+{
+	/* FIXME: not reached S3 path */
+	if (cb->s3resume)
+		fchs3laterestore();
+}
+
+void platform_AfterS3Save(struct sysinfo *cb, AMD_S3SAVE_PARAMS *S3Save)
+{
+	OemS3Save(&S3Save->S3DataBlock);
+}
diff --git a/src/northbridge/amd/agesa/oem_s3.c b/src/northbridge/amd/agesa/oem_s3.c
index 02c384a..f2cef99 100644
--- a/src/northbridge/amd/agesa/oem_s3.c
+++ b/src/northbridge/amd/agesa/oem_s3.c
@@ -20,6 +20,7 @@
 #include <program_loading.h>
 #include <cpu/amd/agesa/s3_resume.h>
 #include <northbridge/amd/agesa/agesawrapper.h>
+#include <northbridge/amd/agesa/state_machine.h>
 #include <AGESA.h>
 #include <northbridge/amd/agesa/agesa_helper.h>
 
diff --git a/src/northbridge/amd/agesa/state_machine.c b/src/northbridge/amd/agesa/state_machine.c
new file mode 100644
index 0000000..0406eee
--- /dev/null
+++ b/src/northbridge/amd/agesa/state_machine.c
@@ -0,0 +1,318 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2011-2012 Advanced Micro Devices, Inc.
+ * Copyright (C) 2016 Kyösti Mälkki
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <stdint.h>
+#include <string.h>
+
+#include <arch/acpi.h>
+#include <bootstate.h>
+
+#include <northbridge/amd/agesa/state_machine.h>
+#include <northbridge/amd/agesa/agesa_helper.h>
+#include <northbridge/amd/agesa/BiosCallOuts.h>
+#include "amdlib.h"
+#include <cpu/amd/agesa/s3_resume.h>
+
+#include "AMD.h"
+#include "heapManager.h"
+#include "Dispatcher.h"
+
+
+#if ENV_ROMSTAGE
+#include <PlatformMemoryConfiguration.h>
+CONST PSO_ENTRY ROMDATA DefaultPlatformMemoryConfiguration[] = {PSO_END};
+#endif
+
+void agesa_set_interface(struct sysinfo *cb)
+{
+	memset(&cb->StdHeader, 0, sizeof(AMD_CONFIG_PARAMS));
+
+	cb->StdHeader.CalloutPtr = GetBiosCallout;
+}
+
+AGESA_STATUS module_dispatch(AGESA_STRUCT_NAME func,
+	AMD_CONFIG_PARAMS *StdHeader)
+{
+	MODULE_ENTRY dispatcher = AmdAgesaDispatcher;
+
+	StdHeader->Func = func;
+	return dispatcher(StdHeader);
+}
+
+static AGESA_STATUS amd_create_struct(AMD_INTERFACE_PARAMS *aip,
+	AGESA_STRUCT_NAME func, void *buf, size_t len)
+{
+	aip->AgesaFunctionName = func;
+	aip->AllocationMethod = 0;
+	aip->NewStructPtr = buf;
+	aip->NewStructSize = len;
+	if (buf != NULL && len != 0)
+		aip->AllocationMethod = ByHost;
+
+	return module_dispatch(AMD_CREATE_STRUCT, &aip->StdHeader);
+}
+
+static AGESA_STATUS amd_release_struct(AMD_INTERFACE_PARAMS *aip)
+{
+	/* Cannot release AMD_LATE_PARAMS until ACPI tables are done. */
+	if (aip->AgesaFunctionName == AMD_INIT_LATE)
+		return AGESA_SUCCESS;
+
+	return module_dispatch(AMD_RELEASE_STRUCT, &aip->StdHeader);
+}
+
+/* By design, for each valid AGESA_STRUCT_NAME, AMD_CONFIG_PARAMS
+ * can be evaluated to apply correct typecast based on Func field.
+ */
+
+static AGESA_STATUS amd_dispatch(struct sysinfo *cb,
+	AGESA_STRUCT_NAME func, AMD_CONFIG_PARAMS *StdHeader)
+{
+	AGESA_STATUS status = AGESA_UNSUPPORTED;
+
+	switch (func)
+	{
+#if ENV_ROMSTAGE
+		case AMD_INIT_RESET:
+		{
+			AMD_RESET_PARAMS *param = (void *)StdHeader;
+			platform_BeforeInitReset(cb, param);
+			board_BeforeInitReset(cb, param);
+			status = module_dispatch(func, StdHeader);
+			break;
+		}
+
+		case AMD_INIT_EARLY:
+		{
+			AMD_EARLY_PARAMS *param = (void *)StdHeader;
+			platform_BeforeInitEarly(cb, param);
+			board_BeforeInitEarly(cb, param);
+			status = module_dispatch(func, StdHeader);
+			break;
+		}
+
+		case AMD_INIT_POST:
+		{
+			AMD_POST_PARAMS *param = (void *)StdHeader;
+			platform_BeforeInitPost(cb, param);
+			board_BeforeInitPost(cb, param);
+			status = module_dispatch(func, StdHeader);
+			platform_AfterInitPost(cb, param);
+			break;
+		}
+
+		case AMD_INIT_RESUME:
+		{
+			AMD_RESUME_PARAMS *param = (void *)StdHeader;
+			platform_BeforeInitResume(cb, param);
+			status = module_dispatch(func, StdHeader);
+			platform_AfterInitResume(cb, param);
+			break;
+		}
+
+		case AMD_INIT_ENV:
+		{
+			AMD_ENV_PARAMS *param = (void *)StdHeader;
+			platform_BeforeInitEnv(cb, param);
+			board_BeforeInitEnv(cb, param);
+			status = module_dispatch(func, StdHeader);
+			platform_AfterInitEnv(cb, param);
+			break;
+		}
+
+		case AMD_S3LATE_RESTORE:
+		{
+			AMD_S3LATE_PARAMS *param = (void *)StdHeader;
+			platform_BeforeS3LateRestore(cb, param);
+			status = module_dispatch(func, StdHeader);
+			platform_AfterS3LateRestore(cb, param);
+			break;
+		}
+#endif
+#if ENV_RAMSTAGE
+		case AMD_INIT_MID:
+		{
+			AMD_MID_PARAMS *param = (void *)StdHeader;
+			platform_BeforeInitMid(cb, param);
+			board_BeforeInitMid(cb, param);
+			status = module_dispatch(func, StdHeader);
+			break;
+		}
+
+		case AMD_S3_SAVE:
+		{
+			AMD_S3SAVE_PARAMS *param = (void *)StdHeader;
+			status = module_dispatch(func, StdHeader);
+			platform_AfterS3Save(cb, param);
+			break;
+		}
+		case AMD_INIT_LATE:
+		{
+			AMD_LATE_PARAMS *param = (void *)StdHeader;
+			status = module_dispatch(func, StdHeader);
+			platform_AfterInitLate(cb, param);
+			completion_InitLate(cb, param);
+			break;
+		}
+#endif
+		default:
+		{
+			break;
+		}
+
+	}
+	return status;
+}
+
+/* DEBUG trace helper */
+
+struct agesa_state
+{
+	u8 apic_id;
+
+	AGESA_STRUCT_NAME func;
+	const char *function_name;
+};
+
+static void state_on_entry(struct agesa_state *task, AGESA_STRUCT_NAME func,
+	const char *struct_name)
+{
+	task->apic_id = (u8) (cpuid_ebx(1) >> 24);
+	task->func = func;
+	task->function_name = struct_name;
+
+	printk(BIOS_DEBUG, "\nAPIC %02d: ** Enter %s [%08x]\n",
+		task->apic_id, task->function_name, task->func);
+}
+
+static void state_on_exit(struct agesa_state *task,
+	AMD_CONFIG_PARAMS *StdHeader)
+{
+	printk(BIOS_DEBUG, "APIC %02d: Heap in %s (%d) at 0x%08x\n",
+		task->apic_id, heap_status_name(StdHeader->HeapStatus),
+		StdHeader->HeapStatus, (u32)StdHeader->HeapBasePtr);
+
+	printk(BIOS_DEBUG, "APIC %02d: ** Exit  %s [%08x]\n",
+		task->apic_id, task->function_name, task->func);
+}
+
+int agesa_execute_state(struct sysinfo *cb, AGESA_STRUCT_NAME func)
+{
+	AMD_INTERFACE_PARAMS aip;
+	union {
+		AMD_RESET_PARAMS reset;
+	} agesa_params;
+	void *buf = NULL;
+	size_t len = 0;
+	const char *state_name = agesa_struct_name(func);
+
+	AGESA_STATUS status, final;
+
+	struct agesa_state task;
+	memset(&task, 0, sizeof(task));
+	state_on_entry(&task, func, state_name);
+
+	aip.StdHeader = cb->StdHeader;
+
+	/* For these calls, heap is not available. */
+	if (func == AMD_INIT_RESET) {
+		buf = (void *) &agesa_params;
+		len = sizeof(agesa_params);
+		memcpy(buf, &cb->StdHeader, sizeof(cb->StdHeader));
+	}
+
+	status = amd_create_struct(&aip, func, buf, len);
+	ASSERT(status == AGESA_SUCCESS);
+
+	/* Must call the function buffer was allocated for.*/
+	AMD_CONFIG_PARAMS *StdHeader = aip.NewStructPtr;
+	ASSERT(StdHeader->Func == func);
+
+	final = amd_dispatch(cb, func, StdHeader);
+
+	agesawrapper_trace(final, StdHeader, state_name);
+	ASSERT(final < AGESA_FATAL);
+
+	status = amd_release_struct(&aip);
+	ASSERT(status == AGESA_SUCCESS);
+
+	state_on_exit(&task, &aip.StdHeader);
+
+	return (final < AGESA_FATAL) ? 0 : -1;
+}
+
+#if ENV_RAMSTAGE
+
+static void amd_bs_ramstage_init(void *arg)
+{
+	struct sysinfo *cb = arg;
+
+	agesa_set_interface(cb);
+}
+
+void sb_After_Pci_Restore_Init(void);
+
+static void amd_bs_dev_enable(void *arg)
+{
+	struct sysinfo *cb = arg;
+
+	if (!acpi_is_wakeup_s3())
+		agesa_execute_state(cb, AMD_INIT_MID);
+
+	/* FIXME */
+	if (IS_ENABLED(CONFIG_AMD_SB_CIMX) && acpi_is_wakeup_s3())
+		sb_After_Pci_Restore_Init();
+}
+
+static void amd_bs_post_device(void *arg)
+{
+	struct sysinfo *cb = arg;
+
+	if (acpi_is_wakeup_s3())
+		return;
+
+	agesa_execute_state(cb, AMD_INIT_LATE);
+
+	if (!acpi_s3_resume_allowed())
+		return;
+
+	agesa_execute_state(cb, AMD_S3_SAVE);
+}
+
+static struct sysinfo state_machine;
+
+BOOT_STATE_INIT_ENTRY(BS_PRE_DEVICE, BS_ON_ENTRY, amd_bs_ramstage_init,
+	&state_machine);
+
+BOOT_STATE_INIT_ENTRY(BS_DEV_ENABLE, BS_ON_ENTRY, amd_bs_dev_enable,
+	&state_machine);
+
+BOOT_STATE_INIT_ENTRY(BS_POST_DEVICE, BS_ON_EXIT, amd_bs_post_device,
+	&state_machine);
+
+#endif /* ENV_RAMSTAGE */
+
+/* Empty stubs for cases board does not need to override anything. */
+void __attribute__((weak))
+board_BeforeInitReset(struct sysinfo *cb, AMD_RESET_PARAMS *Reset) { }
+void __attribute__((weak))
+board_BeforeInitEarly(struct sysinfo *cb, AMD_EARLY_PARAMS *Early) { }
+void __attribute__((weak))
+board_BeforeInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post) { }
+void __attribute__((weak))
+board_BeforeInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env) { }
+void __attribute__((weak))
+board_BeforeInitMid(struct sysinfo *cb, AMD_MID_PARAMS *Mid) { }
diff --git a/src/northbridge/amd/agesa/state_machine.h b/src/northbridge/amd/agesa/state_machine.h
index 86424d1..efa6496 100644
--- a/src/northbridge/amd/agesa/state_machine.h
+++ b/src/northbridge/amd/agesa/state_machine.h
@@ -18,6 +18,10 @@
 
 #include <stdint.h>
 #include <AGESA.h>
+#include <AMD.h>
+
+#define HAS_LEGACY_WRAPPER (IS_ENABLED(CONFIG_AGESA_LEGACY_WRAPPER) || \
+	IS_ENABLED(CONFIG_CPU_AMD_PI))
 
 /* eventlog */
 const char *agesa_struct_name(int state);
@@ -25,8 +29,23 @@
 void agesawrapper_trace(AGESA_STATUS ret, AMD_CONFIG_PARAMS *StdHeader, const char *func);
 AGESA_STATUS agesawrapper_amdreadeventlog(UINT8 HeapStatus);
 
+/* For suspend-to-ram support. */
+
+#if !IS_ENABLED(CONFIG_CPU_AMD_PI)
+/* TODO: With binaryPI we need different interface. */
+AGESA_STATUS OemInitResume(AMD_S3_PARAMS *dataBlock);
+AGESA_STATUS OemS3LateRestore(AMD_S3_PARAMS *dataBlock);
+AGESA_STATUS OemS3Save(AMD_S3_PARAMS *dataBlock);
+#endif
+
+/* For FCH */
+static inline void fchs3earlyrestore(void) { }
+static inline void fchs3laterestore(void) { }
+
 struct sysinfo
 {
+	AMD_CONFIG_PARAMS StdHeader;
+
 	int s3resume;
 };
 
@@ -36,4 +55,44 @@
 void board_BeforeAgesa(struct sysinfo *cb);
 void platform_once(struct sysinfo *cb);
 
+void agesa_set_interface(struct sysinfo *cb);
+int agesa_execute_state(struct sysinfo *cb, AGESA_STRUCT_NAME func);
+
+/* AGESA dispatchers */
+
+AGESA_STATUS module_dispatch(AGESA_STRUCT_NAME func, AMD_CONFIG_PARAMS *StdHeader);
+
+void platform_BeforeInitReset(struct sysinfo *cb, AMD_RESET_PARAMS *Reset);
+void board_BeforeInitReset(struct sysinfo *cb, AMD_RESET_PARAMS *Reset);
+
+void platform_BeforeInitEarly(struct sysinfo *cb, AMD_EARLY_PARAMS *Early);
+void board_BeforeInitEarly(struct sysinfo *cb, AMD_EARLY_PARAMS *Early);
+
+/* Normal boot */
+void platform_BeforeInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post);
+void board_BeforeInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post);
+void platform_AfterInitPost(struct sysinfo *cb, AMD_POST_PARAMS *Post);
+
+void platform_BeforeInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env);
+void board_BeforeInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env);
+void platform_AfterInitEnv(struct sysinfo *cb, AMD_ENV_PARAMS *Env);
+
+void platform_BeforeInitMid(struct sysinfo *cb, AMD_MID_PARAMS *Mid);
+void board_BeforeInitMid(struct sysinfo *cb, AMD_MID_PARAMS *Mid);
+
+void platform_AfterInitLate(struct sysinfo *cb, AMD_LATE_PARAMS *Late);
+void completion_InitLate(struct sysinfo *cb, AMD_LATE_PARAMS *Late);
+
+/* S3 Resume */
+void platform_BeforeInitResume(struct sysinfo *cb, AMD_RESUME_PARAMS *Resume);
+void platform_AfterInitResume(struct sysinfo *cb, AMD_RESUME_PARAMS *Resume);
+
+void platform_BeforeS3LateRestore(struct sysinfo *cb, AMD_S3LATE_PARAMS *S3Late);
+void platform_AfterS3LateRestore(struct sysinfo *cb, AMD_S3LATE_PARAMS *S3Late);
+
+#if IS_ENABLED(CONFIG_CPU_AMD_PI_00660F01)
+typedef void AMD_S3SAVE_PARAMS;
+#endif
+void platform_AfterS3Save(struct sysinfo *cb, AMD_S3SAVE_PARAMS *S3Save);
+
 #endif /* _STATE_MACHINE_H_ */
diff --git a/src/southbridge/amd/agesa/hudson/Makefile.inc b/src/southbridge/amd/agesa/hudson/Makefile.inc
index 7595889..b1c56cb 100644
--- a/src/southbridge/amd/agesa/hudson/Makefile.inc
+++ b/src/southbridge/amd/agesa/hudson/Makefile.inc
@@ -10,7 +10,7 @@
 ramstage-y += pcie.c
 ramstage-y += sd.c
 
-ramstage-y += agesawrapper.c
+ramstage-$(CONFIG_AGESA_LEGACY_WRAPPER) += agesawrapper.c
 
 ramstage-$(CONFIG_HAVE_ACPI_TABLES) += fadt.c
 ramstage-y += reset.c
diff --git a/src/southbridge/amd/cimx/sb800/late.c b/src/southbridge/amd/cimx/sb800/late.c
index 6b3af0e..e764ba0 100644
--- a/src/southbridge/amd/cimx/sb800/late.c
+++ b/src/southbridge/amd/cimx/sb800/late.c
@@ -473,6 +473,7 @@
 	case (0x16 << 3) | 2: /* 0:16:2 EHCI-USB3 */
 		sb_config->USBMODE.UsbMode.Ehci3 = dev->enabled;
 
+#if 1 /* FIXME: IS_ENABLED(CONFIG_AGESA_LEGACY_WRAPPER) */
 		/* call the CIMX entry at the last sb800 device,
 		 * so make sure the mainboard devicetree is complete
 		 */
@@ -480,6 +481,7 @@
 			sb_Before_Pci_Init();
 		else
 			sb_Before_Pci_Restore_Init();
+#endif
 		break;
 
 	default:
diff --git a/src/vendorcode/amd/agesa/common/agesa-entry.c b/src/vendorcode/amd/agesa/common/agesa-entry.c
index e722fc1..29f2069 100644
--- a/src/vendorcode/amd/agesa/common/agesa-entry.c
+++ b/src/vendorcode/amd/agesa/common/agesa-entry.c
@@ -161,11 +161,12 @@
     { AMD_S3LATE_RESTORE, (IMAGE_ENTRY)AmdS3LateRestore },
   #endif
 
+    { AMD_READ_EVENT_LOG, (IMAGE_ENTRY)AmdReadEventLog },
+
   #if AGESA_ENTRY_INIT_GENERAL_SERVICES == TRUE
     { AMD_GET_APIC_ID, (IMAGE_ENTRY)AmdGetApicId },
     { AMD_GET_PCI_ADDRESS, (IMAGE_ENTRY)AmdGetPciAddress },
     { AMD_IDENTIFY_CORE, (IMAGE_ENTRY)AmdIdentifyCore },
-    { AMD_READ_EVENT_LOG, (IMAGE_ENTRY)AmdReadEventLog },
     { AMD_IDENTIFY_DIMMS, (IMAGE_ENTRY)AmdIdentifyDimm },
     { AMD_GET_EXECACHE_SIZE, (IMAGE_ENTRY)AmdGetAvailableExeCacheSize },
   #endif