lib/util-debug.c

   1 /*
   2  *      Debugging Utilities for STM32
   3  *
   4  *      (c) 2018--2019 Martin Mareš <mj@ucw.cz>
   5  */
   6
   7 #include "util.h"
   8
   9 #include <libopencm3/stm32/gpio.h>
  10 #include <libopencm3/stm32/usart.h>
  11
  12 #include <stdarg.h>
  13 #include <string.h>
  14
  15 /*** Configuration ***/
  16
  17 // You should set the following parameters in config.h
  18
  19 // Use the semi-hosting interface for debugging messages
  20 // #define DEBUG_SEMIHOSTING
  21
  22 // Use this USART for debugging messages
  23 // #define DEBUG_USART USART1
  24
  25 // Use this LED for debugging
  26 #ifdef DEBUG_LED_BLUEPILL
  27 #define DEBUG_LED_GPIO GPIOC
  28 #define DEBUG_LED_PIN GPIO13
  29 #define DEBUG_LED_INVERSE
  30 #endif
  31
  32 /*** Implementation ***/
  33
  34 #ifdef DEBUG_SEMIHOSTING
  35
  36 void semi_put_char(char c)
  37 {
  38         // This is tricky, we need to work around GCC bugs
  39         volatile char cc = c;
  40         asm volatile (
  41                 "mov r0, #0x03\n"   /* SYS_WRITEC */
  42                 "mov r1, %[msg]\n"
  43                 "bkpt #0xAB\n"
  44                 :
  45                 : [msg] "r" (&cc)
  46                 : "r0", "r1"
  47         );
  48 }
  49
  50 void semi_write_string(char *c)
  51 {
  52         asm volatile (
  53                 "mov r0, #0x04\n"   /* SYS_WRITE0 */
  54                 "mov r1, %[msg]\n"
  55                 "bkpt #0xAB\n"
  56                 :
  57                 : [msg] "r" (c)
  58                 : "r0", "r1"
  59         );
  60 }
  61
  62 #endif
  63
  64 void debug_putc(int c)
  65 {
  66 #ifdef DEBUG_SEMIHOSTING
  67         static char debug_buf[128];
  68         static int debug_i;
  69         debug_buf[debug_i++] = c;
  70         if (c == '\n' || debug_i >= sizeof(debug_buf) - 1) {
  71                 debug_buf[debug_i] = 0;
  72                 semi_write_string(debug_buf);
  73                 debug_i = 0;
  74         }
  75 #endif
  76 #ifdef DEBUG_USART
  77         if (c == '\n')
  78                 usart_send_blocking(DEBUG_USART, '\r');
  79         usart_send_blocking(DEBUG_USART, c);
  80 #endif
  81 }
  82
  83 void debug_flush(void)
  84 {
  85 #ifdef DEBUG_USART
  86         while (!usart_get_flag(DEBUG_USART, USART_FLAG_TC))
  87                 ;
  88 #endif
  89 }
  90
  91 void debug_puts(const char *s)
  92 {
  93         while (*s)
  94                 debug_putc(*s++);
  95 }
  96
  97 enum printf_flags {
  98         PF_ZERO_PAD = 1,
  99         PF_SIGNED = 2,
 100         PF_NEGATIVE = 4,
 101         PF_UPPERCASE = 8,
 102         PF_LEFT = 16,
 103 };
 104
 105 static void printf_string(const char *s, uint width, uint flags)
 106 {
 107         uint len = strlen(s);
 108         uint pad = (len < width) ? width - len : 0;
 109         char pad_char = (flags & PF_ZERO_PAD) ? '0' : ' ';
 110
 111         if (flags & PF_LEFT)
 112                 debug_puts(s);
 113         while (pad--)
 114                 debug_putc(pad_char);
 115         if (!(flags & PF_LEFT))
 116                 debug_puts(s);
 117 }
 118
 119 static void printf_number(uint i, uint width, uint flags, uint base)
 120 {
 121         char buf[16];
 122         char *w = buf + sizeof(buf);
 123
 124         if (flags & PF_SIGNED) {
 125                 if ((int) i < 0) {
 126                         i = - (int) i;
 127                         flags |= PF_NEGATIVE;
 128                 }
 129         }
 130
 131         *--w = 0;
 132         do {
 133                 uint digit = i % base;
 134                 if (digit < 10)
 135                         *--w = '0' + digit;
 136                 else
 137                         *--w = ((flags & PF_UPPERCASE) ? 'A' : 'a') + digit - 10;
 138                 i /= base;
 139         }
 140         while (i);
 141
 142         if (flags & PF_NEGATIVE)
 143                 *--w = '-';
 144
 145         printf_string(w, width, flags);
 146 }
 147
 148 void debug_printf(const char *fmt, ...)
 149 {
 150         va_list args;
 151         va_start(args, fmt);
 152
 153         while (*fmt) {
 154                 int c = *fmt++;
 155                 if (c != '%') {
 156                         debug_putc(c);
 157                         continue;
 158                 }
 159
 160                 uint width = 0;
 161                 uint flags = 0;
 162
 163                 if (*fmt == '-') {
 164                         fmt++;
 165                         flags |= PF_LEFT;
 166                 }
 167
 168                 if (*fmt == '0') {
 169                         fmt++;
 170                         flags |= PF_ZERO_PAD;
 171                 }
 172
 173                 while (*fmt >= '0' && *fmt <= '9')
 174                         width = 10*width + *fmt++ - '0';
 175
 176                 c = *fmt++;
 177                 switch (c) {
 178                         case 'c':
 179                                 debug_putc(va_arg(args, int));
 180                                 break;
 181                         case 'd':
 182                                 printf_number(va_arg(args, int), width, flags | PF_SIGNED, 10);
 183                                 break;
 184                         case 'u':
 185                                 printf_number(va_arg(args, int), width, flags, 10);
 186                                 break;
 187                         case 'X':
 188                                 flags |= PF_UPPERCASE;
 189                                 // fall-thru
 190                         case 'x':
 191                                 printf_number(va_arg(args, int), width, flags, 16);
 192                                 break;
 193                         case 's':
 194                                 printf_string(va_arg(args, char *), width, flags);
 195                                 break;
 196                         default:
 197                                 debug_putc(c);
 198                                 continue;
 199                 }
 200         }
 201
 202         va_end(args);
 203 }
 204
 205 void debug_led(bool light)
 206 {
 207 #ifdef DEBUG_LED_GPIO
 208 #ifdef DEBUG_LED_INVERSE
 209         light = !light;
 210 #endif
 211         if (light)
 212                 gpio_set(DEBUG_LED_GPIO, DEBUG_LED_PIN);
 213         else
 214                 gpio_clear(DEBUG_LED_GPIO, DEBUG_LED_PIN);
 215 #endif
 216 }
 217
 218 void debug_led_toggle(void)
 219 {
 220 #ifdef DEBUG_LED_GPIO
 221         gpio_toggle(DEBUG_LED_GPIO, DEBUG_LED_PIN);
 222 #endif
 223 }