Data add over CAN working

programs/project.prog

@@ -46,8 +46,6 @@ 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_rx_data0_addr: .word 0x000F0116
-can_rx_data1_addr: .word 0x000F0117
 
 // CAN Constants
 acceptance_code: .word 0x00
@@ -59,7 +57,8 @@ btr1: .word 0x48
 output_control: .word 0x02
 control: .word 0xFE
 id0: .word 0xAA
-id1: .word 0xC1 // data length is also encoded here
+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
@@ -146,6 +145,12 @@ switches_interrupt_handler:
     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
+
     // Unimplemented button function
     
     reti
@@ -161,7 +166,7 @@ can_send_data_clear_frame:
     st08 r0, r3
 
     ldr r0, >can_tx_identifier1_addr
-    ldr r3, >id1
+    ldr r3, >id1_0
     st08 r0, r3
 
     ldr r0, >can_tx_data0_addr
@@ -176,6 +181,39 @@ 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
+    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
+
+    // r2 is still switches reg
+    ldr r0, >can_tx_data1_addr
+    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_handler:
     // Dispatch CAN frame
     ldr r0, >can_rx_data0_addr
@@ -202,10 +240,19 @@ can_rx_handler:
 
 .align
 scrolling_addr_ptr: .word =scrolling_addr
-write_mask_ptr: .word =write_mask_ptr
+write_mask_ptr: .word =write_mask
 
 can_handle_data_add_frame:
     // Expect symbol to add in r10 register
+    ldr r0, >can_rx_data1_addr
+    ld08 r10, r0
+
+    // Release receive buffer
+    ldr r0, >can_command_addr_ptr
+    ld32 r0, r0
+    clr r1
+    addi r1, 0x04
+    st08 r0, r1
 
     ldr r0, >scrolling_addr_ptr
     ld32 r0, r0
@@ -214,7 +261,7 @@ can_handle_data_add_frame:
     ld32 r1, r1
 
     lsh r10, r10, 16
-    or r4, r4, r1
+    or r4, r10, r1
     st32 r0, r4
 
     reti
@@ -222,7 +269,6 @@ can_handle_data_add_frame:
 
 .align
 clear_mask_ptr: .word =clear_mask
-can_command_addr_ptr: .word =can_command_addr
 
 can_handle_data_clear_frame:
     // Release receive buffer

soc/testbench/project_2top.vhd

@@ -120,6 +120,9 @@ BEGIN
 		wait for 3us;
 		btn0 <= "00001"; -- add
 		-- btn0 <= "00010"; -- clear
+		
+		wait for 25us;
+		btn0 <= "00010"; -- clear
 				
 		wait for 300us;
 		assert false report "Simulation terminated!" severity failure;