test-ds/util-debug.c

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