Support for other links sending to master

1 files changed, 13 insertions, 6 deletions

diff --git a/serial_link/protocol/frame_router.c b/serial_link/protocol/frame_router.c
index 480673f0c0..c61c50ebd6 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;