Add SMP support on U3-based G5 systems. This does not yet work perfectly:

at least on my Xserve, getting the decrementer and timebase on APs to tick
requires setting up a clock chip over I2C, which is not yet done.

While here, correct the 64-bit tlbie function to set the CPU to 64-bit
mode correctly.

Hardware donated by:	grehan

3 files changed, 184 insertions, 78 deletions

diff --git a/sys/powerpc/aim/mmu_oea64.c b/sys/powerpc/aim/mmu_oea64.c
index 67e457c..9a40036 100644
--- a/sys/powerpc/aim/mmu_oea64.c
+++ b/sys/powerpc/aim/mmu_oea64.c
@@ -182,35 +182,28 @@ va_to_vsid(pmap_t pm, vm_offset_t va)
  * Just to add to the fun, exceptions must be off as well
  * so that we can't trap in 64-bit mode. What a pain.
  */
+struct mtx	tlbie_mutex;
 
 static __inline void
 TLBIE(pmap_t pmap, vm_offset_t va) {
-	register_t msr;
-	register_t scratch;
-
 	uint64_t vpn;
 	register_t vpn_hi, vpn_lo;
-
-#if 1
-	/*
-	 * CPU documentation says that tlbie takes the VPN, not the
-	 * VA. I think the code below does this correctly. We will see.
-	 */
+	register_t msr;
+	register_t scratch;
 
 	vpn = (uint64_t)(va & ADDR_PIDX);
 	if (pmap != NULL)
 		vpn |= (va_to_vsid(pmap,va) << 28);
-#else
-	vpn = va;
-#endif
 
 	vpn_hi = (uint32_t)(vpn >> 32);
 	vpn_lo = (uint32_t)vpn;
 
+	mtx_lock_spin(&tlbie_mutex);
 	__asm __volatile("\
 	    mfmsr %0; \
 	    clrldi %1,%0,49; \
-	    insrdi %1,1,1,0; \
+	    mtmsr %1; \
+	    insrdi %1,%5,1,0; \
 	    mtmsrd %1; \
 	    ptesync; \
 	    \
@@ -222,7 +215,8 @@ TLBIE(pmap_t pmap, vm_offset_t va) {
 	    eieio; \
 	    tlbsync; \
 	    ptesync;" 
-	: "=r"(msr), "=r"(scratch) : "r"(vpn_hi), "r"(vpn_lo), "r"(32));
+	: "=r"(msr), "=r"(scratch) : "r"(vpn_hi), "r"(vpn_lo), "r"(32), "r"(1));
+	mtx_unlock_spin(&tlbie_mutex);
 }
 
 #define DISABLE_TRANS(msr)	msr = mfmsr(); mtmsr(msr & ~PSL_DR); isync()
@@ -352,7 +346,7 @@ static int		moea64_pte_insert(u_int, struct lpte *);
  * PVO calls.
  */
 static int	moea64_pvo_enter(pmap_t, uma_zone_t, struct pvo_head *,
-		    vm_offset_t, vm_offset_t, uint64_t, int, int);
+		    vm_offset_t, vm_offset_t, uint64_t, int);
 static void	moea64_pvo_remove(struct pvo_entry *, int);
 static struct	pvo_entry *moea64_pvo_find_va(pmap_t, vm_offset_t, int *);
 static struct	lpte *moea64_pvo_to_pte(const struct pvo_entry *, int);
@@ -825,6 +819,11 @@ moea64_bridge_bootstrap(mmu_t mmup, vm_offset_t kernelstart, vm_offset_t kernele
 	    MTX_RECURSE);
 
 	/*
+	 * Initialize the TLBIE lock. TLBIE can only be executed by one CPU.
+	 */
+	mtx_init(&tlbie_mutex, "tlbie mutex", NULL, MTX_SPIN);
+
+	/*
 	 * Initialise the unmanaged pvo pool.
 	 */
 	moea64_bpvo_pool = (struct pvo_entry *)moea64_bootstrap_alloc(
@@ -1254,7 +1253,7 @@ moea64_enter_locked(pmap_t pmap, vm_offset_t va, vm_page_t m, vm_prot_t prot,
 		pvo_flags |= PVO_FAKE;
 
 	error = moea64_pvo_enter(pmap, zone, pvo_head, va, VM_PAGE_TO_PHYS(m),
-	    pte_lo, pvo_flags, 0);
+	    pte_lo, pvo_flags);
 
 	if (pmap == kernel_pmap)
 		TLBIE(pmap, va);
@@ -1427,16 +1426,15 @@ moea64_uma_page_alloc(uma_zone_t zone, int bytes, u_int8_t *flags, int wait)
 	if (pvo_allocator_start >= pvo_allocator_end)
 		panic("Ran out of PVO allocator buffer space!");
 
-	/* Now call pvo_enter in recursive mode */
 	moea64_pvo_enter(kernel_pmap, moea64_upvo_zone,
 	    &moea64_pvo_kunmanaged, va,  VM_PAGE_TO_PHYS(m), LPTE_M, 
-	    PVO_WIRED | PVO_BOOTSTRAP, 1);
+	    PVO_WIRED | PVO_BOOTSTRAP);
 
 	TLBIE(kernel_pmap, va);
-	
+
 	if (needed_lock)
 		PMAP_UNLOCK(kernel_pmap);
-
+	
 	if ((wait & M_ZERO) && (m->flags & PG_ZERO) == 0)
                 bzero((void *)va, PAGE_SIZE);
 
@@ -1579,7 +1577,7 @@ moea64_kenter(mmu_t mmu, vm_offset_t va, vm_offset_t pa)
 	PMAP_LOCK(kernel_pmap);
 	error = moea64_pvo_enter(kernel_pmap, moea64_upvo_zone,
 	    &moea64_pvo_kunmanaged, va, pa, pte_lo, 
-	    PVO_WIRED | VM_PROT_EXECUTE, 0);
+	    PVO_WIRED | VM_PROT_EXECUTE);
 
 	TLBIE(kernel_pmap, va);
 
@@ -1968,14 +1966,29 @@ static void
 tlbia(void)
 {
 	vm_offset_t i;
-
-	for (i = 0; i < 0xFF000; i += 0x00001000) 
-		TLBIE(NULL,i);
+	register_t msr, scratch;
+
+	for (i = 0; i < 0xFF000; i += 0x00001000) {
+		__asm __volatile("\
+		    mfmsr %0; \
+		    mr %1, %0; \
+		    insrdi %1,%3,1,0; \
+		    mtmsrd %1; \
+		    ptesync; \
+		    \
+		    tlbiel %2; \
+		    \
+		    mtmsrd %0; \
+		    eieio; \
+		    tlbsync; \
+		    ptesync;" 
+		: "=r"(msr), "=r"(scratch) : "r"(i), "r"(1));
+	}
 }
 
 static int
 moea64_pvo_enter(pmap_t pm, uma_zone_t zone, struct pvo_head *pvo_head,
-    vm_offset_t va, vm_offset_t pa, uint64_t pte_lo, int flags, int recurse)
+    vm_offset_t va, vm_offset_t pa, uint64_t pte_lo, int flags)
 {
 	struct	 pvo_entry *pvo;
 	uint64_t vsid;
@@ -2011,16 +2024,14 @@ moea64_pvo_enter(pmap_t pm, uma_zone_t zone, struct pvo_head *pvo_head,
 	 * Remove any existing mapping for this page.  Reuse the pvo entry if
 	 * there is a mapping.
 	 */
-	if (!recurse)
-		LOCK_TABLE();
+	LOCK_TABLE();
 
 	LIST_FOREACH(pvo, &moea64_pvo_table[ptegidx], pvo_olink) {
 		if (pvo->pvo_pmap == pm && PVO_VADDR(pvo) == va) {
 			if ((pvo->pvo_pte.lpte.pte_lo & LPTE_RPGN) == pa &&
 			    (pvo->pvo_pte.lpte.pte_lo & LPTE_PP) ==
 			    (pte_lo & LPTE_PP)) {
-				if (!recurse)
-					UNLOCK_TABLE();
+				UNLOCK_TABLE();
 				return (0);
 			}
 			moea64_pvo_remove(pvo, -1);
@@ -2041,12 +2052,19 @@ moea64_pvo_enter(pmap_t pm, uma_zone_t zone, struct pvo_head *pvo_head,
 		moea64_bpvo_pool_index++;
 		bootstrap = 1;
 	} else {
+		/*
+		 * Note: drop the table around the UMA allocation in
+		 * case the UMA allocator needs to manipulate the page
+		 * table. The mapping we are working with is already
+		 * protected by the PMAP lock.
+		 */
+		UNLOCK_TABLE();
 		pvo = uma_zalloc(zone, M_NOWAIT);
+		LOCK_TABLE();
 	}
 
 	if (pvo == NULL) {
-		if (!recurse)
-			UNLOCK_TABLE();
+		UNLOCK_TABLE();
 		return (ENOMEM);
 	}
 
@@ -2093,8 +2111,7 @@ moea64_pvo_enter(pmap_t pm, uma_zone_t zone, struct pvo_head *pvo_head,
 		moea64_pte_overflow++;
 	}
 
-	if (!recurse)
-		UNLOCK_TABLE();
+	UNLOCK_TABLE();
 
 	return (first ? ENOENT : 0);
 }
diff --git a/sys/powerpc/aim/mp_cpudep.c b/sys/powerpc/aim/mp_cpudep.c
index 1dc9525..52f8542 100644
--- a/sys/powerpc/aim/mp_cpudep.c
+++ b/sys/powerpc/aim/mp_cpudep.c
@@ -48,14 +48,34 @@ __FBSDID("$FreeBSD$");
 #include <dev/ofw/openfirm.h>
 #include <machine/ofw_machdep.h>
 
-extern void *rstcode;
-extern register_t l2cr_config;
-extern register_t l3cr_config;
-
 void *ap_pcpu;
 
+static register_t bsp_state[8];
+
+static void cpudep_save_config(void *dummy);
+SYSINIT(cpu_save_config, SI_SUB_CPU, SI_ORDER_ANY, cpudep_save_config, NULL);
+
+uintptr_t
+cpudep_ap_bootstrap(void)
+{
+	register_t msr, sp;
+
+	msr = PSL_KERNSET & ~PSL_EE;
+	mtmsr(msr);
+	isync();
+
+	__asm __volatile("mtsprg 0, %0" :: "r"(ap_pcpu));
+	powerpc_sync();
+
+	pcpup->pc_curthread = pcpup->pc_idlethread;
+	pcpup->pc_curpcb = pcpup->pc_curthread->td_pcb;
+	sp = pcpup->pc_curpcb->pcb_sp;
+
+	return (sp);
+}
+
 static register_t
-l2_enable(void)
+mpc745x_l2_enable(register_t l2cr_config)
 {
 	register_t ccr;
 
@@ -77,7 +97,7 @@ l2_enable(void)
 }
 
 static register_t
-l3_enable(void)
+mpc745x_l3_enable(register_t l3cr_config)
 {
 	register_t ccr;
 
@@ -109,7 +129,7 @@ l3_enable(void)
 }
 
 static register_t
-l1d_enable(void)
+mpc745x_l1d_enable(void)
 {
 	register_t hid;
 
@@ -127,7 +147,7 @@ l1d_enable(void)
 }
 
 static register_t
-l1i_enable(void)
+mpc745x_l1i_enable(void)
 {
 	register_t hid;
 
@@ -144,43 +164,110 @@ l1i_enable(void)
 	return (hid);
 }
 
-uint32_t
-cpudep_ap_bootstrap(void)
+static void
+cpudep_save_config(void *dummy)
 {
-	uint32_t hid, msr, reg, sp;
-
-	// reg = mfspr(SPR_MSSCR0);
-	// mtspr(SPR_MSSCR0, reg | 0x3);
-
-	__asm __volatile("mtsprg 0, %0" :: "r"(ap_pcpu));
-	powerpc_sync();
-
-	__asm __volatile("mtspr 1023,%0" :: "r"(PCPU_GET(cpuid)));
-	__asm __volatile("mfspr %0,1023" : "=r"(pcpup->pc_pir));
-
-	msr = PSL_FP | PSL_IR | PSL_DR | PSL_ME | PSL_RI;
-	powerpc_sync();
-	isync();
-	mtmsr(msr);
-	isync();
-
-	if (l3cr_config != 0)
-		reg = l3_enable();
-	if (l2cr_config != 0)
-		reg = l2_enable();
-	reg = l1d_enable();
-	reg = l1i_enable();
-
-	hid = mfspr(SPR_HID0);
-	hid &= ~(HID0_DOZE | HID0_SLEEP);
-	hid |= HID0_NAP | HID0_DPM;
-	mtspr(SPR_HID0, hid);
-	isync();
-
-	pcpup->pc_curthread = pcpup->pc_idlethread;
-	pcpup->pc_curpcb = pcpup->pc_curthread->td_pcb;
-	sp = pcpup->pc_curpcb->pcb_sp;
+	uint16_t	vers;
+
+	vers = mfpvr() >> 16;
+
+	switch(vers) {
+	case IBM970:
+	case IBM970FX:
+	case IBM970MP:
+		__asm __volatile ("mfspr %0,%2; mr %1,%0; srdi %0,%0,32"
+		    : "=r" (bsp_state[0]),"=r" (bsp_state[1]) : "K" (SPR_HID0));
+		__asm __volatile ("mfspr %0,%2; mr %1,%0; srdi %0,%0,32"
+		    : "=r" (bsp_state[2]),"=r" (bsp_state[3]) : "K" (SPR_HID1));
+		__asm __volatile ("mfspr %0,%2; mr %1,%0; srdi %0,%0,32"
+		    : "=r" (bsp_state[4]),"=r" (bsp_state[5]) : "K" (SPR_HID4));
+		__asm __volatile ("mfspr %0,%2; mr %1,%0; srdi %0,%0,32"
+		    : "=r" (bsp_state[6]),"=r" (bsp_state[7]) : "K" (SPR_HID5));
+
+		break;
+	case MPC7450:
+	case MPC7455:
+	case MPC7457:
+		/* Only MPC745x CPUs have an L3 cache. */
+		bsp_state[3] = mfspr(SPR_L3CR);
+
+		/* Fallthrough */
+	case MPC7400:
+	case MPC7410:
+	case MPC7447A:
+	case MPC7448:
+		bsp_state[2] = mfspr(SPR_L2CR);
+		bsp_state[1] = mfspr(SPR_HID1);
+		bsp_state[0] = mfspr(SPR_HID0);
+		break;
+	}
+}
 
-	return (sp);
+void
+cpudep_ap_setup()
+{ 
+	register_t	reg;
+	uint16_t	vers;
+
+	vers = mfpvr() >> 16;
+
+	switch(vers) {
+	case IBM970:
+	case IBM970FX:
+	case IBM970MP:
+		/* Set HIOR to 0 */
+		__asm __volatile("mtspr 311,%0" :: "r"(0));
+		powerpc_sync();
+
+		/*
+		 * The 970 has strange rules about how to update HID registers.
+		 * See Table 2-3, 970MP manual
+		 */
+
+		__asm __volatile(" \
+			ld	%0,0(%2);				\
+			mtspr	%1, %0;					\
+			mfspr	%0, %1;	mfspr	%0, %1;	mfspr	%0, %1;	\
+			mfspr	%0, %1;	mfspr	%0, %1;	mfspr	%0, %1;"
+		    : "=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;"
+		    : "=r"(reg) : "K"(SPR_HID4), "r"(bsp_state));
+		__asm __volatile("ld %0, 24(%2); sync; mtspr %1, %0; isync;"
+		    : "=r"(reg) : "K"(SPR_HID5), "r"(bsp_state));
+
+		powerpc_sync();
+		break;
+	case MPC7450:
+	case MPC7455:
+	case MPC7457:
+		/* Only MPC745x CPUs have an L3 cache. */
+		reg = mpc745x_l3_enable(bsp_state[3]);
+		
+		/* Fallthrough */
+	case MPC7400:
+	case MPC7410:
+	case MPC7447A:
+	case MPC7448:
+		/* XXX: Program the CPU ID into PIR */
+		__asm __volatile("mtspr 1023,%0" :: "r"(PCPU_GET(cpuid)));
+
+		powerpc_sync();
+		isync();
+
+		mtspr(SPR_HID0, bsp_state[0]); isync();
+		mtspr(SPR_HID1, bsp_state[1]); isync();
+
+		reg = mpc745x_l2_enable(bsp_state[2]);
+		reg = mpc745x_l1d_enable();
+		reg = mpc745x_l1i_enable();
+
+		break;
+	default:
+		printf("WARNING: Unknown CPU type. Cache performace may be "
+		    "suboptimal.\n");
+		break;
+	}
 }
 
diff --git a/sys/powerpc/aim/platform_chrp.c b/sys/powerpc/aim/platform_chrp.c
index 03955ab..2258c12 100644
--- a/sys/powerpc/aim/platform_chrp.c
+++ b/sys/powerpc/aim/platform_chrp.c
@@ -239,12 +239,14 @@ chrp_smp_start_cpu(platform_t plat, struct pcpu *pc)
 	rstvec = rstvec_virtbase + reset;
 
 	*rstvec = 4;
+	(void)(*rstvec);
 	powerpc_sync();
 	DELAY(1);
 	*rstvec = 0;
+	(void)(*rstvec);
 	powerpc_sync();
 
-	timeout = 1000;
+	timeout = 10000;
 	while (!pc->pc_awake && timeout--)
 		DELAY(100);