]> mj.ucw.cz Git - home-hw.git/blob - test-ds/util-debug.c
Merge branch 'master' of ssh://git.ucw.cz/home/mj/GIT/home-hw
[home-hw.git] / 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 }