Add primitive printf ability to usart.{h,c}.

2 files changed, 49 insertions, 0 deletions

diff --git a/02-usart/include/usart.h b/02-usart/include/usart.h
index c645e92..9ed6dbb 100644
--- a/02-usart/include/usart.h
+++ b/02-usart/include/usart.h
@@ -217,5 +217,7 @@ void usart_transmit_bytes_sync(
 
 void usart_transmit_str_sync(__IO usart_t* usart, const char* str);
 
+void usart_printf(__IO usart_t* usart, const char* fmt, ...);
+
 
 #endif /* H__USART_ */
diff --git a/02-usart/src/usart.c b/02-usart/src/usart.c
index d4073d0..07bf90c 100644
--- a/02-usart/src/usart.c
+++ b/02-usart/src/usart.c
@@ -1,5 +1,7 @@
 #include "usart.h"
 #include "delay.h"
+#include "lib.h"
+#include <stdarg.h>
 
 void set_usart1_clock_src(__IO rcc_t* rcc, usart_clk_src_t usart_clk_src)
 {
@@ -96,3 +98,48 @@ void usart_enable_dma(__IO usart_t* usart, usart_enable_t enabled)
       break;
   };
 }
+
+void usart_printf(__IO usart_t* usart, const char* fmt, ...)
+{
+  va_list l;
+  union {
+    void* ptr;
+    int i;
+  } b;
+  char buf[128];
+
+  va_start(l, fmt);
+
+  while (*fmt != 0) {
+    if (*fmt == '%') {
+      switch (*(++fmt)) {
+        case 0:
+          goto end;
+        case '%':
+          usart_transmit_byte_sync(usart, '%');
+          break;
+        case 'p':
+          b.ptr = va_arg(l, void*);
+          hexify(ptr2reg(b.ptr), buf);
+          usart_transmit_str_sync(usart, "0x");
+          usart_transmit_str_sync(usart, buf);
+          break;
+        case 'd':
+        case 'i':
+          b.i = va_arg(l, int);
+          decimalify(b.i, buf);
+          usart_transmit_str_sync(usart, buf);
+          break;
+      }
+      ++ fmt;
+    } else {
+      if (*fmt == '\n') {
+        usart_transmit_byte_sync(usart, '\r');
+      }
+      usart_transmit_byte_sync(usart, *(fmt ++));
+    }
+  }
+
+end:
+  va_end(l);
+}