Src/test.c

   1 #include "main.h"
   2
   3 #include <stdarg.h>
   4 #include <string.h>
   5
   6 typedef unsigned int uint;
   7
   8 void debug_putc(int c)
   9 {
  10   while (!LL_USART_IsActiveFlag_TXE(USART2))
  11     ;
  12   LL_USART_TransmitData8(USART2, c);
  13 }
  14
  15 void debug_puts(const char *s)
  16 {
  17   while (*s)
  18     debug_putc(*s++);
  19 }
  20
  21 enum printf_flags {
  22   PF_ZERO_PAD = 1,
  23   PF_SIGNED = 2,
  24   PF_NEGATIVE = 4,
  25   PF_UPPERCASE = 8,
  26   PF_LEFT = 16,
  27 };
  28
  29 static void printf_string(const char *s, uint width, uint flags)
  30 {
  31   uint len = strlen(s);
  32   uint pad = (len < width) ? width - len : 0;
  33   char pad_char = (flags & PF_ZERO_PAD) ? '0' : ' ';
  34
  35   if (flags & PF_LEFT)
  36     debug_puts(s);
  37   while (pad--)
  38     debug_putc(pad_char);
  39   if (!(flags & PF_LEFT))
  40     debug_puts(s);
  41 }
  42
  43 static void printf_number(uint i, uint width, uint flags, uint base)
  44 {
  45   char buf[16];
  46   char *w = buf + sizeof(buf);
  47
  48   if (flags & PF_SIGNED)
  49     {
  50       if ((int) i < 0)
  51         {
  52           i = - (int) i;
  53           flags |= PF_NEGATIVE;
  54         }
  55     }
  56
  57   *--w = 0;
  58   do
  59     {
  60       uint digit = i % base;
  61       if (digit < 10)
  62         *--w = '0' + digit;
  63       else
  64         *--w = ((flags & PF_UPPERCASE) ? 'A' : 'a') + digit - 10;
  65       i /= base;
  66     }
  67   while (i);
  68
  69   if (flags & PF_NEGATIVE)
  70     *--w = '-';
  71
  72   printf_string(w, width, flags);
  73 }
  74
  75 void debug_printf(const char *fmt, ...)
  76 {
  77   va_list args;
  78   va_start(args, fmt);
  79
  80   while (*fmt)
  81     {
  82       int c = *fmt++;
  83       if (c != '%')
  84         {
  85           debug_putc(c);
  86           continue;
  87         }
  88
  89       uint width = 0;
  90       uint flags = 0;
  91
  92       if (*fmt == '-')
  93         {
  94           fmt++;
  95           flags |= PF_LEFT;
  96         }
  97
  98       if (*fmt == '0')
  99         {
 100           fmt++;
 101           flags |= PF_ZERO_PAD;
 102         }
 103
 104       while (*fmt >= '0' && *fmt <= '9')
 105         width = 10*width + *fmt++ - '0';
 106
 107       c = *fmt++;
 108       switch (c)
 109         {
 110         case 'd':
 111           printf_number(va_arg(args, int), width, flags | PF_SIGNED, 10);
 112           break;
 113         case 'u':
 114           printf_number(va_arg(args, int), width, flags, 10);
 115           break;
 116         case 'X':
 117           flags |= PF_UPPERCASE;
 118           // fall-thru
 119         case 'x':
 120           printf_number(va_arg(args, int), width, flags, 16);
 121           break;
 122         case 's':
 123           printf_string(va_arg(args, char *), width, flags);
 124           break;
 125         default:
 126           debug_putc(c);
 127           continue;
 128         }
 129     }
 130
 131   va_end(args);
 132 }
 133
 134 void run_test(void)
 135 {
 136   for (;;)
 137     {
 138       LL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
 139       debug_printf("Testing printf: %08X %s\r\n", -42, "mnaeiou");
 140       LL_mDelay(200);
 141     }
 142 }