PowerPC common SMP startup and time base rework.

- make mftb() shared, rewrite in C, provide complementary mttb()
- adjust SMP startup per the above, additional comments, minor naming
  changes
- eliminate redundant TB defines, other minor cosmetics

Reviewed by:	marcel, nwhitehorn
Obtained from:	Freescale, Semihalf

6 files changed, 77 insertions, 57 deletions

diff --git a/sys/powerpc/aim/clock.c b/sys/powerpc/aim/clock.c
index 2d3eb6049566..0a5391f7424b 100644
--- a/sys/powerpc/aim/clock.c
+++ b/sys/powerpc/aim/clock.c
@@ -148,22 +148,19 @@ decr_init(void)
 	mtmsr(msr);
 }
 
+#ifdef SMP
 void
-decr_tc_init(void)
+decr_ap_init(void)
 {
-	decr_timecounter.tc_frequency = ticks_per_sec;
-	tc_init(&decr_timecounter);
+
 }
+#endif
 
-static __inline u_quad_t
-mftb(void)
+void
+decr_tc_init(void)
 {
-	u_long		scratch;
-	u_quad_t	tb;
-
-	__asm ("1: mftbu %0; mftb %0+1; mftbu %1; cmpw 0,%0,%1; bne 1b"
-	      : "=r"(tb), "=r"(scratch));
-	return tb;
+	decr_timecounter.tc_frequency = ticks_per_sec;
+	tc_init(&decr_timecounter);
 }
 
 static unsigned
diff --git a/sys/powerpc/booke/clock.c b/sys/powerpc/booke/clock.c
index 6c8874122ce1..7f9848e6d977 100644
--- a/sys/powerpc/booke/clock.c
+++ b/sys/powerpc/booke/clock.c
@@ -151,22 +151,6 @@ decr_init (void)
 	mtmsr(msr);
 }
 
-static __inline u_quad_t
-mftb (void)
-{
-	u_long		scratch;
-	u_quad_t	tb;
-
-	__asm__ __volatile__(
-	    "1:	mftbu %0;"
-	    "	mftb %0+1;"
-	    "	mftbu %1;"
-	    "	cmpw 0,%0,%1;"
-	    "	bne 1b"
-	    : "=r"(tb), "=r"(scratch));
-	return tb;
-}
-
 void
 decr_tc_init(void)
 {
diff --git a/sys/powerpc/include/cpufunc.h b/sys/powerpc/include/cpufunc.h
index 66b5df38ad6f..9cdaba028319 100644
--- a/sys/powerpc/include/cpufunc.h
+++ b/sys/powerpc/include/cpufunc.h
@@ -115,13 +115,37 @@ mfdec(void)
 static __inline register_t
 mfpvr(void)
 {
-	register_t	value;
+	register_t value;
 
 	__asm __volatile ("mfpvr %0" : "=r"(value));
 
 	return (value);
 }
 
+static __inline u_quad_t
+mftb(void)
+{
+	u_quad_t tb;
+	uint32_t *tbup = (uint32_t *)&tb;
+	uint32_t *tblp = tbup + 1;
+
+	do {
+		*tbup = mfspr(TBR_TBU);
+		*tblp = mfspr(TBR_TBL);
+	} while (*tbup != mfspr(TBR_TBU));
+
+	return (tb);
+}
+
+static __inline void
+mttb(u_quad_t time)
+{
+
+	mtspr(TBR_TBWL, 0);
+	mtspr(TBR_TBWU, (uint32_t)(time >> 32));
+	mtspr(TBR_TBWL, (uint32_t)(time & 0xffffffff));
+}
+
 static __inline void
 eieio(void)
 {
diff --git a/sys/powerpc/include/md_var.h b/sys/powerpc/include/md_var.h
index ec81e6cd2360..d1785adfbe0f 100644
--- a/sys/powerpc/include/md_var.h
+++ b/sys/powerpc/include/md_var.h
@@ -56,11 +56,12 @@ int	is_physical_memory(vm_offset_t addr);
 int	mem_valid(vm_offset_t addr, int len);
 
 void	decr_init(void);
+void	decr_ap_init(void);
 void	decr_tc_init(void);
 
 void	cpu_setup(u_int);
 
-struct trapframe;
+struct	trapframe;
 void	powerpc_interrupt(struct trapframe *);
 
 #endif /* !_MACHINE_MD_VAR_H_ */
diff --git a/sys/powerpc/include/spr.h b/sys/powerpc/include/spr.h
index 82f734b4646f..098c78c352db 100644
--- a/sys/powerpc/include/spr.h
+++ b/sys/powerpc/include/spr.h
@@ -129,8 +129,6 @@
 #define	SPR_SPRG7		0x117	/* 4.. SPR General 7 */
 #define	SPR_ASR			0x118	/* ... Address Space Register (PPC64) */
 #define	SPR_EAR			0x11a	/* .68 External Access Register */
-#define	SPR_TBL			0x11c	/* 468 Time Base Lower */
-#define	SPR_TBU			0x11d	/* 468 Time Base Upper */
 #define	SPR_PVR			0x11f	/* 468 Processor Version Register */
 #define	  MPC601		  0x0001
 #define	  MPC603		  0x0003
diff --git a/sys/powerpc/powerpc/mp_machdep.c b/sys/powerpc/powerpc/mp_machdep.c
index cbaa400130a0..0dbbced06cdd 100644
--- a/sys/powerpc/powerpc/mp_machdep.c
+++ b/sys/powerpc/powerpc/mp_machdep.c
@@ -30,6 +30,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/kernel.h>
+#include <sys/ktr.h>
 #include <sys/bus.h>
 #include <sys/pcpu.h>
 #include <sys/proc.h>
@@ -40,6 +41,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/cpu.h>
 #include <machine/intr_machdep.h>
 #include <machine/platform.h>
+#include <machine/md_var.h>
 #include <machine/smp.h>
 
 #include "pic_if.h"
@@ -47,30 +49,35 @@ __FBSDID("$FreeBSD$");
 extern struct pcpu __pcpu[MAXCPU];
 
 volatile static int ap_awake;
-volatile static u_int ap_state;
+volatile static u_int ap_letgo;
 volatile static uint32_t ap_decr;
-volatile static uint32_t ap_tbl;
+volatile static u_quad_t ap_timebase;
+static u_int ipi_msg_cnt[32];
 
 void
 machdep_ap_bootstrap(void)
 {
 
-	pcpup->pc_awake = 1;
+	PCPU_SET(pir, mfspr(SPR_PIR));
+	PCPU_SET(awake, 1);
+	__asm __volatile("msync; isync");
 
-	while (ap_state == 0)
+	while (ap_letgo == 0)
 		;
 
-	mtspr(SPR_TBL, 0);
-	mtspr(SPR_TBU, 0);
-	mtspr(SPR_TBL, ap_tbl);
+	/* Initialize DEC and TB, sync with the BSP values */
+	decr_ap_init();
+	mttb(ap_timebase);
 	__asm __volatile("mtdec %0" :: "r"(ap_decr));
 
-	ap_awake++;
+	atomic_add_int(&ap_awake, 1);
+	CTR1(KTR_SMP, "SMP: AP CPU%d launched", PCPU_GET(cpuid));
 
-	/* Initialize curthread. */
+	/* Initialize curthread */
 	PCPU_SET(curthread, PCPU_GET(idlethread));
 	PCPU_SET(curpcb, curthread->td_pcb);
 
+	/* Let the DEC and external interrupts go */
 	mtmsr(mfmsr() | PSL_EE);
 	sched_throw(NULL);
 }
@@ -149,8 +156,7 @@ cpu_mp_start(void)
 		pc->pc_cpumask = 1 << pc->pc_cpuid;
 		pc->pc_hwref = cpu.cr_hwref;
 		all_cpus |= pc->pc_cpumask;
-
- next:
+next:
 		error = platform_smp_next_cpu(&cpu);
 	}
 }
@@ -176,7 +182,7 @@ static void
 cpu_mp_unleash(void *dummy)
 {
 	struct pcpu *pc;
-	int cpus;
+	int cpus, timeout;
 
 	if (mp_ncpus <= 1)
 		return;
@@ -187,35 +193,47 @@ cpu_mp_unleash(void *dummy)
 		cpus++;
 		pc->pc_other_cpus = all_cpus & ~pc->pc_cpumask;
 		if (!pc->pc_bsp) {
-			printf("Waking up CPU %d (dev=%x)\n", pc->pc_cpuid,
-			    pc->pc_hwref);
+			if (bootverbose)
+				printf("Waking up CPU %d (dev=%x)\n",
+				    pc->pc_cpuid, pc->pc_hwref);
+
 			platform_smp_start_cpu(pc);
+			
+			timeout = 2000;	/* wait 2sec for the AP */
+			while (!pc->pc_awake && --timeout > 0)
+				DELAY(1000);
+
 		} else {
-			__asm __volatile("mfspr %0,1023" : "=r"(pc->pc_pir));
+			PCPU_SET(pir, mfspr(SPR_PIR));
 			pc->pc_awake = 1;
 		}
-		if (pc->pc_awake)
+		if (pc->pc_awake) {
+			if (bootverbose)
+				printf("Adding CPU %d, pir=%x, awake=%x\n",
+				    pc->pc_cpuid, pc->pc_pir, pc->pc_awake);
 			smp_cpus++;
+		} else
+			stopped_cpus |= (1 << pc->pc_cpuid);
 	}
 
 	ap_awake = 1;
 
-	__asm __volatile("mftb %0" : "=r"(ap_tbl));
-	ap_tbl += 10;
+	/* Provide our current DEC and TB values for APs */
 	__asm __volatile("mfdec %0" : "=r"(ap_decr));
-	ap_state++;
-	powerpc_sync();
+	ap_timebase = mftb() + 10;
+	__asm __volatile("msync; isync");
+	
+	/* Let APs continue */
+	atomic_store_rel_int(&ap_letgo, 1);
 
-	mtspr(SPR_TBL, 0);
-	mtspr(SPR_TBU, 0);
-	mtspr(SPR_TBL, ap_tbl);
+	mttb(ap_timebase);
 
 	while (ap_awake < smp_cpus)
 		;
 
 	if (smp_cpus != cpus || cpus != mp_ncpus) {
 		printf("SMP: %d CPUs found; %d CPUs usable; %d CPUs woken\n",
-			mp_ncpus, cpus, smp_cpus);
+		    mp_ncpus, cpus, smp_cpus);
 	}
 
 	smp_active = 1;
@@ -224,8 +242,6 @@ cpu_mp_unleash(void *dummy)
 
 SYSINIT(start_aps, SI_SUB_SMP, SI_ORDER_FIRST, cpu_mp_unleash, NULL);
 
-static u_int ipi_msg_cnt[32];
-
 int
 powerpc_ipi_handler(void *arg)
 {