- Moved hlt() to it's own header.

- Reworked pnp superio device support.  Now complete superio support is less than 100 lines.
- Added support for hard coding resource assignments in Config.lb
- Minor bug fixes to romcc
- Initial support for catching the x86 processor BIST error codes.  I've only seen
  this trigger once in production during a very suspcious reset but...
- added raminit_test to test the code paths in raminit.c for the Opteron
- Removed the IORESOURCE_SET bit and added IORESOURCE_ASSIGNED and IORESOURCE_STORED
  so we can tell what we have really done.
- Added generic AGP/IOMMU setting code to x86
- Added an implementation of memmove and removed reserved identifiers from memcpy
- Added minimal support for booting on pre b3 stepping K8 cores
- Moved the checksum on amd8111 boards because our default location was on top of
  extended RTC registers
- On the Hdama added support for enabling i2c hub so we can get at the temperature
  sensors.  Not that i2c bus was implemented well enough to make that useful.
- Redid the Opteron port so we should only need one reset and most of memory initialization
  is done in cpu_fixup.  This is much, much faster.
- Attempted to make the VGA IO region assigment work.  The code seems to work now...
- Redid the error handling in amdk8/raminit.c to distinguish between a bad value
  and a smbus error, and moved memory clearing out to cpufixup.
- Removed CONFIG_KEYBOARD as it was useless.  See pc87360/superio.c for how to
  setup a legacy keyboard properly.
- Reworked the register values for standard hardware, moving the defintions from
  chip.h into the headers of the initialization routines.  This is much saner
  and is actually implemented.
- Made the hdama port an under clockers BIOS.  I debuged so many interesting problems.
- On amd8111_lpc added setup of architectural/legacy hardware
- Enabled PCI error reporting as much as possible.
- Enhanded build_opt_tbl to generate a header of the cmos option locations so
  that romcc compiled code can query the cmos options.
- In romcc gracefully handle function names that degenerate into function pointers
- Bumped the version to 1.1.6 as we are getting closer to 2.0

  TODO finish optimizing the HT links of non dual boards
  TODO make all Opteron board work again
  TODO convert all superio devices to use the new helpers
  TODO convert the via/epia to freebios2 conventions
  TODO cpu fixup/setup by cpu type


git-svn-id: svn://svn.coreboot.org/coreboot/trunk@1390 2b7e53f0-3cfb-0310-b3e9-8179ed1497e1

13 files changed, 488 insertions, 53 deletions

diff --git a/src/cpu/i386/bist32.inc b/src/cpu/i386/bist32.inc
new file mode 100644
index 0000000000..d2fb98cdc3
--- /dev/null
+++ b/src/cpu/i386/bist32.inc
@@ -0,0 +1,4 @@
+
+	/* Carefully print the failure if the built in self test did not pass */
+	testl	%eax, %eax
+	jnz	bist32_fail
diff --git a/src/cpu/i386/bist32_fail.inc b/src/cpu/i386/bist32_fail.inc
new file mode 100644
index 0000000000..f467b065bc
--- /dev/null
+++ b/src/cpu/i386/bist32_fail.inc
@@ -0,0 +1,44 @@
+
+
+	jmp bist32_fail_0
+bist32_fail:
+	movl	%eax, %ebp
+
+#if 1
+#define SIO_BASE  0x2e
+#define SIO_INDEX SIO_BASE
+#define SIO_DATA  SIO_BASE+1
+#define SIO_WRITE_CONFIG(value, reg) \
+	movb	reg, %al		; \
+	outb	%al, $(SIO_INDEX)	; \
+	movb	value, %al		; \
+	outb	%al, $(SIO_DATA)
+
+#define SIO_READ_CONFIG(reg) \
+	movb	reg, %al		; \
+	outb	%al, $(SIO_INDEX)	; \
+	inb	$(SIO_DATA), %al
+
+#define SIO_SET_LOGICAL_DEVICE(device) \
+	SIO_WRITE_CONFIG(device, $0x07)
+
+	/* Enable serial 1 */
+	SIO_SET_LOGICAL_DEVICE($3)
+	SIO_WRITE_CONFIG($1, $0x30)
+	SIO_WRITE_CONFIG($0x3, $0x60)
+	SIO_WRITE_CONFIG($0xf8, $0x61)
+
+#endif	
+	CALLSP(serial_init)
+	CONSOLE_DEBUG_TX_STRING($str_bist_failed)
+	CONSOLE_DEBUG_TX_HEX32(%ebp)
+	CONSOLE_DEBUG_TX_STRING($str_bist_newline)
+	jmp .Lhlt
+
+bist32_fail_0:
+
+.section ".rom.data"
+str_bist_failed:	.string "BIST failed: "
+str_bist_newline:	.string "\r\n"
+.previous
+
diff --git a/src/cpu/i386/entry16.inc b/src/cpu/i386/entry16.inc
index 7ab3ff3bf0..f9e921f291 100644
--- a/src/cpu/i386/entry16.inc
+++ b/src/cpu/i386/entry16.inc
@@ -33,6 +33,8 @@ it with the version available from LANL.
 
 _start: 
 	cli
+	/* Save the BIST result */
+	movl	%eax, %ebp
 
 /* thanks to kmliu@sis.tw.com for this TBL fix ... */
 /**/
@@ -93,6 +95,9 @@ _start:
 	orl	$0x60000001, %eax /* CD, NW, PE = 1 */
 	movl	%eax, %cr0
 
+	/* Restore BIST to %eax */
+	movl	%ebp, %eax
+
 	/* Now that we are in protected mode jump to a 32 bit code segment. */
 	data32	ljmp	$ROM_CODE_SEG, $__protected_start
 
diff --git a/src/cpu/i386/entry32.inc b/src/cpu/i386/entry32.inc
index 6f55f00203..3d30a3f85f 100644
--- a/src/cpu/i386/entry32.inc
+++ b/src/cpu/i386/entry32.inc
@@ -44,6 +44,9 @@ protected_start:
 	ljmp	$ROM_CODE_SEG, $__protected_start
 	
 __protected_start:
+	/* Save the BIST value */
+	movl	%eax, %ebp
+
 	intel_chip_post_macro(0x10)	/* post 10 */
 
 	movw	$ROM_DATA_SEG, %ax
@@ -53,3 +56,6 @@ __protected_start:
 	movw	%ax, %fs
 	movw	%ax, %gs
 
+	/* Restore the BIST value to %eax */
+	movl	%ebp, %eax
+
diff --git a/src/cpu/k8/cpufixup.c b/src/cpu/k8/cpufixup.c
index 6aa6722579..2d347de829 100644
--- a/src/cpu/k8/cpufixup.c
+++ b/src/cpu/k8/cpufixup.c
@@ -4,8 +4,17 @@
 #include <cpu/p6/msr.h>
 #include <cpu/k8/mtrr.h>
 #include <device/device.h>
-#include "../../northbridge/amd/amdk8/cpu_rev.c"
 #include <device/chip.h>
+#include <device/device.h>
+#include <device/pci.h>
+#include <smp/start_stop.h>
+#include <string.h>
+#include <cpu/p6/msr.h>
+#include <cpu/p6/pgtbl.h>
+#include <pc80/mc146818rtc.h>
+#include <arch/smp/lapic.h>
+#include "../../northbridge/amd/amdk8/amdk8.h"
+#include "../../northbridge/amd/amdk8/cpu_rev.c"
 #include "chip.h"
 
 #define MCI_STATUS 0x401
@@ -14,7 +23,7 @@ static inline void disable_cache(void)
 {
 	unsigned int tmp;
 	/* Disable cache */
-	/* Write back the cache and flush TLB */
+	/* Write back the cache */
 	asm volatile (
 		"movl  %%cr0, %0\n\t"
 		"orl  $0x40000000, %0\n\t"
@@ -57,6 +66,232 @@ static inline void wrmsr_amd(unsigned index, msr_t msr)
                 );
 }
 
+
+
+#define MTRR_COUNT 8
+#define ZERO_CHUNK_KB 0x800UL /* 2M */
+#define TOLM_KB 0x400000UL
+
+struct mtrr {
+	msr_t base;
+	msr_t mask;
+};
+struct mtrr_state {
+	struct mtrr mtrrs[MTRR_COUNT];
+	msr_t top_mem, top_mem2;
+	msr_t def_type;
+};
+
+static void save_mtrr_state(struct mtrr_state *state)
+{
+	int i;
+	for(i = 0; i < MTRR_COUNT; i++) {
+		state->mtrrs[i].base = rdmsr(MTRRphysBase_MSR(i));
+		state->mtrrs[i].mask = rdmsr(MTRRphysMask_MSR(i));
+	}
+	state->top_mem  = rdmsr(TOP_MEM);
+	state->top_mem2 = rdmsr(TOP_MEM2);
+	state->def_type = rdmsr(MTRRdefType_MSR);
+}
+
+static void restore_mtrr_state(struct mtrr_state *state)
+{
+	int i;
+	disable_cache();
+
+	for(i = 0; i < MTRR_COUNT; i++) {
+		wrmsr(MTRRphysBase_MSR(i), state->mtrrs[i].base);
+		wrmsr(MTRRphysMask_MSR(i), state->mtrrs[i].mask);
+	}
+	wrmsr(TOP_MEM,         state->top_mem);
+	wrmsr(TOP_MEM2,        state->top_mem2);
+	wrmsr(MTRRdefType_MSR, state->def_type);
+
+	enable_cache();
+}
+
+
+#if 0
+static void print_mtrr_state(struct mtrr_state *state)
+{
+	int i;
+	for(i = 0; i < MTRR_COUNT; i++) {
+		printk_debug("var mtrr %d: %08x%08x mask: %08x%08x\n",
+			i,
+			state->mtrrs[i].base.hi, state->mtrrs[i].base.lo,
+			state->mtrrs[i].mask.hi, state->mtrrs[i].mask.lo);
+	}
+	printk_debug("top_mem:  %08x%08x\n",
+		state->top_mem.hi, state->top_mem.lo);
+	printk_debug("top_mem2: %08x%08x\n",
+		state->top_mem2.hi, state->top_mem2.lo);
+	printk_debug("def_type: %08x%08x\n",
+		state->def_type.hi, state->def_type.lo);
+}
+#endif
+
+static void set_init_ecc_mtrrs(void)
+{
+	msr_t msr;
+	int i;
+	disable_cache();
+
+	/* First clear all of the msrs to be safe */
+	for(i = 0; i < MTRR_COUNT; i++) {
+		msr_t zero;
+		zero.lo = zero.hi = 0;
+		wrmsr(MTRRphysBase_MSR(i), zero);
+		wrmsr(MTRRphysMask_MSR(i), zero);
+	}
+
+	/* Write back cache the first 1MB */
+	msr.hi = 0x00000000;
+	msr.lo = 0x00000000 | MTRR_TYPE_WRBACK;
+	wrmsr(MTRRphysBase_MSR(0), msr);
+	msr.hi = 0x000000ff;
+	msr.lo = ~((CONFIG_LB_MEM_TOPK << 10) - 1) | 0x800;
+	wrmsr(MTRRphysMask_MSR(0), msr);
+
+	/* Set the default type to write combining */
+	msr.hi = 0x00000000;
+	msr.lo = 0xc00 | MTRR_TYPE_WRCOMB;
+	wrmsr(MTRRdefType_MSR, msr);
+
+	/* Set TOP_MEM to 4G */
+	msr.hi = 0x00000001;
+	msr.lo = 0x00000000;
+	wrmsr(TOP_MEM, msr);
+
+	enable_cache();
+}
+
+
+static void init_ecc_memory(void)
+{
+	unsigned long startk, begink, endk;
+	unsigned long basek;
+	struct mtrr_state mtrr_state;
+	device_t f1_dev, f2_dev, f3_dev;
+	int cpu_index, cpu_id, node_id;
+	int enable_scrubbing;
+	uint32_t dcl;
+	cpu_id = this_processors_id();
+	cpu_index = processor_index(cpu_id);
+	/* For now there is a 1-1 mapping between node_id and cpu_id */
+	node_id = cpu_id;
+
+	f1_dev = dev_find_slot(0, PCI_DEVFN(0x18 + node_id, 1));
+	if (!f1_dev) {
+		die("Cannot find cpu function 1\n");
+	}
+	f2_dev = dev_find_slot(0, PCI_DEVFN(0x18 + node_id, 2));
+	if (!f2_dev) {
+		die("Cannot find cpu function 2\n");
+	}
+	f3_dev = dev_find_slot(0, PCI_DEVFN(0x18 + node_id, 3));
+	if (!f3_dev) {
+		die("Cannot find cpu function 3\n");
+	}
+
+	/* See if we scrubbing should be enabled */
+	enable_scrubbing = 1;
+	get_option(&enable_scrubbing, "hw_scrubber");
+
+	/* Enable cache scrubbing at the lowest possible rate */
+	if (enable_scrubbing) {
+		pci_write_config32(f3_dev, SCRUB_CONTROL,
+			(SCRUB_84ms << 16) | (SCRUB_84ms << 8) | (SCRUB_NONE << 0));
+	} else {
+		pci_write_config32(f3_dev, SCRUB_CONTROL,
+			(SCRUB_NONE << 16) | (SCRUB_NONE << 8) | (SCRUB_NONE << 0));
+		printk_debug("Scrubbing Disabled\n");
+	}
+	
+
+	/* If ecc support is not enabled don't touch memory */
+	dcl = pci_read_config32(f2_dev, DRAM_CONFIG_LOW);
+	if (!(dcl & DCL_DimmEccEn)) {
+		return;
+	}
+
+	startk = (pci_read_config32(f1_dev, 0x40 + (node_id*8)) & 0xffff0000) >> 2;
+	endk   = ((pci_read_config32(f1_dev, 0x44 + (node_id*8)) & 0xffff0000) >> 2) + 0x4000;
+
+	/* Don't start too early */
+	begink = startk;
+	if (begink < CONFIG_LB_MEM_TOPK) {
+		begink = CONFIG_LB_MEM_TOPK;
+	}
+	printk_debug("Clearing memory %uK - %uK: ", startk, endk);
+
+	/* Save the normal state */
+	save_mtrr_state(&mtrr_state);
+
+	/* Switch to the init ecc state */
+	set_init_ecc_mtrrs();
+	disable_lapic();
+
+	/* Walk through 2M chunks and zero them */
+	for(basek = begink; basek < endk; basek = ((basek + ZERO_CHUNK_KB) & ~(ZERO_CHUNK_KB - 1))) {
+		unsigned long limitk;
+		unsigned long size;
+		void *addr;
+
+		/* Report every 64M */
+		if ((basek % (64*1024)) == 0) {
+			/* Restore the normal state */
+			map_2M_page(cpu_index, 0);
+			restore_mtrr_state(&mtrr_state);
+			enable_lapic();
+
+			/* Print a status message */
+			printk_debug("%c", (basek >= TOLM_KB)?'+':'-');
+
+			/* Return to the initialization state */
+			set_init_ecc_mtrrs();
+			disable_lapic();
+		}
+		limitk = (basek + ZERO_CHUNK_KB) & ~(ZERO_CHUNK_KB - 1);
+		if (limitk > endk) {
+			limitk = endk;
+		}
+		size = (limitk - basek) << 10;
+		addr = map_2M_page(cpu_index, basek >> 11);
+		addr = (void *)(((uint32_t)addr) | ((basek & 0x7ff) << 10));
+		if (addr == MAPPING_ERROR) {
+			continue;
+		}
+
+		/* clear memory 2M (limitk - basek) */
+		__asm__ volatile(
+			"1: \n\t"
+			"movl %0, (%1)\n\t"
+			"addl $4,%1\n\t"
+			"subl $4,%2\n\t"
+			"jnz 1b\n\t"
+			:
+			: "a" (0), "D" (addr),	"c" (size)
+			);
+	}
+	/* Restore the normal state */
+	map_2M_page(cpu_index, 0);
+	restore_mtrr_state(&mtrr_state);
+	enable_lapic();
+
+	/* Set the scrub base address registers */
+	pci_write_config32(f3_dev, SCRUB_ADDR_LOW,  startk << 10);
+	pci_write_config32(f3_dev, SCRUB_ADDR_HIGH, startk >> 22);
+
+	/* Enable the scrubber? */
+	if (enable_scrubbing) {
+		/* Enable scrubbing at the lowest possible rate */
+		pci_write_config32(f3_dev, SCRUB_CONTROL,
+			(SCRUB_84ms << 16) | (SCRUB_84ms << 8) | (SCRUB_84ms << 0));
+	}
+
+	printk_debug(" done\n");
+}
+
 void k8_cpufixup(struct mem_range *mem)
 {
 	unsigned long mmio_basek, tomk;
@@ -83,7 +318,11 @@ void k8_cpufixup(struct mem_range *mem)
 	if (mmio_basek > tomk) {
 		mmio_basek = tomk;
 	}
-
+	/* Round mmio_basek down to the nearst size that will fit in TOP_MEM */
+	mmio_basek = mmio_basek & ~TOP_MEM_MASK_KB;
+	/* Round tomk up to the next greater size that will fit in TOP_MEM */
+	tomk = (tomk + TOP_MEM_MASK_KB) & ~TOP_MEM_MASK_KB;
+		
 	/* Setup TOP_MEM */
 	msr.hi = mmio_basek >> 22;
 	msr.lo = mmio_basek << 10;
@@ -101,7 +340,7 @@ void k8_cpufixup(struct mem_range *mem)
 	for(i = IORR_FIRST; i <= IORR_LAST; i++) {
 		wrmsr(i, msr);
 	}
-	
+
 	msr = rdmsr(SYSCFG_MSR);
 	msr.lo |= SYSCFG_MSR_MtrrVarDramEn | SYSCFG_MSR_TOM2En;
 	wrmsr(SYSCFG_MSR, msr);
@@ -118,33 +357,34 @@ void k8_cpufixup(struct mem_range *mem)
 		msr = rdmsr(HWCR_MSR);
 		msr.lo |= (1 << 6);
 		wrmsr(HWCR_MSR, msr);
+
 		/* Erratum 69... */
-#if 1
 		msr = rdmsr_amd(BU_CFG_MSR);
 		msr.hi |= (1 << (45 - 32));
 		wrmsr_amd(BU_CFG_MSR, msr);
-#endif
+
 		/* Erratum 81... */
-#if 1
 		msr = rdmsr_amd(DC_CFG_MSR);
 		msr.lo |=  (1 << 10);
 		wrmsr_amd(DC_CFG_MSR, msr);
-#endif
 			
 	}
-	/* Erratum 89 ... */
-	msr = rdmsr(NB_CFG_MSR);
-	msr.lo |= 1 << 3;
+	/* I can't touch this msr on early buggy cpus */
+	if (!is_cpu_pre_b3()) {
 
-	if (!is_cpu_pre_c0()) {
-		/* Erratum 86 Disable data masking on C0 and 
-		 * later processor revs.
-		 * FIXME this is only needed if ECC is enabled.
-		 */
-		msr.hi |= 1 << (36 - 32);
-	}	
-	wrmsr(NB_CFG_MSR, msr);
-#if 1  /* The following erratum fixes reset the cpu ???? */
+		/* Erratum 89 ... */
+		msr = rdmsr(NB_CFG_MSR);
+		msr.lo |= 1 << 3;
+		
+		if (!is_cpu_pre_c0()) {
+			/* Erratum 86 Disable data masking on C0 and 
+			 * later processor revs.
+			 * FIXME this is only needed if ECC is enabled.
+			 */
+			msr.hi |= 1 << (36 - 32);
+		}	
+		wrmsr(NB_CFG_MSR, msr);
+	}
 	
 	/* Erratum 97 ... */
 	if (!is_cpu_pre_c0()) {
@@ -158,11 +398,14 @@ void k8_cpufixup(struct mem_range *mem)
 	msr.lo |= 1 << 11;
 	wrmsr_amd(IC_CFG_MSR, msr);
 
-#endif
-
 	/* Erratum 91 prefetch miss is handled in the kernel */
 	
 	enable_cache();
+
+	/* Is this a bad location?  In particular can another node prefecth
+	 * data from this node before we have initialized it?
+	 */
+	init_ecc_memory();
 }
 
 static
diff --git a/src/cpu/k8/earlymtrr.c b/src/cpu/k8/earlymtrr.c
index 47ddd12340..5fb63d2e7d 100644
--- a/src/cpu/k8/earlymtrr.c
+++ b/src/cpu/k8/earlymtrr.c
@@ -6,7 +6,7 @@
 
 static void early_mtrr_init(void)
 {
-	static unsigned long mtrr_msrs[] = {
+	static const unsigned long mtrr_msrs[] = {
 		/* fixed mtrr */
 		0x250, 0x258, 0x259,
 		0x268, 0x269, 0x26A
@@ -25,46 +25,46 @@ static void early_mtrr_init(void)
 		0
 	};
 	msr_t msr;
-	unsigned long *msr_addr;
+	const unsigned long *msr_addr;
 
 	/* Inialize all of the relevant msrs to 0 */
 	msr.lo = 0;
 	msr.hi = 0;
-	for(msr_addr = &mtrr_msrs; *msr_addr; msr_addr++) {
+	for(msr_addr = mtrr_msrs; *msr_addr; msr_addr++) {
 		wrmsr(*msr_addr, msr);
 	}
 
-	/* Enable memory access for 0 - 8MB using top_mem */
+	/* Enable memory access for 0 - 1MB using top_mem */
 	msr.hi = 0;
-	msr.lo = 0x08000000;
+	msr.lo = ((CONFIG_LB_MEM_TOPK << 10) + TOP_MEM_MASK) & ~TOP_MEM_MASK;
 	wrmsr(TOP_MEM, msr);
 
-	/* Enable caching for 0 - 128MB using variable mtrr */
+	/* Enable caching for 0 - 1MB using variable mtrr */
 	msr = rdmsr(0x200);
 	msr.hi &= 0xfffffff0;
 	msr.hi |= 0x00000000;
 	msr.lo &= 0x00000f00;
-	msr.lo |= 0x00000006;
+	msr.lo |= 0x00000000 | MTRR_TYPE_WRBACK;
 	wrmsr(0x200, msr);
 
 	msr = rdmsr(0x201);
 	msr.hi &= 0xfffffff0;
 	msr.hi |= 0x0000000f;
 	msr.lo &= 0x000007ff;
-	msr.lo |= 0xf0000800;
+	msr.lo |= (~((CONFIG_LB_MEM_TOPK << 10) - 1)) | 0x800;
 	wrmsr(0x201, msr);
 
 #if defined(XIP_ROM_SIZE) && defined(XIP_ROM_BASE)
-	/* enable write back caching so we can do execute in place
+	/* enable write through caching so we can do execute in place
 	 * on the flash rom.
 	 */
 	msr.hi = 0x00000000;
-	msr.lo = XIP_ROM_BASE | 0x005;
-	wrmsr(0x202);
+	msr.lo = XIP_ROM_BASE | MTRR_TYPE_WRTHROUGH;
+	wrmsr(0x202, msr);
 #error "FIXME verify the type of MTRR I have setup"
 	msr.hi = 0x0000000f;
 	msr.lo = ~(XIP_ROM_SIZE - 1) | 0x800;
-	wrmsr(0x203);
+	wrmsr(0x203, msr);
 #endif
 
 	/* Set the default memory type and enable fixed and variable MTRRs 
@@ -72,7 +72,7 @@ static void early_mtrr_init(void)
 	/* Enable Variable MTRRs */
 	msr.hi = 0x00000000;
 	msr.lo = 0x00000800;
-	wrmsr(0x2ff, msr);
+	wrmsr(MTRRdefType_MSR, msr);
 	
 	/* Enale the MTRRs in SYSCFG */
 	msr = rdmsr(SYSCFG_MSR);
diff --git a/src/cpu/k8/earlymtrr.inc b/src/cpu/k8/earlymtrr.inc
index d14afb1a31..acc917be7d 100644
--- a/src/cpu/k8/earlymtrr.inc
+++ b/src/cpu/k8/earlymtrr.inc
@@ -1,7 +1,11 @@
 #include <cpu/k8/mtrr.h>
 
+
+	/* Save off the BIST value */
+	movl	%eax, %ebp
+
 /* The fixed and variable MTRRs are powered-up with random values, clear them to
- * MTRR_TYPE_UNCACHABLE for safty reason 
+ * MTRR_TYPE_UNCACHEABLE for safty reason 
  */
 
 earlymtrr_start:
@@ -21,20 +25,20 @@ clear_fixed_var_mtrr:
 	jmp	clear_fixed_var_mtrr
 clear_fixed_var_mtrr_out:
 
-/* enable memory access for 0 - 8MB using top_mem */
+/* enable memory access for 0 - 1MB using top_mem */
 	movl	$TOP_MEM, %ecx
 	xorl	%edx, %edx
-	movl	$0x0800000, %eax
+	movl	$(((CONFIG_LB_MEM_TOPK << 10) + TOP_MEM_MASK) & ~TOP_MEM_MASK) , %eax
 	wrmsr
 
 set_var_mtrr:
-	/* enable caching for 0 - 128MB using variable mtrr */
+	/* enable caching for 0 - 1MB using variable mtrr */
 	movl	$0x200, %ecx
 	rdmsr
 	andl	$0xfffffff0, %edx
 	orl	$0x00000000, %edx
 	andl	$0x00000f00, %eax
-	orl	$0x00000006, %eax
+	orl	$(0x00000000 | MTRR_TYPE_WRBACK), %eax
 	wrmsr
 
 	movl	$0x201, %ecx
@@ -42,7 +46,7 @@ set_var_mtrr:
 	andl	$0xfffffff0, %edx
 	orl	$0x0000000f, %edx
 	andl	$0x000007ff, %eax
-	orl	$0xf0000800, %eax
+	orl	$((~((CONFIG_LB_MEM_TOPK << 10) - 1)) | 0x800), %eax
 	wrmsr
 
 #if defined(XIP_ROM_SIZE) && defined(XIP_ROM_BASE)
@@ -51,7 +55,7 @@ set_var_mtrr:
 	 */
 	movl	$0x202, %ecx
 	xorl	%edx, %edx
-	movl	$(XIP_ROM_BASE | 0x006), %eax
+	movl	$(XIP_ROM_BASE | MTRR_TYPE_WRBACK), %eax
 	wrmsr	
 
 	movl	$0x203, %ecx
@@ -62,7 +66,7 @@ set_var_mtrr:
 
 enable_mtrr:	
 	/* Set the default memory type and enable fixed and variable MTRRs */
-	movl	$0x2ff, %ecx
+	movl	$MTRRdefType_MSR, %ecx
 	xorl	%edx, %edx
 	/* Enable Variable MTRRs */
 	movl	$0x00000800, %eax
@@ -97,3 +101,5 @@ mem_top:
 	.long	0xC001001A, 0xC001001D
 	.long	0x000 /* NULL, end of table */
 earlymtrr_end:
+	/* Restore the BIST value */
+	movl	%ebp, %eax
\ No newline at end of file
diff --git a/src/cpu/k8/enable_mmx_sse.inc b/src/cpu/k8/enable_mmx_sse.inc
index 5551525d68..907e817325 100644
--- a/src/cpu/k8/enable_mmx_sse.inc
+++ b/src/cpu/k8/enable_mmx_sse.inc
@@ -1,3 +1,6 @@
+	/* Save the BIST result */
+	movl	%eax, %ebp
+	
 	/*
 	 * Enabling mmx registers is a noop
 	 * Enable the use of the xmm registers
@@ -12,3 +15,6 @@
 	movl	%cr4, %eax
 	orl	$(1<<9), %eax
 	movl	%eax, %cr4
+
+	/* Restore the BIST result */
+	movl	%ebp, %eax
diff --git a/src/cpu/p6/Config.lb b/src/cpu/p6/Config.lb
index c9d483480a..f6e9777bcd 100644
--- a/src/cpu/p6/Config.lb
+++ b/src/cpu/p6/Config.lb
@@ -3,4 +3,5 @@ uses INTEL_PPRO_MTRR
 dir /cpu/p5
 object cpufixup.o
 object mtrr.o
+object pgtbl.o
 #object l2_cache.o
diff --git a/src/cpu/p6/boot_cpu.c b/src/cpu/p6/boot_cpu.c
index 803eecdd5d..77b060b219 100644
--- a/src/cpu/p6/boot_cpu.c
+++ b/src/cpu/p6/boot_cpu.c
@@ -2,8 +2,6 @@
 
 int boot_cpu(void)
 {
-	volatile unsigned long *local_apic;
-	unsigned long apic_id;
 	int bsp;
 	msr_t msr;
 	msr = rdmsr(0x1b);
diff --git a/src/cpu/p6/earlymtrr.c b/src/cpu/p6/earlymtrr.c
index f352f3d791..df74f905c8 100644
--- a/src/cpu/p6/earlymtrr.c
+++ b/src/cpu/p6/earlymtrr.c
@@ -51,7 +51,7 @@ static void early_mtrr_init(void)
 	/* Disable Variable MTRRs */
 	msr.hi = 0x00000000;
 	msr.lo = 0x00000000;
-	wrmsr(0x2ff, msr);
+	wrmsr(MTRRdefType_MSR, msr);
 
 	/* Invalidate the cache again */
 	asm volatile ("invd");
@@ -65,19 +65,19 @@ static void early_mtrr_init(void)
 		wrmsr(*msr_addr, msr);
 	}
 
-	/* Enable caching for 0 - 128MB using variable mtrr */
+	/* Enable caching for 0 - 1MB using variable mtrr */
 	msr = rdmsr(0x200);
 	msr.hi &= 0xfffffff0;
 	msr.hi |= 0x00000000;
 	msr.lo &= 0x00000f00;
-	msr.lo |= 0x00000006;
+	msr.lo |= 0x00000000 | MTRR_TYPE_WRBACK;
 	wrmsr(0x200, msr);
 
 	msr = rdmsr(0x201);
 	msr.hi &= 0xfffffff0;
 	msr.hi |= 0x0000000f;
 	msr.lo &= 0x000007ff;
-	msr.lo |= 0xf0000800;
+	msr.lo |= (~((CONFIG_LB_MEM_TOPK << 10) - 1)) | 0x800;
 	wrmsr(0x201, msr);
 
 #if defined(XIP_ROM_SIZE) && defined(XIP_ROM_BASE)
@@ -98,7 +98,7 @@ static void early_mtrr_init(void)
 	/* Enable Variable MTRRs */
 	msr.hi = 0x00000000;
 	msr.lo = 0x00000800;
-	wrmsr(0x2ff, msr);
+	wrmsr(MTRRdefType_MSR, msr);
 
 	/* Enable the cache */
 	cr0 = read_cr0();
diff --git a/src/cpu/p6/mtrr.c b/src/cpu/p6/mtrr.c
index ac6fd1db37..7e2eb06e95 100644
--- a/src/cpu/p6/mtrr.c
+++ b/src/cpu/p6/mtrr.c
@@ -104,7 +104,8 @@ static void intel_set_var_mtrr(unsigned int reg, unsigned long basek, unsigned l
 	if (sizek < 4*1024*1024) {
 		mask.hi = ADDRESS_MASK_HIGH;
 		mask.lo = ~((sizek << 10) -1);
-	} else {
+	}
+	else {
 		mask.hi = ADDRESS_MASK_HIGH & (~((sizek >> 22) -1));
 		mask.lo = 0;
 	}
@@ -131,6 +132,36 @@ static void intel_set_var_mtrr(unsigned int reg, unsigned long basek, unsigned l
 	enable_cache();
 }
 
+/* setting variable mtrr, comes from linux kernel source */
+void set_var_mtrr(unsigned int reg, unsigned long base, unsigned long size, unsigned char type)
+{
+	if (reg >= 8)
+		return;
+
+	// it is recommended that we disable and enable cache when we 
+	// do this. 
+	disable_cache();
+	if (size == 0) {
+		/* The invalid bit is kept in the mask, so we simply clear the
+		   relevant mask register to disable a range. */
+		msr_t zero;
+		zero.lo = zero.hi = 0;
+		wrmsr (MTRRphysMask_MSR(reg), zero);
+	} else {
+		/* Bit 32-35 of MTRRphysMask should be set to 1 */
+		msr_t basem, maskm;
+		basem.lo = base | type;
+		basem.hi = 0;
+		maskm.lo = ~(size - 1) | 0x800;
+		maskm.hi = 0x0F;
+		wrmsr (MTRRphysBase_MSR(reg), basem);
+		wrmsr (MTRRphysMask_MSR(reg), maskm);
+	}
+
+	// turn cache back on. 
+	enable_cache();
+}
+
 /* fms: find most sigificant bit set, stolen from Linux Kernel Source. */
 static inline unsigned int fms(unsigned int x)
 {
@@ -250,7 +281,7 @@ static unsigned int range_to_mtrr(unsigned int reg,
 		}
 		sizek = 1 << align;
 		printk_debug("Setting variable MTRR %d, base: %4dMB, range: %4dMB, type WB\n",
-			     reg, range_startk >>10, sizek >> 10);
+			reg, range_startk >>10, sizek >> 10);
 		intel_set_var_mtrr(reg++, range_startk, sizek, MTRR_TYPE_WRBACK);
 		range_startk += sizek;
 		range_sizek -= sizek;
@@ -274,7 +305,7 @@ void setup_mtrrs(struct mem_range *mem)
 	/* Initialized the fixed_mtrrs to uncached */
 	printk_debug("Setting fixed MTRRs(%d-%d) type: UC\n", 
 		0, NUM_FIXED_RANGES);
-	set_fixed_mtrrs(0, NUM_FIXED_RANGES, MTRR_TYPE_UNCACHABLE);
+	set_fixed_mtrrs(0, NUM_FIXED_RANGES, MTRR_TYPE_UNCACHEABLE);
 
 	/* Now see which of the fixed mtrrs cover ram.
 	 */
diff --git a/src/cpu/p6/pgtbl.c b/src/cpu/p6/pgtbl.c
new file mode 100644
index 0000000000..e996dd0e69
--- /dev/null
+++ b/src/cpu/p6/pgtbl.c
@@ -0,0 +1,91 @@
+#include <console/console.h>
+#include <smp/start_stop.h>
+#include <cpu/p6/pgtbl.h>
+
+static void paging_off(void)
+{
+	__asm__ __volatile__ (
+		/* Disable paging */
+		"movl	%%cr0, %%eax\n\t"
+		"andl	$0x7FFFFFFF, %%eax\n\t"
+		"movl	%%eax, %%cr0\n\t"
+		/* Disable pae */
+		"movl	%%cr4, %%eax\n\t"
+		"andl	$0xFFFFFFDF, %%eax\n\t"
+		:
+		:
+		: "eax"
+		);
+}
+
+static void paging_on(void *pdp)
+{
+	__asm__ __volatile__(
+		/* Load the page table address */
+		"movl	%0, %%cr3\n\t"
+		/* Enable pae */
+		"movl	%%cr4, %%eax\n\t"
+		"orl	$0x00000020, %%eax\n\t"
+		"movl	%%eax, %%cr4\n\t"
+		/* Enable paging */
+		"movl	%%cr0, %%eax\n\t"
+		"orl	$0x80000000, %%eax\n\t"
+		"movl	%%eax, %%cr0\n\t"
+		:
+		: "r" (pdp)
+		: "eax"
+		);
+}
+
+void *map_2M_page(int cpu_index, unsigned long page) 
+{
+	struct pde {
+		uint32_t addr_lo;
+		uint32_t addr_hi;
+	} __attribute__ ((packed));
+	struct pg_table {
+		struct pde pd[2048];
+		struct pde pdp[512];
+	} __attribute__ ((packed));
+	static struct pg_table pgtbl[CONFIG_MAX_CPUS] __attribute__ ((aligned(4096)));
+	static unsigned long mapped_window[CONFIG_MAX_CPUS];
+	unsigned long window;
+	void *result;
+	int i;
+	if ((cpu_index < 0) || (cpu_index >= CONFIG_MAX_CPUS)) {
+		return MAPPING_ERROR;
+	}
+	window = page >> 10;
+	if (window != mapped_window[cpu_index]) {
+		paging_off();
+		if (window > 1) {
+			struct pde *pd, *pdp;
+			/* Point the page directory pointers at the page directories */
+			memset(&pgtbl[cpu_index].pdp, 0, sizeof(pgtbl[cpu_index].pdp));
+			pd = pgtbl[cpu_index].pd;
+			pdp = pgtbl[cpu_index].pdp;
+			pdp[0].addr_lo = ((uint32_t)&pd[512*0])|1;
+			pdp[1].addr_lo = ((uint32_t)&pd[512*1])|1;
+			pdp[2].addr_lo = ((uint32_t)&pd[512*2])|1;
+			pdp[3].addr_lo = ((uint32_t)&pd[512*3])|1;
+			/* The first half of the page table is identity mapped */
+			for(i = 0; i < 1024; i++) {
+				pd[i].addr_lo = ((i & 0x3ff) << 21)| 0xE3;
+				pd[i].addr_hi = 0;
+			}
+			/* The second half of the page table holds the mapped page */
+			for(i = 1024; i < 2048; i++) {
+				pd[i].addr_lo = ((window & 1) << 31) | ((i & 0x3ff) << 21) | 0xE3;
+				pd[i].addr_hi = (window >> 1);
+			}
+			paging_on(pdp);
+		}
+		mapped_window[cpu_index] = window;
+	}
+	if (window == 0) {
+		result = (void *)(page << 21);
+	} else {
+		result = (void *)(0x80000000 | ((page & 0x3ff) << 21));
+	}
+	return result;
+}