firmware: Implement send_frame()

This transmits a frame via the Serial Line Internet Protocol (SLIP).

1 files changed, 30 insertions, 3 deletions

diff --git a/firmware/src/main.c b/firmware/src/main.c
index a9721ef..c8cddb2 100644
--- a/firmware/src/main.c
+++ b/firmware/src/main.c
@@ -1,13 +1,38 @@
 #include <stdbool.h>
+#include <stdint.h>
 
-#include <zephyr/kernel.h>
 #include <zephyr/device.h>
 #include <zephyr/drivers/uart.h>
+#include <zephyr/kernel.h>
+#include <zephyr/sys/util.h>
 
 #define UART_DEVICE_NODE DT_CHOSEN(zephyr_shell_uart)
 static const struct device *const uart_dev = DEVICE_DT_GET(UART_DEVICE_NODE);
 
-#define SLIP_END 0xC0
+#define SLIP_END         0xC0
+#define SLIP_ESC         0xDB
+#define SLIP_ESC_END     0xDC
+#define SLIP_ESC_ESC     0xDD
+
+void send_frame(uint8_t *buffer, size_t len) {
+	for (size_t i = 0; i < len; i++) {
+		uint8_t octet = *(buffer + i);
+		switch (octet) {
+			case SLIP_END:
+				uart_poll_out(uart_dev, SLIP_ESC);
+				uart_poll_out(uart_dev, SLIP_ESC_END);
+				break;
+			case SLIP_ESC:
+				uart_poll_out(uart_dev, SLIP_ESC);
+				uart_poll_out(uart_dev, SLIP_ESC_ESC);
+				break;
+			default:
+				uart_poll_out(uart_dev, octet);
+				break;
+		}
+	}
+	uart_poll_out(uart_dev, SLIP_END);
+}
 
 int main(void)
 {
@@ -16,9 +41,11 @@ int main(void)
 		return 0;
 	}
 
+	uint8_t frame[] = {0xDE, 0xAD};
+
 	while (true) {
 		k_sleep(K_MSEC(1000));
-		uart_poll_out(uart_dev, SLIP_END);
+		send_frame(frame, ARRAY_SIZE(frame));
 	}
 
 	return 0;