From c28e19c5467e92f532f64db020ad4817b531dbec Mon Sep 17 00:00:00 2001
From: Fred Sundvik <fsundvik@gmail.com>
Date: Sat, 20 Feb 2016 15:02:53 +0200
Subject: [PATCH] First link sending to master

---
 serial_link/protocol/frame_router.c    | 22 +++++++++++++-----
 serial_link/tests/frame_router_tests.c | 32 +++++++++++++++++++++-----
 2 files changed, 42 insertions(+), 12 deletions(-)

diff --git a/serial_link/protocol/frame_router.c b/serial_link/protocol/frame_router.c
index 861c0d7d29..480673f0c0 100644
--- a/serial_link/protocol/frame_router.c
+++ b/serial_link/protocol/frame_router.c
@@ -32,14 +32,24 @@ void router_set_master(bool master) {
 }
 
 void route_incoming_frame(uint8_t link, uint8_t* data, uint16_t size){
-    if (data[size-1] & 1) {
-        transport_recv_frame(0, data, size - 1);
+    if (is_master) {
+        transport_recv_frame(1, data, size);
+    }
+    else {
+        if (data[size-1] & 1) {
+            transport_recv_frame(0, data, size - 1);
+        }
+        data[size-1] >>= 1;
+        validator_send_frame(DOWN_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) {
-    data[size] = destination;
-    validator_send_frame(DOWN_LINK, data, size + 1);
+    if (destination == 0) {
+       validator_send_frame(UP_LINK, data, size);
+    }
+    else {
+        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 7a4ccb0c40..02e6367d21 100644
--- a/serial_link/tests/frame_router_tests.c
+++ b/serial_link/tests/frame_router_tests.c
@@ -46,13 +46,13 @@ router_buffer_t router_buffers[8];
 router_buffer_t* current_router_buffer;
 
 
-Describe(ByteStuffer);
-BeforeEach(ByteStuffer) {
+Describe(FrameRouter);
+BeforeEach(FrameRouter) {
     init_byte_stuffer();
     memset(router_buffers, 0, sizeof(router_buffers));
     current_router_buffer = 0;
 }
-AfterEach(ByteStuffer) {}
+AfterEach(FrameRouter) {}
 
 typedef struct {
     uint32_t data;
@@ -81,6 +81,9 @@ static void activate_router(uint8_t num) {
 static void simulate_transport(uint8_t from, uint8_t to) {
    activate_router(to);
    if (from > to) {
+       receive_data(DOWN_LINK,
+               router_buffers[from].send_buffers[UP_LINK].sent_data,
+               router_buffers[from].send_buffers[UP_LINK].sent_data_size);
    }
    else if(to > from) {
        receive_data(UP_LINK,
@@ -94,7 +97,7 @@ void transport_recv_frame(uint8_t from, uint8_t* data, uint16_t size) {
 }
 
 
-Ensure(ByteStuffer, master_broadcast_is_received_by_everyone) {
+Ensure(FrameRouter, master_broadcast_is_received_by_everyone) {
     frame_buffer_t data;
     data.data = 0xAB7055BB;
     activate_router(0);
@@ -121,8 +124,7 @@ Ensure(ByteStuffer, master_broadcast_is_received_by_everyone) {
     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");
+Ensure(FrameRouter, master_send_is_received_by_targets) {
     frame_buffer_t data;
     data.data = 0xAB7055BB;
     activate_router(0);
@@ -152,3 +154,21 @@ Ensure(ByteStuffer, master_send_is_received_by_targets) {
     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));
 }
+
+Ensure(FrameRouter, first_sends_to_master) {
+    frame_buffer_t data;
+    data.data = 0xAB7055BB;
+    activate_router(1);
+    router_send_frame(0, (uint8_t*)&data, 4);
+    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(1)),
+        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));
+}