reset:
    br always >main
    nop

hardfault:
    reti
    nop

memfault:
    reti
    nop

switches_interrupt:
    br >switches_interrupt_handler
    nop

can_interrupt:
    br >can_interrupt_handler
    nop

.align
led_addr: .word 0x000F0000
switches_addr: .word 0x000F0004
dmem_start_addr: .word 0x00000400
dmem_end_addr: .word 0x000004FC
priority_mask: .word 0xFFFFFF03

// CAN
can_control_addr: .word 0x000F0100
can_command_addr: .word 0x000F0101
can_interrupt_addr: .word 0x000F0103
can_acceptance_code_addr: .word 0x000F0104
can_acceptance_mask_addr: .word 0x000F0105
can_bus_timing0_addr: .word 0x000F0106
can_bus_timing1_addr: .word 0x000F0107
can_output_control_addr: .word 0x000F0108
can_identifier0_addr: .word 0x000F010A
can_identifier1_addr: .word 0x000F010B
can_data0_addr: .word 0x000F010C
can_data1_addr: .word 0x000F010D

// CAN Constants
acceptance_code: .word 0x00
acceptance_mask: .word 0xFF
// btr0: .word 0x45 Real board
// btr1: .word 0x16 Real board
btr0: .word 0x80
btr1: .word 0x48
output_control: .word 0x02
control: .word 0xFE
id0: .word 0xAA
id1: .word 0xC2

main:
    // Initialize stack pointer to the end of the data memory
    ldr r12, >dmem_end_addr

    // Set runtime priority
    ldr r0, >priority_mask
    and r14, r0, r14

    // --- CAN init ---
    ldr r0, >can_acceptance_code_addr
    ldr r3, >acceptance_code
    st08 r0, r3

    ldr r0, >can_acceptance_mask_addr
    ldr r3, >acceptance_mask
    st08 r0, r3

    ldr r0, >can_bus_timing0_addr
    ldr r3, >btr0
    st08 r0, r3

    ldr r0, >can_bus_timing1_addr
    ldr r3, >btr1
    st08 r0, r3

    ldr r0, >can_output_control_addr
    ldr r3, >output_control
    st08 r0, r3

    ldr r0, >can_control_addr
    ldr r3, >control
    st08 r0, r3

    ldr r0, >led_addr
    ldr r1, >switches_addr

    st08 r0, r2
loop:
    br >loop
    nop

switches_interrupt_handler:
    ld32 r2, r1

    // Set LED to pattern
    st08 r0, r2
    
    reti
    nop

can_interrupt_handler:
    reti
    nop

wait:
	clr r7
    clr r8
    addi r8, 16
inc_i:
	cmp neq r7,r8
	br true >inc_i
	addi r7,1
	ret
    nop