2 files changed, 38 insertions, 2 deletions
diff --git a/serial_link/protocol/frame_router.c b/serial_link/protocol/frame_router.c
index 0675cea284..861c0d7d29 100644
--- a/serial_link/protocol/frame_router.c
+++ b/serial_link/protocol/frame_router.c
@@ -32,10 +32,14 @@ void router_set_master(bool master) {
 }
 
 void route_incoming_frame(uint8_t link, uint8_t* data, uint16_t size){
-    transport_recv_frame(0, data, size);
+    if (data[size-1] & 1) {
+        transport_recv_frame(0, data, size - 1);
+    }
+    data[size-1] >>= 1;
     validator_send_frame(DOWN_LINK, data, size);
 }
 
 void router_send_frame(uint8_t destination, uint8_t* data, uint16_t size) {
-    validator_send_frame(DOWN_LINK, data, size);
+    data[size] = destination;
+    validator_send_frame(DOWN_LINK, data, size + 1);
 }
diff --git a/serial_link/tests/frame_router_tests.c b/serial_link/tests/frame_router_tests.c
index eb0c710ea1..7a4ccb0c40 100644
--- a/serial_link/tests/frame_router_tests.c
+++ b/serial_link/tests/frame_router_tests.c
@@ -120,3 +120,35 @@ Ensure(ByteStuffer, master_broadcast_is_received_by_everyone) {
     assert_that(router_buffers[2].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
     assert_that(router_buffers[2].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
 }
+
+Ensure(ByteStuffer, master_send_is_received_by_targets) {
+    printf("Here\n");
+    frame_buffer_t data;
+    data.data = 0xAB7055BB;
+    activate_router(0);
+    router_send_frame((1 << 1) | (1 << 2), (uint8_t*)&data, 4);
+    assert_that(router_buffers[0].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
+    assert_that(router_buffers[0].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
+
+    simulate_transport(0, 1);
+    assert_that(router_buffers[1].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
+    assert_that(router_buffers[1].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
+
+    expect(transport_recv_frame,
+        when(from, is_equal_to(0)),
+        when(size, is_equal_to(4)),
+        when(data, is_equal_to_contents_of(&data.data, 4))
+    );
+    simulate_transport(1, 2);
+    assert_that(router_buffers[2].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
+    assert_that(router_buffers[2].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
+
+    expect(transport_recv_frame,
+        when(from, is_equal_to(0)),
+        when(size, is_equal_to(4)),
+        when(data, is_equal_to_contents_of(&data.data, 4))
+    );
+    simulate_transport(2, 3);
+    assert_that(router_buffers[3].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
+    assert_that(router_buffers[3].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
+}