AGESA: Move API interface under drivers/

New AGESA support files will be used for binaryPI
platforms as well. Furthermore, some of those should
move from split nb/ sb/ directories to soc/, so move
support files for the API under drivers/.

Change-Id: I549788091de91f61de8b9adc223d52ffb5732235
Signed-off-by: Kyösti Mälkki <kyosti.malkki@gmail.com>
Reviewed-on: https://review.coreboot.org/21455
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>
Reviewed-by: Aaron Durbin <adurbin@chromium.org>

12 files changed, 1979 insertions, 0 deletions

diff --git a/src/drivers/amd/agesa/Kconfig b/src/drivers/amd/agesa/Kconfig
new file mode 100644
index 0000000000..f667b95fc5
--- /dev/null
+++ b/src/drivers/amd/agesa/Kconfig
@@ -0,0 +1,15 @@
+#
+# This file is part of the coreboot project.
+#
+# 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.
+#
+
+config DRIVERS_AMD_PI
+	def_bool n
diff --git a/src/drivers/amd/agesa/Makefile.inc b/src/drivers/amd/agesa/Makefile.inc
new file mode 100644
index 0000000000..4d3dbf5cb9
--- /dev/null
+++ b/src/drivers/amd/agesa/Makefile.inc
@@ -0,0 +1,44 @@
+#
+# This file is part of the coreboot project.
+#
+# 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.
+#
+
+ifeq ($(CONFIG_DRIVERS_AMD_PI),y)
+
+ifneq ($(CONFIG_AGESA_LEGACY_WRAPPER)$(CONFIG_BINARYPI_LEGACY_WRAPPER),y)
+
+romstage-y += romstage.c
+romstage-y += mtrr_fixme.c
+romstage-y += state_machine.c
+
+ramstage-y += state_machine.c
+
+cpu_incs-y += $(src)/drivers/amd/agesa/cache_as_ram.S
+postcar-y += cache_as_ram.S
+
+else
+
+romstage-y += heapmanager.c
+
+endif
+
+romstage-y += def_callouts.c
+romstage-y += eventlog.c
+
+ramstage-y += def_callouts.c
+ramstage-y += eventlog.c
+ramstage-y += heapmanager.c
+ramstage-y += acpi_tables.c
+
+romstage-$(CONFIG_CPU_AMD_AGESA) += oem_s3.c
+ramstage-$(CONFIG_CPU_AMD_AGESA) += oem_s3.c s3_mtrr.c
+
+endif
diff --git a/src/drivers/amd/agesa/acpi_tables.c b/src/drivers/amd/agesa/acpi_tables.c
new file mode 100644
index 0000000000..5335c02f5b
--- /dev/null
+++ b/src/drivers/amd/agesa/acpi_tables.c
@@ -0,0 +1,82 @@
+/*
+ * 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 <northbridge/amd/agesa/state_machine.h>
+#include <northbridge/amd/agesa/agesa_helper.h>
+
+#include "AGESA.h"
+
+/* Fields were removed from the structure and we cannot add them back
+ * without new builds of the binaryPI blobs.
+ */
+#if !IS_ENABLED(CONFIG_CPU_AMD_AGESA_BINARY_PI) || \
+	IS_ENABLED(CONFIG_NORTHBRIDGE_AMD_PI_00630F01) || \
+	IS_ENABLED(CONFIG_NORTHBRIDGE_AMD_PI_00730F01)
+
+#define HAS_ACPI_SRAT 	TRUE
+#define HAS_ACPI_SLIT 	TRUE
+#else
+#define HAS_ACPI_SRAT 	FALSE
+#define HAS_ACPI_SLIT 	FALSE
+#endif
+
+/* We will reference AmdLateParams later to copy ACPI tables. */
+static AMD_LATE_PARAMS *AmdLateParams;
+
+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);
+
+	switch (pick) {
+	case PICK_DMI:
+		return AmdLateParams->DmiTable;
+	case PICK_PSTATE:
+		return AmdLateParams->AcpiPState;
+#if HAS_ACPI_SRAT
+	case PICK_SRAT:
+		return AmdLateParams->AcpiSrat;
+#endif
+#if HAS_ACPI_SLIT
+	case PICK_SLIT:
+		return AmdLateParams->AcpiSlit;
+#endif
+	case PICK_WHEA_MCE:
+		return AmdLateParams->AcpiWheaMce;
+	case PICK_WHEA_CMC:
+		return AmdLateParams->AcpiWheaCmc;
+	case PICK_ALIB:
+		return AmdLateParams->AcpiAlib;
+	case PICK_IVRS:
+		return AmdLateParams->AcpiIvrs;
+	case PICK_CRAT:
+		return AmdLateParams->AcpiCrat;
+	case PICK_CDIT:
+		return AmdLateParams->AcpiCdit;
+	default:
+		return NULL;
+	}
+	return NULL;
+}
diff --git a/src/drivers/amd/agesa/cache_as_ram.S b/src/drivers/amd/agesa/cache_as_ram.S
new file mode 100644
index 0000000000..50242f7a54
--- /dev/null
+++ b/src/drivers/amd/agesa/cache_as_ram.S
@@ -0,0 +1,172 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2011 Advanced Micro Devices, Inc.
+ *
+ * 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.
+ */
+
+/******************************************************************************
+ * AMD Generic Encapsulated Software Architecture
+ *
+ * $Workfile:: cache_as_ram.S
+ *
+ * Description: cache_as_ram.S - AGESA Module Entry Point for GCC complier
+ *
+ ******************************************************************************
+ */
+
+#include "gcccar.inc"
+#include <cpu/x86/cache.h>
+#include <cpu/x86/post_code.h>
+
+.code32
+.globl _cache_as_ram_setup, _cache_as_ram_setup_end
+.globl chipset_teardown_car
+
+_cache_as_ram_setup:
+
+  /* Preserve BIST. */
+  movd %eax, %mm0
+
+  post_code(0xa0)
+
+  /* enable SSE2 128bit instructions */
+  /* Turn on OSFXSR [BIT9] and OSXMMEXCPT [BIT10] onto CR4 register */
+
+  movl %cr4, %eax
+  orl $(3 << 9), %eax
+  movl %eax, %cr4
+
+  post_code(0xa1)
+
+  AMD_ENABLE_STACK
+
+  /* Align the stack. */
+  and     $0xFFFFFFF0, %esp
+
+#ifdef __x86_64__
+  /* switch to 64 bit long mode */
+  mov     %esi, %ecx
+  add     $0, %ecx # core number
+  xor     %eax, %eax
+  lea     (0x1000+0x23)(%ecx), %edi
+  mov     %edi, (%ecx)
+  mov     %eax, 4(%ecx)
+
+  lea     0x1000(%ecx), %edi
+  movl    $0x000000e3, 0x00(%edi)
+  movl    %eax, 0x04(%edi)
+  movl    $0x400000e3, 0x08(%edi)
+  movl    %eax, 0x0c(%edi)
+  movl    $0x800000e3, 0x10(%edi)
+  movl    %eax, 0x14(%edi)
+  movl    $0xc00000e3, 0x18(%edi)
+  movl    %eax, 0x1c(%edi)
+
+  # load ROM based identity mapped page tables
+  mov     %ecx, %eax
+  mov     %eax, %cr3
+
+  # enable PAE
+  mov     %cr4, %eax
+  bts     $5, %eax
+  mov     %eax, %cr4
+
+  # enable long mode
+  mov     $0xC0000080, %ecx
+  rdmsr
+  bts     $8, %eax
+  wrmsr
+
+  # enable paging
+  mov     %cr0, %eax
+  bts     $31, %eax
+  mov     %eax, %cr0
+
+  # use call far to switch to 64-bit code segment
+  ljmp $0x18, $1f
+1:
+
+#endif
+
+  call early_all_cores
+
+  /* Must maintain 16-byte stack alignment here. */
+  pushl $0x0
+  pushl $0x0
+  pushl $0x0
+  movd  %mm0, %eax		/* bist */
+  pushl %eax
+  call  romstage_main
+
+#if IS_ENABLED(CONFIG_POSTCAR_STAGE)
+
+/* We do not return. Execution continues with run_postcar_phase()
+ * calling to chipset_teardown_car below.
+ */
+  jmp postcar_entry_failure
+
+chipset_teardown_car:
+
+/*
+ * Retrieve return address from stack as it will get trashed below if
+ * execution is utilizing the cache-as-ram stack.
+ */
+  pop %esp
+
+#else
+
+  movl  %eax, %esp
+
+/* Register %esp is new stacktop for remaining of romstage. */
+
+#endif
+
+  /* Disable cache */
+  movl	%cr0, %eax
+  orl	$CR0_CacheDisable, %eax
+  movl	%eax, %cr0
+
+/* Register %esp is preserved in AMD_DISABLE_STACK. */
+  AMD_DISABLE_STACK
+
+#if IS_ENABLED(CONFIG_POSTCAR_STAGE)
+
+  jmp *%esp
+
+#else
+
+  /* enable cache */
+  movl %cr0, %eax
+  andl $0x9fffffff, %eax
+  movl %eax, %cr0
+
+  call  romstage_after_car
+
+#endif
+
+  /* Should never see this postcode */
+  post_code(0xaf)
+
+stop:
+  hlt
+  jmp stop
+
+/* These are here for linking purposes. */
+.weak early_all_cores, romstage_main
+early_all_cores:
+romstage_main:
+postcar_entry_failure:
+  /* Should never see this postcode */
+  post_code(0xae)
+  jmp stop
+
+_cache_as_ram_setup_end:
diff --git a/src/drivers/amd/agesa/def_callouts.c b/src/drivers/amd/agesa/def_callouts.c
new file mode 100644
index 0000000000..21530a1e00
--- /dev/null
+++ b/src/drivers/amd/agesa/def_callouts.c
@@ -0,0 +1,204 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2011 Advanced Micro Devices, Inc.
+ * Copyright (C) 2013 Sage Electronic Engineering, LLC
+ *
+ * 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 <cbfs.h>
+#include <spd_bin.h>
+#include <string.h>
+
+#include "AGESA.h"
+#include "amdlib.h"
+#include "Ids.h"
+#include <northbridge/amd/agesa/state_machine.h>
+#include <northbridge/amd/agesa/BiosCallOuts.h>
+#include <northbridge/amd/agesa/dimmSpd.h>
+
+#if IS_ENABLED(CONFIG_NORTHBRIDGE_AMD_PI)
+#if IS_ENABLED(CONFIG_ARCH_ROMSTAGE_X86_64) || \
+	IS_ENABLED(CONFIG_ARCH_RAMSTAGE_X86_64)
+#error "FIXME: CALLOUT_ENTRY is UINT32 Data, not UINT Data"
+#endif
+#endif
+
+AGESA_STATUS GetBiosCallout (UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	AGESA_STATUS status;
+	UINTN i;
+
+	if (HAS_LEGACY_WRAPPER || ENV_RAMSTAGE) {
+		/* One HeapManager serves them all. */
+		status = HeapManagerCallout(Func, Data, ConfigPtr);
+		if (status != AGESA_UNSUPPORTED)
+			return status;
+	}
+
+#if HAS_AGESA_FCH_OEM_CALLOUT
+	if (!HAS_LEGACY_WRAPPER && Func == AGESA_FCH_OEM_CALLOUT) {
+		agesa_fch_oem_config(Data, ConfigPtr);
+		return AGESA_SUCCESS;
+	}
+#endif
+
+	for (i = 0; i < BiosCalloutsLen; i++) {
+		if (BiosCallouts[i].CalloutName == Func)
+			break;
+	}
+	if (i >= BiosCalloutsLen)
+		return AGESA_UNSUPPORTED;
+
+	return BiosCallouts[i].CalloutPtr (Func, Data, ConfigPtr);
+}
+
+AGESA_STATUS agesa_NoopUnsupported (UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	return AGESA_UNSUPPORTED;
+}
+
+AGESA_STATUS agesa_NoopSuccess (UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	return AGESA_SUCCESS;
+}
+
+AGESA_STATUS agesa_EmptyIdsInitData (UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	IDS_NV_ITEM *IdsPtr = ((IDS_CALLOUT_STRUCT *) ConfigPtr)->IdsNvPtr;
+	if (Data == IDS_CALLOUT_INIT)
+		IdsPtr[0].IdsNvValue = IdsPtr[0].IdsNvId = 0xffff;
+	return AGESA_SUCCESS;
+}
+
+AGESA_STATUS agesa_Reset (UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	AGESA_STATUS        Status;
+	UINT8                 Value;
+	UINTN               ResetType;
+	AMD_CONFIG_PARAMS   *StdHeader;
+
+	ResetType = Data;
+	StdHeader = ConfigPtr;
+
+	//
+	// Perform the RESET based upon the ResetType. In case of
+	// WARM_RESET_WHENVER and COLD_RESET_WHENEVER, the request will go to
+	// AmdResetManager. During the critical condition, where reset is required
+	// immediately, the reset will be invoked directly by writing 0x04 to port
+	// 0xCF9 (Reset Port).
+	//
+	switch (ResetType) {
+	case WARM_RESET_WHENEVER:
+	case COLD_RESET_WHENEVER:
+		break;
+
+	case WARM_RESET_IMMEDIATELY:
+	case COLD_RESET_IMMEDIATELY:
+		Value = 0x06;
+		LibAmdIoWrite (AccessWidth8, 0xCf9, &Value, StdHeader);
+		break;
+
+	default:
+		break;
+	}
+
+	Status = 0;
+	return Status;
+}
+
+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));
+
+	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);
+#else
+	status = module_dispatch(AMD_LATE_RUN_AP_TASK, &ApExeParams.StdHeader);
+#endif
+
+	ASSERT(status == AGESA_SUCCESS);
+	return status;
+}
+
+#if defined(AGESA_GNB_GFX_GET_VBIOS_IMAGE)
+AGESA_STATUS agesa_GfxGetVbiosImage(UINT32 Func, UINTN FchData, VOID *ConfigPrt)
+{
+	GFX_VBIOS_IMAGE_INFO  *pVbiosImageInfo = (GFX_VBIOS_IMAGE_INFO *)ConfigPrt;
+	pVbiosImageInfo->ImagePtr = cbfs_boot_map_with_leak(
+			"pci"CONFIG_VGA_BIOS_ID".rom",
+			CBFS_TYPE_OPTIONROM, NULL);
+	/* printk(BIOS_DEBUG, "IMGptr=%x\n", pVbiosImageInfo->ImagePtr); */
+	return pVbiosImageInfo->ImagePtr == NULL ? AGESA_WARNING : AGESA_SUCCESS;
+}
+#endif
+
+AGESA_STATUS agesa_ReadSpd (UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	AGESA_STATUS Status = AGESA_UNSUPPORTED;
+#ifdef __PRE_RAM__
+	Status = AmdMemoryReadSPD (Func, Data, ConfigPtr);
+#endif
+	return Status;
+}
+
+AGESA_STATUS agesa_ReadSpd_from_cbfs(UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	AGESA_STATUS Status = AGESA_UNSUPPORTED;
+#ifdef __PRE_RAM__
+	AGESA_READ_SPD_PARAMS *info = ConfigPtr;
+	if (info->MemChannelId > 0)
+		return AGESA_UNSUPPORTED;
+	if (info->SocketId != 0)
+		return AGESA_UNSUPPORTED;
+	if (info->DimmId != 0)
+		return AGESA_UNSUPPORTED;
+
+	/* Read index 0, first SPD_SIZE bytes of spd.bin file. */
+	if (read_ddr3_spd_from_cbfs((u8*)info->Buffer, 0) < 0)
+		die("No SPD data\n");
+
+	Status = AGESA_SUCCESS;
+#endif
+	return Status;
+}
+
+#if HAS_AGESA_FCH_OEM_CALLOUT
+void agesa_fch_oem_config(uintptr_t Data, AMD_CONFIG_PARAMS *StdHeader)
+{
+	/* FIXME: CAR_GLOBAL needed here to pass sysinfo. */
+	struct sysinfo *cb_NA = NULL;
+
+	if (StdHeader->Func == AMD_INIT_RESET) {
+		printk(BIOS_DEBUG, "Fch OEM config in INIT RESET\n");
+		board_FCH_InitReset(cb_NA, (FCH_RESET_DATA_BLOCK *)Data);
+	} else if (StdHeader->Func == AMD_INIT_ENV) {
+		printk(BIOS_DEBUG, "Fch OEM config in INIT ENV\n");
+		board_FCH_InitEnv(cb_NA, (FCH_DATA_BLOCK *)Data);
+	}
+}
+#endif
diff --git a/src/drivers/amd/agesa/eventlog.c b/src/drivers/amd/agesa/eventlog.c
new file mode 100644
index 0000000000..33e5590530
--- /dev/null
+++ b/src/drivers/amd/agesa/eventlog.c
@@ -0,0 +1,171 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * 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 <console/console.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <northbridge/amd/agesa/state_machine.h>
+#include <northbridge/amd/agesa/BiosCallOuts.h>
+#include "amdlib.h"
+#include "AGESA.h"
+#include "AMD.h"
+
+#include <heapManager.h>
+
+static const char undefined[] = "undefined";
+
+/* Match order of enum AGESA_STRUCT_NAME. */
+static const char *AgesaFunctionNameStr[] = {
+	"AmdInitRecovery", "AmdCreateStruct", "AmdInitEarly", "AmdInitEnv", "AmdInitLate",
+	"AmdInitMid", "AmdInitPost", "AmdInitReset", "AmdInitResume", "AmdReleaseStruct",
+	"AmdS3LateRestore","AmdS3Save", "AmdGetApicId", "AmdGetPciAddress", "AmdIdentifyCore",
+	"AmdReadEventLog", "AmdGetAvailableExeCacheSize", "AmdLateRunApTask", "AmdIdentifyDimm",
+};
+
+/* heapManager.h */
+static const char *HeapStatusStr[] = {
+	"DoNotExistYet", "LocalCache", "TempMem", "SystemMem", "DoNotExistAnymore","S3Resume"
+};
+
+/* This function has to match with enumeration of AGESA_STRUCT_NAME defined
+ * inside AMD.h header file. Unfortunately those are different across
+ * different vendorcode subtrees.
+ *
+ * TBD: Fix said header or move this function together with the strings above
+ * under vendorcode/ tree.
+ */
+
+const char *agesa_struct_name(int state)
+{
+#if IS_ENABLED(CONFIG_CPU_AMD_AGESA_OPENSOURCE)
+	if ((state < AMD_INIT_RECOVERY) || (state > AMD_IDENTIFY_DIMMS))
+		return undefined;
+
+	int index = state - AMD_INIT_RECOVERY;
+#else
+	state >>= 12;
+	if ((state < AMD_INIT_RECOVERY >> 12) || (state > AMD_IDENTIFY_DIMMS >> 12))
+		return undefined;
+
+	int index = state - (AMD_INIT_RECOVERY >> 12);
+#endif
+	return AgesaFunctionNameStr[index];
+}
+
+const char *heap_status_name(int status)
+{
+	if ((status < HEAP_DO_NOT_EXIST_YET) || (status > HEAP_S3_RESUME))
+		return undefined;
+
+	int index = status - HEAP_DO_NOT_EXIST_YET;
+	return HeapStatusStr[index];
+}
+
+/*
+ * Possible AGESA_STATUS values:
+ *
+ * 0x0 = AGESA_SUCCESS
+ * 0x1 = AGESA_UNSUPPORTED
+ * 0x2 = AGESA_BOUNDS_CHK
+ * 0x3 = AGESA_ALERT
+ * 0x4 = AGESA_WARNING
+ * 0x5 = AGESA_ERROR
+ * 0x6 = AGESA_CRITICAL
+ * 0x7 = AGESA_FATAL
+ */
+static const char * decodeAGESA_STATUS(AGESA_STATUS sret)
+{
+	const char* statusStrings[] = { "AGESA_SUCCESS", "AGESA_UNSUPPORTED",
+					"AGESA_BOUNDS_CHK", "AGESA_ALERT",
+					"AGESA_WARNING", "AGESA_ERROR",
+					"AGESA_CRITICAL", "AGESA_FATAL"
+					};
+	if (sret > 7) return "unknown"; /* Non-AGESA error code */
+	return statusStrings[sret];
+}
+
+static void show_event(EVENT_PARAMS *Event)
+{
+	printk(BIOS_DEBUG,"\nEventLog:  EventClass = %x, EventInfo = %x.\n",
+			(unsigned int)Event->EventClass,
+			(unsigned int)Event->EventInfo);
+	printk(BIOS_DEBUG,"  Param1 = %x, Param2 = %x.\n",
+			(unsigned int)Event->DataParam1,
+			(unsigned int)Event->DataParam2);
+	printk(BIOS_DEBUG,"  Param3 = %x, Param4 = %x.\n",
+			(unsigned int)Event->DataParam3,
+			(unsigned int)Event->DataParam4);
+}
+
+#define MAX_LOG_ENTRIES 100
+
+static void amd_flush_eventlog(EVENT_PARAMS *Event)
+{
+	int i = 0;
+
+	do {
+		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)
+			return;
+		show_event(Event);
+	} while (++i < MAX_LOG_ENTRIES);
+}
+
+void agesawrapper_trace(AGESA_STATUS ret, AMD_CONFIG_PARAMS *StdHeader,
+	const char *func)
+{
+	EVENT_PARAMS AmdEventParams;
+
+	printk(BIOS_DEBUG, "%s() returned %s\n", func, decodeAGESA_STATUS(ret));
+	if (ret == AGESA_SUCCESS)
+		return;
+
+	memset(&AmdEventParams, 0, sizeof(EVENT_PARAMS));
+
+	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);
+}
+
+AGESA_STATUS agesawrapper_amdreadeventlog (UINT8 HeapStatus)
+{
+	EVENT_PARAMS AmdEventParams;
+
+	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 = HeapStatus;
+
+	amd_flush_eventlog(&AmdEventParams);
+
+	return AGESA_SUCCESS;
+}
diff --git a/src/drivers/amd/agesa/heapmanager.c b/src/drivers/amd/agesa/heapmanager.c
new file mode 100644
index 0000000000..04775cd36e
--- /dev/null
+++ b/src/drivers/amd/agesa/heapmanager.c
@@ -0,0 +1,374 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * 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 "AGESA.h"
+#include "amdlib.h"
+
+#include <cbmem.h>
+#include <northbridge/amd/agesa/agesa_helper.h>
+#include <northbridge/amd/agesa/BiosCallOuts.h>
+
+#include <arch/acpi.h>
+#include <string.h>
+
+/* BIOS_HEAP_START_ADDRESS is only for cold boots. */
+#define BIOS_HEAP_SIZE		0x30000
+#define BIOS_HEAP_START_ADDRESS	0x010000000
+
+#if IS_ENABLED(CONFIG_HAVE_ACPI_RESUME) && (HIGH_MEMORY_SCRATCH < BIOS_HEAP_SIZE)
+#error Increase HIGH_MEMORY_SCRATCH allocation
+#endif
+
+void *GetHeapBase(void)
+{
+	void *heap = (void *)BIOS_HEAP_START_ADDRESS;
+
+	if (acpi_is_wakeup_s3()) {
+		/* FIXME: For S3 resume path, buffer is in CBMEM
+		 * with some arbitrary header. */
+		heap = cbmem_find(CBMEM_ID_RESUME_SCRATCH);
+		heap += 0x10;
+	}
+
+	return heap;
+}
+
+void EmptyHeap(void)
+{
+	void *base = GetHeapBase();
+	memset(base, 0, BIOS_HEAP_SIZE);
+
+	printk(BIOS_DEBUG, "Wiped HEAP at [%08x - %08x]\n",
+		(unsigned int)(uintptr_t) base, (unsigned int)(uintptr_t) base + BIOS_HEAP_SIZE - 1);
+}
+
+#if defined(HEAP_CALLOUT_RUNTIME) && ENV_RAMSTAGE
+
+#define AGESA_RUNTIME_SIZE 4096
+static AGESA_STATUS alloc_cbmem(AGESA_BUFFER_PARAMS *AllocParams)
+{
+	static unsigned int used = 0;
+	void *p = cbmem_find(CBMEM_ID_AGESA_RUNTIME);
+
+	if ((AGESA_RUNTIME_SIZE - used) < AllocParams->BufferLength) {
+		return AGESA_BOUNDS_CHK;
+	}
+
+	/* first time allocation */
+	if (!p) {
+		p = cbmem_add(CBMEM_ID_AGESA_RUNTIME, AGESA_RUNTIME_SIZE);
+		if (!p)
+			return AGESA_BOUNDS_CHK;
+	}
+
+	AllocParams->BufferPointer = p + used;
+	used += AllocParams->BufferLength;
+	return AGESA_SUCCESS;
+}
+#endif
+
+typedef struct _BIOS_HEAP_MANAGER {
+	UINT32 StartOfAllocatedNodes;
+	UINT32 StartOfFreedNodes;
+} BIOS_HEAP_MANAGER;
+
+typedef struct _BIOS_BUFFER_NODE {
+	UINT32 BufferHandle;
+	UINT32 BufferSize;
+	UINT32 NextNodeOffset;
+} BIOS_BUFFER_NODE;
+
+static AGESA_STATUS agesa_AllocateBuffer(BIOS_HEAP_MANAGER *BiosHeapBasePtr,
+	AGESA_BUFFER_PARAMS *AllocParams)
+{
+	UINT32              AvailableHeapSize;
+	UINT8               *BiosHeapBaseAddr = (void *)BiosHeapBasePtr;
+	UINT32              CurrNodeOffset;
+	UINT32              PrevNodeOffset;
+	UINT32              FreedNodeOffset;
+	UINT32              BestFitNodeOffset;
+	UINT32              BestFitPrevNodeOffset;
+	UINT32              NextFreeOffset;
+	BIOS_BUFFER_NODE   *CurrNodePtr;
+	BIOS_BUFFER_NODE   *FreedNodePtr;
+	BIOS_BUFFER_NODE   *BestFitNodePtr;
+	BIOS_BUFFER_NODE   *BestFitPrevNodePtr;
+	BIOS_BUFFER_NODE   *NextFreePtr;
+
+	AllocParams->BufferPointer = NULL;
+	AvailableHeapSize = BIOS_HEAP_SIZE - sizeof(BIOS_HEAP_MANAGER);
+
+	if (BiosHeapBasePtr->StartOfAllocatedNodes == 0) {
+		/* First allocation */
+		CurrNodeOffset = sizeof(BIOS_HEAP_MANAGER);
+		CurrNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + CurrNodeOffset);
+		CurrNodePtr->BufferHandle = AllocParams->BufferHandle;
+		CurrNodePtr->BufferSize = AllocParams->BufferLength;
+		CurrNodePtr->NextNodeOffset = 0;
+		AllocParams->BufferPointer = (UINT8 *) CurrNodePtr + sizeof(BIOS_BUFFER_NODE);
+
+		/* Update the remaining free space */
+		FreedNodeOffset = CurrNodeOffset + CurrNodePtr->BufferSize + sizeof(BIOS_BUFFER_NODE);
+		FreedNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + FreedNodeOffset);
+		FreedNodePtr->BufferSize = AvailableHeapSize - sizeof(BIOS_BUFFER_NODE) - CurrNodePtr->BufferSize;
+		FreedNodePtr->NextNodeOffset = 0;
+
+		/* Update the offsets for Allocated and Freed nodes */
+		BiosHeapBasePtr->StartOfAllocatedNodes = CurrNodeOffset;
+		BiosHeapBasePtr->StartOfFreedNodes = FreedNodeOffset;
+	} else {
+		/* Find out whether BufferHandle has been allocated on the heap.
+		 * If it has, return AGESA_BOUNDS_CHK.
+		 */
+		CurrNodeOffset = BiosHeapBasePtr->StartOfAllocatedNodes;
+		CurrNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + CurrNodeOffset);
+
+		while (CurrNodeOffset != 0) {
+			CurrNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + CurrNodeOffset);
+			if (CurrNodePtr->BufferHandle == AllocParams->BufferHandle) {
+				return AGESA_BOUNDS_CHK;
+			}
+			CurrNodeOffset = CurrNodePtr->NextNodeOffset;
+			/* If BufferHandle has not been allocated on the heap, CurrNodePtr here points
+			 * to the end of the allocated nodes list.
+			 */
+		}
+		/* Find the node that best fits the requested buffer size */
+		FreedNodeOffset = BiosHeapBasePtr->StartOfFreedNodes;
+		PrevNodeOffset = FreedNodeOffset;
+		BestFitNodeOffset = 0;
+		BestFitPrevNodeOffset = 0;
+		while (FreedNodeOffset != 0) {
+			FreedNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + FreedNodeOffset);
+			if (FreedNodePtr->BufferSize >= (AllocParams->BufferLength + sizeof(BIOS_BUFFER_NODE))) {
+				if (BestFitNodeOffset == 0) {
+					/* First node that fits the requested buffer size */
+					BestFitNodeOffset = FreedNodeOffset;
+					BestFitPrevNodeOffset = PrevNodeOffset;
+				} else {
+					/* Find out whether current node is a better fit than the previous nodes */
+					BestFitNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + BestFitNodeOffset);
+					if (BestFitNodePtr->BufferSize > FreedNodePtr->BufferSize) {
+						BestFitNodeOffset = FreedNodeOffset;
+						BestFitPrevNodeOffset = PrevNodeOffset;
+					}
+				}
+			}
+			PrevNodeOffset = FreedNodeOffset;
+			FreedNodeOffset = FreedNodePtr->NextNodeOffset;
+		} /* end of while loop */
+
+		if (BestFitNodeOffset == 0) {
+			/* If we could not find a node that fits the requested buffer
+			 * size, return AGESA_BOUNDS_CHK.
+			 */
+			return AGESA_BOUNDS_CHK;
+		} else {
+			BestFitNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + BestFitNodeOffset);
+			BestFitPrevNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + BestFitPrevNodeOffset);
+
+			/* If BestFitNode is larger than the requested buffer, fragment the node further */
+			if (BestFitNodePtr->BufferSize > (AllocParams->BufferLength + sizeof(BIOS_BUFFER_NODE))) {
+				NextFreeOffset = BestFitNodeOffset + AllocParams->BufferLength + sizeof(BIOS_BUFFER_NODE);
+
+				NextFreePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + NextFreeOffset);
+				NextFreePtr->BufferSize = BestFitNodePtr->BufferSize - (AllocParams->BufferLength + sizeof(BIOS_BUFFER_NODE));
+				NextFreePtr->NextNodeOffset = BestFitNodePtr->NextNodeOffset;
+			} else {
+				/* Otherwise, next free node is NextNodeOffset of BestFitNode */
+				NextFreeOffset = BestFitNodePtr->NextNodeOffset;
+			}
+
+			/* If BestFitNode is the first buffer in the list, then update
+			 * StartOfFreedNodes to reflect the new free node.
+			 */
+			if (BestFitNodeOffset == BiosHeapBasePtr->StartOfFreedNodes) {
+				BiosHeapBasePtr->StartOfFreedNodes = NextFreeOffset;
+			} else {
+				BestFitPrevNodePtr->NextNodeOffset = NextFreeOffset;
+			}
+
+			/* Add BestFitNode to the list of Allocated nodes */
+			CurrNodePtr->NextNodeOffset = BestFitNodeOffset;
+			BestFitNodePtr->BufferSize = AllocParams->BufferLength;
+			BestFitNodePtr->BufferHandle = AllocParams->BufferHandle;
+			BestFitNodePtr->NextNodeOffset = 0;
+
+			/* Remove BestFitNode from list of Freed nodes */
+			AllocParams->BufferPointer = (UINT8 *) BestFitNodePtr + sizeof(BIOS_BUFFER_NODE);
+		}
+	}
+
+	return AGESA_SUCCESS;
+}
+
+static AGESA_STATUS agesa_DeallocateBuffer(BIOS_HEAP_MANAGER *BiosHeapBasePtr,
+	AGESA_BUFFER_PARAMS *AllocParams)
+{
+	UINT8               *BiosHeapBaseAddr = (void *)BiosHeapBasePtr;
+	UINT32              AllocNodeOffset;
+	UINT32              PrevNodeOffset;
+	UINT32              NextNodeOffset;
+	UINT32              FreedNodeOffset;
+	UINT32              EndNodeOffset;
+	BIOS_BUFFER_NODE   *AllocNodePtr;
+	BIOS_BUFFER_NODE   *PrevNodePtr;
+	BIOS_BUFFER_NODE   *FreedNodePtr;
+	BIOS_BUFFER_NODE   *NextNodePtr;
+
+	/* Find target node to deallocate in list of allocated nodes.
+	 * Return AGESA_BOUNDS_CHK if the BufferHandle is not found.
+	 */
+	AllocNodeOffset = BiosHeapBasePtr->StartOfAllocatedNodes;
+	AllocNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + AllocNodeOffset);
+	PrevNodeOffset = AllocNodeOffset;
+
+	while (AllocNodePtr->BufferHandle !=  AllocParams->BufferHandle) {
+		if (AllocNodePtr->NextNodeOffset == 0) {
+			return AGESA_BOUNDS_CHK;
+		}
+		PrevNodeOffset = AllocNodeOffset;
+		AllocNodeOffset = AllocNodePtr->NextNodeOffset;
+		AllocNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + AllocNodeOffset);
+	}
+
+	/* Remove target node from list of allocated nodes */
+	PrevNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + PrevNodeOffset);
+	PrevNodePtr->NextNodeOffset = AllocNodePtr->NextNodeOffset;
+
+	/* Zero out the buffer, and clear the BufferHandle */
+	LibAmdMemFill ((UINT8 *)AllocNodePtr + sizeof(BIOS_BUFFER_NODE), 0, AllocNodePtr->BufferSize, &(AllocParams->StdHeader));
+	AllocNodePtr->BufferHandle = 0;
+	AllocNodePtr->BufferSize += sizeof(BIOS_BUFFER_NODE);
+
+	/* Add deallocated node in order to the list of freed nodes */
+	FreedNodeOffset = BiosHeapBasePtr->StartOfFreedNodes;
+	FreedNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + FreedNodeOffset);
+
+	EndNodeOffset = AllocNodeOffset + AllocNodePtr->BufferSize;
+
+	if (AllocNodeOffset < FreedNodeOffset) {
+		/* Add to the start of the freed list */
+		if (EndNodeOffset == FreedNodeOffset) {
+			/* If the freed node is adjacent to the first node in the list, concatenate both nodes */
+			AllocNodePtr->BufferSize += FreedNodePtr->BufferSize;
+			AllocNodePtr->NextNodeOffset = FreedNodePtr->NextNodeOffset;
+
+			/* Clear the BufferSize and NextNodeOffset of the previous first node */
+			FreedNodePtr->BufferSize = 0;
+			FreedNodePtr->NextNodeOffset = 0;
+
+		} else {
+			/* Otherwise, add freed node to the start of the list
+			 * Update NextNodeOffset and BufferSize to include the
+			 * size of BIOS_BUFFER_NODE.
+			 */
+			AllocNodePtr->NextNodeOffset = FreedNodeOffset;
+		}
+		/* Update StartOfFreedNodes to the new first node */
+		BiosHeapBasePtr->StartOfFreedNodes = AllocNodeOffset;
+	} else {
+		/* Traverse list of freed nodes to find where the deallocated node
+		 * should be placed.
+		 */
+		NextNodeOffset = FreedNodeOffset;
+		NextNodePtr = FreedNodePtr;
+		while (AllocNodeOffset > NextNodeOffset) {
+			PrevNodeOffset = NextNodeOffset;
+			if (NextNodePtr->NextNodeOffset == 0) {
+				break;
+			}
+			NextNodeOffset = NextNodePtr->NextNodeOffset;
+			NextNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + NextNodeOffset);
+		}
+
+		/* If deallocated node is adjacent to the next node,
+		 * concatenate both nodes.
+		 */
+		if (NextNodeOffset == EndNodeOffset) {
+			NextNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + NextNodeOffset);
+			AllocNodePtr->BufferSize += NextNodePtr->BufferSize;
+			AllocNodePtr->NextNodeOffset = NextNodePtr->NextNodeOffset;
+
+			NextNodePtr->BufferSize = 0;
+			NextNodePtr->NextNodeOffset = 0;
+		} else {
+			/*AllocNodePtr->NextNodeOffset = FreedNodePtr->NextNodeOffset; */
+			AllocNodePtr->NextNodeOffset = NextNodeOffset;
+		}
+		/* If deallocated node is adjacent to the previous node,
+		 * concatenate both nodes.
+		 */
+		PrevNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + PrevNodeOffset);
+		EndNodeOffset = PrevNodeOffset + PrevNodePtr->BufferSize;
+		if (AllocNodeOffset == EndNodeOffset) {
+			PrevNodePtr->NextNodeOffset = AllocNodePtr->NextNodeOffset;
+			PrevNodePtr->BufferSize += AllocNodePtr->BufferSize;
+
+			AllocNodePtr->BufferSize = 0;
+			AllocNodePtr->NextNodeOffset = 0;
+		} else {
+			PrevNodePtr->NextNodeOffset = AllocNodeOffset;
+		}
+	}
+	return AGESA_SUCCESS;
+}
+
+static AGESA_STATUS agesa_LocateBuffer(BIOS_HEAP_MANAGER *BiosHeapBasePtr,
+	AGESA_BUFFER_PARAMS *AllocParams)
+{
+	UINT32              AllocNodeOffset;
+	UINT8               *BiosHeapBaseAddr = (void *)BiosHeapBasePtr;
+	BIOS_BUFFER_NODE   *AllocNodePtr;
+
+	AllocNodeOffset = BiosHeapBasePtr->StartOfAllocatedNodes;
+	AllocNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + AllocNodeOffset);
+
+	while (AllocParams->BufferHandle != AllocNodePtr->BufferHandle) {
+		if (AllocNodePtr->NextNodeOffset == 0) {
+			AllocParams->BufferPointer = NULL;
+			AllocParams->BufferLength = 0;
+			return AGESA_BOUNDS_CHK;
+		} else {
+			AllocNodeOffset = AllocNodePtr->NextNodeOffset;
+			AllocNodePtr = (BIOS_BUFFER_NODE *) (BiosHeapBaseAddr + AllocNodeOffset);
+		}
+	}
+
+	AllocParams->BufferPointer = (UINT8 *) ((UINT8 *) AllocNodePtr + sizeof(BIOS_BUFFER_NODE));
+	AllocParams->BufferLength = AllocNodePtr->BufferSize;
+
+	return AGESA_SUCCESS;
+
+}
+
+AGESA_STATUS HeapManagerCallout(UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	AGESA_BUFFER_PARAMS *AllocParams = ConfigPtr;
+
+#if defined(HEAP_CALLOUT_RUNTIME) && ENV_RAMSTAGE
+	if (Func == AGESA_ALLOCATE_BUFFER && Data == HEAP_CALLOUT_RUNTIME)
+		return alloc_cbmem(AllocParams);
+#endif
+
+	/* Must not call GetHeapBase() in AGESA_UNSUPPORTED path. */
+	if (Func == AGESA_LOCATE_BUFFER)
+		return agesa_LocateBuffer(GetHeapBase(), AllocParams);
+	else if (Func == AGESA_ALLOCATE_BUFFER)
+		return agesa_AllocateBuffer(GetHeapBase(), AllocParams);
+	else if (Func == AGESA_DEALLOCATE_BUFFER)
+		return agesa_DeallocateBuffer(GetHeapBase(), AllocParams);
+
+	return AGESA_UNSUPPORTED;
+}
diff --git a/src/drivers/amd/agesa/mtrr_fixme.c b/src/drivers/amd/agesa/mtrr_fixme.c
new file mode 100644
index 0000000000..1fbb55318d
--- /dev/null
+++ b/src/drivers/amd/agesa/mtrr_fixme.c
@@ -0,0 +1,100 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * 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 <arch/cpu.h>
+#include <cbmem.h>
+#include <console/console.h>
+#include <cpu/amd/mtrr.h>
+#include <cpu/cpu.h>
+#include <cpu/x86/cache.h>
+#include <cpu/x86/msr.h>
+#include <cpu/x86/mtrr.h>
+#include <northbridge/amd/agesa/agesa_helper.h>
+
+static void set_range_uc(u32 base, u32 size)
+{
+	int i, max_var_mtrrs;
+	msr_t msr;
+	msr = rdmsr(MTRR_CAP_MSR);
+	max_var_mtrrs = msr.lo & MTRR_CAP_VCNT;
+
+	for (i = 0; i < max_var_mtrrs; i++) {
+		msr = rdmsr(MTRR_PHYS_MASK(i));
+		if (!(msr.lo & MTRR_PHYS_MASK_VALID))
+				break;
+	}
+	if (i == max_var_mtrrs)
+		die("Run out of unused MTRRs\n");
+
+	msr.hi = 0;
+	msr.lo = base | MTRR_TYPE_UNCACHEABLE;
+	wrmsr(MTRR_PHYS_BASE(i), msr);
+
+	msr.hi = (1 << (cpu_phys_address_size() - 32)) - 1;
+	msr.lo = ~(size - 1) | MTRR_PHYS_MASK_VALID;
+	wrmsr(MTRR_PHYS_MASK(i), msr);
+}
+
+void fixup_cbmem_to_UC(int s3resume)
+{
+	if (s3resume)
+		return;
+
+	/* For normal path, INIT_POST has returned with all
+	 * memory set WB cacheable. But we need CBMEM as UC
+	 * to make CAR teardown with invalidation without
+	 * writeback possible.
+	 */
+
+	uintptr_t top_of_ram = (uintptr_t) cbmem_top();
+	top_of_ram = ALIGN_UP(top_of_ram, 4 * MiB);
+
+	set_range_uc(top_of_ram - 4 * MiB, 4 * MiB);
+	set_range_uc(top_of_ram - 8 * MiB, 4 * MiB);
+}
+
+void recover_postcar_frame(struct postcar_frame *pcf, int s3resume)
+{
+	msr_t base, mask;
+	int i;
+
+	/* Replicate non-UC MTRRs as left behind by AGESA.
+	 */
+	for (i = 0; i < pcf->max_var_mtrrs; i++) {
+		mask = rdmsr(MTRR_PHYS_MASK(i));
+		base = rdmsr(MTRR_PHYS_BASE(i));
+		u32 size = ~(mask.lo & ~0xfff) + 1;
+		u8 type = base.lo & 0x7;
+		base.lo &= ~0xfff;
+
+		if (!(mask.lo & MTRR_PHYS_MASK_VALID) ||
+			(type == MTRR_TYPE_UNCACHEABLE))
+			continue;
+
+		postcar_frame_add_mtrr(pcf, base.lo, size, type);
+	}
+
+	/* For S3 resume path, INIT_RESUME does not return with
+	 * memory covering CBMEM set as WB cacheable. For better
+	 * speed make them WB after CAR teardown.
+	 */
+	if (s3resume) {
+		uintptr_t top_of_ram = (uintptr_t) cbmem_top();
+		top_of_ram = ALIGN_DOWN(top_of_ram, 4*MiB);
+
+		postcar_frame_add_mtrr(pcf, top_of_ram - 4*MiB, 4*MiB,
+			MTRR_TYPE_WRBACK);
+		postcar_frame_add_mtrr(pcf, top_of_ram - 8*MiB, 4*MiB,
+			MTRR_TYPE_WRBACK);
+	}
+}
diff --git a/src/drivers/amd/agesa/oem_s3.c b/src/drivers/amd/agesa/oem_s3.c
new file mode 100644
index 0000000000..1ca6e5b2f6
--- /dev/null
+++ b/src/drivers/amd/agesa/oem_s3.c
@@ -0,0 +1,165 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2012 Advanced Micro Devices, Inc.
+ *
+ * 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 <spi-generic.h>
+#include <spi_flash.h>
+#include <string.h>
+#include <cbmem.h>
+#include <program_loading.h>
+#include <northbridge/amd/agesa/state_machine.h>
+#include <AGESA.h>
+#include <northbridge/amd/agesa/agesa_helper.h>
+
+typedef enum {
+	S3DataTypeNonVolatile = 0,	///< NonVolatile Data Type
+	S3DataTypeMTRR			///< MTRR storage
+} S3_DATA_TYPE;
+
+/* The size needs to be 4k aligned, which is the sector size of most flashes. */
+#define S3_DATA_MTRR_SIZE			0x1000
+#define S3_DATA_NONVOLATILE_SIZE	0x1000
+
+#if IS_ENABLED(CONFIG_HAVE_ACPI_RESUME) && \
+	(S3_DATA_MTRR_SIZE + S3_DATA_NONVOLATILE_SIZE) > CONFIG_S3_DATA_SIZE
+#error "Please increase the value of S3_DATA_SIZE"
+#endif
+
+static void get_s3nv_data(S3_DATA_TYPE S3DataType, uintptr_t *pos, uintptr_t *len)
+{
+	/* FIXME: Find file from CBFS. */
+	u32 s3_data = CONFIG_S3_DATA_POS;
+
+	switch (S3DataType) {
+	case S3DataTypeMTRR:
+		*pos = s3_data;
+		*len = S3_DATA_MTRR_SIZE;
+		break;
+	case S3DataTypeNonVolatile:
+		*pos = s3_data + S3_DATA_MTRR_SIZE;
+		*len = S3_DATA_NONVOLATILE_SIZE;
+		break;
+	default:
+		*pos = 0;
+		*len = 0;
+		break;
+	}
+}
+
+AGESA_STATUS OemInitResume(AMD_S3_PARAMS *dataBlock)
+{
+	uintptr_t pos, size;
+	get_s3nv_data(S3DataTypeNonVolatile, &pos, &size);
+
+	u32 len = *(u32*)pos;
+
+	/* Test for uninitialized s3nv data in SPI. */
+	if (len == 0 || len == (u32)-1ULL)
+		return AGESA_FATAL;
+
+	dataBlock->NvStorageSize = len;
+	dataBlock->NvStorage = (void *) (pos + sizeof(u32));
+	return AGESA_SUCCESS;
+}
+
+AGESA_STATUS OemS3LateRestore(AMD_S3_PARAMS *dataBlock)
+{
+	char *heap = cbmem_find(CBMEM_ID_RESUME_SCRATCH);
+	if (heap == NULL)
+		return AGESA_FATAL;
+
+	printk(BIOS_DEBUG, "Using resume HEAP at %08x\n",
+		(unsigned int)(uintptr_t) heap);
+
+	/* Return allocated CBMEM size, we do not keep track of
+	 * how much was actually used.
+	 */
+	dataBlock->VolatileStorageSize = HIGH_MEMORY_SCRATCH;
+	dataBlock->VolatileStorage = heap;
+	return AGESA_SUCCESS;
+}
+
+#if ENV_RAMSTAGE
+
+static int spi_SaveS3info(u32 pos, u32 size, u8 *buf, u32 len)
+{
+#if IS_ENABLED(CONFIG_SPI_FLASH)
+	struct spi_flash flash;
+
+	spi_init();
+	if (spi_flash_probe(0, 0, &flash))
+		return -1;
+
+	spi_flash_volatile_group_begin(&flash);
+
+	spi_flash_erase(&flash, pos, size);
+	spi_flash_write(&flash, pos, sizeof(len), &len);
+	spi_flash_write(&flash, pos + sizeof(len), len, buf);
+
+	spi_flash_volatile_group_end(&flash);
+	return 0;
+#else
+	return -1;
+#endif
+}
+
+static u8 MTRRStorage[S3_DATA_MTRR_SIZE];
+
+AGESA_STATUS OemS3Save(AMD_S3_PARAMS *dataBlock)
+{
+	u32 MTRRStorageSize = 0;
+	uintptr_t pos, size;
+
+	romstage_ram_stack_base(HIGH_ROMSTAGE_STACK_SIZE, ROMSTAGE_STACK_CBMEM);
+
+	/* To be consumed in AmdInitResume. */
+	get_s3nv_data(S3DataTypeNonVolatile, &pos, &size);
+	if (size && dataBlock->NvStorageSize)
+		spi_SaveS3info(pos, size, dataBlock->NvStorage,
+			dataBlock->NvStorageSize);
+	else
+		printk(BIOS_EMERG,
+			"Error: Cannot store memory training results in SPI.\n"
+			"Error: S3 resume will not be possible.\n"
+		);
+
+	/* To be consumed in AmdS3LateRestore. */
+	char *heap = cbmem_add(CBMEM_ID_RESUME_SCRATCH, HIGH_MEMORY_SCRATCH);
+	if (heap) {
+		memset(heap, 0, HIGH_MEMORY_SCRATCH);
+		memcpy(heap, dataBlock->VolatileStorage, dataBlock->VolatileStorageSize);
+	}
+
+	/* Collect MTRR setup. */
+	backup_mtrr(MTRRStorage, &MTRRStorageSize);
+
+	/* To be consumed in restore_mtrr, CPU enumeration in ramstage. */
+	get_s3nv_data(S3DataTypeMTRR, &pos, &size);
+	if (size && MTRRStorageSize)
+		spi_SaveS3info(pos, size, MTRRStorage, MTRRStorageSize);
+
+	return AGESA_SUCCESS;
+}
+
+#endif /* ENV_RAMSTAGE */
+
+const void *OemS3Saved_MTRR_Storage(void)
+{
+	uintptr_t pos, size;
+	get_s3nv_data(S3DataTypeMTRR, &pos, &size);
+	if (!size)
+		return NULL;
+
+	return (void*)(pos + sizeof(UINT32));
+}
diff --git a/src/drivers/amd/agesa/romstage.c b/src/drivers/amd/agesa/romstage.c
new file mode 100644
index 0000000000..bd502b6bad
--- /dev/null
+++ b/src/drivers/amd/agesa/romstage.c
@@ -0,0 +1,120 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2017 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 <arch/acpi.h>
+#include <arch/cpu.h>
+#include <cbmem.h>
+#include <cpu/amd/car.h>
+#include <cpu/x86/bist.h>
+#include <console/console.h>
+#include <halt.h>
+#include <program_loading.h>
+#include <romstage_handoff.h>
+#include <smp/node.h>
+#include <string.h>
+#include <timestamp.h>
+#include <northbridge/amd/agesa/agesa_helper.h>
+#include <northbridge/amd/agesa/state_machine.h>
+
+#if IS_ENABLED(CONFIG_LATE_CBMEM_INIT)
+#error "Only EARLY_CBMEM_INIT is supported."
+#endif
+#if !IS_ENABLED(CONFIG_POSTCAR_STAGE)
+#error "Only POSTCAR_STAGE is supported."
+#endif
+#if HAS_LEGACY_WRAPPER
+#error "LEGACY_WRAPPER code not supported"
+#endif
+
+void asmlinkage early_all_cores(void)
+{
+	amd_initmmio();
+}
+
+void __attribute__((weak)) platform_once(struct sysinfo *cb)
+{
+	board_BeforeAgesa(cb);
+}
+
+static void fill_sysinfo(struct sysinfo *cb)
+{
+	memset(cb, 0, sizeof(*cb));
+	cb->s3resume = acpi_is_wakeup_s3();
+
+	agesa_set_interface(cb);
+}
+
+void * asmlinkage romstage_main(unsigned long bist)
+{
+	struct postcar_frame pcf;
+	struct sysinfo romstage_state;
+	struct sysinfo *cb = &romstage_state;
+	u8 initial_apic_id = (u8) (cpuid_ebx(1) >> 24);
+	int cbmem_initted = 0;
+
+	fill_sysinfo(cb);
+
+	if ((initial_apic_id == 0) && boot_cpu()) {
+
+		timestamp_init(timestamp_get());
+		timestamp_add_now(TS_START_ROMSTAGE);
+
+		platform_once(cb);
+
+		console_init();
+	}
+
+	printk(BIOS_DEBUG, "APIC %02d: CPU Family_Model = %08x\n",
+		initial_apic_id, cpuid_eax(1));
+
+	/* Halt if there was a built in self test failure */
+	report_bist_failure(bist);
+
+	agesa_execute_state(cb, AMD_INIT_RESET);
+
+	agesa_execute_state(cb, AMD_INIT_EARLY);
+
+	timestamp_add_now(TS_BEFORE_INITRAM);
+
+	if (!cb->s3resume)
+		agesa_execute_state(cb, AMD_INIT_POST);
+	else
+		agesa_execute_state(cb, AMD_INIT_RESUME);
+
+	/* FIXME: Detect if TSC frequency changed during raminit? */
+	timestamp_rescale_table(1, 4);
+	timestamp_add_now(TS_AFTER_INITRAM);
+
+	/* Work around AGESA setting all memory as WB on normal
+	 * boot path.
+	 */
+	fixup_cbmem_to_UC(cb->s3resume);
+
+	cbmem_initted = !cbmem_recovery(cb->s3resume);
+
+	if (cb->s3resume && !cbmem_initted) {
+		printk(BIOS_EMERG, "Unable to recover CBMEM\n");
+		halt();
+	}
+
+	romstage_handoff_init(cb->s3resume);
+
+	postcar_frame_init(&pcf, HIGH_ROMSTAGE_STACK_SIZE);
+	recover_postcar_frame(&pcf, cb->s3resume);
+
+	run_postcar_phase(&pcf);
+	/* We do not return. */
+	return NULL;
+}
diff --git a/src/drivers/amd/agesa/s3_mtrr.c b/src/drivers/amd/agesa/s3_mtrr.c
new file mode 100644
index 0000000000..c039abefb6
--- /dev/null
+++ b/src/drivers/amd/agesa/s3_mtrr.c
@@ -0,0 +1,134 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2012 Advanced Micro Devices, Inc.
+ *
+ * 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 <cpu/x86/msr.h>
+#include <cpu/x86/mtrr.h>
+#include <cpu/amd/mtrr.h>
+#include <cpu/x86/cache.h>
+#include <string.h>
+#include <northbridge/amd/agesa/agesa_helper.h>
+
+static void write_mtrr(u8 **p_nvram_pos, unsigned idx)
+{
+	msr_t  msr_data;
+	msr_data = rdmsr(idx);
+
+	memcpy(*p_nvram_pos, &msr_data, sizeof(msr_data));
+	*p_nvram_pos += sizeof(msr_data);
+}
+
+void backup_mtrr(void *mtrr_store, u32 *mtrr_store_size)
+{
+	u8 *nvram_pos = mtrr_store;
+	msr_t  msr_data;
+	u32 i;
+
+	/* Enable access to AMD RdDram and WrDram extension bits */
+	msr_data = rdmsr(SYSCFG_MSR);
+	msr_data.lo |= SYSCFG_MSR_MtrrFixDramModEn;
+	wrmsr(SYSCFG_MSR, msr_data);
+
+	/* Fixed MTRRs */
+	write_mtrr(&nvram_pos, 0x250);
+	write_mtrr(&nvram_pos, 0x258);
+	write_mtrr(&nvram_pos, 0x259);
+
+	for (i = 0x268; i < 0x270; i++)
+		write_mtrr(&nvram_pos, i);
+
+	/* Disable access to AMD RdDram and WrDram extension bits */
+	msr_data = rdmsr(SYSCFG_MSR);
+	msr_data.lo &= ~SYSCFG_MSR_MtrrFixDramModEn;
+	wrmsr(SYSCFG_MSR, msr_data);
+
+	/* Variable MTRRs */
+	for (i = 0x200; i < 0x210; i++)
+		write_mtrr(&nvram_pos, i);
+
+	/* SYSCFG_MSR */
+	write_mtrr(&nvram_pos, SYSCFG_MSR);
+	/* TOM */
+	write_mtrr(&nvram_pos, 0xC001001A);
+	/* TOM2 */
+	write_mtrr(&nvram_pos, 0xC001001D);
+
+	*mtrr_store_size = nvram_pos - (u8*) mtrr_store;
+}
+
+void restore_mtrr(void)
+{
+	volatile u32 *msrPtr = (u32 *) OemS3Saved_MTRR_Storage();
+	u32 msr;
+	msr_t msr_data;
+
+	if (!msrPtr)
+		return;
+
+	disable_cache();
+
+	/* Enable access to AMD RdDram and WrDram extension bits */
+	msr_data = rdmsr(SYSCFG_MSR);
+	msr_data.lo |= SYSCFG_MSR_MtrrFixDramModEn;
+	wrmsr(SYSCFG_MSR, msr_data);
+
+	/* Now restore the Fixed MTRRs */
+	msr_data.lo = *msrPtr;
+	msrPtr ++;
+	msr_data.hi = *msrPtr;
+	msrPtr ++;
+	wrmsr(0x250, msr_data);
+
+	msr_data.lo = *msrPtr;
+	msrPtr ++;
+	msr_data.hi = *msrPtr;
+	msrPtr ++;
+	wrmsr(0x258, msr_data);
+
+	msr_data.lo = *msrPtr;
+	msrPtr ++;
+	msr_data.hi = *msrPtr;
+	msrPtr ++;
+	wrmsr(0x259, msr_data);
+
+	for (msr = 0x268; msr <= 0x26F; msr++) {
+		msr_data.lo = *msrPtr;
+		msrPtr ++;
+		msr_data.hi = *msrPtr;
+		msrPtr ++;
+		wrmsr(msr, msr_data);
+	}
+
+	/* Disable access to AMD RdDram and WrDram extension bits */
+	msr_data = rdmsr(SYSCFG_MSR);
+	msr_data.lo &= ~SYSCFG_MSR_MtrrFixDramModEn;
+	wrmsr(SYSCFG_MSR, msr_data);
+
+	/* Restore the Variable MTRRs */
+	for (msr = 0x200; msr <= 0x20F; msr++) {
+		msr_data.lo = *msrPtr;
+		msrPtr ++;
+		msr_data.hi = *msrPtr;
+		msrPtr ++;
+		wrmsr(msr, msr_data);
+	}
+
+	/* Restore SYSCFG MTRR */
+	msr_data.lo = *msrPtr;
+	msrPtr ++;
+	msr_data.hi = *msrPtr;
+	msrPtr ++;
+	wrmsr(SYSCFG_MSR, msr_data);
+}
diff --git a/src/drivers/amd/agesa/state_machine.c b/src/drivers/amd/agesa/state_machine.c
new file mode 100644
index 0000000000..fdd2e6eeac
--- /dev/null
+++ b/src/drivers/amd/agesa/state_machine.c
@@ -0,0 +1,398 @@
+/*
+ * 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 <cbfs.h>
+#include <cbmem.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 "AMD.h"
+
+#if IS_ENABLED(CONFIG_CPU_AMD_AGESA_OPENSOURCE)
+#include "Dispatcher.h"
+#endif
+
+#if ENV_ROMSTAGE
+#include <PlatformMemoryConfiguration.h>
+CONST PSO_ENTRY ROMDATA DefaultPlatformMemoryConfiguration[] = {PSO_END};
+#endif
+
+static void agesa_locate_image(AMD_CONFIG_PARAMS *StdHeader)
+{
+#if IS_ENABLED(CONFIG_CPU_AMD_AGESA_BINARY_PI)
+	const char ModuleIdentifier[] = AGESA_ID;
+	const void *agesa, *image;
+	size_t file_size;
+
+	agesa = cbfs_boot_map_with_leak((const char *)CONFIG_AGESA_CBFS_NAME,
+			CBFS_TYPE_RAW, &file_size);
+	if (agesa == NULL)
+		return;
+
+	image = LibAmdLocateImage(agesa, agesa + file_size, 4096,
+		ModuleIdentifier);
+	StdHeader->ImageBasePtr = (void*) image;
+#endif
+}
+
+void agesa_set_interface(struct sysinfo *cb)
+{
+	memset(&cb->StdHeader, 0, sizeof(AMD_CONFIG_PARAMS));
+
+	cb->StdHeader.CalloutPtr = GetBiosCallout;
+
+	if (IS_ENABLED(CONFIG_CPU_AMD_AGESA_BINARY_PI)) {
+		agesa_locate_image(&cb->StdHeader);
+		AMD_IMAGE_HEADER *image =
+			(void*)(uintptr_t)cb->StdHeader.ImageBasePtr;
+		ASSERT(image);
+		AMD_MODULE_HEADER *module =
+			(void*)(uintptr_t)image->ModuleInfoOffset;
+		ASSERT(module && module->ModuleDispatcher);
+	}
+}
+
+AGESA_STATUS module_dispatch(AGESA_STRUCT_NAME func,
+	AMD_CONFIG_PARAMS *StdHeader)
+{
+	MODULE_ENTRY dispatcher;
+
+#if IS_ENABLED(CONFIG_CPU_AMD_AGESA_OPENSOURCE)
+	dispatcher = AmdAgesaDispatcher;
+#endif
+#if IS_ENABLED(CONFIG_CPU_AMD_AGESA_BINARY_PI)
+	AMD_IMAGE_HEADER *image = (void*)(uintptr_t)StdHeader->ImageBasePtr;
+	AMD_MODULE_HEADER *module = (void*)(uintptr_t)image->ModuleInfoOffset;
+	dispatcher = module->ModuleDispatcher;
+#endif
+
+	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 romstage_dispatch(struct sysinfo *cb,
+	AGESA_STRUCT_NAME func, AMD_CONFIG_PARAMS *StdHeader)
+{
+	AGESA_STATUS status = AGESA_UNSUPPORTED;
+
+	switch (func)
+	{
+		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;
+		}
+
+		default:
+		{
+			break;
+		}
+
+	}
+	return status;
+}
+
+static AGESA_STATUS ramstage_dispatch(struct sysinfo *cb,
+	AGESA_STRUCT_NAME func, AMD_CONFIG_PARAMS *StdHeader)
+{
+	AGESA_STATUS status = AGESA_UNSUPPORTED;
+
+	switch (func)
+	{
+		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;
+		}
+
+		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;
+		}
+
+		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;
+		AMD_S3LATE_PARAMS s3late;
+	} 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 || func == AMD_S3LATE_RESTORE) {
+		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);
+
+	if (ENV_ROMSTAGE)
+		final = romstage_dispatch(cb, func, StdHeader);
+
+	if (ENV_RAMSTAGE)
+		final = ramstage_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);
+
+	if (!acpi_is_wakeup_s3())
+		agesa_execute_state(cb, AMD_INIT_ENV);
+	else {
+		agesa_execute_state(cb, AMD_S3LATE_RESTORE);
+		fchs3earlyrestore(&cb->StdHeader);
+	}
+}
+
+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()) {
+		fchs3laterestore(&cb->StdHeader);
+		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) { }
+
+AGESA_STATUS __attribute__((weak))
+fchs3earlyrestore(AMD_CONFIG_PARAMS *StdHeader)
+{
+	return AGESA_SUCCESS;
+}
+
+AGESA_STATUS __attribute__((weak))
+fchs3laterestore(AMD_CONFIG_PARAMS *StdHeader)
+{
+	return AGESA_SUCCESS;
+}