1 files changed, 24 insertions, 0 deletions
diff --git a/firmware/src/data_link.c b/firmware/src/data_link.c
new file mode 100644
index 0000000..a6d091a
--- /dev/null
+++ b/firmware/src/data_link.c
@@ -0,0 +1,24 @@
+#include <zephyr/drivers/uart.h>
+
+#include "data_link.h"
+
+void dl_send_frame(const struct device *const uart_dev, uint8_t *buffer,
+                   size_t len) {
+	for (size_t i = 0; i < len; i++) {
+		uint8_t octet = *(buffer + i);
+		switch (octet) {
+			case DL_SLIP_END:
+				uart_poll_out(uart_dev, DL_SLIP_ESC);
+				uart_poll_out(uart_dev, DL_SLIP_ESC_END);
+				break;
+			case DL_SLIP_ESC:
+				uart_poll_out(uart_dev, DL_SLIP_ESC);
+				uart_poll_out(uart_dev, DL_SLIP_ESC_ESC);
+				break;
+			default:
+				uart_poll_out(uart_dev, octet);
+				break;
+		}
+	}
+	uart_poll_out(uart_dev, DL_SLIP_END);
+}