Src/debug.c

   1 #include "util.h"
   2 #include "stm32f1xx_hal.h"
   3
   4 #include <stdarg.h>
   5 #include <string.h>
   6
   7 void semi_put_char(char c)
   8 {
   9 #ifdef DEBUG_SEMIHOSTING
  10   // This is tricky, we need to work around GCC bugs
  11   volatile char cc = c;
  12   asm volatile (
  13     "mov r0, #0x03\n"   /* SYS_WRITEC */
  14     "mov r1, %[msg]\n"
  15     "bkpt #0xAB\n"
  16     :
  17     : [msg] "r" (&cc)
  18     : "r0", "r1"
  19   );
  20 #endif
  21 }
  22
  23 void semi_write_string(char *c)
  24 {
  25 #ifdef DEBUG_SEMIHOSTING
  26   asm volatile (
  27     "mov r0, #0x04\n"   /* SYS_WRITE0 */
  28     "mov r1, %[msg]\n"
  29     "bkpt #0xAB\n"
  30     :
  31     : [msg] "r" (c)
  32     : "r0", "r1"
  33   );
  34 #endif
  35 }
  36
  37 void debug_putc(int c)
  38 {
  39   static char debug_buf[128];
  40   static int debug_i;
  41   debug_buf[debug_i++] = c;
  42   if (c == '\n' || debug_i >= sizeof(debug_buf) - 1)
  43     {
  44       debug_buf[debug_i] = 0;
  45       semi_write_string(debug_buf);
  46       debug_i = 0;
  47     }
  48 }
  49
  50 void debug_puts(const char *s)
  51 {
  52   while (*s)
  53     debug_putc(*s++);
  54 }
  55
  56 enum printf_flags {
  57   PF_ZERO_PAD = 1,
  58   PF_SIGNED = 2,
  59   PF_NEGATIVE = 4,
  60   PF_UPPERCASE = 8,
  61   PF_LEFT = 16,
  62 };
  63
  64 static void printf_string(const char *s, uint width, uint flags)
  65 {
  66   uint len = strlen(s);
  67   uint pad = (len < width) ? width - len : 0;
  68   char pad_char = (flags & PF_ZERO_PAD) ? '0' : ' ';
  69
  70   if (flags & PF_LEFT)
  71     debug_puts(s);
  72   while (pad--)
  73     debug_putc(pad_char);
  74   if (!(flags & PF_LEFT))
  75     debug_puts(s);
  76 }
  77
  78 static void printf_number(uint i, uint width, uint flags, uint base)
  79 {
  80   char buf[16];
  81   char *w = buf + sizeof(buf);
  82
  83   if (flags & PF_SIGNED)
  84     {
  85       if ((int) i < 0)
  86         {
  87           i = - (int) i;
  88           flags |= PF_NEGATIVE;
  89         }
  90     }
  91
  92   *--w = 0;
  93   do
  94     {
  95       uint digit = i % base;
  96       if (digit < 10)
  97         *--w = '0' + digit;
  98       else
  99         *--w = ((flags & PF_UPPERCASE) ? 'A' : 'a') + digit - 10;
 100       i /= base;
 101     }
 102   while (i);
 103
 104   if (flags & PF_NEGATIVE)
 105     *--w = '-';
 106
 107   printf_string(w, width, flags);
 108 }
 109
 110 void debug_printf(const char *fmt, ...)
 111 {
 112   va_list args;
 113   va_start(args, fmt);
 114
 115   while (*fmt)
 116     {
 117       int c = *fmt++;
 118       if (c != '%')
 119         {
 120           debug_putc(c);
 121           continue;
 122         }
 123
 124       uint width = 0;
 125       uint flags = 0;
 126
 127       if (*fmt == '-')
 128         {
 129           fmt++;
 130           flags |= PF_LEFT;
 131         }
 132
 133       if (*fmt == '0')
 134         {
 135           fmt++;
 136           flags |= PF_ZERO_PAD;
 137         }
 138
 139       while (*fmt >= '0' && *fmt <= '9')
 140         width = 10*width + *fmt++ - '0';
 141
 142       c = *fmt++;
 143       switch (c)
 144         {
 145         case 'd':
 146           printf_number(va_arg(args, int), width, flags | PF_SIGNED, 10);
 147           break;
 148         case 'u':
 149           printf_number(va_arg(args, int), width, flags, 10);
 150           break;
 151         case 'X':
 152           flags |= PF_UPPERCASE;
 153           // fall-thru
 154         case 'x':
 155           printf_number(va_arg(args, int), width, flags, 16);
 156           break;
 157         case 's':
 158           printf_string(va_arg(args, char *), width, flags);
 159           break;
 160         default:
 161           debug_putc(c);
 162           continue;
 163         }
 164     }
 165
 166   va_end(args);
 167 }