include syscall.i
  include term.i
  include string.i
  section .text,text
  public traps_init
; Initialize trap handling
traps_init:
  move.l #trap_table, a0
  movec a0, VBR
  rts

generic_handler:
  ; Read the format/vector word and isolate the interrupt vector
  move.w ($6,a7), d0
  andi.l #$FF, d0
  cmpi.l #$20, d0
  ; Only the first 32 traps are named, print generic message for the rest
  bge.b unk_trap
  ; Get the pointer to the trap name in a0
  lsl.w #2, d0
  move.l #trap_name_table, a0
  move.l (a0,d0), a0
  jsr term_print ; Print it
  ; Print " Trap"
  move.l #trap_string, a0
  jsr term_print
  ; Print where the PC was when the trap occured
  move.l ($2,a7), a0
  jsr print_pc
  ; Halt
  stop #$2700
unk_trap:
  ; Print the unknown trap message
  move.l #unk_trap_msg, a0
  jsr term_print
  ; Convert the trap number to a hex string
  move.w ($6,a7), d0
  andi.l #$FF, d0
  move.l #string_buffer, a0
  jsr hex_to_ascii
  ; Print it
  move.l #string_buffer, a0
  jsr term_print
  ; Print where the PC was when the trap occured
  move.l ($2,a7), a0
  jsr print_pc
  ; Halt
  stop #$2700

berr_handler:
  ; Select the correct prefix based on whether the error was in supervisor or user mode
  move.w ($8,a7), d0
  andi.w #$4, d0
  cmpi.w #0, d0
  beq.b .1
  move.l #berr_supervisor_pfx, a0
  bra.b .2
.1:
  move.l #berr_user_pfx, a0
.2:
  jsr term_print ; Print the prefi
  ; Print " bus error "
  move.l #berr_msg, a0
  jsr term_print
  ; Get the error kind out of the special status word (end up with ins flag, word flag, and write flg in that order, shifted left by 2 and put in d0)
  move.l #0, d1
  move.w ($8,a7), d0
  andi.w #$100, d0
  cmpi.w #0, d0
  beq.b .3
  move.l #$4, d1
.3:
  move.w ($8,a7), d0
  andi.w #$200, d0
  cmpi.w #0, d0
  beq.b .4
  ori.l #$8, d1
.4:
  move.w ($8,a7), d0
  andi.w #$2000, d0
  cmpi.w #0, d0
  beq.b .5
  ori.l #$10, d1
.5:
  ; Get the pointer to the correct message for this kind of bus error in a0
  move.l #berr_table, a0
  adda.l d1, a0
  move.l (a0), a0
  jsr term_print ; Print it
  ; Convert the triggering address to a hex string
  move.l ($a,a7), d0
  move.l #string_buffer, a0
  jsr hex_to_ascii
  ; Print it
  move.l #string_buffer, a0
  jsr term_print
  ; Print where the PC was when the trap occured
  move.l ($2,a7), a0
  jsr print_pc
  stop #$2700

int_vector_handler:
  movem.l d0-d7/a0-a6,-(a7)
  lea.l (60,a7), a6
  ; Print "Interrupt Vector "
  move.l #int_vector_msg, a0
  jsr term_print
  ; Get the interrupt vector offset
  move.w ($6,a6), d0
  andi.l #$FF, d0
  subi.l #$40, d0
  ; Convert it to a hex string
  move.l #string_buffer, a0
  jsr hex_to_ascii
  ; Print it
  move.l #string_buffer, a0
  jsr term_println
  movem.l (a7)+, d0-d7/a0-a6
  rte

user_trap_handler:
  movem.l d2-d7/a2-a6,-(a7)
  jsr handle_syscall
  movem.l (a7)+, d2-d7/a2-a6
  rte


; Print " (PC: {a0})\n"
; Clobbers d0, a0, a1, a2
print_pc:
  ; Print the message up to the first variable
  move.l a0, a2
  move.l #pc_string_start, a0
  jsr term_print
  ; Convert the PC value to a hex string
  move.l a2, d0
  move.l #string_buffer, a0
  jsr hex_to_ascii
  ; Print it
  move.l #string_buffer, a0
  jsr term_print
  ; Print the rest of the message
  move.l #pc_string_end, a0
  jsr term_println
  rts


  section .bss,bss
string_buffer:
  ds.b 11

  section .data,data
unk_trap_msg: dc.b "Unknown trap ",0
trap_string: dc.b " Trap",0
berr_msg: dc.b "bus error ",0
pc_string_start: dc.b " (PC: ",0
pc_string_end: dc.b ")",0
int_vector_msg: dc.b "Interrupt Vector ",0
user_trap_msg: dc.b "User Trap ",0
berr_supervisor_pfx: dc.b "Supervsior ",0
berr_user_pfx: dc.b "User ",0
  align 1
berr_table:
  dc.l berr_rdw_msg
  dc.l berr_wdw_msg
  dc.l berr_rdb_msg
  dc.l berr_wdb_msg
  dc.l berr_riw_msg
  dc.l berr_wiw_msg
  dc.l berr_rib_msg
  dc.l berr_wib_msg
berr_rdw_msg: dc.b "reading word from ",0
berr_wdw_msg: dc.b "writing word to ",0
berr_rdb_msg: dc.b "reading byte from ",0
berr_wdb_msg: dc.b "writing byte to ",0
berr_riw_msg: dc.b "reading instruction word from ",0
berr_wiw_msg: dc.b "writing instruction word to ",0
berr_rib_msg: dc.b "reading instruction byte from ",0
berr_wib_msg: dc.b "writing instruction byte to ",0
  align 1
trap_name_table:
  dc.l $0
  dc.l $0
  dc.l berr_msg
  dc.l address_trap_name
  dc.l ill_ins_trap_name
  dc.l zero_div_trap_name
  dc.l chk_ins_trap_name
  dc.l trapv_trap_name
  dc.l priv_viol_trap_name
  dc.l trace_trap_name
  dc.l line_1010_emu_trap_name
  dc.l line_1111_emu_trap_name
  dc.l reserved_trap_name
  dc.l reserved_trap_name
  dc.l format_error_trap_name
  dc.l uninit_int_vect_trap_name
  dc.l reserved_trap_name
  dc.l reserved_trap_name
  dc.l reserved_trap_name
  dc.l reserved_trap_name
  dc.l reserved_trap_name
  dc.l reserved_trap_name
  dc.l reserved_trap_name
  dc.l reserved_trap_name
  dc.l spur_interrupt_trap_name
  dc.l lev_1_autovec_trap_name
  dc.l lev_2_autovec_trap_name
  dc.l lev_3_autovec_trap_name
  dc.l lev_4_autovec_trap_name
  dc.l lev_5_autovec_trap_name
  dc.l lev_6_autovec_trap_name
  dc.l lev_7_autovec_trap_name
address_trap_name: dc.b "Address Error",0
ill_ins_trap_name: dc.b "Illegal Instruction",0
zero_div_trap_name: dc.b "Zero Divide",0
chk_ins_trap_name: dc.b "CHK Instruction",0
trapv_trap_name: dc.b "TRAPV Instruction",0
priv_viol_trap_name: dc.b "Privilege Violation",0
trace_trap_name: dc.b "Trace",0
line_1010_emu_trap_name: dc.b "Line 1010 Emulator",0
line_1111_emu_trap_name: dc.b "Line 1111 Emulator",0
reserved_trap_name: dc.b "Reserved",0
format_error_trap_name: dc.b "Format Error",0
uninit_int_vect_trap_name: dc.b "Uninitialized Interrupt Vector",0
spur_interrupt_trap_name: dc.b "Spurious Interrupt",0
lev_1_autovec_trap_name: dc.b "Level 1 Interrupt Autovector",0
lev_2_autovec_trap_name: dc.b "Level 2 Interrupt Autovector",0
lev_3_autovec_trap_name: dc.b "Level 3 Interrupt Autovector",0
lev_4_autovec_trap_name: dc.b "Level 4 Interrupt Autovector",0
lev_5_autovec_trap_name: dc.b "Level 5 Interrupt Autovector",0
lev_6_autovec_trap_name: dc.b "Level 6 Interrupt Autovector",0
lev_7_autovec_trap_name: dc.b "Level 7 Interrupt Autovector",0
  align 10
trap_table:
  dc.l generic_handler ; Reset initial SSP, should never be called
  dc.l generic_handler ; Reset initial PC, should never be called
  dc.l berr_handler    ; Bus Error
  dc.l generic_handler ; Address Error
  dc.l generic_handler ; Illegal Instruction
  dc.l generic_handler ; Zero Divide
  dc.l generic_handler ; CHK Instruction
  dc.l generic_handler ; TRAPV Instruction
  dc.l generic_handler ; Privilege Violation
  dc.l generic_handler ; Trace
  dc.l generic_handler ; Line 1010 Emulator
  dc.l generic_handler ; Line 1111 Emulator
  dc.l generic_handler ; Reserved
  dc.l generic_handler ; Reserved
  dc.l generic_handler ; Format Error
  dc.l generic_handler ; Uninitialized Interrupt Vector
  rept 8
  dc.l generic_handler ; Reserved
  endr
  dc.l generic_handler ; Spurious Interrupt
  dc.l generic_handler ; Level 1 Interrupt Autovector
  dc.l generic_handler ; Level 2 Interrupt Autovector
  dc.l generic_handler ; Level 3 Interrupt Autovector
  dc.l generic_handler ; Level 4 Interrupt Autovector
  dc.l generic_handler ; Level 5 Interrupt Autovector
  dc.l generic_handler ; Level 6 Interrupt Autovector
  dc.l generic_handler ; Level 7 Interrupt Autovector
  rept 16
  dc.l user_trap_handler
  endr
  rept 16
  dc.l generic_handler ; Reserved
  endr
  rept 192
  dc.l int_vector_handler
  endr