1 files changed, 65 insertions, 0 deletions
diff --git a/assembly/trap-test/trap-test.S b/assembly/trap-test/trap-test.S
new file mode 100644
index 0000000..ec928a7
--- /dev/null
+++ b/assembly/trap-test/trap-test.S
@@ -0,0 +1,65 @@
+la t0,handler
+csrw mtvec,t0 # Set trap handler address and mode (0/direct)
+
+li t0,0xdeadbeef # No RAM here
+lw t1,0(t0) # This should give a load access fault, which should be handled
+
+li a7,4 # Print string
+la a0,readvalmsg # "Read value "
+ecall
+
+li a7,1 # Print integer
+mv a0,t1 # Get the value read earlier
+ecall
+
+li a7,11 # Print character
+li a0,'\n'
+ecall
+
+li a7,10 # Exit program
+ecall
+
+readvalmsg: .asciz "Read value "
+
+.balign 4
+handler:
+# IMPORTANT NOTE: This handler is for demonstration purposes only and clobbers a0/a1/a7/t1!
+# Its "handling" is also roughly the equivalent of Visual Basic's infamous "on error resume next"
+li a7,4 # Print string
+la a0,trapmsg # "Got trap "
+ecall
+
+li a7,1 # Print integer
+csrr a0,mcause # Read trap cause
+ecall
+
+li a7,11 # Print character
+li a0,'\n'
+ecall
+
+li a7,4 # Print string
+la a0,mepcmsg # "at "
+ecall
+
+li a7,1 # Print integer
+csrr a0,mepc # Read faulting address
+ecall
+
+li a7,11 # Print character
+li a0,'\n'
+ecall
+
+csrr a0,mepc # Read faulting address again
+lw a1,0(a0) # Read faulting instruction into a1
+li a7,0x00000003
+andn a1,a7,a1 # Invert it and AND with 0x3 (gives 0 if last two bits were 11, nonzero otherwise)
+addi a0,a0,2 # Move fault address 2 bytes forward (all instructions are at least this long)
+bnez a1,handler_compressed # If it was compressed, that's all the movement needed
+addi a0,a0,2 # Otherwise it needs to go two more bytes forward
+handler_compressed:
+csrw mepc,a0 # Write the adjusted return address back
+li t1,1234 # Fake the load having succeeded
+mret # And go back there
+
+trapmsg: .asciz "Got trap "
+mepcmsg: .asciz "at "