1 files changed, 67 insertions, 0 deletions
diff --git a/src/interrupts.zig b/src/interrupts.zig
index 5850358..bdc1c5f 100644
--- a/src/interrupts.zig
+++ b/src/interrupts.zig
@@ -2,6 +2,8 @@
 //
 // SPDX-License-Identifier: AGPL-3.0-or-later
 
+const std = @import("std");
+
 pub const SupervisorTrapVector = packed struct(usize) {
     pub const Mode = enum(u2) {
         direct = 0,
@@ -18,3 +20,68 @@ pub const SupervisorTrapVector = packed struct(usize) {
         };
     }
 };
+
+pub const TrapFrame = extern struct {
+    general_purpose_registers: [32]usize, // Offset: 0
+    floating_point_registers: [32]usize, // Offset: 256
+    satp: usize, // Offset: 512
+    stack: *u8, // Offset: 520
+    hart_id: usize, // Offset: 528
+};
+
+export fn s_trap(epc: usize, tval: usize, cause: usize, hart_id: usize, status: usize, frame: *TrapFrame) usize {}
+
+export fn supervisor_trap_vector() linksection(".stvec") callconv(.Naked) noreturn {
+    asm volatile (
+        \\ csrrw t6, sscratch, t6
+    );
+
+    inline for (1..31) |i| {
+        save_general_purpose(i);
+    }
+
+    asm volatile (
+        \\ mv t5, t6
+        \\ csrr t6, sscratch
+    );
+
+    save_general_purpose(31, 30);
+
+    asm volatile (
+        \\ csrw mscratch, t5
+        \\
+        \\ csrr a0, sepc
+        \\ csrr a1, stval
+        \\ csrr a2, scause
+        \\ csrr a3, shartid
+        \\ csrr a4, sstatus
+        \\ mv a5, t5
+        \\ ld sp, 520(t5)
+        \\ call s_trap
+        \\
+        \\ csrw sepc, a0
+        \\ csrr t6, sscratch
+    );
+
+    inline for (1..32) |i| {
+        load_general_purpose(i);
+    }
+
+    asm volatile (
+        \\ sret
+    );
+}
+
+inline fn save_general_purpose(comptime i: u5, comptime base_register: ?u5) void {
+    comptime var buf = [_]u8{0} ** 17;
+
+    const offset = i * @sizeOf(usize);
+    asm volatile (std.fmt.bufPrint(buf[0..], "sd x{d}, {d}(x{d})", .{ i, offset, base_register orelse 31 }) catch unreachable);
+}
+
+inline fn load_general_purpose(comptime i: u5, comptime base_register: ?u5) void {
+    comptime var buf = [_]u8{0} ** 17;
+
+    const offset = i * @sizeOf(usize);
+    asm volatile (std.fmt.bufPrint(buf[0..], "ld x{d}, {d}(x{d})", .{ i, offset, base_register orelse 31 }) catch unreachable);
+}