3 files changed, 33 insertions, 0 deletions
diff --git a/src/include/cpu/amd/msr.h b/src/include/cpu/amd/msr.h
index 606ea23bc0..e466e7bbf2 100644
--- a/src/include/cpu/amd/msr.h
+++ b/src/include/cpu/amd/msr.h
@@ -72,6 +72,7 @@
 #define EX_CFG_MSR			0xC001102C
 #define LS_CFG2_MSR			0xC001102D
 #define IBS_OP_DATA3_MSR		0xC0011037
+#define S3_RESUME_EIP_MSR		0xC00110E0
 
 #define MSR_PATCH_LEVEL			0x0000008B
 #define CORE_PERF_BOOST_CTRL		0x15c
diff --git a/src/soc/amd/picasso/bootblock/bootblock.c b/src/soc/amd/picasso/bootblock/bootblock.c
index 606440bfc1..a3935cc7aa 100644
--- a/src/soc/amd/picasso/bootblock/bootblock.c
+++ b/src/soc/amd/picasso/bootblock/bootblock.c
@@ -12,6 +12,9 @@
 #include <soc/southbridge.h>
 #include <soc/i2c.h>
 #include <amdblocks/amd_pci_mmconf.h>
+#include <acpi/acpi.h>
+
+asmlinkage void bootblock_resume_entry(void);
 
 /* PSP performs the memory training and setting up DRAM map prior to x86 cores
    being released. Honor TOP_MEM and set up caching from 0 til TOP_MEM. Likewise,
@@ -84,9 +87,27 @@ static void set_caching(void)
 	enable_cache();
 }
 
+static void write_resume_eip(void)
+{
+	msr_t s3_resume_entry = {
+		.hi = (uint64_t)(uintptr_t)bootblock_resume_entry >> 32,
+		.lo = (uintptr_t)bootblock_resume_entry & 0xffffffff,
+	};
+
+	/*
+	 * Writing to the EIP register can only be done once, otherwise a fault is triggered.
+	 * When this register is written, it will trigger the microcode to stash the CPU state
+	 * (crX , mtrrs, registers, etc) into the CC6 save area. On resume, the state will be
+	 * restored and execution will continue at the EIP.
+	 */
+	if (!acpi_is_wakeup_s3())
+		wrmsr(S3_RESUME_EIP_MSR, s3_resume_entry);
+}
+
 asmlinkage void bootblock_c_entry(uint64_t base_timestamp)
 {
 	set_caching();
+	write_resume_eip();
 	enable_pci_mmconf();
 
 	bootblock_main_with_basetime(base_timestamp);
diff --git a/src/soc/amd/picasso/bootblock/pre_c.S b/src/soc/amd/picasso/bootblock/pre_c.S
index 5c186f1062..83e5491cd5 100644
--- a/src/soc/amd/picasso/bootblock/pre_c.S
+++ b/src/soc/amd/picasso/bootblock/pre_c.S
@@ -2,6 +2,17 @@
 
 #include <cpu/x86/post_code.h>
 
+.global bootblock_resume_entry
+bootblock_resume_entry:
+	post_code(0xb0)
+
+	/* Get an early timestamp */
+	rdtsc
+	movd	%eax, %mm1
+	movd	%edx, %mm2
+
+	/* Fall through to bootblock_pre_c_entry */
+
 /*
  * on entry:
  *   mm0: BIST (ignored)