From 52c288a0564e2167b1e850767e81e43bc24e05e2 Mon Sep 17 00:00:00 2001
From: giomba <giomba@glgprograms.it>
Date: Mon, 31 Dec 2018 17:28:04 +0100
Subject: [PATCH] Improved SWI service routine to handle C functions

---
 src/isr.cpp    |  8 ++++++++
 src/kernel.cpp | 11 +++++++++--
 src/linker.ld  | 34 ++++++++++++++++------------------
 src/startup.s  |  5 ++---
 src/user.s     | 20 ++++++++++++++++++++
 src/vectab.s   | 18 ++++++++++++++----
 6 files changed, 69 insertions(+), 27 deletions(-)
 create mode 100644 src/isr.cpp
 create mode 100644 src/user.s

diff --git a/src/isr.cpp b/src/isr.cpp
new file mode 100644
index 0000000..f754f7a
--- /dev/null
+++ b/src/isr.cpp
@@ -0,0 +1,8 @@
+#include <stdint.h>
+#include <dbg.h>
+
+extern "C" void c_swi_handler(uint32_t type) {
+    printk("Serving SWI # ");
+    printkl(itoa(type));
+    // TODO: to be done =)
+}
diff --git a/src/kernel.cpp b/src/kernel.cpp
index f564c78..b753813 100644
--- a/src/kernel.cpp
+++ b/src/kernel.cpp
@@ -2,14 +2,21 @@
 #include <stdint.h>
 #include <interrupt.h>
 
+extern "C" void go_usr(void);
+
 /* KERNEL MAIN */
 extern "C" int main(int argc, char** argv) {
     printkl("Welcome to STKARM -- Simple and Trivial Kernel for Advanced Reduced Instruction Set Computer Machines");
 
-    printkl("Now firing software interrupt...");
+    printkl("Installing kernel...");
+
+    printkl("Now firing test software interrupt...");
     fireswi();
 
-    printkl("EOK -- End of Kernel -- Now returning to assembly");
+    printkl("Go user...");
+    go_usr();
+
+    printkl("EOK -- End of Kernel -- This point can not be reached");
     return 0;
 }
 
diff --git a/src/linker.ld b/src/linker.ld
index 68cafc9..e41f76e 100644
--- a/src/linker.ld
+++ b/src/linker.ld
@@ -1,31 +1,29 @@
 ENTRY(_start)
 
 MEMORY {
-    m_text  (RX) : ORIGIN = 0x40000000, LENGTH = 16M
-    m_data  (RW) : ORIGIN = 0x41000000, LENGTH = 16M
-    m_stack (RW) : ORIGIN = 0x42000000, LENGTH = 16M
+    m_ram (RWX) : ORIGIN = 0x40000000, LENGTH = 2048M
 }
 
 SECTIONS
 {
-	. = 0x40000000;
-	.text : ALIGN(0x1000) {
-		*(.text)
-	} > m_text
-	.data : ALIGN(0x1000) {
+    .stack : ALIGN(0x100) {
+        . = . + 0x100;  stack_svc = .;
+        . = . + 0x100;  stack_und = .;
+        . = . + 0x100;  stack_abt = .;
+        . = . + 0x100;  stack_irq = .;
+        . = . + 0x100;  stack_fiq = .;
+        . = . + 0x100;  stack_sys = .; stack_usr = .;
+    } > m_ram
+	.data : ALIGN(0x4) {
 		*(.data)
-	} > m_data
-	.bss : ALIGN(0x1000) {
+	} > m_ram
+	.bss : ALIGN(0x4) {
 		*(.bss)
-	} > m_data
+	} > m_ram
 
-    . = ALIGN(0x100);   stack_svc = .;
-    . = . + 0x100;      stack_und = .;
-    . = . + 0x100;      stack_abt = .;
-    . = . + 0x100;      stack_irq = .;
-    . = . + 0x100;      stack_fiq = .;
-    . = . + 0x100;      stack_sys = .;
-    . = . + 0x100;      stack_usr = .;
+	.text : ALIGN(0x4) {
+		*(.text)
+	} > m_ram
 
 }
 
diff --git a/src/startup.s b/src/startup.s
index 429a0d7..0b8e00c 100644
--- a/src/startup.s
+++ b/src/startup.s
@@ -24,9 +24,8 @@ _start:
     cps #0x1f
     ldr sp, =stack_sys
 
-// Leave in usr mode
-//    cps #0x10
-//    ldr sp, =stack_usr
+    /* Enter SuperVisor Mode */
+    cps #0x13
 
     /* Enable Vector Table BAR remapping (clears bit 13 of SCTLR) */
     mrc p15, 0, r0, c1, c0, 0
diff --git a/src/user.s b/src/user.s
new file mode 100644
index 0000000..5e3ba5f
--- /dev/null
+++ b/src/user.s
@@ -0,0 +1,20 @@
+.global go_usr
+go_usr:
+    mrs r0, cpsr
+    bl itoa
+    bl printkl
+
+    /* Actually go to user mode */
+    cps #0x10
+    ldr sp, =stack_usr
+
+    mrs r0, cpsr
+    bl itoa
+    bl printkl
+
+    swi #0xc
+    swi #0x1
+    swi #0xa
+    swi #0x0
+
+    b .
diff --git a/src/vectab.s b/src/vectab.s
index 706cdbb..91daec4 100644
--- a/src/vectab.s
+++ b/src/vectab.s
@@ -37,12 +37,22 @@ und_handler:
     ldmfd sp!, {pc}^
 
 swi_handler:
-    stmfd sp!, {lr}
+    stmfd sp!, {r0-r12, lr}
 
-    ldr r0, =swi_msg
-    bl printkl
+    /* Get SWI number to pass to c_swi_handler */
+    ldr r10, [lr, #-4]
+    and r10, #0x00ffffff
+    mov r0, r10
 
-    ldmfd sp!, {pc}^
+    mov r1, sp
+    mrs r2, spsr
+    stmfd sp!, {r2}
+
+    bl c_swi_handler
+
+    ldmfd sp!, {r2}
+    msr spsr_cxsf, r2
+    ldmfd sp!, {r0-r12, pc}^
 
 // TODO: untested
 prefetch_abt_handler: