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>

42 files changed, 970 insertions, 29 deletions

diff --git a/src/cpu/amd/agesa/Kconfig b/src/cpu/amd/agesa/Kconfig
index cd14975031a2..e43d4b051573 100644
--- a/src/cpu/amd/agesa/Kconfig
+++ b/src/cpu/amd/agesa/Kconfig
@@ -37,6 +37,10 @@ if CPU_AMD_AGESA
 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 19f79756dcea..b5bb8d98a8f5 100644
--- a/src/cpu/amd/agesa/Makefile.inc
+++ b/src/cpu/amd/agesa/Makefile.inc
@@ -32,7 +32,7 @@ endif
 
 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 11a62ad302bb..4422b66bb229 100644
--- a/src/cpu/amd/agesa/romstage.c
+++ b/src/cpu/amd/agesa/romstage.c
@@ -32,6 +32,9 @@ static void fill_sysinfo(struct sysinfo *cb)
 {
 	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 @@ void * asmlinkage romstage_main(unsigned long bist)
 	/* 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 @@ void asmlinkage romstage_after_car(void)
 	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 37fb43fdad03..462d22109d09 100644
--- a/src/mainboard/amd/olivehill/Kconfig
+++ b/src/mainboard/amd/olivehill/Kconfig
@@ -17,6 +17,7 @@ if BOARD_AMD_OLIVEHILL
 
 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 5c85c4b74e3e..348f995ec6b7 100644
--- a/src/mainboard/amd/torpedo/Kconfig
+++ b/src/mainboard/amd/torpedo/Kconfig
@@ -17,6 +17,7 @@ if BOARD_AMD_TORPEDO
 
 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 1c532121243e..8dc53ae40459 100644
--- a/src/mainboard/asrock/e350m1/Kconfig
+++ b/src/mainboard/asrock/e350m1/Kconfig
@@ -17,6 +17,7 @@ if BOARD_ASROCK_E350M1
 
 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 e118bfbe6021..c50ea462334d 100644
--- a/src/mainboard/asrock/imb-a180/Kconfig
+++ b/src/mainboard/asrock/imb-a180/Kconfig
@@ -17,6 +17,7 @@ if BOARD_ASROCK_IMB_A180
 
 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 f3b2d7d3aa1c..651c107e1dbd 100644
--- a/src/mainboard/asus/f2a85-m/Kconfig
+++ b/src/mainboard/asus/f2a85-m/Kconfig
@@ -18,6 +18,7 @@ if BOARD_ASUS_F2A85_M || BOARD_ASUS_F2A85_M_PRO || BOARD_ASUS_F2A85_M_LE
 
 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 6631ac8d7178..ea9fec5abae9 100644
--- a/src/mainboard/bap/ode_e20XX/Kconfig
+++ b/src/mainboard/bap/ode_e20XX/Kconfig
@@ -18,6 +18,7 @@ if BOARD_ODE_E20XX
 
 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 a90130148bdd..9da780c2405a 100644
--- a/src/mainboard/elmex/pcm205400/Kconfig
+++ b/src/mainboard/elmex/pcm205400/Kconfig
@@ -29,6 +29,7 @@ if BOARD_ELMEX_PCM205400 || BOARD_ELMEX_PCM205401
 
 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 4177b1dd876f..766b3b921940 100644
--- a/src/mainboard/gizmosphere/gizmo/Kconfig
+++ b/src/mainboard/gizmosphere/gizmo/Kconfig
@@ -18,6 +18,7 @@ if BOARD_GIZMOSPHERE_GIZMO
 
 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 aac3a8417d51..07dbd1fdc75f 100644
--- a/src/mainboard/lenovo/g505s/Kconfig
+++ b/src/mainboard/lenovo/g505s/Kconfig
@@ -18,6 +18,7 @@ if BOARD_LENOVO_G505S
 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 7a2623b80b50..8187dbe8a40f 100644
--- a/src/mainboard/msi/ms7721/Kconfig
+++ b/src/mainboard/msi/ms7721/Kconfig
@@ -20,6 +20,7 @@ if BOARD_MSI_MS7721
 
 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 5e927cc95e34..8edb36cd95b3 100644
--- a/src/mainboard/pcengines/apu1/Kconfig
+++ b/src/mainboard/pcengines/apu1/Kconfig
@@ -18,6 +18,7 @@ if BOARD_PCENGINES_APU1
 
 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 fff652407699..d735673f1df4 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_TN) += family15tn
 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 7edf43b1263f..5335c02f5b39 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 @@ void agesawrapper_setlateinitptr(void *Late)
 	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 ab7db339f0a6..40b5f2242ef6 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 @@ struct OEM_HOOK
 
 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 4a5f693e51cd..f786a1102a80 100644
--- a/src/northbridge/amd/agesa/def_callouts.c
+++ b/src/northbridge/amd/agesa/def_callouts.c
@@ -108,21 +108,31 @@ AGESA_STATUS agesa_Reset (UINT32 Func, UINTN Data, VOID *ConfigPtr)
 
 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 3291333dca6a..55e75b13ee99 100644
--- a/src/northbridge/amd/agesa/eventlog.c
+++ b/src/northbridge/amd/agesa/eventlog.c
@@ -759,7 +759,12 @@ static void amd_flush_eventlog(EVENT_PARAMS *Event)
 	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 @@ void agesawrapper_trace(AGESA_STATUS ret, AMD_CONFIG_PARAMS *StdHeader,
 
 	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 6b2ca784cdad..41c40c39151b 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 3bf46078dd0f..4995e648a022 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_set_resources(device_t dev)
 
 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 @@ static void domain_enable_resources(device_t dev)
 
 	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 000000000000..ed216c74fbe7
--- /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 6b2ca784cdad..41c40c39151b 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 d5bf73091db9..34e7ce6934df 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_set_resources(device_t dev)
 
 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 @@ static void domain_enable_resources(device_t dev)
 	}
 #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 @@ static void domain_enable_resources(device_t dev)
 	}
 
 	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 000000000000..fbc2d7e1e551
--- /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 3cf4ed13552a..25e3cc8f1f09 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 cef6674f5cc2..16ccf3f18fd1 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_read_resources(device_t dev)
 
 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 @@ static void domain_enable_resources(device_t dev)
 
 	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 000000000000..83eadb0de0d6
--- /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 a188a91fadbe..a84bfc084b56 100644
--- a/src/northbridge/amd/agesa/family15rl/Makefile.inc
+++ b/src/northbridge/amd/agesa/family15rl/Makefile.inc
@@ -17,3 +17,8 @@ romstage-y += dimmSpd.c
 
 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 800d6b132b2f..28b99232fc6f 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_read_resources(struct device *dev)
 
 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 @@ static void domain_enable_resources(struct device *dev)
 		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 a188a91fadbe..d6cbc1f120e8 100644
--- a/src/northbridge/amd/agesa/family15tn/Makefile.inc
+++ b/src/northbridge/amd/agesa/family15tn/Makefile.inc
@@ -17,3 +17,8 @@ romstage-y += dimmSpd.c
 
 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 374c74ff691d..eb705b89e9aa 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_read_resources(device_t dev)
 
 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 @@ static void domain_enable_resources(device_t dev)
 		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 000000000000..ad5a14ca986c
--- /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 3cf4ed13552a..25e3cc8f1f09 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 d586e7a36a96..ab2e8e8980e2 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_read_resources(device_t dev)
 
 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 @@ static void domain_enable_resources(device_t dev)
 		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 000000000000..ad5a14ca986c
--- /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 02c384ac8bfe..f2cef990db1d 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 000000000000..0406eeea6659
--- /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 86424d108470..efa649688d2b 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 @@ const char *heap_status_name(int status);
 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 agesa_postcar(struct sysinfo *cb);
 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 7595889a2f3b..b1c56cb777ca 100644
--- a/src/southbridge/amd/agesa/hudson/Makefile.inc
+++ b/src/southbridge/amd/agesa/hudson/Makefile.inc
@@ -10,7 +10,7 @@ ramstage-y += pci.c
 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 6b3af0ef584d..e764ba09fe65 100644
--- a/src/southbridge/amd/cimx/sb800/late.c
+++ b/src/southbridge/amd/cimx/sb800/late.c
@@ -473,6 +473,7 @@ static void sb800_enable(device_t dev)
 	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 @@ static void sb800_enable(device_t dev)
 			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 e722fc1de2e7..29f2069234a5 100644
--- a/src/vendorcode/amd/agesa/common/agesa-entry.c
+++ b/src/vendorcode/amd/agesa/common/agesa-entry.c
@@ -161,11 +161,12 @@ CONST DISPATCH_TABLE ROMDATA DispatchTable[] =
     { 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