1 files changed, 65 insertions, 1 deletions

diff --git a/serial_link/system/system.c b/serial_link/system/system.c
index 2001f4d5c..2dbd9a4c1 100644
--- a/serial_link/system/system.c
+++ b/serial_link/system/system.c
@@ -22,19 +22,83 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.
 */
 #include "serial_link/system/system.h"
+#include "hal.h"
+#include "serial_link/protocol/byte_stuffer.h"
+#include "serial_link/protocol/transport.h"
+#include "serial_link/protocol/frame_router.h"
+#include <stdbool.h>
+
+static event_source_t new_data_event;
+
+
+// Slow speed for testing
+static SerialConfig config = {
+    .sc_speed = 38400
+};
+
+static uint32_t read_from_serial(SerialDriver* driver) {
+    const uint32_t buffer_size = 16;
+    uint8_t buffer[buffer_size];
+    uint32_t bytes_read = sdAsynchronousRead(driver, buffer, buffer_size);
+    uint8_t* current = buffer;
+    uint8_t* end = current + bytes_read;
+    while(current < end) {
+        byte_stuffer_recv_byte(0, *current);
+        current++;
+    }
+    return bytes_read;
+}
 
 // TODO: Optimize the stack size, this is probably way too big
 static THD_WORKING_AREA(serialThreadStack, 1024);
 static THD_FUNCTION(serialThread, arg) {
     (void)arg;
+    event_listener_t new_data_listener;
+    event_listener_t sd1_listener;
+    event_listener_t sd2_listener;
+    chEvtRegister(&new_data_event, &new_data_listener, 0);
+    chEvtRegisterMaskWithFlags(chnGetEventSource(&SD1),
+        &sd1_listener,
+        EVENT_MASK(1),
+        CHN_INPUT_AVAILABLE);
+    chEvtRegisterMaskWithFlags(chnGetEventSource(&SD2),
+        &sd2_listener,
+        EVENT_MASK(2),
+        CHN_INPUT_AVAILABLE);
+    bool need_wait = false;
+    while(true) {
+        if (need_wait) {
+            chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(1000));
+        }
+        bool is_master = usbGetDriverStateI(&USBD1) == USB_ACTIVE;
+        router_set_master(is_master);
+
+        need_wait = true;
+        need_wait &= read_from_serial(&SD1) == 0;
+        need_wait &= read_from_serial(&SD2) == 0;
+        update_transport();
+    }
+}
+
+void send_data(uint8_t link, const uint8_t* data, uint16_t size) {
+    if (link == 0) {
+        sdWrite(&SD1, data, size);
+    }
+    else {
+        sdWrite(&SD2, data, size);
+    }
 }
 
 
 void init_serial_link(void) {
+    init_byte_stuffer();
+    sdStart(&SD1, &config);
+    sdStart(&SD2, &config);
+    chEvtObjectInit(&new_data_event);
     (void)chThdCreateStatic(serialThreadStack, sizeof(serialThreadStack),
                               LOWPRIO, serialThread, NULL);
 }
 
 void signal_data_written(void) {
-
+    chEvtBroadcast(&new_data_event);
 }