From 532f98eef6a29ae83d7ed61fbc4e25e23ad972d5 Mon Sep 17 00:00:00 2001
From: Fred Sundvik <fsundvik@gmail.com>
Date: Sat, 20 Feb 2016 15:14:40 +0200
Subject: [PATCH] Support for other links sending to master

---
 serial_link/protocol/frame_router.c    | 19 +++++++++++++------
 serial_link/tests/frame_router_tests.c | 25 ++++++++++++++++++++++++-
 2 files changed, 37 insertions(+), 7 deletions(-)

diff --git a/serial_link/protocol/frame_router.c b/serial_link/protocol/frame_router.c
index 480673f0c..c61c50ebd 100644
--- a/serial_link/protocol/frame_router.c
+++ b/serial_link/protocol/frame_router.c
@@ -33,20 +33,27 @@ void router_set_master(bool master) {
 
 void route_incoming_frame(uint8_t link, uint8_t* data, uint16_t size){
     if (is_master) {
-        transport_recv_frame(1, data, size);
+        transport_recv_frame(data[size-1], data, size - 1);
     }
     else {
-        if (data[size-1] & 1) {
-            transport_recv_frame(0, data, size - 1);
+        if (link == UP_LINK) {
+            if (data[size-1] & 1) {
+                transport_recv_frame(0, data, size - 1);
+            }
+            data[size-1] >>= 1;
+            validator_send_frame(DOWN_LINK, data, size);
+        }
+        else {
+            data[size-1]++;
+            validator_send_frame(UP_LINK, data, size);
         }
-        data[size-1] >>= 1;
-        validator_send_frame(DOWN_LINK, data, size);
     }
 }
 
 void router_send_frame(uint8_t destination, uint8_t* data, uint16_t size) {
     if (destination == 0) {
-       validator_send_frame(UP_LINK, data, size);
+        data[size] = 1;
+        validator_send_frame(UP_LINK, data, size + 1);
     }
     else {
         data[size] = destination;
diff --git a/serial_link/tests/frame_router_tests.c b/serial_link/tests/frame_router_tests.c
index 02e6367d2..6b00aca67 100644
--- a/serial_link/tests/frame_router_tests.c
+++ b/serial_link/tests/frame_router_tests.c
@@ -155,7 +155,7 @@ Ensure(FrameRouter, master_send_is_received_by_targets) {
     assert_that(router_buffers[3].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
 }
 
-Ensure(FrameRouter, first_sends_to_master) {
+Ensure(FrameRouter, first_link_sends_to_master) {
     frame_buffer_t data;
     data.data = 0xAB7055BB;
     activate_router(1);
@@ -172,3 +172,26 @@ Ensure(FrameRouter, first_sends_to_master) {
     assert_that(router_buffers[0].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
     assert_that(router_buffers[0].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
 }
+
+Ensure(FrameRouter, second_link_sends_to_master) {
+    printf("Second to master start\n");
+    frame_buffer_t data;
+    data.data = 0xAB7055BB;
+    activate_router(2);
+    router_send_frame(0, (uint8_t*)&data, 4);
+    assert_that(router_buffers[2].send_buffers[UP_LINK].sent_data_size, is_greater_than(0));
+    assert_that(router_buffers[2].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
+
+    simulate_transport(2, 1);
+    assert_that(router_buffers[1].send_buffers[UP_LINK].sent_data_size, is_greater_than(0));
+    assert_that(router_buffers[1].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
+
+    expect(transport_recv_frame,
+        when(from, is_equal_to(2)),
+        when(size, is_equal_to(4)),
+        when(data, is_equal_to_contents_of(&data.data, 4))
+    );
+    simulate_transport(1, 0);
+    assert_that(router_buffers[0].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
+    assert_that(router_buffers[0].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
+}