2 files changed, 30 insertions, 41 deletions

diff --git a/src/soc/intel/skylake/bootblock/cache_as_ram.S b/src/soc/intel/skylake/bootblock/cache_as_ram.S
index eb3d390154..04abba65fd 100644
--- a/src/soc/intel/skylake/bootblock/cache_as_ram.S
+++ b/src/soc/intel/skylake/bootblock/cache_as_ram.S
@@ -231,6 +231,7 @@ find_llc_subleaf:
 	xorl	%edx, %edx
 	wrmsr
 
+	post_code(0x27)
 	/*
 	 * Enable No-Eviction Mode Run State by setting
 	 * NO_EVICT_MODE MSR 2E0h bit [1] = '1'.
@@ -241,53 +242,15 @@ find_llc_subleaf:
 	orl	$0x02, %eax
 	wrmsr
 
-	post_code(0x27)
-	/*
-	 *   Configure the BIOS code region as write-protected (WP) cacheable
-	 *   memory type using a single variable range MTRR.
-	 *
-	 *   Ensure region to cache meets MTRR requirements for
-	 *   size and alignment.
-	 */
-	movl	$(0xFFFFFFFF - CONFIG_ROM_SIZE + 1), %edi /* Code region base */
-	movl	$CONFIG_ROM_SIZE, %eax			  /* Code region size */
-	cmpl	$0, %edi
-	jz	.halt_forever
-	cmpl	$0, %eax
-	jz	.halt_forever
-
-	post_code(0x28)
-	/*
-	 * Program base register
-	 */
-	xorl	%edx, %edx			/* clear upper dword */
-	movl	$MTRR_PHYS_BASE(1), %ecx	/* setup variable mtrr */
-	movl	%edi, %eax
-	orl	$MTRR_TYPE_WRPROT, %eax	/* set type to write protect */
-	wrmsr
-
-	movl	$CONFIG_ROM_SIZE, %eax
-
-	/*
-	 * Compute MTRR mask value:  Mask = NOT (Size - 1)
-	 */
-	dec	%eax	/* eax - size to cache less one byte */
-	not	%eax	/* eax contains low 32 bits of mask */
-	or	$MTRR_PHYS_MASK_VALID, %eax
-	/*
-	 * Program mask register
-	 */
-	movl	$MTRR_PHYS_MASK(1) , %ecx	/* setup variable mtrr */
-	movl	%esi, %edx	/* edx <- MTRR_PHYS_MASK_HIGH */
-	wrmsr
-
 car_init_done:
 
-	post_code(0x29)
+	post_code(0x28)
 
 	/* Setup bootblock stack */
 	mov	$_car_stack_end, %esp
 
+	post_code(0x29)
+
 	/*push TSC value to stack*/
 	movd	%mm2, %eax
 	pushl	%eax	/* tsc[63:32] */
diff --git a/src/soc/intel/skylake/bootblock/cpu.c b/src/soc/intel/skylake/bootblock/cpu.c
index c6ede53937..fab589f611 100644
--- a/src/soc/intel/skylake/bootblock/cpu.c
+++ b/src/soc/intel/skylake/bootblock/cpu.c
@@ -18,8 +18,10 @@
 #include <delay.h>
 #include <arch/io.h>
 #include <console/console.h>
+#include <cpu/x86/mtrr.h>
 #include <cpu/intel/microcode/microcode.c>
 #include <reset.h>
+#include <lib.h>
 #include <soc/bootblock.h>
 #include <soc/cpu.h>
 #include <soc/iomap.h>
@@ -101,8 +103,32 @@ static void set_flex_ratio_to_tdp_nominal(void)
 	soft_reset();
 }
 
+static void cache_bios_region(void)
+{
+	int mtrr;
+	size_t rom_size;
+	uint32_t alignment;
+
+	mtrr = get_free_var_mtrr();
+
+	if (mtrr == -1)
+		return;
+
+	/* Only the IFD BIOS region is memory mapped (at top of 4G) */
+	rom_size = CONFIG_ROM_SIZE;
+
+	if (!rom_size)
+		return;
+
+	/* Round to power of two */
+	alignment = 1 << (log2_ceil(rom_size));
+	rom_size = ALIGN_UP(rom_size, alignment);
+	set_var_mtrr(mtrr, 4ULL*GiB - rom_size, rom_size, MTRR_TYPE_WRPROT);
+}
+
 void bootblock_cpu_init(void)
 {
+	cache_bios_region();
 	/* Set flex ratio and reset if needed */
 	set_flex_ratio_to_tdp_nominal();
 	intel_update_microcode_from_cbfs();