1 files changed, 51 insertions, 0 deletions
diff --git a/src/kern/svc.c b/src/kern/svc.c
new file mode 100644
index 0000000..5527364
--- /dev/null
+++ b/src/kern/svc.c
@@ -0,0 +1,51 @@
+#include "arch.h"
+#include "kern/common.h"
+#include "kern/log.h"
+#include "kern/syscall.h"
+
+void handle_svc_call(
+    uint32_t syscall_id, uint32_t syscall_arg, uint32_t svc_number)
+{
+  switch (svc_number) {
+    case 0x04:
+      switch (syscall_id) {
+#define SYSCALL(id, fn, kernfn, argt) \
+  case id:                    \
+    kernfn((argt)syscall_arg);    \
+    break;
+#include "kern/syscall/syscall_tbl.inc"
+#undef SYSCALL
+      }
+      break;
+
+    default:
+      /* An illegal syscall. TODO kill whatever is happening. */
+      break;
+  }
+}
+
+/* The actual handling for the svc call. Overrides the weak
+ * symbol on_svc in irq.h */
+asm("	.align	2\n"
+    "	.global	on_svc\n"
+    "	.syntax unified\n"
+    "	.fpu softvfp\n"
+    "	.type	on_svc, %function\n"
+    "on_svc:\n"
+    "	tst   lr,#4\n"
+    " ite   eq\n"
+    "	moveq r12,sp\n"
+    "	mrsne r12,psp\n"
+    "	ldr   r12, [r12, #24]\n"
+    "	ldrh  r12, [r12, #-2]\n"
+    "	bics  r12, r12, #0xff00\n"
+    "	push  {r4, lr}\n"
+    "	mov   r2 , r12\n"
+    "	bl    handle_svc_call\n"
+    "	pop   {r4, lr}\n"
+    "	tst   lr, #4\n"
+    " ite   eq\n"
+    "	moveq r12,sp\n"
+    "	mrsne r12,psp\n"
+    "	stm   r12,{r0-r3}\n"
+    "	bx    lr\n");