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
dmem_start_addr: .word 0x00000400
dmem_end_addr: .word 0x000004FC

led_addr: .word 0x000F0000
switches_addr: .word 0x000F0004
scrolling_addr: .word 0x000F00A0
scrolling_count_addr: .word 0x000F00A4

priority_mask: .word 0xFFFFFF03
write_mask: .word 0x1000000
clear_mask: .word 0x100
scrolling_cnt_value: .word 0x10FC000 // for real board
// scrolling_cnt_value: .word 0x500 // for simulation

// 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 Constants
acceptance_code: .word 0x00
acceptance_mask: .word 0xFF
btr0: .word 0x45 // for Real board
btr1: .word 0x16 // for Real board
// btr0: .word 0x80
// btr1: .word 0x48
output_control: .word 0x02
control: .word 0xFE
rx_interrupt_mask: .word 0x01
tx_interrupt_mask: .word 0x02

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

    // Start scrolling controller
    ldr r0, >scrolling_addr
    clr r3
    addi r3, 0x01
    st32 r0, r3

    // (Re)set scrolling speed
    ldr r5, >scrolling_count_addr
    ldr r4, >scrolling_cnt_value
    st32 r5, r4

    // Set LED to state
    ldr r0, >led_addr
    clr r3
    addi r3, 0x7A
    st08 r0, r3

loop:
    br >loop
    nop

.align
button_data_add: .word 0x10000
button_data_clear: .word 0x20000
button_frequency: .word 0x40000

switches_interrupt_handler:
    // Read switch state
    ldr r0, >switches_addr
    ld32 r2, r0

    ldr r0, >led_addr
    st08 r0, r2

    clr r4

    ldr r3, >button_data_clear
    and r5, r2, r3
    cmp neq r5, r4
    br true >can_send_data_clear_frame
    nop

    ldr r3, >button_data_add
    and r5, r2, r3
    cmp neq r5, r4
    br true >can_send_data_add_frame
    nop

    ldr r3, >button_frequency
    and r5, r2, r3
    cmp neq r5, r4
    br true >can_send_frequency_frame
    nop

    // Unimplemented button function
    
    reti
    nop

can_interrupt_handler:
    // Dispatch interrupt event
    ldr r0, >can_interrupt_addr
    ld08 r1, r0
    clr r4

    ldr r2, >rx_interrupt_mask
    and r5, r1, r2
    cmp neq r5, r4
    br true >can_rx_handler
    nop

    ldr r2, >tx_interrupt_mask
    and r5, r1, r2
    cmp neq r5, r4
    br true >can_tx_handler
    nop

    // Unimplemented CAN interrupt
    reti
    nop

.align
can_tx_data0_addr: .word 0x000F010C
can_tx_data1_addr: .word 0x000F010D
can_tx_data2_addr: .word 0x000F010E

can_tx_identifier0_addr: .word 0x000F010A
can_tx_identifier1_addr: .word 0x000F010B

id0: .word 0xAA
id1_0: .word 0xC1 // data length is also encoded here
id1_1: .word 0xC2 // data length is also encoded here
id1_2: .word 0xC3 // data length is also encoded here

frame_data_add: .word 0x00
frame_data_clear: .word 0x01
frame_frequency: .word 0x02

can_command_addr_ptr: .word =can_command_addr

can_send_data_clear_frame:
    ldr r0, >can_tx_identifier0_addr
    ldr r3, >id0
    st08 r0, r3

    ldr r0, >can_tx_identifier1_addr
    ldr r3, >id1_0
    st08 r0, r3

    ldr r0, >can_tx_data0_addr
    ldr r3, >frame_data_clear
    st08 r0, r3

    // Save for tx interrupt
    clr r6
    add r6, r6, r3

    ldr r0, >can_command_addr_ptr
    ld32 r0, r0
    clr r3
    addi r3, 0x01
    st08 r0, r3

    reti
    nop

.align
byte_mask: .word 0xFF

can_send_data_add_frame:
    ldr r0, >can_tx_identifier0_addr
    ldr r3, >id0
    st08 r0, r3

    ldr r0, >can_tx_identifier1_addr
    ldr r3, >id1_1
    st08 r0, r3

    ldr r0, >can_tx_data0_addr
    ldr r3, >frame_data_add
    st08 r0, r3

    // Save for tx interrupt
    clr r6
    add r6, r6, r3

    // r2 is still switches reg
    ldr r0, >can_tx_data1_addr
    st08 r0, r2

    // Save for tx interrupt
    clr r7
    add r7, r7, r2
    ldr r4, >byte_mask
    and r7, r7, r4

    ldr r0, >can_command_addr_ptr
    ld32 r0, r0
    clr r3
    addi r3, 0x01
    st08 r0, r3

    reti
    nop

.align
can_rx_data0_addr: .word 0x000F0116

can_rx_handler:
    // Dispatch CAN frame
    ldr r0, >can_rx_data0_addr
    ld08 r6, r0

    ldr r2, >frame_data_clear
    cmp eq r2, r6
    br true >can_handle_data_clear_frame
    nop

    ldr r2, >frame_data_add
    cmp eq r2, r6
    br true >can_handle_data_add_frame
    nop

    ldr r2, >frame_frequency
    cmp eq r2, r6
    br true >can_handle_frequency_frame
    nop

    // Unimplemented CAN frame
    reti
    nop

can_tx_handler:
    // Dispatch CAN frame

    ldr r2, >frame_data_clear
    cmp eq r2, r6
    br true >handle_data_clear_frame
    nop

    ldr r2, >frame_data_add
    cmp eq r2, r6
    br true >handle_data_add_frame
    nop

    ldr r2, >frame_frequency
    cmp eq r2, r6
    br true >handle_frequency_frame
    nop

    reti
    nop

can_send_frequency_frame:
    ldr r0, >can_tx_identifier0_addr
    ldr r3, >id0
    st08 r0, r3

    ldr r0, >can_tx_identifier1_addr
    ldr r3, >id1_2
    st08 r0, r3

    ldr r0, >can_tx_data0_addr
    ldr r3, >frame_frequency
    st08 r0, r3

    // Save for tx interrupt
    clr r6
    add r6, r6, r3

    // r2 is still switches reg
    ldr r0, >can_tx_data1_addr
    st08 r0, r2

    // Save for tx interrupt
    clr r7
    add r7, r7, r2
    ldr r4, >byte_mask
    and r7, r7, r4

    ldr r0, >can_tx_data2_addr
    rsh r2, r2, 8
    st08 r0, r2

    // Save for tx interrupt
    clr r8
    add r8, r8, r2
    ldr r4, >byte_mask
    and r8, r8, r4

    ldr r0, >can_command_addr_ptr
    ld32 r0, r0
    clr r3
    addi r3, 0x01
    st08 r0, r3

    reti
    nop

can_handle_data_clear_frame:
    // Release receive buffer
    ldr r0, >can_command_addr_ptr
    ld32 r0, r0
    clr r1
    addi r1, 0x04
    st08 r0, r1
handle_data_clear_frame:
    ldr r0, >scrolling_addr_ptr
    ldr r1, >clear_mask_ptr

    ld32 r0, r0
    ld32 r1, r1

    st32 r0, r1

    reti
    nop

.align
scrolling_addr_ptr: .word =scrolling_addr
write_mask_ptr: .word =write_mask
can_command_addr_ptr0: .word =can_command_addr
can_rx_data1_addr: .word 0x000F0117
can_rx_data2_addr: .word 0x000F0118

can_handle_data_add_frame:
    ldr r0, >can_rx_data1_addr
    ld08 r7, r0

    // Release receive buffer
    ldr r0, >can_command_addr_ptr0
    ld32 r0, r0
    clr r1
    addi r1, 0x04
    st08 r0, r1
handle_data_add_frame:
    // Expect symbol to add in r7 register

    ldr r0, >scrolling_addr_ptr
    ld32 r0, r0

    ldr r1, >write_mask_ptr
    ld32 r1, r1

    lsh r7, r7, 16
    or r4, r7, r1
    st32 r0, r4

    reti
    nop

.align
clear_mask_ptr: .word =clear_mask
scrolling_count_addr_ptr: .word =scrolling_count_addr

can_handle_frequency_frame:
    ldr r0, >can_rx_data1_addr
    ld08 r7, r0

    ldr r0, >can_rx_data2_addr
    ld08 r8, r0

    // Release receive buffer
    ldr r0, >can_command_addr_ptr0
    ld32 r0, r0
    clr r1
    addi r1, 0x04
    st08 r0, r1
handle_frequency_frame:
    // Expect count value in r7 and r8 register

    // For real board shift the count value by 16!

    // Concat bits
    lsh r8, r8, 8
    or r8, r8, r7
    lsh r8, r8, 16 // real board !!!

    ldr r0, >scrolling_count_addr_ptr
    ld32 r0, r0

    st32 r0, r8

    reti
    nop
