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