added multiprocessor support

4 files changed, 92 insertions, 21 deletions

diff --git a/sys/kern/entry.S b/sys/kern/entry.S
index 03ff76c..9bf72ea 100644
--- a/sys/kern/entry.S
+++ b/sys/kern/entry.S
@@ -1,8 +1,14 @@
 .section ".text.init"
 
+// entrypoint for boot hart(s). a0: hartid, a1: *fdt
 .globl _start
-_start:
-        /* disable interrupts and poging */ 
+
+
+// entrypoint for other harts. a0: hartid, a1: unused for now
+.global _mpentry 
+
+_start: 
+        /* disable interrupts and paging */ 
         csrw       satp, zero
         csrw       sie, zero
         csrw       sip, zero
@@ -12,7 +18,9 @@ _start:
         la         gp, __global_pointer$
         .option pop
 
-        /* setup stack */
+        /* setup stack. 
+	 * stack grows toward lower address so sp will mark end of frame.
+	 */
         addi       t0, a0, 1
         li         t1, PAGE_SIZE 
         la         sp, __stack_start
@@ -22,11 +30,11 @@ _start:
         /* setup thread pointer */ 
         mv        tp, a0
 
-        /* the label _boot_hart is shared between threads. only one hart will branch before it is no longer 0 */ 
+        /* the label _boot_hart is shared between threads. after the first atomic swap, all other harts will branch to mpentry */ 
         li         a2, 1
         lla        a3, _boot_hart
         amoswap.w  a3, a2, (a3)
-        bnez       a3,   2f
+        bnez       a3, _mpentry
 
         /* clear the bss section */  
         la         a2, __bss_start
@@ -39,9 +47,16 @@ _start:
         call       init
         j          _spin
 
+_mpentry:
+	/* satp = 0, sstatus.sie = 0, a0 = hartid, a1 = opaque	*/
+
+	call	 mpinit
+	/* shouldn't reach this point */
+	j 	_spin
+
 _spin:
 	wfi
-	j _spin
+	j 	_spin
 
 .section ".data"
 _boot_hart: 
@@ -51,3 +66,4 @@ _boot_hart:
 /* linker imports */
 .globl HEAP_START
 HEAP_START: .dword __heap_start
+
diff --git a/sys/kern/init.c b/sys/kern/init.c
index 208df6d..a58d3bc 100644
--- a/sys/kern/init.c
+++ b/sys/kern/init.c
@@ -1,12 +1,17 @@
 #include <fdt.h>
 #include <mm/kalloc.h>
 #include <printf.h>
-#include <stdint.h>
 #include <spinlock.h>
+#include <sbi.h>
+#include <stdint.h>
 
 extern uint64_t HEAP_START;
 
-void 
+#define HLT()\
+	for (;;)\
+		asm("wfi")
+
+void
 init(unsigned long hartid, struct fdt_header *fdt)
 {
 
@@ -22,14 +27,25 @@ init(unsigned long hartid, struct fdt_header *fdt)
         kalloc_init();
         printf("done!\n");
        // printf("printing free pages:\n");
-        //walkfree();
+       ///walkfree();
+       	printf("bringing up other harts...\n");
+	// todo: detect harts from device tree can also be used for more conservative stack allocation
+	for (int i = 0; i < NPROC; ++i) {
+		if (i == hartid) { 
+			printf("skipping hart #%d\n", i);
+			continue;
+		}
+		printf("starting hart #%d\n", i); 
+		struct sbiret r = _start_hart(i, (unsigned long)LOAD_ADDR);
+		if (r.err != SBI_SUCCESS)
+			printf("ERROR");
+	}
 }
 
-/* non boot harts enter here */ 
-void 
-mpinit(unsigned long hartid, struct fdt_header *fdt)
+void
+mpinit(unsigned long hartid)
 {
-        unsigned char *uart = (unsigned char*)0x10000000;
-        *uart = hartid + '0';
-        *(uart + 1) = '\n';
+	printf("mpinit: %d", hartid);
+	HLT();
 }
+
diff --git a/sys/kern/printf.c b/sys/kern/printf.c
index dfd0657..89add9e 100644
--- a/sys/kern/printf.c
+++ b/sys/kern/printf.c
@@ -1,10 +1,14 @@
 #include <printf.h>
 #include <stdint.h>
+#include <spinlock.h>
 #include <sbi.h>
 #include <stdarg.h>
 
 static char digits[] = "0123456789abcdef";
 
+struct spinlock mutex;
+int locked = 0;
+
 int
 puts(const char *str)
 {
@@ -52,9 +56,13 @@ void
 printf(const char *fmt, ...) 
 {
         va_list ap;
-        int i, c;
+        int i, c, _locked;
         char *s;
 
+	_locked = locked;
+	if (_locked)
+		acquire(&mutex);
+
         va_start(ap, fmt);
 
         for (i = 0; (c = fmt[i] & 0xff) != 0; i++) {
@@ -82,4 +90,12 @@ printf(const char *fmt, ...)
                                 break;
                 }
         }
+	if (_locked)
+		release(&mutex);
+}
+
+void printf_init(void)
+{
+	initlock(&mutex);
+	locked = 1;
 }
diff --git a/sys/kern/spinlock.c b/sys/kern/spinlock.c
index a35b8ce..ade270f 100644
--- a/sys/kern/spinlock.c
+++ b/sys/kern/spinlock.c
@@ -1,15 +1,38 @@
+#include <asm.h>
 #include <spinlock.h>
 
 void 
-init_locklock(struct spinlock *l, const char *_name)
+initlock(struct spinlock *l)
 {
-        l->name = _name;
         l->locked = 0;
-        l->cpu = 0;
 }
 
 void 
-acquire_lock(struct spinlock *l) 
+acquire(struct spinlock *l)
 {
-       asm volatile("csrr sie, zero");
+	sie_disable();
+
+	if (l->locked)
+		return;
+
+	while (__sync_lock_test_and_set(&l->locked, 1))
+			;
+
+	__sync_synchronize();
+}
+
+void 
+release(struct spinlock *l) 
+{
+	sie_disable(); // avoid deadlock
+
+	if (l->locked)
+		return;  // interrupts are still disabled, what to do here?
+
+	/* fence */
+	__sync_synchronize();
+
+	__sync_lock_release(&l->locked);
+
+	sie_enable();
 }