diff --git a/programs/project.prog b/programs/project.prog
index be9561c..cb1c966 100644
--- a/programs/project.prog
+++ b/programs/project.prog
@@ -97,28 +97,6 @@ loop:
     br >loop
     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
 button_data_add: .word 0x10000
 button_data_clear: .word 0x20000
@@ -154,9 +132,25 @@ switches_interrupt_handler:
     reti
     nop
 
-can_tx_handler:
-    // Idee: lese hier die tx data register aus und calle die rx handler
-    // dafür mussen diese umgeschrieben werden um die werte nur über register auszuzlesen...
+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
 
@@ -192,6 +186,10 @@ can_send_data_clear_frame:
     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
@@ -201,6 +199,9 @@ can_send_data_clear_frame:
     reti
     nop
 
+.align
+byte_mask: .word 0xFF
+
 can_send_data_add_frame:
     ldr r0, >can_tx_identifier0_addr
     ldr r3, >id0
@@ -214,39 +215,19 @@ can_send_data_add_frame:
     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
-
-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
 
-    ldr r0, >can_tx_data2_addr
-    rsh r2, r2, 8
-    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
@@ -286,6 +267,73 @@ can_rx_handler:
     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
@@ -308,13 +356,14 @@ handle_data_clear_frame:
 .align
 scrolling_addr_ptr: .word =scrolling_addr
 write_mask_ptr: .word =write_mask
+can_command_addr_ptr0: .word =can_command_addr
 
 can_handle_data_add_frame:
     ldr r0, >can_rx_data1_addr
     ld08 r7, r0
 
     // Release receive buffer
-    ldr r0, >can_command_addr_ptr
+    ldr r0, >can_command_addr_ptr0
     ld32 r0, r0
     clr r1
     addi r1, 0x04
@@ -347,7 +396,7 @@ can_handle_frequency_frame:
     ld08 r8, r0
 
     // Release receive buffer
-    ldr r0, >can_command_addr_ptr
+    ldr r0, >can_command_addr_ptr0
     ld32 r0, r0
     clr r1
     addi r1, 0x04
diff --git a/soc/testbench/project_2top.vhd b/soc/testbench/project_2top.vhd
index dfdaba6..0171884 100644
--- a/soc/testbench/project_2top.vhd
+++ b/soc/testbench/project_2top.vhd
@@ -130,8 +130,21 @@ BEGIN
 		wait for 50us;
 		btn0 <= "00100"; -- freq
 		
+        wait for 50us;
+        sw <= x"000B";
+        btn0 <= "00000";
+		btn1 <= "00100"; -- freq
+		
 		wait for 50us;
 		btn0 <= "00010"; -- clear
+		
+        wait for 50us;
+		sw <= x"000C";
+		btn0 <= "00000";
+		btn1 <= "00001"; -- add	
+		
+        wait for 50us;
+		btn1 <= "00010"; -- clear
 				
 		wait for 100us;
 		assert false report "Simulation terminated!" severity failure;