smp now seems to work

+ Had some minor issues with a wraparound of size_t that effectively
  meant that some regions were allocated twice. Also, booting should be
  a bit more reliable now, turned out that the previous iteration of the
  booting was just accidentally working due to the kernel being placed
  'close enough' in RAM to where it was linked to. Fixed by allocating a
  vmem of O1 that maps the kernel to a 2MiB boundary at boot, pretty
  nifty.

9 files changed, 120 insertions, 49 deletions

diff --git a/arch/riscv64/conf/init.c b/arch/riscv64/conf/init.c
index 6b1ce46..c61848e 100644
--- a/arch/riscv64/conf/init.c
+++ b/arch/riscv64/conf/init.c
@@ -274,24 +274,9 @@ static void *sys_req_sharedmem(long tid, unsigned long size, void **cbuf)
 static char *rw_buf = 0;
 static size_t rw_buf_size = 4096;
 
-void callback(long pid, long tid, long d0, long d1, long d2, long d3)
+static void sys_sleep()
 {
-	(void)tid;
-	if (d0 == 1) {
-		void *cbuf = 0;
-		rw_buf = sys_req_sharedmem(pid, rw_buf_size, &cbuf);
-		sys_ipc_resp((long)cbuf, rw_buf_size, 0, 0);
-		__builtin_unreachable();
-
-	} else if (d0 == 2) {
-		puts("Received string: ");
-		puts(rw_buf);
-		sys_ipc_resp(0, 0, 0, 0);
-		__builtin_unreachable();
-	}
-
-	sys_ipc_resp(0, 0, 0, 0);
-	__builtin_unreachable();
+	ecall1(SYS_SLEEP);
 }
 
 #define CSR_TIME "0xc01"
@@ -300,8 +285,19 @@ void callback(long pid, long tid, long d0, long d1, long d2, long d3)
 
 #define CSR_CYCLE "0xc00"
 
-void _start()
+static void handle_kernel(long a0, long a1, long d0, long d1, long d2, long d3)
 {
+	if (a1 != SYS_USER_BOOTED)
+		return;
+
+	long tid = d0;
+	if (tid != 1) {
+		puts("Woo, more cores!\n");
+		while (1)
+			sys_sleep();
+	}
+
+	/* otherwise */
 	sys_noop();
 	puts("Hello, world!\n");
 
@@ -322,9 +318,6 @@ void _start()
 	print_value("Syscalls per second", n);
 	print_value("Executed cycles per second", cend - cstart);
 
-	puts("Setting callback...");
-	sys_ipc_server(callback);
-
 	puts("Starting fork():\n");
 	long pid = sys_fork();
 	if (pid != 0) {
@@ -387,3 +380,28 @@ void _start()
 
 	sys_poweroff(0);
 }
+
+void _start(long a0, long a1, long d0, long d1, long d2, long d3)
+{
+	if (a0 == 0)
+		handle_kernel(a0, a1, d0, d1, d2, d3);
+
+	/* otherwise, implement test functionality */
+	long pid = a0;
+	long tid = a1;
+	if (d0 == 1) {
+		void *cbuf = 0;
+		rw_buf = sys_req_sharedmem(pid, rw_buf_size, &cbuf);
+		sys_ipc_resp((long)cbuf, rw_buf_size, 0, 0);
+		__builtin_unreachable();
+
+	} else if (d0 == 2) {
+		puts("Received string: ");
+		puts(rw_buf);
+		sys_ipc_resp(0, 0, 0, 0);
+		__builtin_unreachable();
+	}
+
+	sys_ipc_resp(0, 0, 0, 0);
+	__builtin_unreachable();
+}
diff --git a/arch/riscv64/conf/kernel-link.S b/arch/riscv64/conf/kernel-link.S
index 832a886..88f2e75 100644
--- a/arch/riscv64/conf/kernel-link.S
+++ b/arch/riscv64/conf/kernel-link.S
@@ -11,7 +11,7 @@ SECTIONS {
 	 * hack.
 	 * @todo look into this further.
 	 */
-	. = VM_DMAP;
+	. = ABSOLUTE(VM_KERNEL);
 	__kernel_start = .;
 	.text ALIGN(4K) : AT(0) {
 		*(.kernel.start);
diff --git a/arch/riscv64/config.h b/arch/riscv64/config.h
index 2c513a5..952c0ca 100644
--- a/arch/riscv64/config.h
+++ b/arch/riscv64/config.h
@@ -32,6 +32,12 @@
 /** Direct map offset. */
 #define VM_DMAP (0xffffffc000000000)         /* testing for now */
 
+/** Page reserved for kernel mapping in O1 */
+#define KERNEL_PAGE 510UL
+
+/** Kernel virtual address */
+#define VM_KERNEL (0xffffffff80000000)
+
 /** Page reserved for kernel I/O. */
 #define IO_PAGE 511UL
 
diff --git a/arch/riscv64/kernel/core_bringup.S b/arch/riscv64/kernel/core_bringup.S
index 83b017c..8a5a6f5 100644
--- a/arch/riscv64/kernel/core_bringup.S
+++ b/arch/riscv64/kernel/core_bringup.S
@@ -12,12 +12,17 @@ riscv_bringup:
 	sfence.vma
 
 	/* fetch the stack allocated to us at our hart index in smp_init_stacks */
-	lla t0, smp_init_stacks
+	lui t0, %hi(smp_init_stacks)
+	addi t0, t0, %lo(smp_init_stacks)
 	slli t1, a0, RW_SHIFT
 	add t0, t0, t1
 	lr sp, 0(t0)
 	mv tp, sp
 
+	/* calculate actual address in kernelspace where we should jump to */
+	/* a0 has RAM base */
+	lui t0, %hi(core_bringup)
+	addi t0, t0, %lo(core_bringup)
+
 	/* jump to C to handle rest of bringup */
-	lla t0, core_bringup
 	jr t0
diff --git a/arch/riscv64/kernel/entry.S b/arch/riscv64/kernel/entry.S
index e37d5cc..6096844 100644
--- a/arch/riscv64/kernel/entry.S
+++ b/arch/riscv64/kernel/entry.S
@@ -130,7 +130,7 @@ fast_dispatch:
 	csrr    t5, CSR_SEPC
 	sr	t5, offsetof_exec(tp)
 	/* jump to C */
-	jal	dispatch
+	call	dispatch
 	/* if we had a thread switch, load kernel stack of current thread and
 	 * restore its context */
 	/* get associated kernel stack */
diff --git a/arch/riscv64/kernel/proc.c b/arch/riscv64/kernel/proc.c
index a4746b4..af4b2f3 100644
--- a/arch/riscv64/kernel/proc.c
+++ b/arch/riscv64/kernel/proc.c
@@ -18,9 +18,8 @@
 
 void run_init(struct tcb *t, vm_t fdt, vm_t initrd)
 {
-	/** \todo actually map fdt and initrd into the target address space */
 	csr_write(CSR_SSCRATCH, t);
-	csr_write(CSR_SEPC, t->exec);
+	csr_write(CSR_SEPC, t->callback);
 	/* gcc gives a warning 'the value of the stack pointer after an asm
 	 * statement must be the same as it was before the statement', so this
 	 * is technically speaking undefined behavior, I think.
@@ -31,13 +30,16 @@ void run_init(struct tcb *t, vm_t fdt, vm_t initrd)
 	vm_t stack_top = t->thread_stack + t->thread_stack_size;
 	bkl_unlock();
 	__asm__ volatile ("mv sp, %0\n"
-	                  "mv a0, %1\n"
-	                  "mv a1, %2\n"
+	                  "li a0, %1\n"
+	                  "li a1, %2\n"
 	                  "mv a2, %3\n"
+			  "mv a3, %4\n"
+			  "mv a4, %5\n"
 	                  "sret\n"
 	                  :
-	                  : "r" (stack_top), "r" (t->tid), "r" (fdt),
-	                  "r" (initrd)
+	                  : "r" (stack_top),
+				"K"(0), "K"(SYS_USER_BOOTED),
+				"r" (t->tid), "r" (fdt), "r" (initrd)
 	                  : "memory");
 	/* we should never reach this */
 	unreachable();
diff --git a/arch/riscv64/kernel/smp.c b/arch/riscv64/kernel/smp.c
index 74cc930..a878b90 100644
--- a/arch/riscv64/kernel/smp.c
+++ b/arch/riscv64/kernel/smp.c
@@ -7,9 +7,13 @@
  */
 
 #include <kmi/debug.h>
-#include <arch/smp.h>
+#include <kmi/bkl.h>
 #include <kmi/tcb.h>
 #include <libfdt.h>
+
+#include <arch/proc.h>
+#include <arch/arch.h>
+#include <arch/smp.h>
 #include "arch.h"
 #include "sbi.h"
 
@@ -94,7 +98,10 @@ void smp_bringup(struct vmem *b, void *fdt)
 		smp_init_stacks[hartid] = (void *)alloc_page(BASE_PAGE) +
 		                          BASE_PAGE_SIZE;
 
-		pm_t bringup = (pm_t)__pa(riscv_bringup);
+		/* fixup physical address of bringup */
+		pm_t bringup = (pm_t)riscv_bringup;
+		bringup = bringup - VM_KERNEL + get_load_addr();
+
 		r = sbi_hart_start(hartid, bringup, satp);
 
 		if (r.error) {
@@ -111,8 +118,9 @@ void smp_bringup(struct vmem *b, void *fdt)
  *
  * @param hartid Hart that's being brought up.
  */
-void core_bringup(long hartid)
+__noreturn void core_bringup(long hartid)
 {
+	bkl_lock();
 	/* assume smp_bringup assigned our cpuid correctly */
 	id_t cpuid = hartid_to_cpuid(hartid);
 
@@ -125,12 +133,25 @@ void core_bringup(long hartid)
 
 	/* add us as a thread to init program that cpu 0 is hopefully running by
 	 * now */
-	struct tcb *t = create_thread(cpu_tcb(0));
+	struct tcb *init = get_tcb(1);
+	assert(init);
+
+	struct tcb *t = create_thread(init);
+	assert(t);
+
+	alloc_stack(t);
+	/* init is special in that all threads jump to the entrypoint of the
+	 * program */
+	t->callback = init->callback;
+	t->exec = init->exec;
 	t->cpu_id = cpuid;
+	/** @todo this is pretty hacky, should really be a separate function? */
+	setup_irq(NULL);
+	setup_arch(NULL);
 	tcb_assign(t);
 	use_tcb(t);
 
-	/* eventually we should jump to init and start running stuff, but for
-	 * now take it easy */
-	while (1);
+	info("core %ld releasing BKL\n", (long)cpuid);
+	run_init(t, NULL, NULL);
+	unreachable();
 }
diff --git a/arch/riscv64/kernel/start.S b/arch/riscv64/kernel/start.S
index 73a442e..5a2ed61 100644
--- a/arch/riscv64/kernel/start.S
+++ b/arch/riscv64/kernel/start.S
@@ -24,12 +24,11 @@ ret
 
 .section .text
 
-/* a0 is fdt, a1 is load_addr, a2 is d, a3 is ram_base, a4 is DMAP */
+/* a0 is fdt, a1 is load_addr, a2 is d, a3 is ram_base */
 .global to_kernelspace
 to_kernelspace:
-lla t0, kernel
-add t0, t0, a4
-sub t0, t0, a3
-add sp, sp, a4
-sub sp, sp, a3
+li t1, VM_DMAP
+add sp, sp, t1
+lui t0, %hi(kernel)
+addi t0, t0, %lo(kernel)
 jr t0
diff --git a/arch/riscv64/kernel/vmem.c b/arch/riscv64/kernel/vmem.c
index 97b9cce..68869a3 100644
--- a/arch/riscv64/kernel/vmem.c
+++ b/arch/riscv64/kernel/vmem.c
@@ -416,14 +416,19 @@ long riscv_init_stack[4096 / sizeof(long)];
  * and so the kernel itself has to be on a 4K boundary. */
 __aligned(4096) struct vmem bootvmem;
 
-struct vmem *direct_mapping()
+/** Page entry for mapping kernel on a O1 page level, similar to how Linux does
+ * it. */
+__aligned(4096) struct vmem kvmem;
+
+struct vmem *init_mapping()
 {
 	rpc_pages = order_size(MM_O1) / BASE_PAGE_SIZE;
+	kvmem.leaf[0] = (struct vmem *)to_pte(get_load_addr(),
+			VM_A | VM_G | VM_D | VM_R | VM_W | VM_X | VM_V);
 
 	__populate_dmap(&bootvmem);
 	populate_kvmem(&bootvmem);
 	__use_vmem(&bootvmem, DEFAULT_Sv_MODE);
-
 	return &bootvmem;
 }
 
@@ -456,16 +461,31 @@ void destroy_vmem(struct vmem *b)
 	__destroy_branch(b);
 }
 
+static void map_kernel(struct vmem *b)
+{
+	intptr_t addr = (pm_t)&kvmem;
+
+	/* virtual memory is negative, unsure if this applies everywhere but I
+	 * guess it's good enough for us */
+	if (addr < 0)
+		addr = addr - VM_KERNEL + get_load_addr();
+
+	b->leaf[KERNEL_PAGE] = (struct vmem *)to_pte((pm_t)addr, VM_V);
+}
+
 stat_t populate_kvmem(struct vmem *b)
 {
 	size_t flags = VM_V | VM_R | VM_W | VM_X | VM_G | VM_D | VM_A;
-	for (size_t i = KSTART_PAGE; i < IO_PAGE; ++i)
+	for (size_t i = KSTART_PAGE; i < KERNEL_PAGE; ++i)
 		b->leaf[i] = (struct vmem *)to_pte(
-			get_ram_base() + TOP_PAGE_SIZE * (i - KSTART_PAGE),
+			TOP_PAGE_SIZE * (i - KSTART_PAGE),
 			flags);
 
-	/* map in IO region */
+	/* map in IO region if debugging is specified */
 	map_io_dbg(b);
+
+	/* map actual kernel */
+	map_kernel(b);
 	return OK;
 }