Add primitive printf functionality to the USART.

5 files changed, 204 insertions, 6 deletions

diff --git a/03-refactor/include/printf.h b/03-refactor/include/printf.h
new file mode 100644
index 0000000..ec3eec0
--- /dev/null
+++ b/03-refactor/include/printf.h
@@ -0,0 +1,15 @@
+#ifndef PRINTF_H_
+#define PRINTF_H_
+
+#include <stdarg.h>
+#include <stdlib.h>
+
+typedef void(*printf_callback_t)(volatile void*, char);
+
+void printf_format(
+    const char* fmt,
+    printf_callback_t callback,
+    volatile void* callback_closure,
+    va_list lst);
+
+#endif /* PRINTF_H_ */
diff --git a/03-refactor/include/usart.h b/03-refactor/include/usart.h
index 3fea253..377048c 100644
--- a/03-refactor/include/usart.h
+++ b/03-refactor/include/usart.h
@@ -207,5 +207,7 @@ void usart_transmit_bytes(
 
 void usart_transmit_str(__IO usart_t* usart, const char* str);
 
+void usart_printf(__IO usart_t* usart, const char* fmt, ...);
+
 
 #endif /* H__USART_ */
diff --git a/03-refactor/src/main.c b/03-refactor/src/main.c
index 0545087..562fda9 100644
--- a/03-refactor/src/main.c
+++ b/03-refactor/src/main.c
@@ -73,21 +73,39 @@ int enable_usart1(uint32_t baud_rate)
   asm volatile(" cpsie i ");
 }
 
+void dwn() {
+  int val = 19;
+
+  while (val > 1) {
+    usart_printf(&USART2, "Value: 0x%02x\r\n", val);
+    if ((val & 1) == 0) {
+      val /= 2;
+    } else {
+      val = val * 3 + 1;
+    }
+  }
+  usart_printf(&USART2, "Value: 0x%02x\r\n", val);
+}
+
 /* Main function. This gets executed from the interrupt vector defined above. */
 int main()
 {
   /* Enable the GPIO port B. */
-
-  __IO gpio_port_t* port_b = enable_gpio(GPIO_PORT_B);
-  gpio_output_pin_t pin3 = set_gpio_pin_output(port_b, PIN_3);
-  gpio_output_pin_t pin1 = set_gpio_pin_output(port_b, PIN_1);
+  // __IO gpio_port_t* port_b = enable_gpio(GPIO_PORT_B);
+  // gpio_output_pin_t pin3 = set_gpio_pin_output(port_b, PIN_3);
+  // gpio_output_pin_t pin1 = set_gpio_pin_output(port_b, PIN_1);
 
   /* Enable a higher clock frequency. */
   set_system_clock_MHz(80);
 
   enable_usart2(115200);
 
-  pin_on(pin3);
-  usart_transmit_str(&USART2, "Hello, World\n");
+  // pin_on(pin3);
+  dwn();
+  usart_printf(&USART2, "Hello, %d!\r\n", -15);
   for(;;);
 }
+
+void do_thing(void(*fn)()) {
+  fn();
+}
diff --git a/03-refactor/src/printf.c b/03-refactor/src/printf.c
new file mode 100644
index 0000000..fa7f519
--- /dev/null
+++ b/03-refactor/src/printf.c
@@ -0,0 +1,152 @@
+#include "printf.h"
+
+enum PRINTF_TYPE {
+  PRINTF_TYPE_STRING = 0,
+  PRINTF_TYPE_INT = 1,
+  PRINTF_TYPE_PERCENT = 2,
+  PRINTF_TYPE_UNKNOWN = 999
+};
+
+static enum PRINTF_TYPE get_printf_type(const char* cur)
+{
+  while (*cur >= 0x30 && *cur < 0x3a) ++ cur;
+
+  if (*cur == 's') {
+    return PRINTF_TYPE_STRING;
+  } else if (*cur == 'd' || *cur == 'x' || *cur == 'X' || *cur == 'u') {
+    return PRINTF_TYPE_INT;
+  }
+
+  return PRINTF_TYPE_UNKNOWN;
+}
+
+static const char* printf_handle_string(
+  const char* fmt,
+  const char* next,
+  printf_callback_t callback,
+  volatile void* closure)
+{
+  const char* cur = next;
+  for (; *cur != 0; ++ cur) {
+    callback(closure, *cur);
+  }
+
+  return fmt;
+}
+
+static char printf_toupper(char ch)
+{
+  if (ch <= 0x7a && ch > 0x60) {
+    return ch - 0x20;
+  }
+  return ch;
+}
+
+static const char* parse_fmt(
+    const char* fmt,
+    int* out_padding,
+    char* out_pad_char)
+{
+  if (*fmt == '0') {
+    *out_pad_char = '0';
+    ++ fmt;
+  } else {
+    *out_pad_char = ' ';
+  }
+  int padding = 0;
+  while (*fmt < 0x3a && *fmt >= 0x30) {
+    padding *= 10;
+    padding += (*fmt ++) - 0x30;
+  }
+
+  *out_padding = padding;
+  return fmt;
+}
+
+static const char* mapping = "0123456789abcdef";
+static const char* printf_handle_int(
+  const char* fmt, unsigned int next, printf_callback_t callback, volatile void* closure)
+{
+  int base = 10;
+  char chars[32];
+  int pos = 31;
+  int upper = 0;
+  char pad_char;
+  int padding;
+  int is_unsigned = 0;
+
+  fmt = parse_fmt(fmt, &padding, &pad_char);
+
+  if (*fmt == 'x' || *fmt == 'X') {
+    base = 16;
+    is_unsigned = 1;
+  } else if (*fmt == 'u') {
+    is_unsigned = 1;
+  }
+
+  upper = *fmt == 'X';
+
+  int next_i = (int) next;
+  if (next_i < 0 && !is_unsigned) {
+    callback(closure, '-');
+    next_i *= -1;
+    next = (unsigned) next_i;
+  }
+
+  if (next == 0) {
+    callback(closure, '0');
+  } else {
+    while (next > 0) {
+      char next_ch = mapping[next % base];
+      if (upper) next_ch = printf_toupper(next_ch);
+      chars[pos --] = next_ch;
+      padding --;
+      next /= base;
+    }
+
+    while ((padding--) > 0) {
+      chars[pos --] = pad_char;
+    }
+
+    for (++ pos; pos < 32; ++ pos) {
+      callback(closure, chars[pos]);
+    }
+  }
+
+  return fmt;
+}
+
+void printf_format(
+    const char* fmt,
+    printf_callback_t callback,
+    volatile void* closure,
+    va_list ap) 
+{
+  const char* cur = fmt;
+
+  const char* next_string;
+  int next_int;
+
+  for (; *cur != 0; ++ cur) {
+    if (*cur == '%') {
+      // Handle the formatting here.
+      ++ cur;
+      enum PRINTF_TYPE printf_type = get_printf_type(cur);
+
+      switch (printf_type) {
+      case PRINTF_TYPE_PERCENT:
+        callback(closure, '%');
+        break;
+      case PRINTF_TYPE_STRING:
+        next_string = va_arg(ap, const char*);
+        cur = printf_handle_string(cur, next_string, callback, closure);
+        break;
+      case PRINTF_TYPE_INT:
+        next_int = va_arg(ap, int);
+        cur = printf_handle_int(cur, next_int, callback, closure);
+      }
+    } else {
+      callback(closure, *cur);
+    }
+  }
+}
diff --git a/03-refactor/src/usart.c b/03-refactor/src/usart.c
index fc58467..a64ad8b 100644
--- a/03-refactor/src/usart.c
+++ b/03-refactor/src/usart.c
@@ -1,5 +1,6 @@
 #include "usart.h"
 #include "delay.h"
+#include "printf.h"
 
 void set_usart1_clock_src(__IO rcc_t* rcc, usart_clk_src_t usart_clk_src)
 {
@@ -78,3 +79,13 @@ void usart_transmit_str(__IO usart_t* usart, const char* str)
     usart_transmit_byte(usart, *(str ++));
   }
 }
+
+void usart_printf(__IO usart_t* usart, const char* fmt, ...)
+{
+  printf_callback_t callback = (printf_callback_t) usart_transmit_byte;
+  volatile void* closure = usart;
+  va_list ap; 
+  va_start(ap, fmt);
+  printf_format(fmt, callback, closure, ap);
+  va_end(ap);
+}