- Major cleanup of the bootpath

- Changes to allow more code to be compiled both ways
- Working SMP support


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

27 files changed, 804 insertions, 604 deletions

diff --git a/src/arch/i386/include/arch/io.h b/src/arch/i386/include/arch/io.h
index bcba9a9..c1207b3 100644
--- a/src/arch/i386/include/arch/io.h
+++ b/src/arch/i386/include/arch/io.h
@@ -1,76 +1,142 @@
 #ifndef _ASM_IO_H
 #define _ASM_IO_H
 
+#include <stdint.h>
+
 /*
  * This file contains the definitions for the x86 IO instructions
  * inb/inw/inl/outb/outw/outl and the "string versions" of the same
- * (insb/insw/insl/outsb/outsw/outsl).
- *
- * This file is not meant to be obfuscating: it's just complicated
- * to (a) handle it all in a way that makes gcc able to optimize it
- * as well as possible and (b) trying to avoid writing the same thing
- * over and over again with slight variations and possibly making a
- * mistake somewhere.
- */
-
- /*
-  *  Bit simplified and optimized by Jan Hubicka
-  *  Support of BIGMEM added by Gerhard Wichert, Siemens AG, July 1999.
-  */
-
-/*
- * Talk about misusing macros..
+ * (insb/insw/insl/outsb/outsw/outsl). You can also use "pausing"
+ * versions of the single-IO instructions (inb_p/inw_p/..).
  */
-#define __OUT1(s,x) \
-extern inline void out##s(unsigned x value, unsigned short port) {
-
-#define __OUT2(s,s1,s2) \
-__asm__ __volatile__ ("out" #s " %" s1 "0,%" s2 "1"
-
-#define __OUT(s,s1,x) \
-__OUT1(s,x) __OUT2(s,s1,"w") : : "a" (value), "Nd" (port)); } 
-
-#define __IN1(s) \
-extern inline RETURN_TYPE in##s(unsigned short port) { RETURN_TYPE _v;
-
-#define __IN2(s,s1,s2) \
-__asm__ __volatile__ ("in" #s " %" s2 "1,%" s1 "0"
-
-#define __IN(s,s1,i...) \
-__IN1(s) __IN2(s,s1,"w") : "=a" (_v) : "Nd" (port) ,##i ); return _v; } 
-
-#define __INS(s) \
-extern inline void ins##s(unsigned short port, void * addr, unsigned long count) \
-{ __asm__ __volatile__ ("cld ; rep ; ins" #s \
-: "=D" (addr), "=c" (count) : "d" (port),"0" (addr),"1" (count)); }
-
-#define __OUTS(s) \
-extern inline void outs##s(unsigned short port, const void * addr, unsigned long count) \
-{ __asm__ __volatile__ ("cld ; rep ; outs" #s \
-: "=S" (addr), "=c" (count) : "d" (port),"0" (addr),"1" (count)); }
-
-#define RETURN_TYPE unsigned char
-__IN(b,"")
-#undef RETURN_TYPE
-#define RETURN_TYPE unsigned short
-__IN(w,"")
-#undef RETURN_TYPE
-#define RETURN_TYPE unsigned int
-__IN(l,"")
-#undef RETURN_TYPE
-
-__OUT(b,"b",char)
-__OUT(w,"w",short)
-__OUT(l,,int)
-
-__INS(b)
-__INS(w)
-__INS(l)
-
-__OUTS(b)
-__OUTS(w)
-__OUTS(l)
 
+#ifdef __ROMCC__
+static void outb(unsigned char value, unsigned short port)
+{
+	__builtin_outb(value, port);
+}
+
+static void outw(unsigned short value, unsigned short port)
+{
+	__builtin_outw(value, port);
+}
+
+static void outl(unsigned int value, unsigned short port)
+{
+	__builtin_outl(value, port);
+}
+
+
+static unsigned char inb(unsigned short port)
+{
+	return __builtin_inb(port);
+}
+
+
+static unsigned char inw(unsigned short port)
+{
+	return __builtin_inw(port);
+}
+
+static unsigned char inl(unsigned short port)
+{
+	return __builtin_inl(port);
+}
+
+#else
+
+static inline void outb(uint8_t value, uint16_t port)
+{
+	__asm__ __volatile__ ("outb %b0, %w1" : : "a" (value), "Nd" (port));
+}
+
+static inline void outw(uint16_t value, uint16_t port)
+{
+	__asm__ __volatile__ ("outw %w0, %w1" : : "a" (value), "Nd" (port));
+}
+
+static inline void outl(uint32_t value, uint16_t port)
+{
+	__asm__ __volatile__ ("outl %0, %w1" : : "a" (value), "Nd" (port));
+}
+
+static inline uint8_t inb(uint16_t port)
+{
+	uint8_t value;
+	__asm__ __volatile__ ("inb %w1, %b0" : "=a"(value) : "Nd" (port));
+	return value;
+}
+
+static inline uint16_t inw(uint16_t port)
+{
+	uint16_t value;
+	__asm__ __volatile__ ("inw %w1, %w0" : "=a"(value) : "Nd" (port));
+	return value;
+}
+
+static inline uint32_t inl(uint16_t port)
+{
+	uint32_t value;
+	__asm__ __volatile__ ("inl %w1, %0" : "=a"(value) : "Nd" (port));
+	return value;
+}
+
+#endif /* __ROMCC__ */
+
+static inline void outsb(uint16_t port, const void *addr, unsigned long count)
+{
+	__asm__ __volatile__ (
+		"cld ; rep ; outsb " 
+		: "=S" (addr), "=c" (count)
+		: "d"(port), "0"(addr), "1" (count)
+		);
+}
+
+static inline void outsw(uint16_t port, const void *addr, unsigned long count)
+{
+	__asm__ __volatile__ (
+		"cld ; rep ; outsw " 
+		: "=S" (addr), "=c" (count)
+		: "d"(port), "0"(addr), "1" (count)
+		);
+}
+
+static inline void outsl(uint16_t port, const void *addr, unsigned long count)
+{
+	__asm__ __volatile__ (
+		"cld ; rep ; outsl " 
+		: "=S" (addr), "=c" (count)
+		: "d"(port), "0"(addr), "1" (count)
+		);
+}
+
+
+static inline void insb(uint16_t port, void *addr, unsigned long count)
+{
+	__asm__ __volatile__ (
+		"cld ; rep ; insb " 
+		: "=D" (addr), "=c" (count)
+		: "d"(port), "0"(addr), "1" (count)
+		);
+}
+
+static inline void insw(uint16_t port, void *addr, unsigned long count)
+{
+	__asm__ __volatile__ (
+		"cld ; rep ; insw " 
+		: "=D" (addr), "=c" (count)
+		: "d"(port), "0"(addr), "1" (count)
+		);
+}
+
+static inline void insl(uint16_t port, void *addr, unsigned long count)
+{
+	__asm__ __volatile__ (
+		"cld ; rep ; insl " 
+		: "=D" (addr), "=c" (count)
+		: "d"(port), "0"(addr), "1" (count)
+		);
+}
 
 #endif
 
diff --git a/src/arch/i386/include/arch/romcc_io.h b/src/arch/i386/include/arch/romcc_io.h
index f8618b0..1a64635 100644
--- a/src/arch/i386/include/arch/romcc_io.h
+++ b/src/arch/i386/include/arch/romcc_io.h
@@ -1,37 +1,6 @@
 #ifndef ARCH_ROMCC_IO_H
 #define ARCH_ROMCC_IO_H 1
 
-static void outb(unsigned char value, unsigned short port)
-{
-	__builtin_outb(value, port);
-}
-
-static void outw(unsigned short value, unsigned short port)
-{
-	__builtin_outw(value, port);
-}
-
-static void outl(unsigned int value, unsigned short port)
-{
-	__builtin_outl(value, port);
-}
-
-
-static unsigned char inb(unsigned short port)
-{
-	return __builtin_inb(port);
-}
-
-
-static unsigned char inw(unsigned short port)
-{
-	return __builtin_inw(port);
-}
-
-static unsigned char inl(unsigned short port)
-{
-	return __builtin_inl(port);
-}
 
 static void hlt(void)
 {
@@ -76,38 +45,6 @@ int log2(int value)
 	return __builtin_bsr(value);
 }
 
-
-typedef __builtin_msr_t msr_t;
-
-static msr_t rdmsr(unsigned long index)
-{
-	return __builtin_rdmsr(index);
-}
-
-static void wrmsr(unsigned long index, msr_t msr)
-{
-	__builtin_wrmsr(index, msr.lo, msr.hi);
-}
-
-
-struct tsc_struct {
-	unsigned lo;
-	unsigned hi;
-};
-typedef struct tsc_struct tsc_t;
-
-static tsc_t rdtsc(void)
-{
-	tsc_t res;
-	asm ("rdtsc"
-		: "=a" (res.lo), "=d"(res.hi) /* outputs */
-		: /* inputs */
-		: /* Clobbers */
-		);
-	return res;
-}
-
-
 #define PCI_ADDR(BUS, DEV, FN, WHERE) ( \
 	(((BUS) & 0xFF) << 16) | \
 	(((DEV) & 0x1f) << 11) | \
diff --git a/src/arch/i386/lib/cpu.c b/src/arch/i386/lib/cpu.c
index 19020fe..95dba5f 100644
--- a/src/arch/i386/lib/cpu.c
+++ b/src/arch/i386/lib/cpu.c
@@ -57,16 +57,16 @@ static void interrupts_on()
 
 #if defined(APIC)
 	/* Only Pentium Pro and later have those MSR stuff */
-	unsigned long low, high;
+	msr_t msr;
 
 	printk_info("Setting up local apic...");
 
 	/* Enable the local apic */
-	rdmsr(APIC_BASE_MSR, low, high);
-	low |= APIC_BASE_MSR_ENABLE;
-	low &= ~APIC_BASE_MSR_ADDR_MASK;
-	low |= APIC_DEFAULT_BASE;
-	wrmsr(APIC_BASE_MSR, low, high);
+	msr = rdmsr(APIC_BASE_MSR);
+	msr.lo |= APIC_BASE_MSR_ENABLE;
+	msr.lo &= ~APIC_BASE_MSR_ADDR_MASK;
+	msr.lo |= APIC_DEFAULT_BASE;
+	wrmsr(APIC_BASE_MSR, msr);
 
 	/*
 	 * Set Task Priority to 'accept all'.
@@ -103,13 +103,13 @@ static void interrupts_on()
 #else /* APIC */
 #ifdef i686
 	/* Only Pentium Pro and later have those MSR stuff */
-	unsigned long low, high;
+	msr_t msr;
 
 	printk_info("Disabling local apic...");
 
-	rdmsr(APIC_BASE_MSR, low, high);
-	low &= ~APIC_BASE_MSR_ENABLE;
-	wrmsr(APIC_BASE_MSR, low, high);
+	msr = rdmsr(APIC_BASE_MSR);
+	msr.lo &= ~APIC_BASE_MSR_ENABLE;
+	wrmsr(APIC_BASE_MSR, msr);
 #endif /* i686 */
 #endif /* APIC */
 	printk_info("done.\n");
@@ -143,6 +143,7 @@ unsigned long cpu_initialize(struct mem_range *mem)
 	configure_l2_cache();
 #endif
 	interrupts_on();
+	processor_id = this_processors_id();
 	printk_info("CPU #%d Initialized\n", processor_id);
 	return processor_id;
 }
diff --git a/src/arch/i386/smp/secondary.S b/src/arch/i386/smp/secondary.S
new file mode 100644
index 0000000..78c5576
--- /dev/null
+++ b/src/arch/i386/smp/secondary.S
@@ -0,0 +1,76 @@
+#include <arch/asm.h>
+#include <arch/intel.h>
+#include <cpu/p6/mtrr.h>
+#include <cpu/p6/apic.h>
+	.text
+	.globl _secondary_start
+	.balign 4096
+_secondary_start:
+	.code16
+	cli
+	xorl	%eax, %eax
+	movl	%eax, %cr3    /* Invalidate TLB*/
+
+	/* On hyper threaded cpus invalidating the cache here is
+	 * very very bad.  Don't.
+	 */
+
+	/* setup the data segment */
+	movw	%cs, %ax
+	movw	%ax, %ds
+
+	data32	lgdt	gdtaddr  - _secondary_start
+
+	movl	%cr0, %eax
+	andl	$0x7FFAFFD1, %eax /* PG,AM,WP,NE,TS,EM,MP = 0 */
+	orl	$0x60000001, %eax /* CD, NW, PE = 1 */
+	movl	%eax, %cr0
+
+	ljmpl $0x10, $1f
+1:	
+	.code32
+	movw	$0x18, %ax
+	movw	%ax, %ds
+	movw	%ax, %es
+	movw	%ax, %ss
+	movw	%ax, %fs
+	movw	%ax, %gs
+
+	/* Enable the local apic, and map it where we expext it */
+	movl	$APIC_BASE_MSR, %ecx
+	rdmsr
+	orl	$APIC_BASE_MSR_ENABLE, %eax
+	andl	$(~APIC_BASE_MSR_ADDR_MASK), %eax
+	orl	$APIC_DEFAULT_BASE, %eax
+	wrmsr
+
+	/* Get the apic_id */
+	movl	(APIC_ID + APIC_DEFAULT_BASE), %edi
+	shrl	$24, %edi
+
+	/* Get the cpu index (MAX_CPUS on error) */
+	movl	$-4, %ebx
+1:	addl	$4, %ebx
+	cmpl	$(MAX_CPUS << 2), %ebx
+	je	2
+	cmpl	%edi, initial_apicid(%ebx)
+	jne	1b
+2:	shrl	$2, %ebx
+	
+	/* set the stack pointer */
+	movl	$_estack, %esp
+	movl	%ebx, %eax
+	movl	$STACK_SIZE, %ebx
+	mull	%ebx
+	subl	%eax, %esp
+
+	call	secondary_cpu_init
+1:	hlt
+	jmp	1b
+
+gdtaddr:
+	.word	gdt_limit	/* the table limit */
+	.long	gdt		/* we know the offset */
+
+
+.code32
diff --git a/src/arch/i386/smp/start_stop.c b/src/arch/i386/smp/start_stop.c
index 018ec30..bb8868a 100644
--- a/src/arch/i386/smp/start_stop.c
+++ b/src/arch/i386/smp/start_stop.c
@@ -3,6 +3,7 @@
 #include <cpu/p6/apic.h>
 #include <delay.h>
 #include <string.h>
+#include <console/console.h>
 
 #ifndef START_CPU_SEG
 #define START_CPU_SEG 0x90000
diff --git a/src/boot/hardwaremain.c b/src/boot/hardwaremain.c
index 4e7b9e3..1443727 100644
--- a/src/boot/hardwaremain.c
+++ b/src/boot/hardwaremain.c
@@ -35,6 +35,7 @@ it with the version available from LANL.
 #include <part/sizeram.h>
 #include <device/device.h>
 #include <device/pci.h>
+#include <delay.h>
 #if 0
 #include <part/mainboard.h>
 #endif
@@ -74,7 +75,7 @@ static struct mem_range *get_ramsize(void)
 }
 
 
-#if SMP == 1
+#if CONFIG_SMP == 1
 /* Number of cpus that are currently running in linuxbios */
 static atomic_t active_cpus = ATOMIC_INIT(1);
 
@@ -111,10 +112,9 @@ static void wait_for_other_cpus(void)
 	}
 	for(i = 0; i < MAX_CPUS; i++) {
 		if (!(processor_map[i] & CPU_ENABLED)) {
-			printk_err("CPU %d/%u did not initialize!\n",
-				i, initial_apicid[i]);
+			printk_err("CPU %d did not initialize!\n", i);
 			processor_map[i] = 0;
-			mainboard_cpu_fixup(i);
+#warning "FIXME do I need a mainboard_cpu_fixup function?"
 		}
 	}
 	printk_debug("All AP CPUs stopped\n");
@@ -159,7 +159,7 @@ void hardwaremain(int boot_complete)
 		hard_reset();
 	}
 #endif
-#if 1
+	init_timer();
 
 	/* pick how to scan the bus. This is first so we can get at memory size. */
 	printk_info("Finding PCI configuration type.\n");
@@ -181,7 +181,6 @@ void hardwaremain(int boot_complete)
 
 	dev_initialize();
 	post_code(0x89);
-#endif
 
 	mem = get_ramsize();
 	post_code(0x70);
diff --git a/src/cpu/k8/apic_timer.c b/src/cpu/k8/apic_timer.c
new file mode 100644
index 0000000..fa7e9b9
--- /dev/null
+++ b/src/cpu/k8/apic_timer.c
@@ -0,0 +1,26 @@
+#include <stdint.h>
+#include <delay.h>
+#include <cpu/p6/msr.h>
+#include <cpu/p6/apic.h>
+
+void init_timer(void)
+{
+	/* Set the apic timer to no interrupts and periodic mode */
+	apic_write(APIC_LVTT, (1 << 17)|(1<< 16)|(0 << 12)|(0 << 0));
+	/* Set the divider to 1, no divider */
+	apic_write(APIC_TDCR, APIC_TDR_DIV_1);
+	/* Set the initial counter to 0xffffffff */
+	apic_write(APIC_TMICT, 0xffffffff);
+}
+
+void udelay(unsigned usecs)
+{
+	uint32_t start, value, ticks;
+	/* Calculate the number of ticks to run, our FSB runs a 200Mhz */
+	ticks = usecs * 200;
+	start = apic_read(APIC_TMCCT);
+	do {
+		value = apic_read(APIC_TMCCT);
+	} while((start - value) < ticks);
+	
+}
diff --git a/src/cpu/k8/cpufixup.c b/src/cpu/k8/cpufixup.c
index 9f306d1..8e7ad95 100644
--- a/src/cpu/k8/cpufixup.c
+++ b/src/cpu/k8/cpufixup.c
@@ -13,7 +13,8 @@
 
 void k8_cpufixup(struct mem_range *mem)
 {
-	unsigned long lo = 0, hi = 0, i;
+	msr_t msr;
+	unsigned long i;
 	unsigned long ram_megabytes;
 
 	/* For now no Athlon board has significant holes in it's
@@ -27,33 +28,34 @@ void k8_cpufixup(struct mem_range *mem)
 	ram_megabytes = (mem[i-1].basek + mem[i-1].sizek) *1024;
 		
 
+#warning "FIXME handle > 4GB of ram"
 	// 8 MB alignment please
 	ram_megabytes += 0x7fffff;
 	ram_megabytes &= (~0x7fffff);
 
 	// set top_mem registers to ram size
 	printk_spew("Setting top_mem to 0x%x\n", ram_megabytes);
-	rdmsr(TOP_MEM, lo, hi);
-	printk_spew("TOPMEM was 0x%02x:0x%02x\n", hi, lo);
-	hi = 0;
-	lo = ram_megabytes;
-	wrmsr(TOP_MEM, lo, hi);
+	msr = rdmsr(TOP_MEM);
+	printk_spew("TOPMEM was 0x%02x:0x%02x\n", msr.hi, msr.lo);
+	msr.hi = 0;
+	msr.lo = ram_megabytes;
+	wrmsr(TOP_MEM, msr);
 
 	// I am setting this even though I won't enable it
-	wrmsr(TOP_MEM2, lo, hi);
+	wrmsr(TOP_MEM2, msr);
 
 	/* zero the IORR's before we enable to prevent
 	 * undefined side effects
 	 */
-	lo = hi = 0;
+	msr.lo = msr.hi = 0;
 	for (i = IORR_FIRST; i <= IORR_LAST; i++)
-		wrmsr(i, lo, hi);
-
-	rdmsr(SYSCFG, lo, hi);
-	printk_spew("SYSCFG was 0x%x:0x%x\n", hi, lo);
-	lo |= MTRRVARDRAMEN;
-	wrmsr(SYSCFG, lo, hi);
-	rdmsr(SYSCFG, lo, hi);
-	printk_spew("SYSCFG IS NOW 0x%x:0x%x\n", hi, lo);
+		wrmsr(i, msr);
+
+	msr = rdmsr(SYSCFG);
+	printk_spew("SYSCFG was 0x%x:0x%x\n", msr.hi, msr.lo);
+	msr.lo |= MTRRVARDRAMEN;
+	wrmsr(SYSCFG, msr);
+	msr = rdmsr(SYSCFG);
+	printk_spew("SYSCFG IS NOW 0x%x:0x%x\n", msr.hi, msr.lo);
 }
 
diff --git a/src/cpu/k8/earlymtrr.c b/src/cpu/k8/earlymtrr.c
new file mode 100644
index 0000000..47ddd12
--- /dev/null
+++ b/src/cpu/k8/earlymtrr.c
@@ -0,0 +1,87 @@
+#include <cpu/k8/mtrr.h>
+
+/* the fixed and variable MTTRs are power-up with random values,
+ * clear them to MTRR_TYPE_UNCACHEABLE for safty.
+ */
+
+static void early_mtrr_init(void)
+{
+	static unsigned long mtrr_msrs[] = {
+		/* fixed mtrr */
+		0x250, 0x258, 0x259,
+		0x268, 0x269, 0x26A
+		0x26B, 0x26C, 0x26D
+		0x26E, 0x26F,
+		/* var mtrr */
+		0x200, 0x201, 0x202, 0x203,
+		0x204, 0x205, 0x206, 0x207,
+		0x208, 0x209, 0x20A, 0x20B,
+		0x20C, 0x20D, 0x20E, 0x20F,
+		/* var iorr msr */
+		0xC0010016, 0xC0010017, 0xC0010018, 0xC0010019,
+		/* mem top */
+		0xC001001A, 0xC001001D,
+		/* NULL end of table */
+		0
+	};
+	msr_t msr;
+	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++) {
+		wrmsr(*msr_addr, msr);
+	}
+
+	/* Enable memory access for 0 - 8MB using top_mem */
+	msr.hi = 0;
+	msr.lo = 0x08000000;
+	wrmsr(TOP_MEM, msr);
+
+	/* Enable caching for 0 - 128MB using variable mtrr */
+	msr = rdmsr(0x200);
+	msr.hi &= 0xfffffff0;
+	msr.hi |= 0x00000000;
+	msr.lo &= 0x00000f00;
+	msr.lo |= 0x00000006;
+	wrmsr(0x200, msr);
+
+	msr = rdmsr(0x201);
+	msr.hi &= 0xfffffff0;
+	msr.hi |= 0x0000000f;
+	msr.lo &= 0x000007ff;
+	msr.lo |= 0xf0000800;
+	wrmsr(0x201, msr);
+
+#if defined(XIP_ROM_SIZE) && defined(XIP_ROM_BASE)
+	/* enable write back caching so we can do execute in place
+	 * on the flash rom.
+	 */
+	msr.hi = 0x00000000;
+	msr.lo = XIP_ROM_BASE | 0x005;
+	wrmsr(0x202);
+#error "FIXME verify the type of MTRR I have setup"
+	msr.hi = 0x0000000f;
+	msr.lo = ~(XIP_ROM_SIZE - 1) | 0x800;
+	wrmsr(0x203);
+#endif
+
+	/* Set the default memory type and enable fixed and variable MTRRs 
+	 */
+	/* Enable Variable MTRRs */
+	msr.hi = 0x00000000;
+	msr.lo = 0x00000800;
+	wrmsr(0x2ff, msr);
+	
+	/* Enale the MTRRs in SYSCFG */
+	msr = rdmsr(SYSCFG_MSR);
+	msr.lo |= SYSCFG_MSR_MtrrrVarDramEn;
+	wrmsr(SYSCFG_MSR, msr);
+
+	/* Enable the cache */
+	unsigned long cr0;
+	cr0 = read_cr0();
+	cr0 &= 0x9fffffff;
+	write_cr0(cr0);
+}
diff --git a/src/cpu/p5/cpuid.c b/src/cpu/p5/cpuid.c
index d98ce13..2d3d3a8 100644
--- a/src/cpu/p5/cpuid.c
+++ b/src/cpu/p5/cpuid.c
@@ -9,21 +9,20 @@ int mtrr_check(void)
 {
 #ifdef i686
 	/* Only Pentium Pro and later have MTRR */
-	unsigned long low, high;
-
+	msr_t msr;
 	printk_debug("\nMTRR check\n");
 
-	rdmsr(0x2ff, low, high);
-	low = low >> 10;
+	msr = rdmsr(0x2ff);
+	msr.lo >>= 10;
 
 	printk_debug("Fixed MTRRs   : ");
-	if (low & 0x01)
+	if (msr.lo & 0x01)
 		printk_debug("Enabled\n");
 	else
 		printk_debug("Disabled\n");
 
 	printk_debug("Variable MTRRs: ");
-	if (low & 0x02)
+	if (msr.lo & 0x02)
 		printk_debug("Enabled\n");
 	else
 		printk_debug("Disabled\n");
@@ -31,7 +30,7 @@ int mtrr_check(void)
 	printk_debug("\n");
 
 	post_code(0x93);
-	return ((int) low);
+	return ((int) msr.lo);
 #else /* !i686 */
 	return 0;
 #endif /* i686 */
diff --git a/src/cpu/p6/boot_cpu.c b/src/cpu/p6/boot_cpu.c
new file mode 100644
index 0000000..803eecd
--- /dev/null
+++ b/src/cpu/p6/boot_cpu.c
@@ -0,0 +1,12 @@
+#include <cpu/p6/msr.h>
+
+int boot_cpu(void)
+{
+	volatile unsigned long *local_apic;
+	unsigned long apic_id;
+	int bsp;
+	msr_t msr;
+	msr = rdmsr(0x1b);
+	bsp = !!(msr.lo & (1 << 8));
+	return bsp;
+}
diff --git a/src/cpu/p6/mtrr.c b/src/cpu/p6/mtrr.c
index 413acb1..b067883 100644
--- a/src/cpu/p6/mtrr.c
+++ b/src/cpu/p6/mtrr.c
@@ -40,20 +40,20 @@ static unsigned int mtrr_msr[] = {
 
 static void intel_enable_fixed_mtrr(void)
 {
-	unsigned long low, high;
+	msr_t msr;
 
-	rdmsr(MTRRdefType_MSR, low, high);
-	low |= 0xc00;
-	wrmsr(MTRRdefType_MSR, low, high);
+	msr = rdmsr(MTRRdefType_MSR);
+	msr.lo |= 0xc00;
+	wrmsr(MTRRdefType_MSR, msr);
 }
 
 static void intel_enable_var_mtrr(void)
 {
-	unsigned long low, high;
+	msr_t msr;
 
-	rdmsr(MTRRdefType_MSR, low, high);
-	low |= 0x800;
-	wrmsr(MTRRdefType_MSR, low, high);
+	msr = rdmsr(MTRRdefType_MSR);
+	msr.lo |= 0x800;
+	wrmsr(MTRRdefType_MSR, msr);
 }
 
 static inline void disable_cache(void)
@@ -86,19 +86,18 @@ static inline void enable_cache(void)
 /* setting variable mtrr, comes from linux kernel source */
 static void intel_set_var_mtrr(unsigned int reg, unsigned long basek, unsigned long sizek, unsigned char type)
 {
-	unsigned long base_high, base_low;
-	unsigned long  mask_high, mask_low;
+	msr_t base, mask;
 
-	base_high = basek >> 22;
-	base_low  = basek << 10;
+	base.hi = basek >> 22;
+	base.lo  = basek << 10;
 
 	if (sizek < 4*1024*1024) {
-		mask_high = 0x0F;
-		mask_low = ~((sizek << 10) -1);
+		mask.hi = 0x0F;
+		mask.lo = ~((sizek << 10) -1);
 	}
 	else {
-		mask_high = 0x0F & (~((sizek >> 22) -1));
-		mask_low = 0;
+		mask.hi = 0x0F & (~((sizek >> 22) -1));
+		mask.lo = 0;
 	}
 
 	if (reg >= 8)
@@ -108,13 +107,17 @@ static void intel_set_var_mtrr(unsigned int reg, unsigned long basek, unsigned l
 	// do this. 
 	disable_cache();
 	if (sizek == 0) {
+		msr_t zero;
+		zero.lo = zero.hi = 0;
 		/* The invalid bit is kept in the mask, so we simply clear the
 		   relevant mask register to disable a range. */
-		wrmsr (MTRRphysMask_MSR (reg), 0, 0);
+		wrmsr (MTRRphysMask_MSR(reg), zero);
 	} else {
 		/* Bit 32-35 of MTRRphysMask should be set to 1 */
-		wrmsr (MTRRphysBase_MSR(reg), base_low | type, base_high);
-		wrmsr (MTRRphysMask_MSR(reg), mask_low | 0x800, mask_high);
+		base.lo |= type;
+		mask.lo |= 0x800;
+		wrmsr (MTRRphysBase_MSR(reg), base);
+		wrmsr (MTRRphysMask_MSR(reg), mask);
 	}
 	enable_cache();
 }
@@ -131,11 +134,18 @@ void set_var_mtrr(unsigned int reg, unsigned long base, unsigned long size, unsi
 	if (size == 0) {
 		/* The invalid bit is kept in the mask, so we simply clear the
 		   relevant mask register to disable a range. */
-		wrmsr (MTRRphysMask_MSR (reg), 0, 0);
+		msr_t zero;
+		zero.lo = zero.hi = 0;
+		wrmsr (MTRRphysMask_MSR(reg), zero);
 	} else {
 		/* Bit 32-35 of MTRRphysMask should be set to 1 */
-		wrmsr (MTRRphysBase_MSR (reg), base | type, 0);
-		wrmsr (MTRRphysMask_MSR (reg), ~(size - 1) | 0x800, 0x0F);
+		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. 
@@ -197,32 +207,32 @@ static void set_fixed_mtrrs(unsigned int first, unsigned int last, unsigned char
 {
 	unsigned int i;
 	unsigned int fixed_msr = NUM_FIXED_RANGES >> 3;
-	unsigned long low, high;
-	low = high = 0; /* Shut up gcc */
+	msr_t msr;
+	msr.lo = msr.hi = 0; /* Shut up gcc */
 	for(i = first; i < last; i++) {
 		/* When I switch to a new msr read it in */
 		if (fixed_msr != i >> 3) {
 			/* But first write out the old msr */
 			if (fixed_msr < (NUM_FIXED_RANGES >> 3)) {
 				disable_cache();
-				wrmsr(mtrr_msr[fixed_msr], low, high);
+				wrmsr(mtrr_msr[fixed_msr], msr);
 				enable_cache();
 			}
 			fixed_msr = i>>3;
-			rdmsr(mtrr_msr[fixed_msr], low, high);
+			msr = rdmsr(mtrr_msr[fixed_msr]);
 		}
 		if ((i & 7) < 4) {
-			low &= ~(0xff << ((i&3)*8));
-			low |= type << ((i&3)*8);
+			msr.lo &= ~(0xff << ((i&3)*8));
+			msr.lo |= type << ((i&3)*8);
 		} else {
-			high &= ~(0xff << ((i&3)*8));
-			high |= type << ((i&3)*8);
+			msr.hi &= ~(0xff << ((i&3)*8));
+			msr.hi |= type << ((i&3)*8);
 		}
 	}
 	/* Write out the final msr */
 	if (fixed_msr < (NUM_FIXED_RANGES >> 3)) {
 		disable_cache();
-		wrmsr(mtrr_msr[fixed_msr], low, high);
+		wrmsr(mtrr_msr[fixed_msr], msr);
 		enable_cache();
 	}
 }
diff --git a/src/include/console/console.h b/src/include/console/console.h
index 57ba9da..2a9f388 100644
--- a/src/include/console/console.h
+++ b/src/include/console/console.h
@@ -72,5 +72,54 @@ int do_printk(int msg_level, const char *fmt, ...);
 #define printk_spew(fmt, arg...)    do {} while(0)
 #endif
 
+#define print_emerg(STR)   printk_emerg  ("%s", (STR))
+#define print_alert(STR)   printk_alert  ("%s", (STR))
+#define print_crit(STR)    printk_crit   ("%s", (STR))
+#define print_err(STR)     printk_err    ("%s", (STR))
+#define print_warning(STR) printk_warning("%s", (STR))
+#define print_notice(STR)  printk_notice ("%s", (STR))
+#define print_info(STR)    printk_info   ("%s", (STR))
+#define print_debug(STR)   printk_debug  ("%s", (STR))
+#define print_spew(STR)    printk_spew   ("%s", (STR))
+
+#define print_emerg_char(CH)   printk_emerg  ("%c", (CH))
+#define print_alert_char(CH)   printk_alert  ("%c", (CH))
+#define print_crit_char(CH)    printk_crit   ("%c", (CH))
+#define print_err_char(CH)     printk_err    ("%c", (CH))
+#define print_warning_char(CH) printk_warning("%c", (CH))
+#define print_notice_char(CH)  printk_notice ("%c", (CH))
+#define print_info_char(CH)    printk_info   ("%c", (CH))
+#define print_debug_char(CH)   printk_debug  ("%c", (CH))
+#define print_spew_char(CH)    printk_spew   ("%c", (CH))
+
+#define print_emerg_hex8(HEX)   printk_emerg  ("%08x",  (HEX))
+#define print_alert_hex8(HEX)   printk_alert  ("%08x",  (HEX))
+#define print_crit_hex8(HEX)    printk_crit   ("%08x",  (HEX))
+#define print_err_hex8(HEX)     printk_err    ("%08x",  (HEX))
+#define print_warning_hex8(HEX) printk_warning("%08x",  (HEX))
+#define print_notice_hex8(HEX)  printk_notice ("%08x",  (HEX))
+#define print_info_hex8(HEX)    printk_info   ("%08x",  (HEX))
+#define print_debug_hex8(HEX)   printk_debug  ("%08x",  (HEX))
+#define print_spew_hex8(HEX)    printk_spew   ("%08x",  (HEX))
+
+#define print_emerg_hex16(HEX)   printk_emerg  ("%016x", (HEX))
+#define print_alert_hex16(HEX)   printk_alert  ("%016x", (HEX))
+#define print_crit_hex16(HEX)    printk_crit   ("%016x", (HEX))
+#define print_err_hex16(HEX)     printk_err    ("%016x", (HEX))
+#define print_warning_hex16(HEX) printk_warning("%016x", (HEX))
+#define print_notice_hex16(HEX)  printk_notice ("%016x", (HEX))
+#define print_info_hex16(HEX)    printk_info   ("%016x", (HEX))
+#define print_debug_hex16(HEX)   printk_debug  ("%016x", (HEX))
+#define print_spew_hex16(HEX)    printk_spew   ("%016x", (HEX))
+
+#define print_emerg_hex32(HEX)   printk_emerg  ("%032x", (HEX))
+#define print_alert_hex32(HEX)   printk_alert  ("%032x", (HEX))
+#define print_crit_hex32(HEX)    printk_crit   ("%032x", (HEX))
+#define print_err_hex32(HEX)     printk_err    ("%032x", (HEX))
+#define print_warning_hex32(HEX) printk_warning("%032x", (HEX))
+#define print_notice_hex32(HEX)  printk_notice ("%032x", (HEX))
+#define print_info_hex32(HEX)    printk_info   ("%032x", (HEX))
+#define print_debug_hex32(HEX)   printk_debug  ("%032x", (HEX))
+#define print_spew_hex32(HEX)    printk_spew   ("%032x", (HEX))
 
 #endif /* CONSOLE_CONSOLE_H_ */
diff --git a/src/include/cpu/p6/apic.h b/src/include/cpu/p6/apic.h
index 65a1460..7e8b2f4 100644
--- a/src/include/cpu/p6/apic.h
+++ b/src/include/cpu/p6/apic.h
@@ -76,12 +76,40 @@
 #define			APIC_MODE_EXINT		0x7
 #define APIC_LVT1	0x360
 #define APIC_LVTERR	0x370
+#define	APIC_TMICT	0x380
+#define	APIC_TMCCT	0x390
+#define	APIC_TDCR	0x3E0
+#define		APIC_TDR_DIV_TMBASE	(1<<2)
+#define		APIC_TDR_DIV_1		0xB
+#define		APIC_TDR_DIV_2		0x0
+#define		APIC_TDR_DIV_4		0x1
+#define		APIC_TDR_DIV_8		0x2
+#define		APIC_TDR_DIV_16		0x3
+#define		APIC_TDR_DIV_32		0x8
+#define		APIC_TDR_DIV_64		0x9
+#define		APIC_TDR_DIV_128	0xA
+
+#if defined(__ROMCC__) || !defined(ASSEMBLY)
 
+static inline unsigned long apic_read(unsigned long reg)
+{
+	return *((volatile unsigned long *)(APIC_DEFAULT_BASE+reg));
+}
 
-#if !defined(ASSEMBLY)
+static inline void apic_write(unsigned long reg, unsigned long v)
+{
+	*((volatile unsigned long *)(APIC_DEFAULT_BASE+reg)) = v;
+}
+
+static inline void apic_wait_icr_idle(void)
+{
+	do { } while ( apic_read( APIC_ICR ) & APIC_ICR_BUSY );
+}
 
-#include <console/console.h>
 
+#endif
+
+#if !defined(ASSEMBLY)
 
 #define xchg(ptr,v) ((__typeof__(*(ptr)))__xchg((unsigned long)(v),(ptr),sizeof(*(ptr))))
 
@@ -119,25 +147,11 @@ static inline unsigned long __xchg(unsigned long x, volatile void * ptr, int siz
 }
 
 
-static inline unsigned long apic_read(unsigned long reg)
-{
-	return *((volatile unsigned long *)(APIC_DEFAULT_BASE+reg));
-}
-
 extern inline void apic_write_atomic(unsigned long reg, unsigned long v)
 {
 	xchg((volatile unsigned long *)(APIC_DEFAULT_BASE+reg), v);
 }
 
-static inline void apic_write(unsigned long reg, unsigned long v)
-{
-	*((volatile unsigned long *)(APIC_DEFAULT_BASE+reg)) = v;
-}
-
-static inline void apic_wait_icr_idle(void)
-{
-	do { } while ( apic_read( APIC_ICR ) & APIC_ICR_BUSY );
-}
 
 #ifdef CONFIG_X86_GOOD_APIC
 # define FORCE_READ_AROUND_WRITE 0
diff --git a/src/include/cpu/p6/msr.h b/src/include/cpu/p6/msr.h
index 4977b02..d7632ba 100644
--- a/src/include/cpu/p6/msr.h
+++ b/src/include/cpu/p6/msr.h
@@ -1,33 +1,101 @@
 #ifndef CPU_P6_MSR_H
 #define CPU_P6_MSR_H
 
-/*
- * Access to machine-specific registers (available on 586 and better only)
- * Note: the rd* operations modify the parameters directly (without using
- * pointer indirection), this allows gcc to optimize better
- */
-
-#define rdmsr(msr,val1,val2) \
-       __asm__ __volatile__("rdmsr" \
-			    : "=a" (val1), "=d" (val2) \
-			    : "c" (msr))
-
-#define wrmsr(msr,val1,val2) \
-     __asm__ __volatile__("wrmsr" \
-			  : /* no outputs */ \
-			  : "c" (msr), "a" (val1), "d" (val2))
-
-#define rdtsc(low,high) \
-     __asm__ __volatile__("rdtsc" : "=a" (low), "=d" (high))
-
-#define rdtscl(low) \
-     __asm__ __volatile__ ("rdtsc" : "=a" (low) : : "edx")
-
-#define rdtscll(val) \
-     __asm__ __volatile__ ("rdtsc" : "=A" (val))
-
-#define rdpmc(counter,low,high) \
-     __asm__ __volatile__("rdpmc" \
-			  : "=a" (low), "=d" (high) \
-			  : "c" (counter))
+
+#ifdef __ROMCC__
+
+typedef __builtin_msr_t msr_t;
+
+static msr_t rdmsr(unsigned long index)
+{
+	return __builtin_rdmsr(index);
+}
+
+static void wrmsr(unsigned long index, msr_t msr)
+{
+	__builtin_wrmsr(index, msr.lo, msr.hi);
+}
+
+
+struct tsc_struct {
+	unsigned lo;
+	unsigned hi;
+};
+typedef struct tsc_struct tsc_t;
+
+static tsc_t rdtsc(void)
+{
+	tsc_t res;
+	asm ("rdtsc"
+		: "=a" (res.lo), "=d"(res.hi) /* outputs */
+		: /* inputs */
+		: /* Clobbers */
+		);
+	return res;
+}
+#endif
+
+#ifdef __GNUC__
+
+typedef struct msr_struct 
+{
+	unsigned lo;
+	unsigned hi;
+} msr_t;
+
+static inline msr_t rdmsr(unsigned index)
+{
+	msr_t result;
+	__asm__ __volatile__ (
+		"rdmsr"
+		: "=a" (result.lo), "=d" (result.hi)
+		: "c" (index)
+		);
+	return result;
+}
+
+static inline void wrmsr(unsigned index, msr_t msr)
+{
+	__asm__ __volatile__ (
+		"wrmsr"
+		: /* No outputs */
+		: "c" (index), "a" (msr.lo), "d" (msr.hi)
+		);
+}
+
+typedef struct tsc_struct 
+{
+	unsigned lo;
+	unsigned hi;
+} tsc_t;
+
+static inline tsc_t rdtsc(void)
+{
+	tsc_t result;
+	__asm__ __volatile__(
+		"rdtsc"
+		: "=a"  (result.lo), "=d" (result.hi)
+		);
+	return result;
+}
+
+typedef struct pmc_struct 
+{
+	unsigned lo;
+	unsigned hi;
+} pmc_t; 
+
+static inline pmc_t rdpmc(unsigned counter)
+{
+	pmc_t result;
+	__asm__ __volatile__(
+		"rdpmc"
+		: "=a" (result.lo), "=d" (result.hi)
+		: "c" (counter)
+		);
+	return result;
+}
+
+#endif
+
 #endif /* CPU_P6_MSR_H */
diff --git a/src/include/delay.h b/src/include/delay.h
index bffb719..57bff0b 100644
--- a/src/include/delay.h
+++ b/src/include/delay.h
@@ -1,8 +1,11 @@
 #ifndef DELAY_H
 #define DELAY_H
+#ifndef __ROMCC__
 
-void udelay(int usecs);
-void mdelay(int msecs);
-void delay(int secs);
+void init_timer(void);
+void udelay(unsigned usecs);
+void mdelay(unsigned msecs);
+void delay(unsigned secs);
 
+#endif
 #endif /* DELAY_H */
diff --git a/src/include/device/pnp.h b/src/include/device/pnp.h
new file mode 100644
index 0000000..0d39fc1
--- /dev/null
+++ b/src/include/device/pnp.h
@@ -0,0 +1,58 @@
+#ifndef DEVICE_PNP_H
+#define DEVICE_PNP_H
+
+static inline void pnp_write_config(unsigned char port, unsigned char value, unsigned char reg)
+{
+	outb(reg, port);
+	outb(value, port +1);
+}
+
+static inline unsigned char pnp_read_config(unsigned char port, unsigned char reg)
+{
+	outb(reg, port);
+	return inb(port +1);
+}
+
+static inline void pnp_set_logical_device(unsigned char port, int device)
+{
+	pnp_write_config(port, device, 0x07);
+}
+
+static inline void pnp_set_enable(unsigned char port, int enable)
+{
+	pnp_write_config(port, enable?0x1:0x0, 0x30);
+}
+
+static inline int pnp_read_enable(unsigned char port)
+{
+	return !!pnp_read_config(port, 0x30);
+}
+
+static inline void pnp_set_iobase0(unsigned char port, unsigned iobase)
+{
+	pnp_write_config(port, (iobase >> 8) & 0xff, 0x60);
+	pnp_write_config(port, iobase & 0xff, 0x61);
+}
+
+static inline void pnp_set_iobase1(unsigned char port, unsigned iobase)
+{
+	pnp_write_config(port, (iobase >> 8) & 0xff, 0x62);
+	pnp_write_config(port, iobase & 0xff, 0x63);
+}
+
+static inline void pnp_set_irq0(unsigned char port, unsigned irq)
+{
+	pnp_write_config(port, irq, 0x70);
+}
+
+static inline void pnp_set_irq1(unsigned char port, unsigned irq)
+{
+	pnp_write_config(port, irq, 0x72);
+}
+
+static inline void pnp_set_drq(unsigned char port, unsigned drq)
+{
+	pnp_write_config(port, drq & 0xff, 0x74);
+}
+
+#endif /* DEVICE_PNP_H */
diff --git a/src/include/smp/start_stop.h b/src/include/smp/start_stop.h
index c0eebd0..84fd331 100644
--- a/src/include/smp/start_stop.h
+++ b/src/include/smp/start_stop.h
@@ -1,7 +1,7 @@
 #ifndef SMP_START_STOP_H
 #define SMP_START_STOP_H
 
-#if SMP == 1
+#if CONFIG_SMP == 1
 #include <smp/atomic.h>
 unsigned long this_processors_id(void);
 int processor_index(unsigned long processor_id);
diff --git a/src/lib/delay.c b/src/lib/delay.c
index af5f786..207c629 100644
--- a/src/lib/delay.c
+++ b/src/lib/delay.c
@@ -1,14 +1,14 @@
 #include <delay.h>
-void mdelay(int msecs)
+void mdelay(unsigned msecs)
 {
-	int i;
+	unsigned i;
 	for(i = 0; i < msecs; i++) {
 		udelay(1000);
 	}
 }
-void delay(int secs)
+void delay(unsigned secs)
 {
-	int i;
+	unsigned i;
 	for(i = 0; i < secs; i++) {
 		mdelay(1000);
 	}
diff --git a/src/mainboard/arima/hdama/auto.c b/src/mainboard/arima/hdama/auto.c
index 3a361c4..83f7598 100644
--- a/src/mainboard/arima/hdama/auto.c
+++ b/src/mainboard/arima/hdama/auto.c
@@ -1,225 +1,21 @@
 #define ASSEMBLY 1
 #include <stdint.h>
 #include <device/pci_def.h>
-#include "arch/romcc_io.h"
+#include <cpu/p6/apic.h>
+#include <arch/io.h>
+#include <device/pnp.h>
+#include <arch/romcc_io.h>
 #include "pc80/serial.c"
 #include "arch/i386/lib/console.c"
 #include "ram/ramtest.c"
 #include "northbridge/amd/amdk8/early_ht.c"
 #include "southbridge/amd/amd8111/amd8111_early_smbus.c"
 #include "northbridge/amd/amdk8/raminit.h"
-
-
-static void print_debug_pci_dev(unsigned dev)
-{
-	print_debug("PCI: ");
-	print_debug_hex8((dev >> 16) & 0xff);
-	print_debug_char(':');
-	print_debug_hex8((dev >> 11) & 0x1f);
-	print_debug_char('.');
-	print_debug_hex8((dev >> 8) & 7);
-}
-
-static void print_pci_devices(void)
-{
-	device_t dev;
-	for(dev = PCI_DEV(0, 0, 0); 
-		dev <= PCI_DEV(0, 0x1f, 0x7); 
-		dev += PCI_DEV(0,0,1)) {
-		uint32_t id;
-		id = pci_read_config32(dev, PCI_VENDOR_ID);
-		if (((id & 0xffff) == 0x0000) || ((id & 0xffff) == 0xffff) ||
-			(((id >> 16) & 0xffff) == 0xffff) ||
-			(((id >> 16) & 0xffff) == 0x0000)) {
-			continue;
-		}
-		print_debug_pci_dev(dev);
-		print_debug("\r\n");
-	}
-}
-
-static void dump_pci_device(unsigned dev)
-{
-	int i;
-	print_debug_pci_dev(dev);
-	print_debug("\r\n");
-	
-	for(i = 0; i <= 255; i++) {
-		unsigned char val;
-		if ((i & 0x0f) == 0) {
-			print_debug_hex8(i);
-			print_debug_char(':');
-		}
-		val = pci_read_config8(dev, i);
-		print_debug_char(' ');
-		print_debug_hex8(val);
-		if ((i & 0x0f) == 0x0f) {
-			print_debug("\r\n");
-		}
-	}
-}
-
-static void dump_pci_devices(void)
-{
-	device_t dev;
-	for(dev = PCI_DEV(0, 0, 0); 
-		dev <= PCI_DEV(0, 0x1f, 0x7); 
-		dev += PCI_DEV(0,0,1)) {
-		uint32_t id;
-		id = pci_read_config32(dev, PCI_VENDOR_ID);
-		if (((id & 0xffff) == 0x0000) || ((id & 0xffff) == 0xffff) ||
-			(((id >> 16) & 0xffff) == 0xffff) ||
-			(((id >> 16) & 0xffff) == 0x0000)) {
-			continue;
-		}
-		dump_pci_device(dev);
-	}
-}
-
-static void dump_spd_registers(const struct mem_controller *ctrl)
-{
-	int i;
-	print_debug("\r\n");
-	for(i = 0; i < 4; i++) {
-		unsigned device;
-		device = ctrl->channel0[i];
-		if (device) {
-			int j;
-			print_debug("dimm: "); 
-			print_debug_hex8(i); 
-			print_debug(".0: ");
-			print_debug_hex8(device);
-			for(j = 0; j < 256; j++) {
-				int status;
-				unsigned char byte;
-				if ((j & 0xf) == 0) {
-					print_debug("\r\n");
-					print_debug_hex8(j);
-					print_debug(": ");
-				}
-				status = smbus_read_byte(device, j);
-				if (status < 0) {
-					print_debug("bad device\r\n");
-					break;
-				}
-				byte = status & 0xff;
-				print_debug_hex8(byte);
-				print_debug_char(' ');
-			}
-			print_debug("\r\n");
-		}
-		device = ctrl->channel1[i];
-		if (device) {
-			int j;
-			print_debug("dimm: "); 
-			print_debug_hex8(i); 
-			print_debug(".1: ");
-			print_debug_hex8(device);
-			for(j = 0; j < 256; j++) {
-				int status;
-				unsigned char byte;
-				if ((j & 0xf) == 0) {
-					print_debug("\r\n");
-					print_debug_hex8(j);
-					print_debug(": ");
-				}
-				status = smbus_read_byte(device, j);
-				if (status < 0) {
-					print_debug("bad device\r\n");
-					break;
-				}
-				byte = status & 0xff;
-				print_debug_hex8(byte);
-				print_debug_char(' ');
-			}
-			print_debug("\r\n");
-		}
-	}
-}
-
-#warning "FIXME move these delay functions somewhere more appropriate"
-#warning "FIXME use the apic timer instead it needs no calibration on an Opteron it runs at 200Mhz"
-static void print_clock_multiplier(void)
-{
-	msr_t msr;
-	print_debug("clock multipler: 0x");
-	msr = rdmsr(0xc0010042);
-	print_debug_hex32(msr.lo & 0x3f);
-	print_debug(" = 0x");
-	print_debug_hex32(((msr.lo & 0x3f) + 8) * 100);
-	print_debug("Mhz\r\n");
-}
-
-static unsigned usecs_to_ticks(unsigned usecs)
-{
-#warning "FIXME make usecs_to_ticks work properly"
-#if 1
-	return usecs *2000;
-#else
-	/* This can only be done if cpuid says fid changing is supported
-	 * I need to look up the base frequency another way for other
-	 * cpus.  Is it worth dedicating a global register to this?
-	 * Are the PET timers useable for this purpose?
-	 */
-	msr_t msr;
-	msr = rdmsr(0xc0010042);
-	return ((msr.lo & 0x3f) + 8) * 100 *usecs;
-#endif
-}
-
-static void init_apic_timer(void)
-{
-	volatile uint32_t *apic_reg = (volatile uint32_t *)0xfee00000;
-	uint32_t start, end;
-	/* Set the apic timer to no interrupts and periodic mode */
-	apic_reg[0x320 >> 2] = (1 << 17)|(1<< 16)|(0 << 12)|(0 << 0);
-	/* Set the divider to 1, no divider */
-	apic_reg[0x3e0 >> 2] = (1 << 3) | 3;
-	/* Set the initial counter to 0xffffffff */
-	apic_reg[0x380 >> 2] = 0xffffffff;
-}
-
-static void udelay(unsigned usecs)
-{
-#if 1
-	uint32_t start, ticks;
-	tsc_t tsc;
-	/* Calculate the number of ticks to run for */
-	ticks = usecs_to_ticks(usecs);
-	/* Find the current time */
-	tsc = rdtsc();
-	start = tsc.lo;
-	do {
-		tsc = rdtsc();
-	} while((tsc.lo - start) < ticks);
-#else
-	volatile uint32_t *apic_reg = (volatile uint32_t *)0xfee00000;
-	uint32_t start, value, ticks;
-	/* Calculate the number of ticks to run for */
-	ticks = usecs * 200;
-	start = apic_reg[0x390 >> 2];
-	do {
-		value = apic_reg[0x390 >> 2];
-	} while((start - value) < ticks);
-#endif
-}
-
-static void mdelay(unsigned msecs)
-{
-	int i;
-	for(i = 0; i < msecs; i++) {
-		udelay(1000);
-	}
-}
-
-static void delay(unsigned secs)
-{
-	int i;
-	for(i = 0; i < secs; i++) {
-		mdelay(1000);
-	}
-}
-
+#include "cpu/k8/apic_timer.c"
+#include "lib/delay.c"
+#include "cpu/p6/boot_cpu.c"
+#include "northbridge/amd/amdk8/reset_test.c"
+#include "debug.c"
 
 static void memreset_setup(const struct mem_controller *ctrl)
 {
@@ -285,112 +81,39 @@ static unsigned int generate_row(uint8_t node, uint8_t row, uint8_t maxnodes)
 #include "northbridge/amd/amdk8/coherent_ht.c"
 #include "sdram/generic_sdram.c"
 
-#define NODE_ID		0x60
-#define	HT_INIT_CONTROL 0x6c
-
-#define HTIC_ColdR_Detect  (1<<4)
-#define HTIC_BIOSR_Detect  (1<<5)
-#define HTIC_INIT_Detect   (1<<6)
 
-static int boot_cpu(void)
+static void enable_lapic(void)
 {
-	volatile unsigned long *local_apic;
-	unsigned long apic_id;
-	int bsp;
+
 	msr_t msr;
 	msr = rdmsr(0x1b);
-	bsp = !!(msr.lo & (1 << 8));
-	if (bsp) {
-		print_debug("Bootstrap cpu\r\n");
-	}
-
-	return bsp;
+	msr.hi &= 0xffffff00;
+	msr.lo &= 0x000007ff;
+	msr.lo |= APIC_DEFAULT_BASE | (1 << 11);
+	wrmsr(0x1b, msr);
 }
 
-static int cpu_init_detected(void)
+static void stop_this_cpu(void)
 {
-	unsigned long dcl;
-	int cpu_init;
+	unsigned apicid;
+	apicid = apic_read(APIC_ID) >> 24;
 
-	unsigned long htic;
+	/* Send an APIC INIT to myself */
+	apic_write(APIC_ICR2, SET_APIC_DEST_FIELD(apicid));
+	apic_write(APIC_ICR, APIC_INT_LEVELTRIG | APIC_INT_ASSERT | APIC_DM_INIT);
+	/* Wait for the ipi send to finish */
+	apic_wait_icr_idle();
 
-	htic = pci_read_config32(PCI_DEV(0, 0x18, 0), HT_INIT_CONTROL);
-#if 0
-	print_debug("htic: ");
-	print_debug_hex32(htic);
-	print_debug("\r\n");
+	/* Deassert the APIC INIT */
+	apic_write(APIC_ICR2, SET_APIC_DEST_FIELD(apicid));
+	apic_write(APIC_ICR,  APIC_INT_LEVELTRIG | APIC_DM_INIT);
+	/* Wait for the ipi send to finish */
+	apic_wait_icr_idle();
 
-	if (!(htic & HTIC_ColdR_Detect)) {
-		print_debug("Cold Reset.\r\n");
+	/* If I haven't halted spin forever */
+	for(;;) {
+		hlt();
 	}
-	if ((htic & HTIC_ColdR_Detect) && !(htic & HTIC_BIOSR_Detect)) {
-		print_debug("BIOS generated Reset.\r\n");
-	}
-	if (htic & HTIC_INIT_Detect) {
-		print_debug("Init event.\r\n");
-	}
-#endif
-	cpu_init = (htic & HTIC_INIT_Detect);
-	if (cpu_init) {
-		print_debug("CPU INIT Detected.\r\n");
-	}
-	return cpu_init;
-}
-
-
-
-static void pnp_write_config(unsigned char port, unsigned char value, unsigned char reg)
-{
-	outb(reg, port);
-	outb(value, port +1);
-}
-
-static unsigned char pnp_read_config(unsigned char port, unsigned char reg)
-{
-	outb(reg, port);
-	return inb(port +1);
-}
-
-static void pnp_set_logical_device(unsigned char port, int device)
-{
-	pnp_write_config(port, device, 0x07);
-}
-
-static void pnp_set_enable(unsigned char port, int enable)
-{
-	pnp_write_config(port, enable?0x1:0x0, 0x30);
-}
-
-static int pnp_read_enable(unsigned char port)
-{
-	return !!pnp_read_config(port, 0x30);
-}
-
-static void pnp_set_iobase0(unsigned char port, unsigned iobase)
-{
-	pnp_write_config(port, (iobase >> 8) & 0xff, 0x60);
-	pnp_write_config(port, iobase & 0xff, 0x61);
-}
-
-static void pnp_set_iobase1(unsigned char port, unsigned iobase)
-{
-	pnp_write_config(port, (iobase >> 8) & 0xff, 0x62);
-	pnp_write_config(port, iobase & 0xff, 0x63);
-}
-
-static void pnp_set_irq0(unsigned char port, unsigned irq)
-{
-	pnp_write_config(port, irq, 0x70);
-}
-
-static void pnp_set_irq1(unsigned char port, unsigned irq)
-{
-	pnp_write_config(port, irq, 0x72);
-}
-
-static void pnp_set_drq(unsigned char port, unsigned drq)
-{
-	pnp_write_config(port, drq & 0xff, 0x74);
 }
 
 #define PC87360_FDC  0x00
@@ -435,46 +158,51 @@ static void main(void)
 		.channel0 = { (0xa<<3)|4, (0xa<<3)|6, 0, 0 },
 		.channel1 = { (0xa<<3)|5, (0xa<<3)|7, 0, 0 },
 	};
+	if (cpu_init_detected()) {
+		asm("jmp __cpu_reset");
+	}
 	pc87360_enable_serial();
 	uart_init();
 	console_init();
-	if (boot_cpu() && !cpu_init_detected()) {
-#if 0
-		init_apic_timer();
+	enable_lapic();
+	if (!boot_cpu()) {
+		stop_this_cpu();
+	}
+	init_timer();
+	setup_default_resource_map();
+	setup_coherent_ht_domain();
+	enumerate_ht_chain(0);
+	distinguish_cpu_resets();
+	
+#if 1 
+	print_pci_devices();
 #endif
-		setup_default_resource_map();
-		setup_coherent_ht_domain();
-		enumerate_ht_chain();
-		print_pci_devices();
-		enable_smbus();
+	enable_smbus();
 #if 0
-		dump_spd_registers(&cpu0);
+	dump_spd_registers(&cpu0);
 #endif
-		sdram_initialize(&cpu0);
+	sdram_initialize(&cpu0);
 
 #if 1
-		dump_pci_devices();
+	dump_pci_devices();
 #endif
 #if 0
-		dump_pci_device(PCI_DEV(0, 0x18, 2));
+	dump_pci_device(PCI_DEV(0, 0x18, 2));
 #endif
 
-		/* Check all of memory */
+	/* Check all of memory */
 #if 0
-		msr_t msr;
-		msr = rdmsr(TOP_MEM);
-		print_debug("TOP_MEM: ");
-		print_debug_hex32(msr.hi);
-		print_debug_hex32(msr.lo);
-		print_debug("\r\n");
+	msr_t msr;
+	msr = rdmsr(TOP_MEM);
+	print_debug("TOP_MEM: ");
+	print_debug_hex32(msr.hi);
+	print_debug_hex32(msr.lo);
+	print_debug("\r\n");
 #endif
 #if 0
-		ram_check(0x00000000, msr.lo);
+	ram_check(0x00000000, msr.lo);
 #else
-#if 1
-		/* Check 16MB of memory */
-		ram_check(0x00000000, 0x01000000);
-#endif
+	/* Check 16MB of memory */
+	ram_check(0x00000000, 0x01000000);
 #endif
-	}
 }
diff --git a/src/mainboard/arima/hdama/failover.c b/src/mainboard/arima/hdama/failover.c
index 017b038..8eeeaef 100644
--- a/src/mainboard/arima/hdama/failover.c
+++ b/src/mainboard/arima/hdama/failover.c
@@ -2,22 +2,37 @@
 #include <stdint.h>
 #include <device/pci_def.h>
 #include <device/pci_ids.h>
+#include <arch/io.h>
 #include "arch/romcc_io.h"
 #include "pc80/mc146818rtc_early.c"
 #include "southbridge/amd/amd8111/amd8111_enable_rom.c"
 #include "northbridge/amd/amdk8/early_ht.c"
+#include "cpu/p6/boot_cpu.c"
+#include "northbridge/amd/amdk8/reset_test.c"
 
 static void main(void)
 {
 	/* Nothing special needs to be done to find bus 0 */
 	/* Allow the HT devices to be found */
-	enumerate_ht_chain();
+	enumerate_ht_chain(0);
 
 	/* Setup the 8111 */
 	amd8111_enable_rom();
 
-	if (do_normal_boot()) {
-		/* Jump to the normal image */
+	/* Is this a cpu reset? */
+	if (cpu_init_detected()) {
+		if (last_boot_normal()) {
+			asm("jmp __normal_image");
+		} else {
+			asm("jmp __cpu_reset");
+		}
+	}
+	/* Is this a secondary cpu? */
+	else if (!boot_cpu() && last_boot_normal()) {
+		asm("jmp __normal_image");
+	}
+	/* This is the primary cpu how should I boot? */
+	else if (do_normal_boot()) {
 		asm("jmp __normal_image");
 	}
 }
diff --git a/src/mainboard/arima/hdama/mainboard.c b/src/mainboard/arima/hdama/mainboard.c
index 5690bd5..2118e83 100644
--- a/src/mainboard/arima/hdama/mainboard.c
+++ b/src/mainboard/arima/hdama/mainboard.c
@@ -7,5 +7,5 @@
 
 unsigned long initial_apicid[MAX_CPUS] =
 {
-	0
+	0, 1,
 };
diff --git a/src/northbridge/amd/amdk8/coherent_ht.c b/src/northbridge/amd/amdk8/coherent_ht.c
index 460284b..23ebd60 100644
--- a/src/northbridge/amd/amdk8/coherent_ht.c
+++ b/src/northbridge/amd/amdk8/coherent_ht.c
@@ -440,13 +440,7 @@ static void enable_routing(u8 node)
 	print_debug_hex32(node);
 
 	val=pci_read_config32(NODE_HT(node), 0x6c);
-	val |= (1 << 6) | (1 << 5) | (1 << 4);
-#if 0
 	val &= ~((1<<1)|(1<<0));
-#else
-	/* Don't enable requests here as the indicated processor starts booting */
-	val &= ~(1<<0);
-#endif
 	pci_write_config32(NODE_HT(node), 0x6c, val);
 
 	print_debug(" done.\r\n");
@@ -456,7 +450,7 @@ static void enable_routing(u8 node)
 
 static void rename_temp_node(u8 node)
 {
-	u32 val;
+	uint32_t val;
 
 	print_debug("Renaming current temp node to ");
 	print_debug_hex32(node);
@@ -678,8 +672,8 @@ static u8 setup_smp(void)
 	/* We found 2 nodes so far */
 	setup_node(0, cpus);	/* Node 1 is there. Setup Node 0 correctly */
 	setup_remote_node(1, cpus);  /* Setup the routes on the remote node */
-	enable_routing(7);	/* Enable routing on Node 1 */
 	rename_temp_node(1);	/* Rename Node 7 to Node 1  */
+	enable_routing(1);	/* Enable routing on Node 1 */
 	
 	clear_temp_row(0);	/* delete temporary connection */
 	
@@ -716,14 +710,14 @@ static u8 setup_smp(void)
 	
 	setup_temp_row(0,2,cpus);
 	setup_temp_node(2,cpus);
-	enable_routing(7);
 	rename_temp_node(2);
+	enable_routing(2);
 
 	setup_temp_row(0,1,cpus);
 	setup_temp_row(1,3,cpus);
 	setup_temp_node(3,cpus);
-	enable_routing(7);	/* enable routing on node 3 (temp.) */
 	rename_temp_node(3);
+	enable_routing(3);	/* enable routing on node 3 (temp.) */
 	
 	clear_temp_row(0);
 	clear_temp_row(1);
@@ -820,13 +814,14 @@ static void coherent_ht_finalize(unsigned cpus)
 	}
 
 #if 1
-	print_debug("done\n");
+	print_debug("done\r\n");
 #endif
 }
 
-static void setup_coherent_ht_domain(void)
+static int setup_coherent_ht_domain(void)
 {
 	unsigned cpus;
+	int reset_needed = 0;
 
 	enable_bsp_routing();
 
@@ -837,6 +832,8 @@ static void setup_coherent_ht_domain(void)
 	cpus=detect_mp_capabilities(cpus);
 #endif
 	coherent_ht_finalize(cpus);
+
+	return reset_needed;
 }
 
 #endif
diff --git a/src/northbridge/amd/amdk8/early_ht.c b/src/northbridge/amd/amdk8/early_ht.c
index b8262d5..4de8fa1 100644
--- a/src/northbridge/amd/amdk8/early_ht.c
+++ b/src/northbridge/amd/amdk8/early_ht.c
@@ -1,11 +1,12 @@
-static void enumerate_ht_chain(void)
+static int enumerate_ht_chain(unsigned link)
 {
 	/* Assumption the HT chain that is bus 0 has the HT I/O Hub on it.
 	 * On most boards this just happens.  If a cpu has multiple
 	 * non Coherent links the appropriate bus registers for the
 	 * links needs to be programed to point at bus 0.
 	 */
-	unsigned next_unitid, last_unitid;;
+	unsigned next_unitid, last_unitid;
+	int reset_needed = 0;
 	next_unitid = 1;
 	do {
 		uint32_t id;
@@ -46,4 +47,5 @@ static void enumerate_ht_chain(void)
 			pos = pci_read_config8(PCI_DEV(0, 0, 0), pos + PCI_CAP_LIST_NEXT);
 		}
 	} while((last_unitid != next_unitid) && (next_unitid <= 0x1f));
+	return reset_needed;
 }
diff --git a/src/northbridge/amd/amdk8/raminit.c b/src/northbridge/amd/amdk8/raminit.c
index fd59567..41a93b5 100644
--- a/src/northbridge/amd/amdk8/raminit.c
+++ b/src/northbridge/amd/amdk8/raminit.c
@@ -1135,7 +1135,7 @@ static void route_dram_accesses(const struct mem_controller *ctrl,
 	pci_write_config32(ctrl->f1, 0x44, limit | (0 << 8) | (node_id << 0));
 	pci_write_config32(ctrl->f1, 0x40, base  | (0 << 8) | (1<<1) | (1<<0));
 
-#if 0
+#if 1
 	pci_write_config32(PCI_DEV(0, 0x19, 1), 0x44, limit | (0 << 8) | (1 << 4) | (node_id << 0));
 	pci_write_config32(PCI_DEV(0, 0x19, 1), 0x40, base  | (0 << 8) | (1<<1) | (1<<0));
 #endif
diff --git a/src/northbridge/amd/amdk8/reset_test.c b/src/northbridge/amd/amdk8/reset_test.c
new file mode 100644
index 0000000..949bd7c
--- /dev/null
+++ b/src/northbridge/amd/amdk8/reset_test.c
@@ -0,0 +1,43 @@
+#include <stdint.h>
+#define NODE_ID		0x60
+#define	HT_INIT_CONTROL 0x6c
+
+#define HTIC_ColdR_Detect  (1<<4)
+#define HTIC_BIOSR_Detect  (1<<5)
+#define HTIC_INIT_Detect   (1<<6)
+
+
+static int cpu_init_detected(void)
+{
+	unsigned long dcl;
+	int cpu_init;
+
+	unsigned long htic;
+
+	htic = pci_read_config32(PCI_DEV(0, 0x18, 0), HT_INIT_CONTROL);
+#if 0
+	print_debug("htic: ");
+	print_debug_hex32(htic);
+	print_debug("\r\n");
+
+	if (!(htic & HTIC_ColdR_Detect)) {
+		print_debug("Cold Reset.\r\n");
+	}
+	if ((htic & HTIC_ColdR_Detect) && !(htic & HTIC_BIOSR_Detect)) {
+		print_debug("BIOS generated Reset.\r\n");
+	}
+	if (htic & HTIC_INIT_Detect) {
+		print_debug("Init event.\r\n");
+	}
+#endif
+	cpu_init = (htic & HTIC_INIT_Detect);
+	return cpu_init;
+}
+
+static void distinguish_cpu_resets(void)
+{
+	uint32_t htic;
+	htic = pci_read_config32(PCI_DEV(0, 0x18, 0), HT_INIT_CONTROL);
+	htic |= HTIC_ColdR_Detect | HTIC_BIOSR_Detect | HTIC_INIT_Detect;
+	pci_write_config32(PCI_DEV(0, 0x18, 0), HT_INIT_CONTROL, htic);
+}
diff --git a/src/pc80/mc146818rtc_early.c b/src/pc80/mc146818rtc_early.c
index 96f8f5d..3036945 100644
--- a/src/pc80/mc146818rtc_early.c
+++ b/src/pc80/mc146818rtc_early.c
@@ -4,7 +4,7 @@
 #ifndef MAX_REBOOT_CNT
 #error "MAX_REBOOT_CNT not defined"
 #endif
-#if  MAX_REBOOT_CNT > 15
+#if  MAX_REBOOT_CNT > 14
 #error "MAX_REBOOT_CNT too high"
 #endif
 
@@ -47,6 +47,13 @@ static int cmos_chksum_valid(void)
 }
 
 
+static int last_boot_normal(void)
+{
+	unsigned char byte;
+	byte = cmos_read(RTC_BOOT_BYTE);
+	return (byte & (1 << 1));
+}
+
 static int do_normal_boot(void)
 {
 	unsigned char byte;