From dab90f68edd7c8d89e8dbcb395ca55e533fadc28 Mon Sep 17 00:00:00 2001
From: Nathan Whitehorn <nwhitehorn@FreeBSD.org>
Date: Fri, 23 Oct 2009 21:36:33 +0000
Subject: [PATCH] Add some more paranoia to setting HID registers, and update
 the AIM clock routines to work better with SMP. This makes SMP work fully and
 stably on an Xserve G5.

Obtained from:	Book-E (clock bits)
---
 sys/powerpc/aim/clock.c     | 17 +++++++++++------
 sys/powerpc/aim/machdep.c   |  2 ++
 sys/powerpc/aim/mp_cpudep.c | 20 ++++++++++++++------
 3 files changed, 27 insertions(+), 12 deletions(-)

diff --git a/sys/powerpc/aim/clock.c b/sys/powerpc/aim/clock.c
index 0a5391f7424..a60878c83c5 100644
--- a/sys/powerpc/aim/clock.c
+++ b/sys/powerpc/aim/clock.c
@@ -95,8 +95,7 @@ static struct timecounter	decr_timecounter = {
 void
 decr_intr(struct trapframe *frame)
 {
-	long		tick;
-	int		nticks;
+	int32_t		tick, nticks;
 
 	/*
 	 * Check whether we are initialized.
@@ -112,13 +111,17 @@ decr_intr(struct trapframe *frame)
 	for (nticks = 0; tick < 0; nticks++)
 		tick += ticks_per_intr;
 	mtdec(tick);
+if (nticks > 5) printf("BIG NTICKS on CPU %d: %x\n",PCPU_GET(cpuid),nticks);
 
-	if (PCPU_GET(cpuid) == 0) {
-		while (nticks-- > 0)
+	while (nticks-- > 0) {
+		if (PCPU_GET(cpuid) == 0)
 			hardclock(TRAPF_USERMODE(frame), TRAPF_PC(frame));
-	} else {
-		while (nticks-- > 0)
+		else
 			hardclock_cpu(TRAPF_USERMODE(frame));
+
+		statclock(TRAPF_USERMODE(frame));
+		if (profprocs != 0)
+			profclock(TRAPF_USERMODE(frame), TRAPF_PC(frame));
 	}
 }
 
@@ -145,6 +148,8 @@ decr_init(void)
 	ticks_per_intr = ticks_per_sec / hz;
 	mtdec(ticks_per_intr);
 
+	set_cputicker(mftb, ticks_per_sec, 0);
+
 	mtmsr(msr);
 }
 
diff --git a/sys/powerpc/aim/machdep.c b/sys/powerpc/aim/machdep.c
index 339e64abfd8..da73dfab3ff 100644
--- a/sys/powerpc/aim/machdep.c
+++ b/sys/powerpc/aim/machdep.c
@@ -885,6 +885,8 @@ cpu_initclocks(void)
 {
 
 	decr_tc_init();
+	stathz = hz;
+	profhz = hz;
 }
 
 /*
diff --git a/sys/powerpc/aim/mp_cpudep.c b/sys/powerpc/aim/mp_cpudep.c
index 52f8542ac78..50f64d79441 100644
--- a/sys/powerpc/aim/mp_cpudep.c
+++ b/sys/powerpc/aim/mp_cpudep.c
@@ -50,7 +50,7 @@ __FBSDID("$FreeBSD$");
 
 void *ap_pcpu;
 
-static register_t bsp_state[8];
+static register_t bsp_state[8] __aligned(8);
 
 static void cpudep_save_config(void *dummy);
 SYSINIT(cpu_save_config, SI_SUB_CPU, SI_ORDER_ANY, cpudep_save_config, NULL);
@@ -184,6 +184,8 @@ cpudep_save_config(void *dummy)
 		__asm __volatile ("mfspr %0,%2; mr %1,%0; srdi %0,%0,32"
 		    : "=r" (bsp_state[6]),"=r" (bsp_state[7]) : "K" (SPR_HID5));
 
+		powerpc_sync();
+
 		break;
 	case MPC7450:
 	case MPC7455:
@@ -224,17 +226,23 @@ cpudep_ap_setup()
 		 * See Table 2-3, 970MP manual
 		 */
 
+		__asm __volatile("mtasr %0; sync" :: "r"(0));
 		__asm __volatile(" \
 			ld	%0,0(%2);				\
+			sync; isync;					\
 			mtspr	%1, %0;					\
 			mfspr	%0, %1;	mfspr	%0, %1;	mfspr	%0, %1;	\
-			mfspr	%0, %1;	mfspr	%0, %1;	mfspr	%0, %1;"
+			mfspr	%0, %1;	mfspr	%0, %1;	mfspr	%0, %1; \
+			sync; isync" 
 		    : "=r"(reg) : "K"(SPR_HID0), "r"(bsp_state));
-		__asm __volatile("ld %0, 8(%2); mtspr %1, %0; mtspr %1, %0; \
-		    isync" : "=r"(reg) : "K"(SPR_HID1), "r"(bsp_state));
-		__asm __volatile("ld %0, 16(%2); sync; mtspr %1, %0; isync;"
+		__asm __volatile("ld %0, 8(%2); sync; isync;	\
+		    mtspr %1, %0; mtspr %1, %0; sync; isync"
+		    : "=r"(reg) : "K"(SPR_HID1), "r"(bsp_state));
+		__asm __volatile("ld %0, 16(%2); sync; isync;	\
+		    mtspr %1, %0; sync; isync;"
 		    : "=r"(reg) : "K"(SPR_HID4), "r"(bsp_state));
-		__asm __volatile("ld %0, 24(%2); sync; mtspr %1, %0; isync;"
+		__asm __volatile("ld %0, 24(%2); sync; isync;	\
+		    mtspr %1, %0; sync; isync;"
 		    : "=r"(reg) : "K"(SPR_HID5), "r"(bsp_state));
 
 		powerpc_sync();