Better implement SMP support:

o  Do not use a special struct to keep track of CPUs we found;
   instead, use struct pcpu. This handles all the magic WRT
   thread creation (yay!).
o  Respect MAXCPU.
o  Use the vhpt_base and vhpt_size values to initialize the AP.
o  Style fixes.

Note that this commit temporarily breaks SMP configurations.
Previously APs didn't do anything, but they now enter the
scheduler. They hold sched_lock for more than 5 secs though
and cause a panic. That's what I call progress :-)

3 files changed, 180 insertions, 101 deletions

diff --git a/sys/ia64/ia64/locore.S b/sys/ia64/ia64/locore.S
index 544d192..0b79f91 100644
--- a/sys/ia64/ia64/locore.S
+++ b/sys/ia64/ia64/locore.S
@@ -145,7 +145,6 @@ ENTRY(mi_startup_trampoline, 0)
 END(mi_startup_trampoline)
 
 #ifdef SMP
-
 /*
  * AP wake-up entry point. The handoff state is similar as for the BSP,
  * as described on page 3-9 of the IPF SAL Specification. The difference
@@ -205,7 +204,7 @@ ENTRY(os_boot_rendez,0)
 	;;
 1:	mov	r16 = ip
 	add	r17 = 2f-1b, r17
-	movl	r18 = (IA64_PSR_AC|IA64_PSR_IC|IA64_PSR_DT|IA64_PSR_RT|IA64_PSR_IT|IA64_PSR_BN)
+	movl	r18 = (IA64_PSR_AC|IA64_PSR_DT|IA64_PSR_RT|IA64_PSR_IT|IA64_PSR_BN)
 	;;
 	add	r17 = r17, r16
 	mov	cr.ipsr = r18
@@ -217,29 +216,55 @@ ENTRY(os_boot_rendez,0)
 
 	.align	32
 2:	movl	r16 = ia64_vector_table			// set up IVT early
-	movl	r17 = ia64_vhpt+(1<<8)+(15<<2)+1	// and VHPT
 	;;
 	mov	cr.iva = r16
-	mov	cr.pta = r17
 	;;
 	srlz.i
 	;;
-	srlz.d
 	movl	r16 = ap_stack
+	movl	r17 = ap_pcpu
 	mov	ar.rsc = 0
 	movl	gp = __gp
 	;;
 	ld8	r16 = [r16]
-	mov	r17 = KSTACK_PAGES*PAGE_SIZE-SIZEOF_PCB-SIZEOF_TRAPFRAME-16
+	ld8	r17 = [r17]
+	mov	r18 = KSTACK_PAGES*PAGE_SIZE-SIZEOF_PCB-SIZEOF_TRAPFRAME-16
 	;;
-	add	sp = r17, r16
+	add	sp = r18, r16
 	mov	ar.bspstore = r16
+	mov	ar.k4 = r17
+	mov	r13 = r17	/* gas doesn't know tp as an alias for r13 */
 	;;
 	loadrs
+	movl	r16 = ia64_pal_base
 	;;
 	mov	ar.rsc = 3
+	ld8	r16 = [r16]
+	;;
+	cmp.eq	p1, p0 = 0, r16
+(p1)	br.cond.spnt	1f
+	;;
+	mov	r18 = 28<<2
+	movl	r17 = 7<<61
+	;;
+	mov	cr.itir = r18
+	or	r17 = r17, r16
+	mov	r16 = (PTE_P|PTE_MA_WB|PTE_A|PTE_D|PTE_PL_KERN|PTE_AR_RWX)
+	;;
+	mov	cr.ifa = r17
+	extr.u  r18 = r17, 12, 38
+	;;
+	srlz.i
+	shl	r18 = r18, 12
+	;;
+	add	r17 = 1, r0
+	or	r16 = r16, r18
+	;;
+	itr.i	itr[r17] = r16
+	;;
+	srlz.i
 	;;
-	alloc	r16 = ar.pfs, 0, 0, 0, 0
+1:	alloc	r16 = ar.pfs, 0, 0, 0, 0
 	;;
 	br.call.sptk.few rp = ia64_ap_startup
 	/* NOT REACHED */
diff --git a/sys/ia64/ia64/locore.s b/sys/ia64/ia64/locore.s
index 544d192..0b79f91 100644
--- a/sys/ia64/ia64/locore.s
+++ b/sys/ia64/ia64/locore.s
@@ -145,7 +145,6 @@ ENTRY(mi_startup_trampoline, 0)
 END(mi_startup_trampoline)
 
 #ifdef SMP
-
 /*
  * AP wake-up entry point. The handoff state is similar as for the BSP,
  * as described on page 3-9 of the IPF SAL Specification. The difference
@@ -205,7 +204,7 @@ ENTRY(os_boot_rendez,0)
 	;;
 1:	mov	r16 = ip
 	add	r17 = 2f-1b, r17
-	movl	r18 = (IA64_PSR_AC|IA64_PSR_IC|IA64_PSR_DT|IA64_PSR_RT|IA64_PSR_IT|IA64_PSR_BN)
+	movl	r18 = (IA64_PSR_AC|IA64_PSR_DT|IA64_PSR_RT|IA64_PSR_IT|IA64_PSR_BN)
 	;;
 	add	r17 = r17, r16
 	mov	cr.ipsr = r18
@@ -217,29 +216,55 @@ ENTRY(os_boot_rendez,0)
 
 	.align	32
 2:	movl	r16 = ia64_vector_table			// set up IVT early
-	movl	r17 = ia64_vhpt+(1<<8)+(15<<2)+1	// and VHPT
 	;;
 	mov	cr.iva = r16
-	mov	cr.pta = r17
 	;;
 	srlz.i
 	;;
-	srlz.d
 	movl	r16 = ap_stack
+	movl	r17 = ap_pcpu
 	mov	ar.rsc = 0
 	movl	gp = __gp
 	;;
 	ld8	r16 = [r16]
-	mov	r17 = KSTACK_PAGES*PAGE_SIZE-SIZEOF_PCB-SIZEOF_TRAPFRAME-16
+	ld8	r17 = [r17]
+	mov	r18 = KSTACK_PAGES*PAGE_SIZE-SIZEOF_PCB-SIZEOF_TRAPFRAME-16
 	;;
-	add	sp = r17, r16
+	add	sp = r18, r16
 	mov	ar.bspstore = r16
+	mov	ar.k4 = r17
+	mov	r13 = r17	/* gas doesn't know tp as an alias for r13 */
 	;;
 	loadrs
+	movl	r16 = ia64_pal_base
 	;;
 	mov	ar.rsc = 3
+	ld8	r16 = [r16]
+	;;
+	cmp.eq	p1, p0 = 0, r16
+(p1)	br.cond.spnt	1f
+	;;
+	mov	r18 = 28<<2
+	movl	r17 = 7<<61
+	;;
+	mov	cr.itir = r18
+	or	r17 = r17, r16
+	mov	r16 = (PTE_P|PTE_MA_WB|PTE_A|PTE_D|PTE_PL_KERN|PTE_AR_RWX)
+	;;
+	mov	cr.ifa = r17
+	extr.u  r18 = r17, 12, 38
+	;;
+	srlz.i
+	shl	r18 = r18, 12
+	;;
+	add	r17 = 1, r0
+	or	r16 = r16, r18
+	;;
+	itr.i	itr[r17] = r16
+	;;
+	srlz.i
 	;;
-	alloc	r16 = ar.pfs, 0, 0, 0, 0
+1:	alloc	r16 = ar.pfs, 0, 0, 0, 0
 	;;
 	br.call.sptk.few rp = ia64_ap_startup
 	/* NOT REACHED */
diff --git a/sys/ia64/ia64/mp_machdep.c b/sys/ia64/ia64/mp_machdep.c
index 3fa8b2b..18f5b7b 100644
--- a/sys/ia64/ia64/mp_machdep.c
+++ b/sys/ia64/ia64/mp_machdep.c
@@ -40,6 +40,8 @@
 
 #include <vm/vm.h>
 #include <vm/pmap.h>
+#include <vm/vm_extern.h>
+#include <vm/vm_kern.h>
 
 #include <machine/atomic.h>
 #include <machine/pal.h>
@@ -47,57 +49,52 @@
 #include <machine/clock.h>
 #include <machine/sal.h>
 
-#define	LID_SAPIC_ID(x)		((int)((x) >> 24) & 0xff)
-#define	LID_SAPIC_EID(x)	((int)((x) >> 16) & 0xff)
-
-static MALLOC_DEFINE(M_SMP, "smp", "SMP structures");
+void cpu_mp_add(uint, uint, uint);
+void ia64_ap_startup(void);
+void map_pal_code(void);
 
-static void ipi_send(u_int64_t, int);
-static void cpu_mp_unleash(void *);
+extern vm_offset_t vhpt_base, vhpt_size;
 
-struct mp_cpu {
-	TAILQ_ENTRY(mp_cpu) cpu_next;
-	u_int64_t	cpu_lid;	/* Local processor ID */
-	int32_t		cpu_no;		/* Sequential CPU number */
-	u_int32_t	cpu_bsp:1;	/* 1=BSP; 0=AP */
-	u_int32_t	cpu_awake:1;	/* 1=Awake; 0=sleeping */
-	void		*cpu_stack;
-};
+#define	LID_SAPIC_ID(x)		((int)((x) >> 24) & 0xff)
+#define	LID_SAPIC_EID(x)	((int)((x) >> 16) & 0xff)
+#define	LID_SAPIC_SET(id,eid)	(((id & 0xff) << 8 | (eid & 0xff)) << 16);
+#define	LID_SAPIC_MASK		0xffff0000UL
 
 int	mp_hardware = 0;
 int	mp_ipi_vector[IPI_COUNT];
 int	mp_ipi_test = 0;
 
 /* Variables used by os_boot_rendez */
-volatile void *ap_stack;
+volatile vm_offset_t ap_stack;
+volatile struct pcpu *ap_pcpu;
 volatile int ap_delay;
 volatile int ap_awake;
 
-TAILQ_HEAD(, mp_cpu) ia64_cpus = TAILQ_HEAD_INITIALIZER(ia64_cpus);
+static void ipi_send(u_int64_t, int);
+static void cpu_mp_unleash(void *);
 
 void
 ia64_ap_startup(void)
 {
-#if 0
-	struct mp_cpu *cpu;
-	u_int64_t lid = ia64_get_lid() & 0xffff0000L;
-#endif
+	__asm __volatile("mov cr.pta=%0;; srlz.i;;" ::
+	    "r" (vhpt_base + (1<<8) + (vhpt_size<<2) + 1));
 
 	ap_awake = 1;
 	ap_delay = 0;
-	while (1);
 
-#if 0
-	TAILQ_FOREACH(cpu, &ia64_cpus, cpu_next) {
-		if (cpu->cpu_lid == lid)
-			break;
-	}
+	/* Wait until it's time for us to be unleashed */
+	while (!smp_started);
 
-	KASSERT(cpu != NULL, ("foo!"));
+	CTR1(KTR_SMP, "SMP: cpu%d launched", PCPU_GET(cpuid));
 
-	cpu->cpu_lid = ia64_get_lid();
-	cpu->cpu_awake = 1;
-#endif
+	__asm __volatile("ssm psr.ic|psr.i;; srlz.i;;");
+
+	microuptime(PCPU_PTR(switchtime));
+	PCPU_SET(switchticks, ticks);
+
+	mtx_lock_spin(&sched_lock);
+	cpu_throw();
+	panic(__func__ ": cpu_throw() returned");
 }
 
 int
@@ -113,66 +110,90 @@ cpu_mp_probe()
 void
 cpu_mp_add(uint acpiid, uint apicid, uint apiceid)
 {
-	struct mp_cpu *cpu;
-	u_int64_t bsp = ia64_get_lid() & 0xffff0000L;
+	struct pcpu *pc;
+	u_int64_t lid;
 
-	cpu = malloc(sizeof(*cpu), M_SMP, M_WAITOK|M_ZERO);
-	if (cpu == NULL)
+	/* Count all CPUs, even the ones we cannot use */
+	mp_ncpus++;
+
+	/* Ignore any processor numbers outside our range */
+	if (acpiid >= MAXCPU) {
+		printf("SMP: cpu%d skipped; increase MAXCPU\n", acpiid);
 		return;
+	}
 
-	TAILQ_INSERT_TAIL(&ia64_cpus, cpu, cpu_next);
-	cpu->cpu_no = acpiid;
-	cpu->cpu_lid = ((apicid & 0xff) << 8 | (apiceid & 0xff)) << 16;
-	if (cpu->cpu_lid == bsp)
-		cpu->cpu_bsp = 1;
-	all_cpus |= (1 << acpiid);
-	mp_ncpus++;
+	KASSERT((all_cpus & (1UL << acpiid)) == 0,
+	    (__func__ ": cpu%d already in CPU map", acpiid));
+
+	lid = LID_SAPIC_SET(apicid, apiceid);
+
+	if ((ia64_get_lid() & LID_SAPIC_MASK) == lid) {
+		KASSERT(acpiid == 0,
+		    (__func__ ": the BSP must be cpu0"));
+	}
+
+	if (acpiid != 0) {
+		pc = (struct pcpu *)kmem_alloc(kernel_map, PAGE_SIZE);
+		pcpu_init(pc, acpiid, PAGE_SIZE);
+	} else
+		pc = pcpup;
+
+	pc->pc_lid = lid;
+	all_cpus |= (1UL << acpiid);
 }
 
 void
 cpu_mp_announce()
 {
-	struct mp_cpu *cpu;
-
-	TAILQ_FOREACH(cpu, &ia64_cpus, cpu_next) {
-		printf("cpu%d: SAPIC Id=%x, SAPIC Eid=%x", cpu->cpu_no,
-		    LID_SAPIC_ID(cpu->cpu_lid), LID_SAPIC_EID(cpu->cpu_lid));
-		if (cpu->cpu_bsp)
-			printf(" (BSP)\n");
-		else
-			printf("\n");
+	struct pcpu *pc;
+	int i;
+
+	for (i = 0; i < MAXCPU; i++) {
+		pc = pcpu_find(i);
+		if (pc != NULL) {
+			printf("cpu%d: SAPIC Id=%x, SAPIC Eid=%x", i,
+			    LID_SAPIC_ID(pc->pc_lid),
+			    LID_SAPIC_EID(pc->pc_lid));
+			if (i == 0)
+				printf(" (BSP)\n");
+			else
+				printf("\n");
+		}
 	}
 }
 
 void
 cpu_mp_start()
 {
-	struct mp_cpu *cpu;
+	struct pcpu *pc;
 
-	TAILQ_FOREACH(cpu, &ia64_cpus, cpu_next) {
-		if (!cpu->cpu_bsp) {
-			cpu->cpu_stack = malloc(KSTACK_PAGES * PAGE_SIZE,
-			    M_SMP, M_WAITOK);
+	SLIST_FOREACH(pc, &cpuhead, pc_allcpu) {
+#if 0
+		pc->pc_current_pmap = PCPU_GET(current_pmap);
+#endif
+		pc->pc_other_cpus = all_cpus & ~(1UL << pc->pc_cpuid);
+		if (pc->pc_cpuid > 0) {
+			ap_stack = kmem_alloc(kernel_map,
+			    KSTACK_PAGES * PAGE_SIZE);
+			ap_pcpu = pc;
+			ap_delay = 2000;
+			ap_awake = 0;
 
 			if (bootverbose)
-				printf("SMP: waking up cpu%d\n", cpu->cpu_no);
+				printf("SMP: waking up cpu%d\n", pc->pc_cpuid);
 
-			ap_stack = cpu->cpu_stack;
-			ap_delay = 2000;
-			ap_awake = 0;
-			ipi_send(cpu->cpu_lid, IPI_AP_WAKEUP);
+			ipi_send(pc->pc_lid, IPI_AP_WAKEUP);
 
 			do {
 				DELAY(1000);
 			} while (--ap_delay > 0);
-			cpu->cpu_awake = (ap_awake) ? 1 : 0;
+			pc->pc_awake = ap_awake;
 
-			if (bootverbose && !ap_awake)
+			if (!ap_awake)
 				printf("SMP: WARNING: cpu%d did not wake up\n",
-				    cpu->cpu_no);
+				    pc->pc_cpuid);
 		} else {
-			cpu->cpu_lid = ia64_get_lid();
-			cpu->cpu_awake = 1;
+			pc->pc_awake = 1;
 			ipi_self(IPI_TEST);
 		}
 	}
@@ -181,21 +202,30 @@ cpu_mp_start()
 static void
 cpu_mp_unleash(void *dummy)
 {
-	struct mp_cpu *cpu;
-	int awake = 0;
+	struct pcpu *pc;
+	int cpus;
 
 	if (!mp_hardware)
 		return;
 
 	if (mp_ipi_test != 1)
-		printf("SMP: sending of a test IPI to BSP failed\n");
+		printf("SMP: WARNING: sending of a test IPI failed\n");
+
+	cpus = 0;
+	smp_cpus = 0;
+	SLIST_FOREACH(pc, &cpuhead, pc_allcpu) {
+		cpus++;
+		if (pc->pc_awake)
+			smp_cpus++;
+	}
 
-	TAILQ_FOREACH(cpu, &ia64_cpus, cpu_next) {
-		awake += cpu->cpu_awake;
+	if (smp_cpus != cpus || cpus != mp_ncpus) {
+		printf("SMP: %d CPUs found; %d CPUs usable; %d CPUs woken\n",
+		    mp_ncpus, cpus, smp_cpus);
 	}
 
-	if (awake != mp_ncpus)
-		printf("SMP: %d CPU(s) didn't get woken\n", mp_ncpus - awake);
+	smp_active = 1;
+	smp_started = 1;
 }
 
 /*
@@ -204,11 +234,11 @@ cpu_mp_unleash(void *dummy)
 void
 ipi_selected(u_int64_t cpus, int ipi)
 {
-	struct mp_cpu *cpu;
+	struct pcpu *pc;
 
-	TAILQ_FOREACH(cpu, &ia64_cpus, cpu_next) {
-		if (cpus & (1 << cpu->cpu_no))
-			ipi_send(cpu->cpu_lid, ipi);
+	SLIST_FOREACH(pc, &cpuhead, pc_allcpu) {
+		if (cpus & (1UL << pc->pc_cpuid))
+			ipi_send(pc->pc_lid, ipi);
 	}
 }
 
@@ -218,10 +248,10 @@ ipi_selected(u_int64_t cpus, int ipi)
 void
 ipi_all(int ipi)
 {
-	struct mp_cpu *cpu;
+	struct pcpu *pc;
 
-	TAILQ_FOREACH(cpu, &ia64_cpus, cpu_next) {
-		ipi_send(cpu->cpu_lid, ipi);
+	SLIST_FOREACH(pc, &cpuhead, pc_allcpu) {
+		ipi_send(pc->pc_lid, ipi);
 	}
 }
 
@@ -231,12 +261,11 @@ ipi_all(int ipi)
 void
 ipi_all_but_self(int ipi)
 {
-	struct mp_cpu *cpu;
-	u_int64_t lid = ia64_get_lid();
+	struct pcpu *pc;
 
-	TAILQ_FOREACH(cpu, &ia64_cpus, cpu_next) {
-		if (cpu->cpu_lid != lid)
-			ipi_send(cpu->cpu_lid, ipi);
+	SLIST_FOREACH(pc, &cpuhead, pc_allcpu) {
+		if (pc != pcpup)
+			ipi_send(pc->pc_lid, ipi);
 	}
 }
 
@@ -262,7 +291,7 @@ ipi_send(u_int64_t lid, int ipi)
 	u_int64_t vector;
 
 	pipi = ia64_memory_address(PAL_PIB_DEFAULT_ADDR |
-	    ((lid >> 12) & 0xFFFF0L));
+	    ((lid & LID_SAPIC_MASK) >> 12));
 	vector = (u_int64_t)(mp_ipi_vector[ipi] & 0xff);
 	*pipi = vector;
 	ia64_mf_a();