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_puts(const char *s)
  84 {
  85         while (*s)
  86                 debug_putc(*s++);
  87 }
  88
  89 enum printf_flags {
  90         PF_ZERO_PAD = 1,
  91         PF_SIGNED = 2,
  92         PF_NEGATIVE = 4,
  93         PF_UPPERCASE = 8,
  94         PF_LEFT = 16,
  95 };
  96
  97 static void printf_string(const char *s, uint width, uint flags)
  98 {
  99         uint len = strlen(s);
 100         uint pad = (len < width) ? width - len : 0;
 101         char pad_char = (flags & PF_ZERO_PAD) ? '0' : ' ';
 102
 103         if (flags & PF_LEFT)
 104                 debug_puts(s);
 105         while (pad--)
 106                 debug_putc(pad_char);
 107         if (!(flags & PF_LEFT))
 108                 debug_puts(s);
 109 }
 110
 111 static void printf_number(uint i, uint width, uint flags, uint base)
 112 {
 113         char buf[16];
 114         char *w = buf + sizeof(buf);
 115
 116         if (flags & PF_SIGNED) {
 117                 if ((int) i < 0) {
 118                         i = - (int) i;
 119                         flags |= PF_NEGATIVE;
 120                 }
 121         }
 122
 123         *--w = 0;
 124         do {
 125                 uint digit = i % base;
 126                 if (digit < 10)
 127                         *--w = '0' + digit;
 128                 else
 129                         *--w = ((flags & PF_UPPERCASE) ? 'A' : 'a') + digit - 10;
 130                 i /= base;
 131         }
 132         while (i);
 133
 134         if (flags & PF_NEGATIVE)
 135                 *--w = '-';
 136
 137         printf_string(w, width, flags);
 138 }
 139
 140 void debug_printf(const char *fmt, ...)
 141 {
 142         va_list args;
 143         va_start(args, fmt);
 144
 145         while (*fmt) {
 146                 int c = *fmt++;
 147                 if (c != '%') {
 148                         debug_putc(c);
 149                         continue;
 150                 }
 151
 152                 uint width = 0;
 153                 uint flags = 0;
 154
 155                 if (*fmt == '-') {
 156                         fmt++;
 157                         flags |= PF_LEFT;
 158                 }
 159
 160                 if (*fmt == '0') {
 161                         fmt++;
 162                         flags |= PF_ZERO_PAD;
 163                 }
 164
 165                 while (*fmt >= '0' && *fmt <= '9')
 166                         width = 10*width + *fmt++ - '0';
 167
 168                 c = *fmt++;
 169                 switch (c) {
 170                         case 'c':
 171                                 debug_putc(va_arg(args, int));
 172                                 break;
 173                         case 'd':
 174                                 printf_number(va_arg(args, int), width, flags | PF_SIGNED, 10);
 175                                 break;
 176                         case 'u':
 177                                 printf_number(va_arg(args, int), width, flags, 10);
 178                                 break;
 179                         case 'X':
 180                                 flags |= PF_UPPERCASE;
 181                                 // fall-thru
 182                         case 'x':
 183                                 printf_number(va_arg(args, int), width, flags, 16);
 184                                 break;
 185                         case 's':
 186                                 printf_string(va_arg(args, char *), width, flags);
 187                                 break;
 188                         default:
 189                                 debug_putc(c);
 190                                 continue;
 191                 }
 192         }
 193
 194         va_end(args);
 195 }
 196
 197 void debug_led(bool light)
 198 {
 199 #ifdef DEBUG_LED_GPIO
 200 #ifdef DEBUG_LED_INVERSE
 201         light = !light;
 202 #endif
 203         if (light)
 204                 gpio_set(DEBUG_LED_GPIO, DEBUG_LED_PIN);
 205         else
 206                 gpio_clear(DEBUG_LED_GPIO, DEBUG_LED_PIN);
 207 #endif
 208 }
 209
 210 void debug_led_toggle(void)
 211 {
 212 #ifdef DEBUG_LED_GPIO
 213         gpio_toggle(DEBUG_LED_GPIO, DEBUG_LED_PIN);
 214 #endif
 215 }