1 files changed, 29 insertions, 0 deletions
diff --git a/controller/fw/src/serial.c b/controller/fw/src/serial.c
index 71e1868..2d09f80 100644
--- a/controller/fw/src/serial.c
+++ b/controller/fw/src/serial.c
@@ -11,6 +11,8 @@
 static void usart_schedule_dma(volatile struct usart_desc *us);
 static void usart_dma_reset(volatile struct usart_desc *us);
 static void usart_putc_nonblocking_tpf(void *us, char c);
+static void usart_wait_chunk_free(volatile struct usart_desc *us);
+static void usart_putc_blocking_tpf(void *us, char c);
 
 void usart_dma_reset(volatile struct usart_desc *us) {
     us->tx_buf.xfr_start = -1;
@@ -117,10 +119,24 @@ int usart_putc_nonblocking(volatile struct usart_desc *us, char c) {
     return 0;
 }
 
+int usart_putc_blocking(volatile struct usart_desc *us, char c) {
+    volatile struct dma_tx_buf *buf = &us->tx_buf;
+
+    while (buf->wr_pos == buf->xfr_start)
+        ;
+
+    buf->data[buf->wr_pos] = c;
+    buf->wr_pos = (buf->wr_pos + 1) % sizeof(us->data);
+    return 0;
+}
+
 void usart_putc_nonblocking_tpf(void *us, char c) {
     usart_putc_nonblocking((struct usart_desc *)us, c);
 }
 
+void usart_putc_blocking_tpf(void *us, char c) {
+    usart_putc_blocking((struct usart_desc *)us, c);
+}
 
 int usart_send_chunk_nonblocking(volatile struct usart_desc *us, const char *chunk, size_t chunk_len) {
     for (size_t i=0; i<chunk_len; i++)
@@ -129,6 +145,11 @@ int usart_send_chunk_nonblocking(volatile struct usart_desc *us, const char *chu
     return usart_flush(us);
 }
 
+void usart_wait_chunk_free(volatile struct usart_desc *us) {
+    while (us->tx_buf.chunk_end[us->tx_buf.wr_idx] != -1)
+        ;
+}
+
 int usart_flush(volatile struct usart_desc *us) {
     /* Find a free slot for this chunk */
     if (us->tx_buf.chunk_end[us->tx_buf.wr_idx] != -1) {
@@ -151,3 +172,11 @@ int usart_printf(volatile struct usart_desc *us, char *fmt, ...) {
     return usart_flush(us);
 }
 
+int usart_printf_blocking(volatile struct usart_desc *us, char *fmt, ...) {
+    va_list va;
+    va_start(va, fmt);
+    tfp_format((void *)us, usart_putc_blocking_tpf, fmt, va);
+    usart_wait_chunk_free(us);
+    return usart_flush(us);
+}
+