ssr/Src/debug.c

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