1 files changed, 105 insertions, 0 deletions
diff --git a/debug_rom/debug_rom.S b/debug_rom/debug_rom.S
new file mode 100755
index 0000000..230f4b4
--- /dev/null
+++ b/debug_rom/debug_rom.S
@@ -0,0 +1,105 @@
+# This code should be functional. Doesn't have to be optimal.
+# I'm writing it to prove that it can be done.
+
+# TODO: Update these constants once they're finalized in the doc.
+
+#define DCSR                    0x790
+#define DCSR_CAUSE_DEBINT       3
+#define DCSR_HALT_OFFSET        3
+#define DCSR_DEBUGINT_OFFSET    10
+
+#define DSCRATCH                0x792
+
+#define MCPUID                  0xf00
+#define MHARTID                 0xf10
+
+#define DEBUG_RAM               0x400
+#define DEBUG_RAM_SIZE          64
+
+#define SETHALTNOT              0x100
+#define CLEARHALTNOT            0x104
+#define CLEARDEBINT             0x108
+
+        .global entry
+        .global resume
+
+        # Automatically called when Debug Mode is first entered.
+entry:  j       _entry
+        # Should be called by Debug RAM code that has finished execution and
+        # wants to return to Debug Mode.
+resume:
+        # Clear debug interrupt.
+clear_debint:
+        csrr    s1, MHARTID
+        sw      s1, CLEARDEBINT(zero)
+clear_debint_loop:
+        csrr    s1, DCSR
+        andi    s1, s1, (1<<DCSR_DEBUGINT_OFFSET)
+        bnez    s1, wait_for_interrupt
+
+        # Restore s1.
+        csrr    s1, MCPUID
+        bltz    s1, restore_not_32
+restore_32:
+        lw      s1, (DEBUG_RAM + DEBUG_RAM_SIZE - 4)(zero)
+        j       check_halt
+restore_not_32:
+        slli    s1, s1, 1
+        bltz    s1, restore_128
+restore_64:
+        ld      s1, (DEBUG_RAM + DEBUG_RAM_SIZE - 8)(zero)
+        j       check_halt
+restore_128:
+        nop     #lq      s1, (DEBUG_RAM + DEBUG_RAM_SIZE - 16)(zero)
+
+check_halt:
+        csrr    s0, DCSR
+        andi    s0, s0, (1<<DCSR_HALT_OFFSET)
+        beqz    s0, exit
+        j       wait_for_interrupt
+
+exit:
+        # Restore s0.
+        csrr    s0, DSCRATCH
+        eret
+
+
+_entry:
+        # Save s0 in DSCRATCH
+        csrw    DSCRATCH, s0
+
+        # Check why we're here
+        csrr    s0, DCSR
+        # cause is in bits 2:0 of dcsr
+        andi    s0, s0, 7
+        addi    s0, s0, -DCSR_CAUSE_DEBINT
+        bnez    s0, spontaneous_halt
+
+jdebugram:
+        # Save s1 so that the debug program can use two registers.
+        csrr    s0, MCPUID
+        bltz    s0, save_not_32
+save_32:
+        sw      s1, (DEBUG_RAM + DEBUG_RAM_SIZE - 4)(zero)
+        jr      zero, DEBUG_RAM
+save_not_32:
+        slli    s0, s0, 1
+        bltz    s0, save_128
+save_64:
+        sd      s1, (DEBUG_RAM + DEBUG_RAM_SIZE - 8)(zero)
+        jr      zero, DEBUG_RAM
+save_128:
+        nop     #sq      s1, (DEBUG_RAM + DEBUG_RAM_SIZE - 16)(zero)
+        jr      zero, DEBUG_RAM
+
+spontaneous_halt:
+        csrr    s0, MHARTID
+        sw      s0, SETHALTNOT(zero)
+        csrsi   DCSR, DCSR_HALT_OFFSET
+
+wait_for_interrupt:
+        csrr    s0, DCSR
+        andi    s0, s0, (1<<DCSR_DEBUGINT_OFFSET)
+        beqz    s0, wait_for_interrupt
+
+        j       jdebugram