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