+#include "main.h"
+
+#include <stdarg.h>
+#include <string.h>
+
+typedef unsigned int uint;
+
+void debug_putc(int c)
+{
+ while (!LL_USART_IsActiveFlag_TXE(USART2))
+ ;
+ LL_USART_TransmitData8(USART2, c);
+}
+
+void debug_puts(const char *s)
+{
+ while (*s)
+ debug_putc(*s++);
+}
+
+enum printf_flags {
+ PF_ZERO_PAD = 1,
+ PF_SIGNED = 2,
+ PF_NEGATIVE = 4,
+ PF_UPPERCASE = 8,
+ PF_LEFT = 16,
+};
+
+static void printf_string(const char *s, uint width, uint flags)
+{
+ uint len = strlen(s);
+ uint pad = (len < width) ? width - len : 0;
+ char pad_char = (flags & PF_ZERO_PAD) ? '0' : ' ';
+
+ if (flags & PF_LEFT)
+ debug_puts(s);
+ while (pad--)
+ debug_putc(pad_char);
+ if (!(flags & PF_LEFT))
+ debug_puts(s);
+}
+
+static void printf_number(uint i, uint width, uint flags, uint base)
+{
+ char buf[16];
+ char *w = buf + sizeof(buf);
+
+ if (flags & PF_SIGNED)
+ {
+ if ((int) i < 0)
+ {
+ i = - (int) i;
+ flags |= PF_NEGATIVE;
+ }
+ }
+
+ *--w = 0;
+ do
+ {
+ uint digit = i % base;
+ if (digit < 10)
+ *--w = '0' + digit;
+ else
+ *--w = ((flags & PF_UPPERCASE) ? 'A' : 'a') + digit - 10;
+ i /= base;
+ }
+ while (i);
+
+ if (flags & PF_NEGATIVE)
+ *--w = '-';
+
+ printf_string(w, width, flags);
+}
+
+void debug_printf(const char *fmt, ...)
+{
+ va_list args;
+ va_start(args, fmt);
+
+ while (*fmt)
+ {
+ int c = *fmt++;
+ if (c != '%')
+ {
+ debug_putc(c);
+ continue;
+ }
+
+ uint width = 0;
+ uint flags = 0;
+
+ if (*fmt == '-')
+ {
+ fmt++;
+ flags |= PF_LEFT;
+ }
+
+ if (*fmt == '0')
+ {
+ fmt++;
+ flags |= PF_ZERO_PAD;
+ }
+
+ while (*fmt >= '0' && *fmt <= '9')
+ width = 10*width + *fmt++ - '0';
+
+ c = *fmt++;
+ switch (c)
+ {
+ case 'd':
+ printf_number(va_arg(args, int), width, flags | PF_SIGNED, 10);
+ break;
+ case 'u':
+ printf_number(va_arg(args, int), width, flags, 10);
+ break;
+ case 'X':
+ flags |= PF_UPPERCASE;
+ // fall-thru
+ case 'x':
+ printf_number(va_arg(args, int), width, flags, 16);
+ break;
+ case 's':
+ printf_string(va_arg(args, char *), width, flags);
+ break;
+ default:
+ debug_putc(c);
+ continue;
+ }
+ }
+
+ va_end(args);
+}
+
+void run_test(void)
+{
+ for (;;)
+ {
+ LL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
+ debug_printf("Testing printf: %08X %s\r\n", -42, "mnaeiou");
+ LL_mDelay(200);
+ }
+}