add sbi-disk mcall routinessbi-disk

5 files changed, 110 insertions, 1 deletions

diff --git a/machine/htif.c b/machine/htif.c
index fa3db53..18457d6 100644
--- a/machine/htif.c
+++ b/machine/htif.c
@@ -2,6 +2,8 @@
 #include "atomic.h"
 #include "mtrap.h"
 
+#include <string.h>
+
 volatile uint64_t tohost __attribute__((section(".htif")));
 volatile uint64_t fromhost __attribute__((section(".htif")));
 volatile int htif_console_buf;
@@ -85,3 +87,58 @@ void htif_poweroff()
     tohost = 1;
   }
 }
+
+struct request {
+  uint64_t addr;
+  uint64_t offset;
+  uint64_t size;
+  uint64_t tag;
+};
+
+void htif_disk_read(uintptr_t addr, uintptr_t offset, size_t size)
+{
+  struct request req;
+
+  req.addr = addr;
+  req.offset = offset;
+  req.size = size;
+  req.tag = 0;
+
+  do_tohost_fromhost(2, 0, (uintptr_t) &req);
+}
+
+void htif_disk_write(uintptr_t addr, uintptr_t offset, size_t size)
+{
+  struct request req;
+
+  req.addr = addr;
+  req.offset = offset;
+  req.size = size;
+  req.tag = 0;
+
+  do_tohost_fromhost(2, 1, (uintptr_t) &req);
+}
+
+unsigned long htif_disk_size(void)
+{
+  char idbuf[128];
+  uintptr_t addr = (uintptr_t) idbuf;
+  unsigned long payload;
+  char *id = idbuf, *s;
+
+  // The buffer address needs to be aligned to 64 bytes
+  if (addr % 64 != 0) {
+    unsigned long inc = 64 - (addr % 64);
+    addr += inc;
+    id += inc;
+  }
+
+  payload = (addr << 8) | 0xff;
+  do_tohost_fromhost(2, 255, payload);
+
+  s = strstr(id, "size=");
+  if (s == NULL)
+    return 0;
+  s += 5;
+  return atol(s);
+}
diff --git a/machine/htif.h b/machine/htif.h
index fa768d8..09b99e2 100644
--- a/machine/htif.h
+++ b/machine/htif.h
@@ -2,6 +2,7 @@
 #define _RISCV_HTIF_H
 
 #include <stdint.h>
+#include <stdlib.h>
 
 #if __riscv_xlen == 64
 # define TOHOST_CMD(dev, cmd, payload) \
@@ -19,5 +20,8 @@ void htif_console_putchar(uint8_t);
 int htif_console_getchar();
 void htif_poweroff() __attribute__((noreturn));
 void htif_syscall(uintptr_t);
+void htif_disk_read(uintptr_t addr, uintptr_t offset, size_t size);
+void htif_disk_write(uintptr_t addr, uintptr_t offset, size_t size);
+unsigned long htif_disk_size(void);
 
 #endif
diff --git a/machine/mcall.h b/machine/mcall.h
index 6a2c037..8469be3 100644
--- a/machine/mcall.h
+++ b/machine/mcall.h
@@ -10,5 +10,8 @@
 #define SBI_REMOTE_SFENCE_VMA 6
 #define SBI_REMOTE_SFENCE_VMA_ASID 7
 #define SBI_SHUTDOWN 8
+#define SBI_DISK_READ 9
+#define SBI_DISK_WRITE 10
+#define SBI_DISK_SIZE 11
 
 #endif
diff --git a/machine/mtrap.c b/machine/mtrap.c
index d0c1684..f7f397c 100644
--- a/machine/mtrap.c
+++ b/machine/mtrap.c
@@ -84,6 +84,23 @@ static uintptr_t mcall_set_timer(uint64_t when)
   return 0;
 }
 
+static uintptr_t mcall_disk_read(uintptr_t addr, uintptr_t offset, size_t size)
+{
+  htif_disk_read(addr, offset, size);
+  return 0;
+}
+
+static uintptr_t mcall_disk_write(uintptr_t addr, uintptr_t offset, size_t size)
+{
+  htif_disk_write(addr, offset, size);
+  return 0;
+}
+
+static uintptr_t mcall_disk_size(void)
+{
+  return htif_disk_size();
+}
+
 static void send_ipi_many(uintptr_t* pmask, int event)
 {
   _Static_assert(MAX_HARTS <= 8 * sizeof(*pmask), "# harts > uintptr_t bits");
@@ -118,7 +135,8 @@ void mcall_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
 {
   write_csr(mepc, mepc + 4);
 
-  uintptr_t n = regs[17], arg0 = regs[10], arg1 = regs[11], retval, ipi_type;
+  uintptr_t n = regs[17], arg0 = regs[10], arg1 = regs[11], arg2 = regs[12];
+  uintptr_t retval, ipi_type;
 
   switch (n)
   {
@@ -154,6 +172,15 @@ send_ipi:
       retval = mcall_set_timer(arg0);
 #endif
       break;
+    case SBI_DISK_READ:
+      retval = mcall_disk_read(arg0, arg1, arg2);
+      break;
+    case SBI_DISK_WRITE:
+      retval = mcall_disk_write(arg0, arg1, arg2);
+      break;
+    case SBI_DISK_SIZE:
+      retval = mcall_disk_size();
+      break;
     default:
       retval = -ENOSYS;
       break;
diff --git a/util/string.c b/util/string.c
index 41855c2..7287bf8 100644
--- a/util/string.c
+++ b/util/string.c
@@ -88,3 +88,21 @@ long atol(const char* str)
 
   return sign ? -res : res;
 }
+
+char *strstr(const char *haystack, const char *needle)
+{
+  while (*haystack) {
+    int i;
+    int found = 1;
+    for (i = 0; needle[i]; i++) {
+      if (haystack[i] != needle[i]) {
+       found = 0;
+       break;
+      }
+    }
+    if (found)
+      return (char *) haystack;
+    haystack++;
+  }
+  return NULL;
+}