Update frequency over CAN working

programs/project.prog

@@ -42,10 +42,6 @@ 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_tx_identifier0_addr: .word 0x000F010A
-can_tx_identifier1_addr: .word 0x000F010B
-can_tx_data0_addr: .word 0x000F010C
-can_tx_data1_addr: .word 0x000F010D
 
 // CAN Constants
 acceptance_code: .word 0x00
@@ -56,17 +52,8 @@ btr0: .word 0x80
 btr1: .word 0x48
 output_control: .word 0x02
 control: .word 0xFE
-id0: .word 0xAA
-id1_0: .word 0xC1 // data length is also encoded here
-id1_1: .word 0xC2 // data length is also encoded here
 rx_interrupt_mask: .word 0x01
 tx_interrupt_mask: .word 0x02
-frame_data_add: .word 0x00
-frame_data_clear: .word 0x01
-frame_frequency: .word 0x02
-button_data_add: .word 0x10000
-button_data_clear: .word 0x20000
-button_frequency: .word 0x40000
 
 main:
     // Initialize stack pointer to the end of the data memory
@@ -132,6 +119,11 @@ can_interrupt_handler:
     reti
     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
@@ -151,6 +143,12 @@ switches_interrupt_handler:
     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
@@ -160,6 +158,25 @@ can_tx_handler:
     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
@@ -173,7 +190,8 @@ can_send_data_clear_frame:
     ldr r3, >frame_data_clear
     st08 r0, r3
 
-    ldr r0, >can_command_addr
+    ldr r0, >can_command_addr_ptr
+    ld32 r0, r0
     clr r3
     addi r3, 0x01
     st08 r0, r3
@@ -181,9 +199,6 @@ can_send_data_clear_frame:
     reti
     nop
 
-.align
-can_command_addr_ptr: .word =can_command_addr
-
 can_send_data_add_frame:
     ldr r0, >can_tx_identifier0_addr
     ldr r3, >id0
@@ -210,9 +225,40 @@ can_send_data_add_frame:
     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
+
+    // r2 is still switches reg
+    ldr r0, >can_tx_data1_addr
+    st08 r0, r2
+
+    ldr r0, >can_tx_data2_addr
+    rsh r2, r2, 8
+    st08 r0, r2
+
+    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_data1_addr: .word 0x000F0117
+can_rx_data2_addr: .word 0x000F0118
 
 can_rx_handler:
     // Dispatch CAN frame
@@ -289,8 +335,35 @@ can_handle_data_clear_frame:
     reti
     nop
 
+.align
+scrolling_count_addr_ptr: .word =scrolling_count_addr
+
 can_handle_frequency_frame:
-    // TODO
+    // For real board shift the count value by 16!
+
+    ldr r0, >can_rx_data1_addr
+    ld08 r9, r0
+
+    ldr r0, >can_rx_data2_addr
+    ld08 r10, r0
+
+    // Concat bits
+    lsh r10, r10, 8
+    or r10, r10, r9
+    // lsh r10, r10, 16 // real board !!!
+
+    // Release receive buffer
+    ldr r0, >can_command_addr_ptr
+    ld32 r0, r0
+    clr r1
+    addi r1, 0x04
+    st08 r0, r1
+
+    ldr r0, >scrolling_count_addr_ptr
+    ld32 r0, r0
+
+    st32 r0, r10
+
     reti
     nop