Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 1 | #include <stdint.h> |
| 2 | #include <string.h> |
| 3 | #include <stdarg.h> |
| 4 | |
| 5 | #include "std.h" |
| 6 | #include "misc.h" |
| 7 | #include "systick.h" |
| 8 | #include "mpconfig.h" |
| 9 | #include "qstr.h" |
| 10 | #include "obj.h" |
Dave Hylands | baf6f14 | 2014-03-30 21:06:50 -0700 | [diff] [blame] | 11 | #include "pfenv.h" |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 12 | #if 0 |
| 13 | #include "lcd.h" |
| 14 | #endif |
Damien George | 7533700 | 2014-04-21 12:03:09 +0100 | [diff] [blame] | 15 | #include "uart.h" |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 16 | #include "usb.h" |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 17 | |
| 18 | #if MICROPY_ENABLE_FLOAT |
| 19 | #include "formatfloat.h" |
| 20 | #endif |
| 21 | |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 22 | void pfenv_prints(const pfenv_t *pfenv, const char *str) { |
| 23 | pfenv->print_strn(pfenv->data, str, strlen(str)); |
| 24 | } |
| 25 | |
| 26 | int pfenv_printf(const pfenv_t *pfenv, const char *fmt, va_list args) { |
| 27 | int chrs = 0; |
| 28 | for (;;) { |
| 29 | { |
| 30 | const char *f = fmt; |
| 31 | while (*f != '\0' && *f != '%') { |
| 32 | ++f; // XXX UTF8 advance char |
| 33 | } |
| 34 | if (f > fmt) { |
| 35 | pfenv->print_strn(pfenv->data, fmt, f - fmt); |
| 36 | chrs += f - fmt; |
| 37 | fmt = f; |
| 38 | } |
| 39 | } |
| 40 | |
| 41 | if (*fmt == '\0') { |
| 42 | break; |
| 43 | } |
| 44 | |
| 45 | // move past % character |
| 46 | ++fmt; |
| 47 | |
| 48 | // parse flags, if they exist |
| 49 | int flags = 0; |
Dave Hylands | baf6f14 | 2014-03-30 21:06:50 -0700 | [diff] [blame] | 50 | char fill = ' '; |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 51 | while (*fmt != '\0') { |
| 52 | if (*fmt == '-') flags |= PF_FLAG_LEFT_ADJUST; |
| 53 | else if (*fmt == '+') flags |= PF_FLAG_SHOW_SIGN; |
| 54 | else if (*fmt == ' ') flags |= PF_FLAG_SPACE_SIGN; |
| 55 | else if (*fmt == '!') flags |= PF_FLAG_NO_TRAILZ; |
Dave Hylands | baf6f14 | 2014-03-30 21:06:50 -0700 | [diff] [blame] | 56 | else if (*fmt == '0') { |
| 57 | flags |= PF_FLAG_PAD_AFTER_SIGN; |
| 58 | fill = '0'; |
| 59 | } else break; |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 60 | ++fmt; |
| 61 | } |
| 62 | |
| 63 | // parse width, if it exists |
| 64 | int width = 0; |
| 65 | for (; '0' <= *fmt && *fmt <= '9'; ++fmt) { |
| 66 | width = width * 10 + *fmt - '0'; |
| 67 | } |
| 68 | |
| 69 | // parse precision, if it exists |
| 70 | int prec = -1; |
| 71 | if (*fmt == '.') { |
| 72 | ++fmt; |
| 73 | if (*fmt == '*') { |
| 74 | ++fmt; |
| 75 | prec = va_arg(args, int); |
| 76 | } else { |
| 77 | prec = 0; |
| 78 | for (; '0' <= *fmt && *fmt <= '9'; ++fmt) { |
| 79 | prec = prec * 10 + *fmt - '0'; |
| 80 | } |
| 81 | } |
| 82 | if (prec < 0) { |
| 83 | prec = 0; |
| 84 | } |
| 85 | } |
| 86 | |
| 87 | // parse long specifiers (current not used) |
| 88 | //bool long_arg = false; |
| 89 | if (*fmt == 'l') { |
| 90 | ++fmt; |
| 91 | //long_arg = true; |
| 92 | } |
| 93 | |
| 94 | if (*fmt == '\0') { |
| 95 | break; |
| 96 | } |
| 97 | |
| 98 | switch (*fmt) { |
| 99 | case 'b': |
| 100 | if (va_arg(args, int)) { |
Dave Hylands | baf6f14 | 2014-03-30 21:06:50 -0700 | [diff] [blame] | 101 | chrs += pfenv_print_strn(pfenv, "true", 4, flags, fill, width); |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 102 | } else { |
Dave Hylands | baf6f14 | 2014-03-30 21:06:50 -0700 | [diff] [blame] | 103 | chrs += pfenv_print_strn(pfenv, "false", 5, flags, fill, width); |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 104 | } |
| 105 | break; |
| 106 | case 'c': |
| 107 | { |
| 108 | char str = va_arg(args, int); |
Dave Hylands | baf6f14 | 2014-03-30 21:06:50 -0700 | [diff] [blame] | 109 | chrs += pfenv_print_strn(pfenv, &str, 1, flags, fill, width); |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 110 | break; |
| 111 | } |
| 112 | case 's': |
| 113 | { |
| 114 | const char *str = va_arg(args, const char*); |
| 115 | if (str) { |
| 116 | if (prec < 0) { |
| 117 | prec = strlen(str); |
| 118 | } |
Dave Hylands | baf6f14 | 2014-03-30 21:06:50 -0700 | [diff] [blame] | 119 | chrs += pfenv_print_strn(pfenv, str, prec, flags, fill, width); |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 120 | } else { |
Dave Hylands | baf6f14 | 2014-03-30 21:06:50 -0700 | [diff] [blame] | 121 | chrs += pfenv_print_strn(pfenv, "(null)", 6, flags, fill, width); |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 122 | } |
| 123 | break; |
| 124 | } |
| 125 | case 'u': |
Dave Hylands | baf6f14 | 2014-03-30 21:06:50 -0700 | [diff] [blame] | 126 | chrs += pfenv_print_int(pfenv, va_arg(args, int), 0, 10, 'a', flags, fill, width); |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 127 | break; |
| 128 | case 'd': |
Dave Hylands | baf6f14 | 2014-03-30 21:06:50 -0700 | [diff] [blame] | 129 | chrs += pfenv_print_int(pfenv, va_arg(args, int), 1, 10, 'a', flags, fill, width); |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 130 | break; |
| 131 | case 'x': |
| 132 | case 'p': // ? |
Dave Hylands | baf6f14 | 2014-03-30 21:06:50 -0700 | [diff] [blame] | 133 | chrs += pfenv_print_int(pfenv, va_arg(args, int), 0, 16, 'a', flags, fill, width); |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 134 | break; |
| 135 | case 'X': |
| 136 | case 'P': // ? |
Dave Hylands | baf6f14 | 2014-03-30 21:06:50 -0700 | [diff] [blame] | 137 | chrs += pfenv_print_int(pfenv, va_arg(args, int), 0, 16, 'A', flags, fill, width); |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 138 | break; |
| 139 | #if MICROPY_ENABLE_FLOAT |
| 140 | case 'e': |
| 141 | case 'E': |
| 142 | case 'f': |
| 143 | case 'F': |
| 144 | case 'g': |
| 145 | case 'G': |
| 146 | { |
Dave Hylands | baf6f14 | 2014-03-30 21:06:50 -0700 | [diff] [blame] | 147 | #if MICROPY_FLOAT_IMPL == MICROPY_FLOAT_IMPL_FLOAT |
| 148 | mp_float_t f = va_arg(args, double); |
| 149 | chrs += pfenv_print_float(pfenv, f, *fmt, flags, fill, width, prec); |
| 150 | #elif MICROPY_FLOAT_IMPL == MICROPY_FLOAT_IMPL_DOUBLE |
| 151 | // Currently pfenv_print_float uses snprintf, so if you want |
| 152 | // to use pfenv_print_float with doubles then you'll need |
| 153 | // fix it to not use snprintf first. Otherwise you'll have |
| 154 | // inifinite recursion. |
| 155 | #error Calling pfenv_print_float with double not supported from within printf |
| 156 | #else |
| 157 | #error Unknown MICROPY FLOAT IMPL |
| 158 | #endif |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 159 | break; |
| 160 | } |
| 161 | #endif |
| 162 | default: |
| 163 | pfenv->print_strn(pfenv->data, fmt, 1); |
| 164 | chrs += 1; |
| 165 | break; |
| 166 | } |
| 167 | ++fmt; |
| 168 | } |
| 169 | return chrs; |
| 170 | } |
| 171 | |
| 172 | void stdout_print_strn(void *data, const char *str, unsigned int len) { |
Damien George | 7533700 | 2014-04-21 12:03:09 +0100 | [diff] [blame] | 173 | // send stdout to UART, USB CDC VCP, and LCD if nothing else |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 174 | bool any = false; |
| 175 | |
Damien George | 7533700 | 2014-04-21 12:03:09 +0100 | [diff] [blame] | 176 | if (pyb_uart_global_debug != PYB_UART_NONE) { |
| 177 | uart_tx_strn_cooked(pyb_uart_global_debug, str, len); |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 178 | any = true; |
| 179 | } |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 180 | if (usb_vcp_is_enabled()) { |
| 181 | usb_vcp_send_strn_cooked(str, len); |
| 182 | any = true; |
| 183 | } |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 184 | if (!any) { |
| 185 | #if 0 |
| 186 | #if MICROPY_HW_HAS_LCD |
| 187 | lcd_print_strn(str, len); |
| 188 | #endif |
| 189 | #endif |
| 190 | } |
| 191 | } |
| 192 | |
| 193 | static const pfenv_t pfenv_stdout = {0, stdout_print_strn}; |
| 194 | |
| 195 | int printf(const char *fmt, ...) { |
| 196 | va_list ap; |
| 197 | va_start(ap, fmt); |
| 198 | int ret = pfenv_printf(&pfenv_stdout, fmt, ap); |
| 199 | va_end(ap); |
| 200 | return ret; |
| 201 | } |
| 202 | |
| 203 | int vprintf(const char *fmt, va_list ap) { |
| 204 | return pfenv_printf(&pfenv_stdout, fmt, ap); |
| 205 | } |
| 206 | |
| 207 | #if MICROPY_DEBUG_PRINTERS |
| 208 | int DEBUG_printf(const char *fmt, ...) { |
| 209 | (void)stream; |
| 210 | va_list ap; |
| 211 | va_start(ap, fmt); |
| 212 | int ret = pfenv_printf(&pfenv_stdout, fmt, ap); |
| 213 | va_end(ap); |
| 214 | return ret; |
| 215 | } |
| 216 | #endif |
| 217 | |
| 218 | // need this because gcc optimises printf("%c", c) -> putchar(c), and printf("a") -> putchar('a') |
| 219 | int putchar(int c) { |
| 220 | char chr = c; |
| 221 | stdout_print_strn(0, &chr, 1); |
| 222 | return chr; |
| 223 | } |
| 224 | |
| 225 | // need this because gcc optimises printf("string\n") -> puts("string") |
| 226 | int puts(const char *s) { |
| 227 | stdout_print_strn(0, s, strlen(s)); |
| 228 | char chr = '\n'; |
| 229 | stdout_print_strn(0, &chr, 1); |
| 230 | return 1; |
| 231 | } |
| 232 | |
| 233 | typedef struct _strn_pfenv_t { |
| 234 | char *cur; |
| 235 | size_t remain; |
| 236 | } strn_pfenv_t; |
| 237 | |
| 238 | void strn_print_strn(void *data, const char *str, unsigned int len) { |
| 239 | strn_pfenv_t *strn_pfenv = data; |
| 240 | if (len > strn_pfenv->remain) { |
| 241 | len = strn_pfenv->remain; |
| 242 | } |
| 243 | memcpy(strn_pfenv->cur, str, len); |
| 244 | strn_pfenv->cur += len; |
| 245 | strn_pfenv->remain -= len; |
| 246 | } |
Damien George | d0f9f6c | 2014-04-17 18:58:09 +0100 | [diff] [blame] | 247 | |
Dave Hylands | f14b92b | 2014-03-12 18:06:26 -0700 | [diff] [blame] | 248 | int vsnprintf(char *str, size_t size, const char *fmt, va_list ap) { |
| 249 | strn_pfenv_t strn_pfenv; |
| 250 | strn_pfenv.cur = str; |
| 251 | strn_pfenv.remain = size; |
| 252 | pfenv_t pfenv; |
| 253 | pfenv.data = &strn_pfenv; |
| 254 | pfenv.print_strn = strn_print_strn; |
| 255 | int len = pfenv_printf(&pfenv, fmt, ap); |
| 256 | // add terminating null byte |
| 257 | if (size > 0) { |
| 258 | if (strn_pfenv.remain == 0) { |
| 259 | strn_pfenv.cur[-1] = 0; |
| 260 | } else { |
| 261 | strn_pfenv.cur[0] = 0; |
| 262 | } |
| 263 | } |
| 264 | return len; |
| 265 | } |
| 266 | |
| 267 | int snprintf(char *str, size_t size, const char *fmt, ...) { |
| 268 | va_list ap; |
| 269 | va_start(ap, fmt); |
| 270 | int ret = vsnprintf(str, size, fmt, ap); |
| 271 | va_end(ap); |
| 272 | return ret; |
| 273 | } |