4 files changed, 48 insertions, 7 deletions

diff --git a/src/northbridge/amd/agesa/state_machine.c b/src/northbridge/amd/agesa/state_machine.c
index 3f61c584aa..7743603f0b 100644
--- a/src/northbridge/amd/agesa/state_machine.c
+++ b/src/northbridge/amd/agesa/state_machine.c
@@ -330,6 +330,7 @@ static void amd_bs_ramstage_init(void *arg)
 		if (IS_ENABLED(CONFIG_LATE_CBMEM_INIT))
 			cbmem_initialize();
 		agesa_execute_state(cb, AMD_S3LATE_RESTORE);
+		fchs3earlyrestore(&cb->StdHeader);
 	}
 }
 
@@ -351,8 +352,10 @@ static void amd_bs_post_device(void *arg)
 {
 	struct sysinfo *cb = arg;
 
-	if (acpi_is_wakeup_s3())
+	if (acpi_is_wakeup_s3()) {
+		fchs3laterestore(&cb->StdHeader);
 		return;
+	}
 
 	agesa_execute_state(cb, AMD_INIT_LATE);
 
@@ -386,3 +389,15 @@ 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;
+}
diff --git a/src/northbridge/amd/agesa/state_machine.h b/src/northbridge/amd/agesa/state_machine.h
index 3d8e53d3d4..f2551de5d9 100644
--- a/src/northbridge/amd/agesa/state_machine.h
+++ b/src/northbridge/amd/agesa/state_machine.h
@@ -38,6 +38,10 @@ AGESA_STATUS OemS3LateRestore(AMD_S3_PARAMS *dataBlock);
 AGESA_STATUS OemS3Save(AMD_S3_PARAMS *dataBlock);
 #endif
 
+/* For FCH */
+AGESA_STATUS fchs3earlyrestore(AMD_CONFIG_PARAMS *StdHeader);
+AGESA_STATUS fchs3laterestore(AMD_CONFIG_PARAMS *StdHeader);
+
 struct sysinfo
 {
 	AMD_CONFIG_PARAMS StdHeader;
diff --git a/src/southbridge/amd/agesa/hudson/hudson.h b/src/southbridge/amd/agesa/hudson/hudson.h
index d870845055..c89f6825f7 100644
--- a/src/southbridge/amd/agesa/hudson/hudson.h
+++ b/src/southbridge/amd/agesa/hudson/hudson.h
@@ -80,7 +80,6 @@ int s3_load_nvram_early(int size, u32 *old_dword, int nvram_pos);
 
 #else
 void hudson_enable(device_t dev);
-void s3_resume_init_data(void *FchParams);
 
 #endif /* __PRE_RAM__ */
 #endif /* __SMM__ */
diff --git a/src/southbridge/amd/agesa/hudson/resume.c b/src/southbridge/amd/agesa/hudson/resume.c
index cbfa9f421a..e680fadf31 100644
--- a/src/southbridge/amd/agesa/hudson/resume.c
+++ b/src/southbridge/amd/agesa/hudson/resume.c
@@ -20,6 +20,8 @@
 #include "hudson.h"
 #include "AGESA.h"
 
+#include <northbridge/amd/agesa/state_machine.h>
+
 extern FCH_DATA_BLOCK InitEnvCfgDefault;
 extern FCH_INTERFACE FchInterfaceDefault;
 extern FCH_RESET_DATA_BLOCK  InitResetCfgDefault;
@@ -27,13 +29,9 @@ extern FCH_RESET_INTERFACE FchResetInterfaceDefault;
 
 #define DUMP_FCH_SETTING 0
 
-void s3_resume_init_data(void *data)
+static void s3_resume_init_data(FCH_DATA_BLOCK *FchParams)
 {
-	FCH_DATA_BLOCK *FchParams = (FCH_DATA_BLOCK *)data;
-	AMD_CONFIG_PARAMS *StdHeader = FchParams->StdHeader;
-
 	*FchParams = InitEnvCfgDefault;
-	FchParams->StdHeader = StdHeader;
 
 	FchParams->Usb.Xhci0Enable              = InitResetCfgDefault.FchReset.Xhci0Enable;
 	FchParams->Usb.Xhci1Enable              = InitResetCfgDefault.FchReset.Xhci1Enable;
@@ -120,3 +118,28 @@ void s3_resume_init_data(void *data)
 	}
 #endif
 }
+
+AGESA_STATUS fchs3earlyrestore(AMD_CONFIG_PARAMS *StdHeader)
+{
+	FCH_DATA_BLOCK      FchParams;
+
+	/* FIXME: Recover FCH_DATA_BLOCK from CBMEM. */
+	s3_resume_init_data(&FchParams);
+
+	FchParams.StdHeader = StdHeader;
+	FchInitS3EarlyRestore(&FchParams);
+	return AGESA_SUCCESS;
+}
+
+AGESA_STATUS fchs3laterestore(AMD_CONFIG_PARAMS *StdHeader)
+{
+	FCH_DATA_BLOCK      FchParams;
+
+	/* FIXME: Recover FCH_DATA_BLOCK from CBMEM. */
+	s3_resume_init_data(&FchParams);
+
+	FchParams.StdHeader = StdHeader;
+	FchInitS3LateRestore(&FchParams);
+
+	return AGESA_SUCCESS;
+}