Add smp support

Signed-off-by: Richard Henderson <rth@twiddle.net>

4 files changed, 88 insertions, 45 deletions

diff --git a/Makefile b/Makefile
index 2025599..ec3c55f 100644
--- a/Makefile
+++ b/Makefile
@@ -27,7 +27,7 @@ clean:
 	rm -f palcode-*
 
 pal.o: pal.S osf.h sys-$(SYSTEM).h core-$(CORE).h
-init.o: init.c hwrpb.h osf.h uart.h sys-$(SYSTEM).h core-$(CORE).h
+init.o: init.c protos.h hwrpb.h osf.h uart.h sys-$(SYSTEM).h core-$(CORE).h
 printf.o: printf.c uart.h
 uart.o: uart.c uart.h protos.h
 crb.o: crb.c hwrpb.h protos.h console.h uart.h
diff --git a/init.c b/init.c
index 324bc91..e7b506f 100644
--- a/init.c
+++ b/init.c
@@ -40,7 +40,7 @@
 
 struct hwrpb_combine {
   struct hwrpb_struct hwrpb;
-  struct percpu_struct processor;
+  struct percpu_struct processor[4];
   struct memdesc_struct md;
   struct memclust_struct mc[2];
   struct crb_struct crb;
@@ -138,10 +138,11 @@ init_page_table(void)
 }
 
 static void
-init_hwrpb (unsigned long memsize)
+init_hwrpb (unsigned long memsize, unsigned long cpus)
 {
   unsigned long pal_pages;
   unsigned long amask;
+  unsigned long i;
   
   hwrpb.hwrpb.phys_addr = PA(&hwrpb);
 
@@ -186,14 +187,19 @@ init_hwrpb (unsigned long memsize)
   hwrpb.hwrpb.sys_type = SYS_TYPE;
   hwrpb.hwrpb.sys_variation = SYS_VARIATION;
   hwrpb.hwrpb.sys_revision = SYS_REVISION;
-  hwrpb.processor.type = hwrpb.hwrpb.cpuid;
+  for (i = 0; i < cpus; ++i)
+    {
+      /* ??? Look up these bits.  Snagging the value examined by the kernel. */
+      hwrpb.processor[i].flags = 0x1cc;
+      hwrpb.processor[i].type = hwrpb.hwrpb.cpuid;
+    }
 
   hwrpb.hwrpb.intr_freq = HZ * 4096;
   hwrpb.hwrpb.cycle_freq = 250000000;	/* QEMU architects 250MHz.  */
 
   hwrpb.hwrpb.vptb = VPTPTR;
 
-  hwrpb.hwrpb.nr_processors = 1;
+  hwrpb.hwrpb.nr_processors = cpus;
   hwrpb.hwrpb.processor_size = sizeof(struct percpu_struct);
   hwrpb.hwrpb.processor_offset = offsetof(struct hwrpb_combine, processor);
 
@@ -268,13 +274,25 @@ init_i8259 (void)
   outb(0x20, PORT_PIC1_CMD);
 }
 
+static void __attribute__((noreturn))
+swppal(void *entry, void *pcb)
+{
+  register int variant __asm__("$16") = 2;	/* OSF/1 PALcode */
+  register void *pc __asm__("$17") = entry;
+  register unsigned long pa_pcb __asm__("$18") = PA(pcb);
+  register unsigned long vptptr __asm__("$19") = VPTPTR;
+
+  asm("call_pal 0x0a" : : "r"(variant), "r"(pc), "r"(pa_pcb), "r"(vptptr));
+  __builtin_unreachable ();
+}
+
 void
-do_start(unsigned long memsize, void (*kernel_entry)(void), long cpus)
+do_start(unsigned long memsize, void (*kernel_entry)(void), unsigned long cpus)
 {
   last_alloc = _end;
 
   init_page_table();
-  init_hwrpb(memsize);
+  init_hwrpb(memsize, cpus);
   init_pcb();
   init_i8259();
   uart_init();
@@ -282,29 +300,38 @@ do_start(unsigned long memsize, void (*kernel_entry)(void), long cpus)
   pci_setup();
   vgahw_init();
 
-  {
-    register int variant __asm__("$16") = 2;	/* OSF/1 PALcode */
-    register void (*pc)(void) __asm__("$17");
-    register unsigned long pa_pcb __asm__("$18");
-    register unsigned long vptptr __asm__("$19");
-
-    pc = (kernel_entry ? kernel_entry : do_console);
-    pa_pcb = PA(&pcb);
-    vptptr = VPTPTR;
-    asm("call_pal 0x0a" : : "r"(variant), "r"(pc), "r"(pa_pcb), "r"(vptptr));
-  }
-  __builtin_unreachable ();
+  swppal(kernel_entry, &pcb);
 }
 
 void
-do_start_wait(void)
+do_start_wait(unsigned long cpuid)
 {
   while (1)
     {
-      // WtInt with interrupts off.  Rely on the fact that QEMU will
-      // un-halt the CPU when an interrupt arrives.
-      asm("lda $16,-1\n\tcall_pal 0x3e" : : : "$0", "$16");
-
-      // FIXME do something with the IPI.
+      /* Wait 1ms for the kernel to wake us.  */
+      ndelay(1000000);
+
+      if (hwrpb.hwrpb.rxrdy & (1ull << cpuid))
+	{
+	  /* ??? The only message I know of is "START\r\n".
+	     I can't be bothered to verify more than 4 characters.  */
+	  /* ??? The Linux kernel fills in, but does not require,
+	     CPU_restart_data.  It just sets that to the same address
+	     as CPU_restart itself.  Our swppal *does* put the PC into
+	     $26 and $27, the latter of which the kernel does rely upon.  */
+
+	  unsigned int len = hwrpb.processor[cpuid].ipc_buffer[0];
+	  unsigned int msg = hwrpb.processor[cpuid].ipc_buffer[1];
+	  void *CPU_restart = hwrpb.hwrpb.CPU_restart;
+	  __sync_synchronize();
+	  hwrpb.hwrpb.rxrdy = 0;
+
+          if (len == 7 && msg == ('S' | 'T' << 8 | 'A' << 16 | 'R' << 24))
+	    {
+	      /* Set bootstrap in progress */
+	      hwrpb.processor[cpuid].flags |= 1;
+	      swppal(CPU_restart, hwrpb.processor[cpuid].hwpcb);
+	    }
+	}
     }
 }
diff --git a/pal.S b/pal.S
index 1befc9f..1781c4b 100644
--- a/pal.S
+++ b/pal.S
@@ -97,9 +97,9 @@ __start:
 	bne	a0, 1f
 
 	// Load boot arguments
-	mfpr	a0, qemu_trap_arg0
-	mfpr	a1, qemu_trap_arg1
-	mfpr	a2, qemu_trap_arg2
+	mfpr	a0, qemu_trap_arg0		// memsize
+	mfpr	a1, qemu_trap_arg1		// kernel entry
+	mfpr	a2, qemu_trap_arg2		// ncpus
 
 	// Continue in do_start, outside PALmode.
 	ldah	$27, do_start($gp)		!gprelhigh
@@ -400,10 +400,25 @@ CallPal_Draina:
 	hw_rei
 ENDFN	CallPal_Draina
 
+/*
+ * Delay for N nanoseconds.
+ *
+ * This is unique to QEMU, used only within PALcode itself.
+ */
 	ORG_CALL_PAL_PRIV(0x03)
-CallPal_OpcDec03:
-	br	CallPal_OpcDec
-ENDFN	CallPal_OpcDec03
+CallPal_Ndelay:
+	mfpr	p0, qemu_vmtime
+	addq	p0, a0, p0
+	mtpr	p0, qemu_alarm
+
+	mtpr	$31, qemu_wait
+
+	SYS_ACK_CLK p2, p3, p4
+
+	mfpr	v0, qemu_vmtime
+	subq	p0, v0, v0
+	hw_rei
+ENDFN	CallPal_Ndelay
 
 	ORG_CALL_PAL_PRIV(0x04)
 CallPal_OpcDec04:
@@ -1416,6 +1431,7 @@ ENDFN	CallPal_OpcDec85
  */ 
         ORG_CALL_PAL_UNPRIV(0x86)
 CallPal_Imb:
+	mb
 	hw_rei
 ENDFN	CallPal_Imb
 
@@ -1914,5 +1930,5 @@ Sys_EnterConsole:
 	.align 3
 	.globl	stack
 	.type	stack,@object
-	.size	stack,STACK_SIZE
-stack:	.skip	STACK_SIZE
+	.size	stack,STACK_SIZE * 4
+stack:	.skip	STACK_SIZE * 4
diff --git a/util.c b/util.c
index 1444dd6..0e943be 100644
--- a/util.c
+++ b/util.c
@@ -20,20 +20,20 @@
 
 #include "protos.h"
 
+static inline long
+ndelay_with_int(unsigned long nsec)
+{
+  register long a0 __asm__("16") = nsec;
+  register long v0 __asm__("0");
+  asm volatile ("call_pal 3" : "=r"(v0) : "r"(a0));
+  return v0;
+}
 
 void
 ndelay(unsigned long nsec)
 {
-  unsigned long target, now;
-
-  /* ??? Fix race between setting an alarm and waiting for an interrupt,
-     so that we can use wtint here.  This isn't used much except for 
-     during startup, so it probably doesn't matter much.  */
-
-  now = get_wall_time();
-  target = now + nsec;
-
-  do
-    now = get_wall_time();
-  while (now < target);
+  long left = nsec;
+  do {
+    left = ndelay_with_int(left);
+  } while (left > 0);
 }