Read and use devicetree -- THIS BREAKS ROCKET FOR NOW

pk/console.c

@@ -6,7 +6,7 @@
 #include <stdbool.h>
 #include <string.h>
 
-static int vsnprintf(char* out, size_t n, const char* s, va_list vl)
+int vsnprintf(char* out, size_t n, const char* s, va_list vl)
 {
   bool format = false;
   bool longarg = false;

pk/devicetree.c

@@ -0,0 +1,95 @@
+#include "devicetree.h"
+#include "encoding.h"
+#include "pk.h"
+#include "mtrap.h"
+#include <arpa/inet.h>
+#include <stdbool.h>
+
+static uintptr_t max_hart_id;
+
+static uint64_t fdt_read_uint64(uint32_t* addr) {
+  return ((uint64_t)ntohl(addr[0]) << 32) | ntohl(addr[1]);
+}
+
+static void fdt_handle_cpu(const char* isa, uint32_t* reg_addr, uint32_t reg_len)
+{
+  int xlen = sizeof(long) * 8;
+  kassert(reg_len == 8);
+  kassert(isa && isa[0]=='r' && isa[1]=='v' && isa[2]=='0'+(xlen/10));
+
+  uintptr_t* base_addr = (uintptr_t*)(uintptr_t)fdt_read_uint64(reg_addr);
+  debug_printk("at %p, ", base_addr);
+  uintptr_t hart_id = *(uintptr_t*)(base_addr + CSR_MHARTID);
+  debug_printk("found hart %ld\n", hart_id);
+  hls_init(hart_id, base_addr);
+  num_harts++;
+  max_hart_id = MAX(max_hart_id, hart_id);
+}
+
+static void fdt_handle_mem(uint32_t* reg_addr, uint32_t reg_len)
+{
+  kassert(reg_len == 16);
+  uint64_t base = fdt_read_uint64(reg_addr);
+  uint64_t size = fdt_read_uint64(reg_addr+2);
+  debug_printk("at %p, found %d MiB of memory\n", base, (int)(size >> 20));
+  kassert(base == 0);
+  mem_size = size;
+}
+
+// This code makes the following assumptions about FDTs:
+// - They are trusted and don't need to be sanitized
+// - All addresses and sizes are 64 bits (we don't parse #address-cells etc)
+
+static uint32_t* parse_node(uint32_t* token, char* strings)
+{
+  const char* nodename = (const char*)token, *s, *dev_type = 0, *isa = 0;
+  uint32_t reg_len = 0, *reg_addr = 0;
+  token = (uint32_t*)nodename + strlen(nodename)/4+1;
+
+  while (1) switch (ntohl(*token)) {
+    case FDT_PROP: {
+      token++;
+      uint32_t len = ntohl(*token++);
+      const char* name = strings + ntohl(*token++);
+      if (strcmp(name, "device_type") == 0) {
+        dev_type = (char*)token;
+      } else if (strcmp(name, "isa") == 0) {
+        isa = (char*)token;
+      } else if (strcmp(name, "reg") == 0) {
+        reg_len = len;
+        reg_addr = token;
+      }
+      token += (len+3)/4;
+      continue;
+    }
+    case FDT_BEGIN_NODE:
+      token = parse_node(token+1, strings);
+      continue;
+    case FDT_END_NODE:
+      goto out;
+    case FDT_NOP:
+      continue;
+    default:
+      kassert(0);
+  }
+
+out:
+  if (dev_type && strcmp(dev_type, "cpu") == 0)
+    fdt_handle_cpu(isa, reg_addr, reg_len);
+  else if (dev_type && strcmp(dev_type, "memory") == 0)
+    fdt_handle_mem(reg_addr, reg_len);
+
+  return token+1;
+}
+
+void parse_device_tree()
+{
+  struct fdt_header* hdr = (struct fdt_header*)read_csr(miobase);
+  debug_printk("reading device tree at %p\n", hdr);
+  kassert(ntohl(hdr->magic) == FDT_MAGIC);
+  char* strings = (char*)hdr + ntohl(hdr->off_dt_strings);
+  uint32_t* root = (uint32_t*)((char*)hdr + ntohl(hdr->off_dt_struct));
+  while (ntohl(*root++) != FDT_BEGIN_NODE);
+  parse_node(root, strings);
+  kassert(max_hart_id == num_harts-1);
+}

pk/devicetree.h

@@ -0,0 +1,37 @@
+// See LICENSE for license details.
+
+#ifndef _PK_DEVICETREE_H
+#define _PK_DEVICETREE_H
+
+#include <stdint.h>
+
+#define FDT_MAGIC 0xd00dfeedU
+#define FDT_VERSION 17
+#define FDT_COMP_VERSION 16
+#define FDT_BEGIN_NODE 1
+#define FDT_END_NODE 2
+#define FDT_PROP 3
+#define FDT_NOP 4
+#define FDT_END 9
+
+struct fdt_header {
+  uint32_t magic;
+  uint32_t totalsize;
+  uint32_t off_dt_struct;
+  uint32_t off_dt_strings;
+  uint32_t off_rsvmap;
+  uint32_t version;
+  uint32_t last_comp_version;
+  uint32_t boot_cpuid_phys;
+  uint32_t size_dt_strings;
+  uint32_t size_dt_struct;
+};
+
+struct fdt_reserve_entry {
+  uint64_t address;
+  uint64_t size;
+};
+
+void parse_device_tree();
+
+#endif

pk/init.c

@@ -44,31 +44,10 @@ static void handle_option(const char* s)
       uarch_counters_enabled = 1;
       break;
 
-    case 'm': // memory capacity in MiB
-    {
-      uintptr_t mem_mb = atol(&s[2]);
-      if (!mem_mb)
-        goto need_nonzero_int;
-      mem_size = mem_mb << 20;
-      if ((mem_size >> 20) < mem_mb)
-        mem_size = (typeof(mem_size))-1 & -RISCV_PGSIZE;
-      break;
-    }
-
-    case 'p': // number of harts
-      num_harts = atol(&s[2]);
-      if (!num_harts)
-        goto need_nonzero_int;
-      break;
-
     default:
       panic("unrecognized option: `%c'", s[1]);
       break;
   }
-  return;
-
-need_nonzero_int:
-  panic("the -%c flag requires a nonzero argument", s[1]);
 }
 
 struct mainvars* parse_args(struct mainvars* args)

pk/minit.c

@@ -1,5 +1,6 @@
 #include "vm.h"
 #include "mtrap.h"
+#include "devicetree.h"
 
 uintptr_t mem_size;
 uint32_t num_harts;
@@ -54,15 +55,19 @@ static void fp_init()
 #endif
 }
 
-static void hls_init(uint32_t hart_id)
+void hls_init(uint32_t hart_id, uintptr_t* csrs)
 {
-  memset(HLS(), 0, sizeof(*HLS()));
-  HLS()->hart_id = hart_id;
+  hls_t* hls = OTHER_HLS(hart_id);
+  memset(hls, 0, sizeof(*hls));
+  hls->hart_id = hart_id;
+  hls->csrs = csrs;
 }
 
 static void init_first_hart()
 {
+  memset(HLS(), 0, sizeof(*HLS()));
   file_init();
+  parse_device_tree();
 
   struct mainvars arg_buffer;
   struct mainvars *args = parse_args(&arg_buffer);
@@ -72,16 +77,17 @@ static void init_first_hart()
   boot_loader(args);
 }
 
-static void init_other_hart()
+static void init_other_hart(uint32_t hart_id)
 {
   // wait until virtual memory is enabled
-  while (root_page_table == NULL)
-    asm volatile ("" ::: "memory");
-  mb();
+  do {
+    mb();
+  } while (root_page_table == NULL);
+
   write_csr(sptbr, root_page_table);
 
   // then make sure we're in bounds
-  if (HLS()->hart_id >= num_harts) {
+  if (hart_id >= num_harts) {
     while (1)
       wfi();
   }
@@ -91,12 +97,11 @@ static void init_other_hart()
 
 void machine_init(uint32_t hart_id)
 {
-  hls_init(hart_id);
   mstatus_init();
   fp_init();
 
   if (hart_id == 0)
     init_first_hart();
   else
-    init_other_hart();
+    init_other_hart(hart_id);
 }

pk/mtrap.c

@@ -130,9 +130,19 @@ static uintptr_t mcall_console_putchar(uint8_t ch)
   return 0;
 }
 
-#define printm(str, ...) ({ \
-  char buf[128], *p = buf; snprintf(buf, sizeof(buf), str, __VA_ARGS__); \
-  while (*p) mcall_console_putchar(*p++); })
+void printm(const char* s, ...)
+{
+  char buf[128];
+  va_list vl;
+
+  va_start(vl, s);
+  vsnprintf(buf, sizeof buf, s, vl);
+  va_end(vl);
+
+  char* p = buf;
+  while (*p)
+    mcall_console_putchar(*p++);
+}
 
 static uintptr_t mcall_dev_req(sbi_device_message *m)
 {
@@ -183,7 +193,7 @@ static uintptr_t mcall_send_ipi(uintptr_t recipient)
 
   if (atomic_swap(&OTHER_HLS(recipient)->ipi_pending, 1) == 0) {
     mb();
-    write_csr(send_ipi, recipient);
+    OTHER_HLS(recipient)->csrs[CSR_MIPI] = 0;
   }
 
   return 0;

pk/mtrap.h

@@ -225,10 +225,13 @@ typedef struct {
   sbi_device_message* device_response_queue_head;
   sbi_device_message* device_response_queue_tail;
 
+  uintptr_t* csrs;
   int hart_id;
   int ipi_pending;
 } hls_t;
 
+void hls_init(uint32_t hart_id, uintptr_t* csrs);
+
 #define MACHINE_STACK_TOP() ({ \
   register uintptr_t sp asm ("sp"); \
   (void*)((sp + RISCV_PGSIZE) & -RISCV_PGSIZE); })

pk/pk.h

@@ -5,9 +5,12 @@
 
 #ifndef __ASSEMBLER__
 
+#define debug_printk(s, ...) //printk(s, __VA_ARGS__)
+
+#include "encoding.h"
 #include <stdint.h>
 #include <string.h>
-#include "encoding.h"
+#include <stdarg.h>
 
 typedef struct
 {
@@ -49,6 +52,8 @@ extern uint32_t num_harts_booted;
 
 struct mainvars* parse_args(struct mainvars*);
 void printk(const char* s, ...);
+void printm(const char* s, ...);
+int vsnprintf(char* out, size_t n, const char* s, va_list vl);
 int snprintf(char* out, size_t n, const char* s, ...);
 void init_tf(trapframe_t*, long pc, long sp, int user64);
 void start_user(trapframe_t* tf) __attribute__((noreturn));

pk/pk.mk.in

@@ -25,6 +25,7 @@ pk_c_srcs = \
 	vm.c \
 	string.c \
 	logo.c \
+	devicetree.c \
 
 pk_asm_srcs = \
 	mentry.S \

pk/string.c

@@ -45,6 +45,18 @@ size_t strlen(const char *s)
   return p - s;
 }
 
+int strcmp(const char* s1, const char* s2)
+{
+  unsigned char c1, c2;
+
+  do {
+    c1 = *s1++;
+    c2 = *s2++;
+  } while (c1 != 0 && c1 == c2);
+
+  return c1 - c2;
+}
+
 char* strcpy(char* dest, const char* src)
 {
   char* d = dest;

pk/vm.c

@@ -423,6 +423,7 @@ static uintptr_t sbi_top_paddr()
 
 void vm_init()
 {
+  mem_size = mem_size / SUPERPAGE_SIZE * SUPERPAGE_SIZE;
   current.first_free_paddr = first_free_paddr();
 
   size_t mem_pages = mem_size >> RISCV_PGSHIFT;