From dcff3fd588af36cab270631ba83b47d9b27ec78c Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Mon, 9 Oct 2023 19:26:57 -0500
Subject: [PATCH] fix(v5te): account for high vectors in swi

---
 src/arm/cpu/arm/software_interrupt.zig   | 8 +++++++-
 src/arm/cpu/thumb/software_interrupt.zig | 8 +++++++-
 2 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/src/arm/cpu/arm/software_interrupt.zig b/src/arm/cpu/arm/software_interrupt.zig
index d922e86..51ef4d9 100644
--- a/src/arm/cpu/arm/software_interrupt.zig
+++ b/src/arm/cpu/arm/software_interrupt.zig
@@ -14,7 +14,13 @@ pub fn armSoftwareInterrupt(comptime InstrFn: type) InstrFn {
 
             cpu.r[14] = ret_addr; // Resume Execution
             cpu.spsr.raw = cpsr; // Previous mode CPSR
-            cpu.r[15] = 0x0000_0008;
+            cpu.r[15] = switch (Arm32.arch) {
+                .v4t => 0x0000_0008,
+                .v5te => blk: {
+                    const ctrl = cpu.cp15.read(0, 1, 0, 0);
+                    break :blk if (ctrl >> 13 & 1 == 1) 0xFFFF_0008 else 0x0000_0008;
+                },
+            };
             cpu.pipe.reload(cpu);
         }
     }.inner;
diff --git a/src/arm/cpu/thumb/software_interrupt.zig b/src/arm/cpu/thumb/software_interrupt.zig
index 20b1a33..46ea142 100644
--- a/src/arm/cpu/thumb/software_interrupt.zig
+++ b/src/arm/cpu/thumb/software_interrupt.zig
@@ -14,7 +14,13 @@ pub fn fmt17(comptime InstrFn: type) InstrFn {
 
             cpu.r[14] = ret_addr; // Resume Execution
             cpu.spsr.raw = cpsr; // Previous mode CPSR
-            cpu.r[15] = 0x0000_0008;
+            cpu.r[15] = switch (Arm32.arch) {
+                .v4t => 0x0000_0008,
+                .v5te => blk: {
+                    const ctrl = cpu.cp15.read(0, 1, 0, 0);
+                    break :blk if (ctrl >> 13 & 1 == 1) 0xFFFF_0008 else 0x0000_0008;
+                },
+            };
             cpu.pipe.reload(cpu);
         }
     }.inner;