drivers/amd/agesa/s3_mtrr.c: Save MSR for S3 using an array

The size of the data used is fixed in this function so there is no
need for this aritmetic.

The function signature will be changed in a followup commit.

The cache_disable call is dropped as all the codepaths calling the
restore_mtrr function do this already.

TESTED on pcengines/apu1 and lenovo/g505s: S3 resume works fine.

Change-Id: I3c6df8951d39695cddd4635360d6407d4d001b0a
Signed-off-by: Arthur Heymans <arthur@aheymans.xyz>
Reviewed-on: https://review.coreboot.org/c/coreboot/+/44293
Reviewed-by: Mike Banon <mikebdp2@gmail.com>
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>
Reviewed-by: Kyösti Mälkki <kyosti.malkki@gmail.com>
Reviewed-by: Michał Żygowski <michal.zygowski@3mdeb.com>

2 files changed, 55 insertions, 93 deletions

diff --git a/src/drivers/amd/agesa/oem_s3.c b/src/drivers/amd/agesa/oem_s3.c
index e47daf3711..10a8390b00 100644
--- a/src/drivers/amd/agesa/oem_s3.c
+++ b/src/drivers/amd/agesa/oem_s3.c
@@ -101,7 +101,7 @@ static int spi_SaveS3info(u32 pos, u32 size, u8 *buf, u32 len)
 #endif
 }
 
-static u8 MTRRStorage[S3_DATA_MTRR_SIZE];
+__aligned((sizeof(msr_t))) static u8 MTRRStorage[S3_DATA_MTRR_SIZE];
 
 AGESA_STATUS OemS3Save(AMD_S3_PARAMS *dataBlock)
 {
diff --git a/src/drivers/amd/agesa/s3_mtrr.c b/src/drivers/amd/agesa/s3_mtrr.c
index 6c7ba5fffe..fea9d7cc37 100644
--- a/src/drivers/amd/agesa/s3_mtrr.c
+++ b/src/drivers/amd/agesa/s3_mtrr.c
@@ -8,114 +8,76 @@
 #include <string.h>
 #include <northbridge/amd/agesa/agesa_helper.h>
 
-static void write_mtrr(u8 **p_nvram_pos, unsigned int idx)
-{
-	msr_t  msr_data;
-	msr_data = rdmsr(idx);
-
-	memcpy(*p_nvram_pos, &msr_data, sizeof(msr_data));
-	*p_nvram_pos += sizeof(msr_data);
-}
+/* TODO: Do we want MTRR_DEF_TYPE_MSR too? */
+static const uint32_t msr_backup[] = {
+	MTRR_FIX_64K_00000,
+	MTRR_FIX_16K_80000,
+	MTRR_FIX_16K_A0000,
+	MTRR_FIX_4K_C0000,
+	MTRR_FIX_4K_C8000,
+	MTRR_FIX_4K_D0000,
+	MTRR_FIX_4K_D8000,
+	MTRR_FIX_4K_E0000,
+	MTRR_FIX_4K_E8000,
+	MTRR_FIX_4K_F0000,
+	MTRR_FIX_4K_F8000,
+	MTRR_PHYS_BASE(0),
+	MTRR_PHYS_MASK(0),
+	MTRR_PHYS_BASE(1),
+	MTRR_PHYS_MASK(1),
+	MTRR_PHYS_BASE(2),
+	MTRR_PHYS_MASK(2),
+	MTRR_PHYS_BASE(3),
+	MTRR_PHYS_MASK(3),
+	MTRR_PHYS_BASE(4),
+	MTRR_PHYS_MASK(4),
+	MTRR_PHYS_BASE(5),
+	MTRR_PHYS_MASK(5),
+	MTRR_PHYS_BASE(6),
+	MTRR_PHYS_MASK(6),
+	MTRR_PHYS_BASE(7),
+	MTRR_PHYS_MASK(7),
+	SYSCFG_MSR,
+	TOP_MEM,
+	TOP_MEM2,
+};
 
 void backup_mtrr(void *mtrr_store, u32 *mtrr_store_size)
 {
-	u8 *nvram_pos = mtrr_store;
-	msr_t  msr_data;
-	u32 i;
+	msr_t syscfg_msr;
+	msr_t *mtrr_save = (msr_t *)mtrr_store;
 
 	/* 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, MTRR_FIX_64K_00000);
-	write_mtrr(&nvram_pos, MTRR_FIX_16K_80000);
-	write_mtrr(&nvram_pos, MTRR_FIX_16K_A0000);
+	syscfg_msr = rdmsr(SYSCFG_MSR);
+	syscfg_msr.lo |= SYSCFG_MSR_MtrrFixDramModEn;
+	wrmsr(SYSCFG_MSR, syscfg_msr);
 
-	for (i = MTRR_FIX_4K_C0000; i <= MTRR_FIX_4K_F8000; i++)
-		write_mtrr(&nvram_pos, i);
+	for (int i = 0; i < ARRAY_SIZE(msr_backup); i++)
+		*mtrr_save++ = rdmsr(msr_backup[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);
+	syscfg_msr = rdmsr(SYSCFG_MSR);
+	syscfg_msr.lo &= ~SYSCFG_MSR_MtrrFixDramModEn;
+	wrmsr(SYSCFG_MSR, syscfg_msr);
 
-	/* Variable MTRRs */
-	for (i = MTRR_PHYS_BASE(0); i < MTRR_PHYS_BASE(8); i++)
-		write_mtrr(&nvram_pos, i);
-
-	/* SYSCFG_MSR */
-	write_mtrr(&nvram_pos, SYSCFG_MSR);
-	/* TOM */
-	write_mtrr(&nvram_pos, TOP_MEM);
-	/* TOM2 */
-	write_mtrr(&nvram_pos, TOP_MEM2);
-
-	*mtrr_store_size = nvram_pos - (u8*) mtrr_store;
+	*mtrr_store_size = sizeof(msr_t) * ARRAY_SIZE(msr_backup);
 }
 
 void restore_mtrr(void)
 {
-	volatile u32 *msrPtr = (u32 *) OemS3Saved_MTRR_Storage();
-	u32 msr;
-	msr_t msr_data;
-
-	if (!msrPtr)
-		return;
-
-	disable_cache();
+	msr_t syscfg_msr;
+	msr_t *mtrr_save = (msr_t *)OemS3Saved_MTRR_Storage();
 
 	/* 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(MTRR_FIX_64K_00000, msr_data);
-
-	msr_data.lo = *msrPtr;
-	msrPtr ++;
-	msr_data.hi = *msrPtr;
-	msrPtr ++;
-	wrmsr(MTRR_FIX_16K_80000, msr_data);
+	syscfg_msr = rdmsr(SYSCFG_MSR);
+	syscfg_msr.lo |= SYSCFG_MSR_MtrrFixDramModEn;
+	wrmsr(SYSCFG_MSR, syscfg_msr);
 
-	msr_data.lo = *msrPtr;
-	msrPtr ++;
-	msr_data.hi = *msrPtr;
-	msrPtr ++;
-	wrmsr(MTRR_FIX_16K_A0000, msr_data);
-
-	for (msr = MTRR_FIX_4K_C0000; msr <= MTRR_FIX_4K_F8000; msr++) {
-		msr_data.lo = *msrPtr;
-		msrPtr ++;
-		msr_data.hi = *msrPtr;
-		msrPtr ++;
-		wrmsr(msr, msr_data);
-	}
+	for (int i = 0; i < ARRAY_SIZE(msr_backup); i++)
+		wrmsr(msr_backup[i], *mtrr_save++);
 
 	/* 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 = MTRR_PHYS_BASE(0); msr <= MTRR_PHYS_MASK(7); 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);
+	syscfg_msr = rdmsr(SYSCFG_MSR);
+	syscfg_msr.lo &= ~SYSCFG_MSR_MtrrFixDramModEn;
+	wrmsr(SYSCFG_MSR, syscfg_msr);
 }