soc/amd/common: Move AGESA related source files

Move AGESA related source files in soc/amd/common under block directory.
Folder soc/amd/common/block subfolders should mimic soc/intel/common/block
subfolders (one subfolder per subject).

BUG=b:69262110
TEST=Build with no error gardenia and kahlee (no code change, just folder
reorg).

Change-Id: I497cdefe64e8dff00aaff7153c4ffa9c57c9acf8
Signed-off-by: Richard Spiegel <richard.spiegel@silverbackltd.com>
Reviewed-on: https://review.coreboot.org/22792
Reviewed-by: Aaron Durbin <adurbin@chromium.org>
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>

6 files changed, 1096 insertions, 0 deletions

diff --git a/src/soc/amd/common/block/pi/Kconfig b/src/soc/amd/common/block/pi/Kconfig
new file mode 100644
index 0000000000..273598b0bd
--- /dev/null
+++ b/src/soc/amd/common/block/pi/Kconfig
@@ -0,0 +1,5 @@
+config SOC_AMD_COMMON_BLOCK_PI
+	bool
+	default n
+	help
+	  This option builds functions that interface AMD's AGESA.
diff --git a/src/soc/amd/common/block/pi/Makefile.inc b/src/soc/amd/common/block/pi/Makefile.inc
new file mode 100644
index 0000000000..de6bf78b03
--- /dev/null
+++ b/src/soc/amd/common/block/pi/Makefile.inc
@@ -0,0 +1,16 @@
+ifeq ($(CONFIG_SOC_AMD_COMMON_BLOCK_PI),y)
+
+bootblock-y += agesawrapper.c
+bootblock-y += def_callouts.c
+bootblock-y += heapmanager.c
+
+romstage-y += agesawrapper.c
+romstage-y += def_callouts.c
+romstage-y += heapmanager.c
+
+ramstage-y += agesawrapper.c
+ramstage-y += amd_late_init.c
+ramstage-y += def_callouts.c
+ramstage-y += heapmanager.c
+
+endif
diff --git a/src/soc/amd/common/block/pi/agesawrapper.c b/src/soc/amd/common/block/pi/agesawrapper.c
new file mode 100644
index 0000000000..b1f03cb178
--- /dev/null
+++ b/src/soc/amd/common/block/pi/agesawrapper.c
@@ -0,0 +1,421 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2012 - 2017 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 <amdblocks/agesawrapper.h>
+#include <arch/early_variables.h>
+#include <cbfs.h>
+#include <cbmem.h>
+#include <delay.h>
+#include <cpu/x86/mtrr.h>
+#include <amdblocks/BiosCallOuts.h>
+#include <string.h>
+#include <timestamp.h>
+
+void __attribute__((weak)) SetMemParams(AMD_POST_PARAMS *PostParams) {}
+void __attribute__((weak)) OemPostParams(AMD_POST_PARAMS *PostParams) {}
+
+#ifndef __PRE_RAM__
+/* ACPI table pointers returned by AmdInitLate */
+static void *DmiTable;
+static void *AcpiPstate;
+static void *AcpiSrat;
+static void *AcpiSlit;
+
+static void *AcpiWheaMce;
+static void *AcpiWheaCmc;
+static void *AcpiAlib;
+static void *AcpiIvrs;
+static void *AcpiCrat;
+#endif /* #ifndef __PRE_RAM__ */
+
+AGESA_STATUS agesawrapper_amdinitreset(void)
+{
+	AGESA_STATUS status;
+	AMD_INTERFACE_PARAMS AmdParamStruct;
+	AMD_RESET_PARAMS AmdResetParams;
+
+	memset(&AmdParamStruct, 0, sizeof(AmdParamStruct));
+	memset(&AmdResetParams, 0, sizeof(AmdResetParams));
+
+	AmdParamStruct.AgesaFunctionName = AMD_INIT_RESET;
+	AmdParamStruct.AllocationMethod = ByHost;
+	AmdParamStruct.NewStructSize = sizeof(AMD_RESET_PARAMS);
+	AmdParamStruct.NewStructPtr = &AmdResetParams;
+	AmdParamStruct.StdHeader.AltImageBasePtr = 0;
+	AmdParamStruct.StdHeader.CalloutPtr = &GetBiosCallout;
+	AmdParamStruct.StdHeader.Func = 0;
+	AmdParamStruct.StdHeader.ImageBasePtr = 0;
+	AmdCreateStruct (&AmdParamStruct);
+
+	AmdResetParams.FchInterface.Xhci0Enable = IS_ENABLED(CONFIG_STONEYRIDGE_XHCI_ENABLE);
+
+	AmdResetParams.FchInterface.SataEnable = !((CONFIG_STONEYRIDGE_SATA_MODE == 0) || (CONFIG_STONEYRIDGE_SATA_MODE == 3));
+	AmdResetParams.FchInterface.IdeEnable = (CONFIG_STONEYRIDGE_SATA_MODE == 0) || (CONFIG_STONEYRIDGE_SATA_MODE == 3);
+
+	timestamp_add_now(TS_AGESA_INIT_RESET_START);
+	status = AmdInitReset(&AmdResetParams);
+	timestamp_add_now(TS_AGESA_INIT_RESET_DONE);
+
+	if (status != AGESA_SUCCESS) agesawrapper_amdreadeventlog(AmdParamStruct.StdHeader.HeapStatus);
+	AmdReleaseStruct (&AmdParamStruct);
+	return status;
+}
+
+AGESA_STATUS agesawrapper_amdinitearly(void)
+{
+	AGESA_STATUS status;
+	AMD_INTERFACE_PARAMS AmdParamStruct;
+	AMD_EARLY_PARAMS     *AmdEarlyParamsPtr;
+
+	memset(&AmdParamStruct, 0, sizeof(AmdParamStruct));
+
+	AmdParamStruct.AgesaFunctionName = AMD_INIT_EARLY;
+	AmdParamStruct.AllocationMethod = PreMemHeap;
+	AmdParamStruct.StdHeader.AltImageBasePtr = 0;
+	AmdParamStruct.StdHeader.CalloutPtr = &GetBiosCallout;
+	AmdParamStruct.StdHeader.Func = 0;
+	AmdParamStruct.StdHeader.ImageBasePtr = 0;
+	AmdCreateStruct (&AmdParamStruct);
+
+	AmdEarlyParamsPtr = (AMD_EARLY_PARAMS *)AmdParamStruct.NewStructPtr;
+	OemCustomizeInitEarly (AmdEarlyParamsPtr);
+
+	AmdEarlyParamsPtr->GnbConfig.PsppPolicy = PsppDisabled;
+
+	timestamp_add_now(TS_AGESA_INIT_EARLY_START);
+	status = AmdInitEarly ((AMD_EARLY_PARAMS *)AmdParamStruct.NewStructPtr);
+	timestamp_add_now(TS_AGESA_INIT_EARLY_DONE);
+
+	if (status != AGESA_SUCCESS) agesawrapper_amdreadeventlog(AmdParamStruct.StdHeader.HeapStatus);
+	AmdReleaseStruct (&AmdParamStruct);
+
+	return status;
+}
+
+AGESA_STATUS agesawrapper_amdinitpost(void)
+{
+	AGESA_STATUS status;
+	AMD_INTERFACE_PARAMS  AmdParamStruct;
+	AMD_POST_PARAMS       *PostParams;
+
+	memset(&AmdParamStruct, 0, sizeof(AmdParamStruct));
+
+	AmdParamStruct.AgesaFunctionName = AMD_INIT_POST;
+	AmdParamStruct.AllocationMethod = PreMemHeap;
+	AmdParamStruct.StdHeader.AltImageBasePtr = 0;
+	AmdParamStruct.StdHeader.CalloutPtr = &GetBiosCallout;
+	AmdParamStruct.StdHeader.Func = 0;
+	AmdParamStruct.StdHeader.ImageBasePtr = 0;
+
+	AmdCreateStruct (&AmdParamStruct);
+	PostParams = (AMD_POST_PARAMS *)AmdParamStruct.NewStructPtr;
+
+	PostParams->MemConfig.UmaMode = CONFIG_GFXUMA ? UMA_AUTO : UMA_NONE;
+	PostParams->MemConfig.UmaSize = 0;
+	PostParams->MemConfig.BottomIo = (UINT16)
+					 (CONFIG_BOTTOMIO_POSITION >> 24);
+
+	SetMemParams(PostParams);
+	OemPostParams(PostParams);
+	printk(BIOS_SPEW, "DRAM clear on reset: %s\n",
+		(PostParams->MemConfig.EnableMemClr == FALSE) ? "Keep" :
+		(PostParams->MemConfig.EnableMemClr == TRUE) ? "Clear" :
+		"unknown"
+	);
+
+	timestamp_add_now(TS_AGESA_INIT_POST_START);
+	status = AmdInitPost (PostParams);
+	timestamp_add_now(TS_AGESA_INIT_POST_DONE);
+
+	/* If UMA is enabled we currently have it below TOP_MEM as well.
+	 * UMA may or may not be cacheable, so Sub4GCacheTop could be
+	 * higher than UmaBase. With UMA_NONE we see UmaBase==0. */
+	if (PostParams->MemConfig.UmaBase)
+		backup_top_of_low_cacheable(PostParams->MemConfig.UmaBase << 16);
+	else
+		backup_top_of_low_cacheable(PostParams->MemConfig.Sub4GCacheTop);
+
+
+	printk(
+			BIOS_SPEW,
+			"setup_uma_memory: umamode %s\n",
+			(PostParams->MemConfig.UmaMode == UMA_AUTO) ? "UMA_AUTO" :
+			(PostParams->MemConfig.UmaMode == UMA_SPECIFIED) ? "UMA_SPECIFIED" :
+			(PostParams->MemConfig.UmaMode == UMA_NONE) ? "UMA_NONE" :
+			"unknown"
+	);
+	printk(
+			BIOS_SPEW,
+			"setup_uma_memory: syslimit 0x%08llX, bottomio 0x%08lx\n",
+			(unsigned long long)(PostParams->MemConfig.SysLimit) << 16,
+			(unsigned long)(PostParams->MemConfig.BottomIo) << 16
+	);
+	printk(
+			BIOS_SPEW,
+			"setup_uma_memory: uma size %luMB, uma start 0x%08lx\n",
+			(unsigned long)(PostParams->MemConfig.UmaSize) >> (20 - 16),
+			(unsigned long)(PostParams->MemConfig.UmaBase) << 16
+	);
+
+	if (status != AGESA_SUCCESS) agesawrapper_amdreadeventlog(PostParams->StdHeader.HeapStatus);
+	AmdReleaseStruct (&AmdParamStruct);
+
+	return status;
+}
+
+AGESA_STATUS agesawrapper_amdinitenv(void)
+{
+	AGESA_STATUS status;
+	AMD_INTERFACE_PARAMS AmdParamStruct;
+	AMD_ENV_PARAMS       *EnvParam;
+
+	memset(&AmdParamStruct, 0, sizeof(AmdParamStruct));
+
+	AmdParamStruct.AgesaFunctionName = AMD_INIT_ENV;
+	AmdParamStruct.AllocationMethod = PostMemDram;
+	AmdParamStruct.StdHeader.AltImageBasePtr = 0;
+	AmdParamStruct.StdHeader.CalloutPtr = &GetBiosCallout;
+	AmdParamStruct.StdHeader.Func = 0;
+	AmdParamStruct.StdHeader.ImageBasePtr = 0;
+	status = AmdCreateStruct (&AmdParamStruct);
+	EnvParam = (AMD_ENV_PARAMS *)AmdParamStruct.NewStructPtr;
+
+	EnvParam->FchInterface.AzaliaController = AzEnable;
+	EnvParam->FchInterface.SataClass = CONFIG_STONEYRIDGE_SATA_MODE;
+	EnvParam->FchInterface.SataEnable = !((CONFIG_STONEYRIDGE_SATA_MODE == 0) || (CONFIG_STONEYRIDGE_SATA_MODE == 3));
+	EnvParam->FchInterface.IdeEnable = (CONFIG_STONEYRIDGE_SATA_MODE == 0) || (CONFIG_STONEYRIDGE_SATA_MODE == 3);
+	EnvParam->FchInterface.SataIdeMode = (CONFIG_STONEYRIDGE_SATA_MODE == 3);
+	EnvParam->GnbEnvConfiguration.IommuSupport = FALSE;
+
+	timestamp_add_now(TS_AGESA_INIT_ENV_START);
+	status = AmdInitEnv (EnvParam);
+	timestamp_add_now(TS_AGESA_INIT_ENV_DONE);
+
+	if (status != AGESA_SUCCESS) agesawrapper_amdreadeventlog(EnvParam->StdHeader.HeapStatus);
+	/* Initialize Subordinate Bus Number and Secondary Bus Number
+	 * In platform BIOS this address is allocated by PCI enumeration code
+	 Modify D1F0x18
+	*/
+
+	return status;
+}
+
+#ifndef __PRE_RAM__
+VOID* agesawrapper_getlateinitptr (int pick)
+{
+	switch (pick) {
+	case PICK_DMI:
+		return DmiTable;
+	case PICK_PSTATE:
+		return AcpiPstate;
+	case PICK_SRAT:
+		return AcpiSrat;
+	case PICK_SLIT:
+		return AcpiSlit;
+	case PICK_WHEA_MCE:
+		return AcpiWheaMce;
+	case PICK_WHEA_CMC:
+		return AcpiWheaCmc;
+	case PICK_ALIB:
+		return AcpiAlib;
+	case PICK_IVRS:
+		return AcpiIvrs;
+	case PICK_CRAT:
+		return AcpiCrat;
+	default:
+		return NULL;
+	}
+}
+#endif /* #ifndef __PRE_RAM__ */
+
+AGESA_STATUS agesawrapper_amdinitmid(void)
+{
+	AGESA_STATUS status;
+	AMD_INTERFACE_PARAMS AmdParamStruct;
+	AMD_MID_PARAMS *MidParam;
+
+	/* Enable MMIO on AMD CPU Address Map Controller */
+	amd_initcpuio ();
+
+	memset(&AmdParamStruct, 0, sizeof(AmdParamStruct));
+
+	AmdParamStruct.AgesaFunctionName = AMD_INIT_MID;
+	AmdParamStruct.AllocationMethod = PostMemDram;
+	AmdParamStruct.StdHeader.AltImageBasePtr = 0;
+	AmdParamStruct.StdHeader.CalloutPtr = &GetBiosCallout;
+	AmdParamStruct.StdHeader.Func = 0;
+	AmdParamStruct.StdHeader.ImageBasePtr = 0;
+
+	AmdCreateStruct (&AmdParamStruct);
+	MidParam = (AMD_MID_PARAMS *)AmdParamStruct.NewStructPtr;
+
+	MidParam->GnbMidConfiguration.iGpuVgaMode = 0;/* 0 iGpuVgaAdapter, 1 iGpuVgaNonAdapter; */
+	MidParam->GnbMidConfiguration.GnbIoapicAddress = 0xFEC20000;
+
+	MidParam->FchInterface.AzaliaController = AzEnable;
+	MidParam->FchInterface.SataClass = CONFIG_STONEYRIDGE_SATA_MODE;
+	MidParam->FchInterface.SataEnable = !((CONFIG_STONEYRIDGE_SATA_MODE == 0) || (CONFIG_STONEYRIDGE_SATA_MODE == 3));
+	MidParam->FchInterface.IdeEnable = (CONFIG_STONEYRIDGE_SATA_MODE == 0) || (CONFIG_STONEYRIDGE_SATA_MODE == 3);
+	MidParam->FchInterface.SataIdeMode = (CONFIG_STONEYRIDGE_SATA_MODE == 3);
+
+	timestamp_add_now(TS_AGESA_INIT_MID_START);
+	status = AmdInitMid ((AMD_MID_PARAMS *)AmdParamStruct.NewStructPtr);
+	timestamp_add_now(TS_AGESA_INIT_MID_DONE);
+
+	if (status != AGESA_SUCCESS) agesawrapper_amdreadeventlog(AmdParamStruct.StdHeader.HeapStatus);
+	AmdReleaseStruct (&AmdParamStruct);
+
+	return status;
+}
+
+#ifndef __PRE_RAM__
+AGESA_STATUS agesawrapper_amdinitlate(void)
+{
+	AGESA_STATUS Status;
+	AMD_INTERFACE_PARAMS AmdParamStruct;
+	AMD_LATE_PARAMS *AmdLateParams;
+
+	memset(&AmdParamStruct, 0, sizeof(AmdParamStruct));
+
+	AmdParamStruct.AgesaFunctionName = AMD_INIT_LATE;
+	AmdParamStruct.AllocationMethod = PostMemDram;
+	AmdParamStruct.StdHeader.AltImageBasePtr = 0;
+	AmdParamStruct.StdHeader.CalloutPtr = &GetBiosCallout;
+	AmdParamStruct.StdHeader.HeapStatus = HEAP_SYSTEM_MEM;
+	AmdParamStruct.StdHeader.Func = 0;
+	AmdParamStruct.StdHeader.ImageBasePtr = 0;
+
+	/* NOTE: if not call amdcreatestruct, the initializer(AmdInitLateInitializer) would not be called */
+	AmdCreateStruct(&AmdParamStruct);
+	AmdLateParams = (AMD_LATE_PARAMS *)AmdParamStruct.NewStructPtr;
+
+	timestamp_add_now(TS_AGESA_INIT_LATE_START);
+	Status = AmdInitLate(AmdLateParams);
+	timestamp_add_now(TS_AGESA_INIT_LATE_DONE);
+
+	if (Status != AGESA_SUCCESS) {
+		agesawrapper_amdreadeventlog(AmdLateParams->StdHeader.HeapStatus);
+		ASSERT(Status == AGESA_SUCCESS);
+	}
+
+	DmiTable    = AmdLateParams->DmiTable;
+	AcpiPstate  = AmdLateParams->AcpiPState;
+
+	AcpiWheaMce = AmdLateParams->AcpiWheaMce;
+	AcpiWheaCmc = AmdLateParams->AcpiWheaCmc;
+	AcpiAlib    = AmdLateParams->AcpiAlib;
+	AcpiIvrs    = AmdLateParams->AcpiIvrs;
+	AcpiCrat    = AmdLateParams->AcpiCrat;
+
+	printk(BIOS_DEBUG, "DmiTable:%x, AcpiPstatein: %x, AcpiSrat:%x,"
+	       "AcpiSlit:%x, Mce:%x, Cmc:%x,"
+	       "Alib:%x, AcpiIvrs:%x in %s\n",
+	       (unsigned int)DmiTable, (unsigned int)AcpiPstate, (unsigned int)AcpiSrat,
+	       (unsigned int)AcpiSlit, (unsigned int)AcpiWheaMce, (unsigned int)AcpiWheaCmc,
+	       (unsigned int)AcpiAlib, (unsigned int)AcpiIvrs, __func__);
+
+	/* AmdReleaseStruct (&AmdParamStruct); */
+	return Status;
+}
+#endif /* #ifndef __PRE_RAM__ */
+
+AGESA_STATUS agesawrapper_amdlaterunaptask (
+	UINT32 Func,
+	UINTN Data,
+	VOID *ConfigPtr
+	)
+{
+	AGESA_STATUS Status;
+	AP_EXE_PARAMS ApExeParams;
+
+	memset(&ApExeParams, 0, sizeof(ApExeParams));
+
+	ApExeParams.StdHeader.AltImageBasePtr = 0;
+	ApExeParams.StdHeader.CalloutPtr = &GetBiosCallout;
+	ApExeParams.StdHeader.Func = 0;
+	ApExeParams.StdHeader.ImageBasePtr = 0;
+	ApExeParams.FunctionNumber = Func;
+	ApExeParams.RelatedDataBlock = ConfigPtr;
+
+	Status = AmdLateRunApTask (&ApExeParams);
+	if (Status != AGESA_SUCCESS) {
+		/* agesawrapper_amdreadeventlog(); */
+		ASSERT(Status == AGESA_SUCCESS);
+	}
+
+	return Status;
+}
+
+AGESA_STATUS agesawrapper_amdreadeventlog (UINT8 HeapStatus)
+{
+	AGESA_STATUS Status;
+	EVENT_PARAMS AmdEventParams;
+
+	memset(&AmdEventParams, 0, sizeof(AmdEventParams));
+
+	AmdEventParams.StdHeader.AltImageBasePtr = 0;
+	AmdEventParams.StdHeader.CalloutPtr = &GetBiosCallout;
+	AmdEventParams.StdHeader.Func = 0;
+	AmdEventParams.StdHeader.ImageBasePtr = 0;
+	AmdEventParams.StdHeader.HeapStatus = HeapStatus;
+	Status = AmdReadEventLog (&AmdEventParams);
+	while (AmdEventParams.EventClass != 0) {
+		printk(BIOS_DEBUG,"\nEventLog:  EventClass = %x, EventInfo = %x.\n", (unsigned int)AmdEventParams.EventClass,(unsigned int)AmdEventParams.EventInfo);
+		printk(BIOS_DEBUG,"  Param1 = %x, Param2 = %x.\n",(unsigned int)AmdEventParams.DataParam1, (unsigned int)AmdEventParams.DataParam2);
+		printk(BIOS_DEBUG,"  Param3 = %x, Param4 = %x.\n",(unsigned int)AmdEventParams.DataParam3, (unsigned int)AmdEventParams.DataParam4);
+		Status = AmdReadEventLog (&AmdEventParams);
+	}
+
+	return Status;
+}
+
+const void *agesawrapper_locate_module (const CHAR8 name[8])
+{
+	const void* agesa;
+	const AMD_IMAGE_HEADER* image;
+	const AMD_MODULE_HEADER* module;
+	size_t file_size;
+
+	agesa = cbfs_boot_map_with_leak((const char *)CONFIG_AGESA_CBFS_NAME,
+						CBFS_TYPE_RAW, &file_size);
+
+	if (!agesa)
+		return NULL;
+	image =  LibAmdLocateImage(agesa, agesa + file_size - 1, 4096, name);
+	module = (AMD_MODULE_HEADER*)image->ModuleInfoOffset;
+
+	return module;
+}
+
+static MODULE_ENTRY agesa_dispatcher CAR_GLOBAL;
+
+MODULE_ENTRY agesa_get_dispatcher(void)
+{
+	const AMD_MODULE_HEADER *module;
+	static const CHAR8 id[8] = AGESA_ID;
+	MODULE_ENTRY val = car_get_var(agesa_dispatcher);
+
+	if (val != NULL)
+		return val;
+
+	module = agesawrapper_locate_module(id);
+	if (!module)
+		return NULL;
+
+	val = module->ModuleDispatcher;
+	car_set_var(agesa_dispatcher, val);
+
+	return val;
+}
diff --git a/src/soc/amd/common/block/pi/amd_late_init.c b/src/soc/amd/common/block/pi/amd_late_init.c
new file mode 100644
index 0000000000..65667b9bdc
--- /dev/null
+++ b/src/soc/amd/common/block/pi/amd_late_init.c
@@ -0,0 +1,40 @@
+/*
+ * 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 <arch/acpi.h>
+#include <bootstate.h>
+#include <console/console.h>
+#include <device/device.h>
+#include <device/pci_def.h>
+#include <device/pci_ops.h>
+
+#include <amdblocks/agesawrapper.h>
+#include <amdblocks/agesawrapper_call.h>
+
+static void agesawrapper_post_device(void *unused)
+{
+	if (acpi_is_wakeup_s3())
+		return;
+
+	AGESAWRAPPER(amdinitlate);
+
+	if (!acpi_s3_resume_allowed())
+		return;
+
+	AGESAWRAPPER(amdS3Save);
+}
+
+BOOT_STATE_INIT_ENTRY(BS_POST_DEVICE, BS_ON_EXIT,
+			agesawrapper_post_device, NULL);
diff --git a/src/soc/amd/common/block/pi/def_callouts.c b/src/soc/amd/common/block/pi/def_callouts.c
new file mode 100644
index 0000000000..c05d4de8cc
--- /dev/null
+++ b/src/soc/amd/common/block/pi/def_callouts.c
@@ -0,0 +1,251 @@
+/*
+ * 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 <cpu/x86/lapic.h>
+#include <cpu/x86/mp.h>
+#include <timer.h>
+#include <amdlib.h>
+#include <amdblocks/BiosCallOuts.h>
+#include <amdblocks/agesawrapper.h>
+#include <amdblocks/agesawrapper_call.h>
+#include <reset.h>
+#include <soc/southbridge.h>
+
+#if ENV_BOOTBLOCK
+const BIOS_CALLOUT_STRUCT BiosCallouts[] = {
+	{ AGESA_DO_RESET,                 agesa_Reset },
+	{ AGESA_FCH_OEM_CALLOUT,          agesa_fch_initreset },
+	{ AGESA_GNB_PCIE_SLOT_RESET,      agesa_PcieSlotResetControl }
+};
+#else
+const BIOS_CALLOUT_STRUCT BiosCallouts[] = {
+	/* Required callouts */
+	{ AGESA_ALLOCATE_BUFFER,          agesa_AllocateBuffer },
+	{ AGESA_DEALLOCATE_BUFFER,        agesa_DeallocateBuffer },
+	{ AGESA_DO_RESET,                 agesa_Reset },
+	{ AGESA_LOCATE_BUFFER,            agesa_LocateBuffer },
+	{ AGESA_READ_SPD,                 agesa_ReadSpd },
+	{ AGESA_GNB_PCIE_SLOT_RESET,      agesa_PcieSlotResetControl },
+#if ENV_RAMSTAGE
+	{ AGESA_RUNFUNC_ONAP,             agesa_RunFuncOnAp },
+	{ AGESA_RUNFUNC_ON_ALL_APS,       agesa_RunFcnOnAllAps },
+	{ AGESA_WAIT_FOR_ALL_APS,         agesa_WaitForAllApsFinished },
+	{ AGESA_IDLE_AN_AP,               agesa_IdleAnAp },
+#endif /* ENV_RAMSTAGE */
+
+	/* Optional callouts */
+	{ AGESA_GET_IDS_INIT_DATA,             agesa_EmptyIdsInitData },
+	//AgesaHeapRebase - Hook ID?
+	{ AGESA_HOOKBEFORE_DRAM_INIT,          agesa_NoopUnsupported },
+	{ AGESA_HOOKBEFORE_DQS_TRAINING,       agesa_NoopUnsupported },
+	{ AGESA_EXTERNAL_2D_TRAIN_VREF_CHANGE, agesa_NoopUnsupported },
+	{ AGESA_HOOKBEFORE_EXIT_SELF_REF,      agesa_NoopUnsupported },
+	{ AGESA_GNB_GFX_GET_VBIOS_IMAGE,       agesa_GfxGetVbiosImage },
+	{ AGESA_FCH_OEM_CALLOUT,               agesa_fch_initenv },
+	{ AGESA_EXTERNAL_VOLTAGE_ADJUST,       agesa_NoopUnsupported },
+	{ AGESA_GNB_PCIE_CLK_REQ,              agesa_NoopUnsupported },
+
+	/* Deprecated */
+	{ AGESA_HOOKBEFORE_DRAM_INIT_RECOVERY, agesa_NoopUnsupported},
+	{ AGESA_READ_SPD_RECOVERY,             agesa_NoopUnsupported },
+
+};
+#endif
+
+const int BiosCalloutsLen = ARRAY_SIZE(BiosCallouts);
+
+AGESA_STATUS GetBiosCallout(UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	UINTN i;
+
+	for (i = 0 ; i < BiosCalloutsLen ; i++) {
+		if (BiosCallouts[i].CalloutName == Func)
+			break;
+	}
+
+	if (i >= BiosCalloutsLen) {
+		printk(BIOS_ERR, "ERROR: AGESA Callout Not Supported: 0x%x",
+			(u32)Func);
+		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;
+	UINTN ResetType;
+	AMD_CONFIG_PARAMS *StdHeader;
+
+	ResetType = Data;
+	StdHeader = ConfigPtr;
+
+	/*
+	 * This should perform the RESET based upon the ResetType, but coreboot
+	 * doesn't have a reset manager to handle a WHENEVER case. Do all
+	 * resets immediately.
+	 */
+	switch (ResetType) {
+	case WARM_RESET_WHENEVER:
+	case WARM_RESET_IMMEDIATELY:
+		do_soft_reset();
+		break;
+
+	case COLD_RESET_WHENEVER:
+	case COLD_RESET_IMMEDIATELY:
+		do_hard_reset();
+		break;
+
+	default:
+		break;
+	}
+
+	Status = 0;
+	return Status;
+}
+
+AGESA_STATUS agesa_GfxGetVbiosImage(UINT32 Func, UINTN FchData,
+							VOID *ConfigPrt)
+{
+	GFX_VBIOS_IMAGE_INFO *pVbiosImageInfo;
+
+	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, "agesa_GfxGetVbiosImage: IMGptr=%p\n",
+						pVbiosImageInfo->ImagePtr);
+	return pVbiosImageInfo->ImagePtr ? AGESA_SUCCESS : AGESA_WARNING;
+}
+
+AGESA_STATUS __attribute__((weak)) platform_PcieSlotResetControl(UINT32 Func,
+	UINTN Data, VOID *ConfigPtr)
+{
+	printk(BIOS_WARNING, "Warning - AGESA callout: %s not supported\n",
+		__func__);
+	return AGESA_UNSUPPORTED;
+}
+
+AGESA_STATUS agesa_PcieSlotResetControl(UINT32 Func, UINTN Data,
+	VOID *ConfigPtr)
+{
+	return platform_PcieSlotResetControl(Func, Data, ConfigPtr);
+}
+
+/*
+ * Application Processor callouts:
+ * agesa_RunFuncOnAp() and agesa_RunFcnOnAllAps() are called after main memory
+ * has been initialized and coreboot has taken control of AP task dispatching.
+ * These functions execute callout_ap_entry() on each AP, which calls the
+ * AmdLateRunApTask() entry point if it is a targeted AP.
+ */
+
+/*
+ * Global data for APs.
+ * Passed from the AGESA_Callout for the agesawrapper_amdlaterunaptask.
+ */
+static struct agesa_data {
+	UINT32 Func;
+	UINTN Data;
+	VOID *ConfigPtr;
+} agesadata;
+
+/*
+ * BSP deploys APs to callout_ap_entry(), which calls
+ * agesawrapper_amdlaterunaptask with the agesadata.
+ */
+static void callout_ap_entry(void)
+{
+	AGESA_STATUS Status = AGESA_UNSUPPORTED;
+
+	printk(BIOS_DEBUG, "%s Func: 0x%x,  Data: 0x%lx, Ptr: 0x%p \n",
+		__func__, agesadata.Func, agesadata.Data, agesadata.ConfigPtr);
+
+	/* Check if this AP should run the function */
+	if (!((agesadata.Func == AGESA_RUNFUNC_ONAP) &&
+	    (agesadata.Data == lapicid())))
+		return;
+
+	Status = agesawrapper_amdlaterunaptask(agesadata.Func, agesadata.Data,
+			agesadata.ConfigPtr);
+
+	if (Status)
+		printk(BIOS_DEBUG, "There was a problem with %lx returned %s\n",
+			lapicid(), decodeAGESA_STATUS(Status));
+}
+
+AGESA_STATUS agesa_RunFuncOnAp(UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	printk(BIOS_DEBUG, "%s\n", __func__);
+
+	agesadata.Func = Func;
+	agesadata.Data = Data;
+	agesadata.ConfigPtr = ConfigPtr;
+	mp_run_on_aps(callout_ap_entry, 100 * USECS_PER_MSEC);
+
+	return AGESA_SUCCESS;
+}
+
+AGESA_STATUS agesa_RunFcnOnAllAps(UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	printk(BIOS_DEBUG, "%s\n", __func__);
+
+	agesadata.Func = Func;
+	agesadata.Data = Data;
+	agesadata.ConfigPtr = ConfigPtr;
+	mp_run_on_aps(callout_ap_entry, 100 * USECS_PER_MSEC);
+
+	return AGESA_SUCCESS;
+}
+
+AGESA_STATUS agesa_WaitForAllApsFinished(UINT32 Func, UINTN Data,
+	VOID *ConfigPtr)
+{
+	printk(BIOS_WARNING, "Warning - AGESA callout: %s not supported\n",
+		__func__);
+	AGESA_STATUS Status = AGESA_UNSUPPORTED;
+
+	return Status;
+}
+
+AGESA_STATUS agesa_IdleAnAp(UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	printk(BIOS_WARNING, "Warning - AGESA callout: %s no supported\n",
+		__func__);
+	AGESA_STATUS Status = AGESA_UNSUPPORTED;
+
+	return Status;
+}
diff --git a/src/soc/amd/common/block/pi/heapmanager.c b/src/soc/amd/common/block/pi/heapmanager.c
new file mode 100644
index 0000000000..bda521f07f
--- /dev/null
+++ b/src/soc/amd/common/block/pi/heapmanager.c
@@ -0,0 +1,363 @@
+/*
+ * 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 <amdblocks/agesawrapper.h>
+#include <amdlib.h>
+#include <arch/acpi.h>
+#include <amdblocks/BiosCallOuts.h>
+#include <cbmem.h>
+#include <string.h>
+
+static void *GetHeapBase(void)
+{
+	return cbmem_add(CBMEM_ID_RESUME_SCRATCH, BIOS_HEAP_SIZE);
+}
+
+static void EmptyHeap(int unused)
+{
+	void *BiosManagerPtr = GetHeapBase();
+	memset(BiosManagerPtr, 0, BIOS_HEAP_SIZE);
+}
+
+#if IS_ENABLED(CONFIG_LATE_CBMEM_INIT)
+#error "Only EARLY_CBMEM_INIT is supported."
+#endif
+ROMSTAGE_CBMEM_INIT_HOOK(EmptyHeap)
+
+AGESA_STATUS agesa_AllocateBuffer (UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	UINT32              AvailableHeapSize;
+	UINT8               *BiosHeapBaseAddr;
+	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;
+	BIOS_HEAP_MANAGER  *BiosHeapBasePtr;
+	AGESA_BUFFER_PARAMS *AllocParams;
+
+	AllocParams = ((AGESA_BUFFER_PARAMS *)ConfigPtr);
+	AllocParams->BufferPointer = NULL;
+
+	AvailableHeapSize = BIOS_HEAP_SIZE - sizeof(BIOS_HEAP_MANAGER);
+	BiosHeapBaseAddr = GetHeapBase();
+	BiosHeapBasePtr = (BIOS_HEAP_MANAGER *)BiosHeapBaseAddr;
+
+	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) { /* todo: simplify this */
+			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 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;
+}
+
+AGESA_STATUS agesa_DeallocateBuffer (UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+
+	UINT8               *BiosHeapBaseAddr;
+	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;
+	BIOS_HEAP_MANAGER  *BiosHeapBasePtr;
+	AGESA_BUFFER_PARAMS *AllocParams;
+
+	AllocParams = (AGESA_BUFFER_PARAMS *)ConfigPtr;
+
+	BiosHeapBaseAddr = GetHeapBase();
+	BiosHeapBasePtr = (BIOS_HEAP_MANAGER *)BiosHeapBaseAddr;
+
+	/* 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 */
+	memset((UINT8 *)AllocNodePtr + sizeof(BIOS_BUFFER_NODE), 0,
+		AllocNodePtr->BufferSize);
+	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;
+}
+
+AGESA_STATUS agesa_LocateBuffer (UINT32 Func, UINTN Data, VOID *ConfigPtr)
+{
+	UINT32              AllocNodeOffset;
+	UINT8               *BiosHeapBaseAddr;
+	BIOS_BUFFER_NODE    *AllocNodePtr;
+	BIOS_HEAP_MANAGER   *BiosHeapBasePtr;
+	AGESA_BUFFER_PARAMS *AllocParams;
+
+	AllocParams = (AGESA_BUFFER_PARAMS *)ConfigPtr;
+
+	BiosHeapBaseAddr = GetHeapBase();
+	BiosHeapBasePtr = (BIOS_HEAP_MANAGER *)BiosHeapBaseAddr;
+
+	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;
+
+}