From b401524ea50b1f917efcf694a38ea544c26b3cc1 Mon Sep 17 00:00:00 2001
From: Matthew Berry <me@mattrb.com>
Date: Mon, 19 Oct 2020 00:00:05 -0700
Subject: [PATCH] arm / thumb software interrupts, register banking

---
 src/crab/arm/arm.cr                  |  2 +-
 src/crab/arm/software_interrupt.cr   | 10 +++++++
 src/crab/cpu.cr                      | 43 ++++++++++++++++++++++++++++
 src/crab/thumb/software_interrupt.cr | 11 +++++++
 src/crab/thumb/thumb.cr              |  2 +-
 5 files changed, 66 insertions(+), 2 deletions(-)
 create mode 100644 src/crab/arm/software_interrupt.cr
 create mode 100644 src/crab/thumb/software_interrupt.cr

diff --git a/src/crab/arm/arm.cr b/src/crab/arm/arm.cr
index 7c80032..b36a800 100644
--- a/src/crab/arm/arm.cr
+++ b/src/crab/arm/arm.cr
@@ -21,7 +21,7 @@ module ARM
     lut = Slice(Proc(Word, Nil)).new 4096, ->arm_unimplemented(Word)
     4096.times do |idx|
       if idx & 0b111100000000 == 0b111100000000
-        # software interrupt
+        lut[idx] = ->arm_software_interrupt(Word)
       elsif idx & 0b111100000001 == 0b111000000001
         # coprocessor register transfer
       elsif idx & 0b111100000001 == 0b111000000001
diff --git a/src/crab/arm/software_interrupt.cr b/src/crab/arm/software_interrupt.cr
new file mode 100644
index 0000000..f95bebf
--- /dev/null
+++ b/src/crab/arm/software_interrupt.cr
@@ -0,0 +1,10 @@
+module ARM
+  def arm_software_interrupt(instr : Word) : Nil
+    lr = @r[15] - 4
+    switch_mode CPU::Mode::SVC
+    @r[14] = lr
+    @cpsr.irq_disable = true
+    @r[15] = 0x08
+    clear_pipeline
+  end
+end
diff --git a/src/crab/cpu.cr b/src/crab/cpu.cr
index 63b25ca..4aea7a5 100644
--- a/src/crab/cpu.cr
+++ b/src/crab/cpu.cr
@@ -6,6 +6,27 @@ class CPU
   include ARM
   include THUMB
 
+  enum Mode
+    USR = 0b10000
+    FIQ = 0b10001
+    IRQ = 0b10010
+    SVC = 0b10011
+    ABT = 0b10111
+    UND = 0b11011
+    SYS = 0b11111
+
+    def bank : Int
+      case self
+      in Mode::USR, Mode::SYS then 0 # todo maybe some cpsr bits can't be changed in user mode?
+      in Mode::FIQ            then 1
+      in Mode::IRQ            then 2
+      in Mode::SVC            then 3
+      in Mode::ABT            then 4
+      in Mode::UND            then 5
+      end
+    end
+  end
+
   class PSR < BitField(UInt32)
     bool negative
     bool zero
@@ -23,6 +44,7 @@ class CPU
   @pipeline = Pipeline.new
   getter lut : Slice(Proc(Word, Nil)) { fill_lut }
   getter thumb_lut : Slice(Proc(Word, Nil)) { fill_thumb_lut }
+  @reg_banks = Array(Array(Word)).new 6 { Array(Word).new 8, 0 }
 
   def initialize(@gba : GBA)
     @r[0] = 0x08000000
@@ -32,6 +54,27 @@ class CPU
     @cpsr = PSR.new 0x6000001F
   end
 
+  def switch_mode(new_mode : Mode) : Nil
+    old_mode = Mode.from_value @cpsr.mode
+    return if new_mode == old_mode
+    new_bank = new_mode.bank
+    old_bank = old_mode.bank
+    if new_mode == Mode::FIQ || old_mode == Mode::FIQ
+      5.times do |idx|
+        @reg_banks[old_bank][idx] = @r[8 + idx]
+        @r[8 + idx] = @reg_banks[new_bank][idx]
+      end
+    end
+    # store old regs
+    @reg_banks[old_bank][5] = @r[13]
+    @reg_banks[old_bank][6] = @r[14]
+    @reg_banks[old_bank][7] = @cpsr.value
+    # load new regs
+    @r[13] = @reg_banks[new_bank][5]
+    @r[14] = @reg_banks[new_bank][6]
+    @cpsr.value = @reg_banks[new_bank][7]
+  end
+
   def fill_pipeline : Nil
     while @pipeline.size < 2
       if @cpsr.thumb
diff --git a/src/crab/thumb/software_interrupt.cr b/src/crab/thumb/software_interrupt.cr
new file mode 100644
index 0000000..a4d612a
--- /dev/null
+++ b/src/crab/thumb/software_interrupt.cr
@@ -0,0 +1,11 @@
+module THUMB
+  def thumb_software_interrupt(instr : Word) : Nil
+    lr = @r[15] - 4
+    switch_mode CPU::Mode::SVC
+    @r[14] = lr
+    @cpsr.irq_disable = true
+    @cpsr.thumb = false
+    @r[15] = 0x08
+    clear_pipeline
+  end
+end
diff --git a/src/crab/thumb/thumb.cr b/src/crab/thumb/thumb.cr
index f5310b9..dd1561f 100644
--- a/src/crab/thumb/thumb.cr
+++ b/src/crab/thumb/thumb.cr
@@ -11,7 +11,7 @@ module THUMB
       elsif idx & 0b11111000 == 0b11100000
         lut[idx] = ->thumb_unconditional_branch(Word)
       elsif idx & 0b11111111 == 0b11011111
-        # software interrupt
+        lut[idx] = ->thumb_software_interrupt(Word)
       elsif idx & 0b11110000 == 0b11010000
         lut[idx] = ->thumb_conditional_branch(Word)
       elsif idx & 0b11110000 == 0b11000000