Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1702 | - | 1 | // -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- |
| 2 | /* |
||
| 3 | Adapted from the avr-libc vfprintf: |
||
| 4 | |||
| 5 | Copyright (c) 2002, Alexander Popov (sasho@vip.bg) |
||
| 6 | Copyright (c) 2002,2004,2005 Joerg Wunsch |
||
| 7 | Copyright (c) 2005, Helmut Wallner |
||
| 8 | Copyright (c) 2007, Dmitry Xmelkov |
||
| 9 | All rights reserved. |
||
| 10 | |||
| 11 | Redistribution and use in source and binary forms, with or without |
||
| 12 | modification, are permitted provided that the following conditions are met: |
||
| 13 | |||
| 14 | * Redistributions of source code must retain the above copyright |
||
| 15 | notice, this list of conditions and the following disclaimer. |
||
| 16 | * Redistributions in binary form must reproduce the above copyright |
||
| 17 | notice, this list of conditions and the following disclaimer in |
||
| 18 | the documentation and/or other materials provided with the |
||
| 19 | distribution. |
||
| 20 | * Neither the name of the copyright holders nor the names of |
||
| 21 | contributors may be used to endorse or promote products derived |
||
| 22 | from this software without specific prior written permission. |
||
| 23 | |||
| 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
||
| 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
||
| 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
||
| 27 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
||
| 28 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
||
| 29 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
||
| 30 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
||
| 31 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
||
| 32 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
||
| 33 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
| 34 | POSSIBILITY OF SUCH DAMAGE. |
||
| 35 | */ |
||
| 36 | |||
| 37 | /* From: Id: printf_p_new.c,v 1.1.1.9 2002/10/15 20:10:28 joerg_wunsch Exp */ |
||
| 38 | /* $Id: vfprintf.c,v 1.18.2.1 2009/04/01 23:12:06 arcanum Exp $ */ |
||
| 39 | |||
| 40 | #include "BetterStream.h" |
||
| 41 | |||
| 42 | #include <avr/pgmspace.h> |
||
| 43 | #include <stdarg.h> |
||
| 44 | #include <string.h> |
||
| 45 | extern "C" { |
||
| 46 | #include "ftoa_engine.h" |
||
| 47 | #include "ntz.h" |
||
| 48 | #include "xtoa_fast.h" |
||
| 49 | } |
||
| 50 | |||
| 51 | // workaround for GCC bug c++/34734 |
||
| 52 | #undef PROGMEM |
||
| 53 | #define PROGMEM __attribute__(( section(".progmem.data") )) |
||
| 54 | #undef PSTR |
||
| 55 | #define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) |
||
| 56 | |||
| 57 | #define GETBYTE(flag, mask, pnt) ({ \ |
||
| 58 | unsigned char __c; \ |
||
| 59 | asm ( \ |
||
| 60 | "sbrc %2,%3 \n\t" \ |
||
| 61 | "lpm %0,Z+ \n\t" \ |
||
| 62 | "sbrs %2,%3 \n\t" \ |
||
| 63 | "ld %0,Z+ " \ |
||
| 64 | : "=r" (__c), \ |
||
| 65 | "+z" (pnt) \ |
||
| 66 | : "r" (flag), \ |
||
| 67 | "I" (ntz(mask)) \ |
||
| 68 | ); \ |
||
| 69 | __c; \ |
||
| 70 | }) |
||
| 71 | |||
| 72 | #define FL_ZFILL 0x01 |
||
| 73 | #define FL_PLUS 0x02 |
||
| 74 | #define FL_SPACE 0x04 |
||
| 75 | #define FL_LPAD 0x08 |
||
| 76 | #define FL_ALT 0x10 |
||
| 77 | #define FL_WIDTH 0x20 |
||
| 78 | #define FL_PREC 0x40 |
||
| 79 | #define FL_LONG 0x80 |
||
| 80 | |||
| 81 | #define FL_PGMSTRING FL_LONG |
||
| 82 | #define FL_NEGATIVE FL_LONG |
||
| 83 | |||
| 84 | #define FL_ALTUPP FL_PLUS |
||
| 85 | #define FL_ALTHEX FL_SPACE |
||
| 86 | |||
| 87 | #define FL_FLTUPP FL_ALT |
||
| 88 | #define FL_FLTEXP FL_PREC |
||
| 89 | #define FL_FLTFIX FL_LONG |
||
| 90 | |||
| 91 | #ifdef DESKTOP_BUILD |
||
| 92 | void |
||
| 93 | BetterStream::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap) |
||
| 94 | { |
||
| 95 | char *str = NULL; |
||
| 96 | int i; |
||
| 97 | char *fmt2 = strdup(fmt); |
||
| 98 | for (i=0; fmt2[i]; i++) { |
||
| 99 | // cope with %S |
||
| 100 | if (fmt2[i] == '%' && fmt2[i+1] == 'S') { |
||
| 101 | fmt2[i+1] = 's'; |
||
| 102 | } |
||
| 103 | } |
||
| 104 | vasprintf(&str, fmt2, ap); |
||
| 105 | for (i=0; str[i]; i++) { |
||
| 106 | write(str[i]); |
||
| 107 | } |
||
| 108 | free(str); |
||
| 109 | free(fmt2); |
||
| 110 | } |
||
| 111 | #else |
||
| 112 | void |
||
| 113 | BetterStream::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap) |
||
| 114 | { |
||
| 115 | unsigned char c; /* holds a char from the format string */ |
||
| 116 | unsigned char flags; |
||
| 117 | unsigned char width; |
||
| 118 | unsigned char prec; |
||
| 119 | unsigned char buf[11]; /* size for -1 in octal, without '\0' */ |
||
| 120 | |||
| 121 | for (;;) { |
||
| 122 | |||
| 123 | /* |
||
| 124 | * Process non-format characters |
||
| 125 | */ |
||
| 126 | for (;;) { |
||
| 127 | c = GETBYTE (in_progmem, 1, fmt); |
||
| 128 | if (!c) return; |
||
| 129 | if (c == '%') { |
||
| 130 | c = GETBYTE (in_progmem, 1, fmt); |
||
| 131 | if (c != '%') break; |
||
| 132 | } |
||
| 133 | /* emit cr before lf to make most terminals happy */ |
||
| 134 | if (c == '\n') |
||
| 135 | write('\r'); |
||
| 136 | write(c); |
||
| 137 | } |
||
| 138 | |||
| 139 | flags = 0; |
||
| 140 | width = 0; |
||
| 141 | prec = 0; |
||
| 142 | |||
| 143 | /* |
||
| 144 | * Process format adjustment characters, precision, width. |
||
| 145 | */ |
||
| 146 | do { |
||
| 147 | if (flags < FL_WIDTH) { |
||
| 148 | switch (c) { |
||
| 149 | case '0': |
||
| 150 | flags |= FL_ZFILL; |
||
| 151 | continue; |
||
| 152 | case '+': |
||
| 153 | flags |= FL_PLUS; |
||
| 154 | /* FALLTHROUGH */ |
||
| 155 | case ' ': |
||
| 156 | flags |= FL_SPACE; |
||
| 157 | continue; |
||
| 158 | case '-': |
||
| 159 | flags |= FL_LPAD; |
||
| 160 | continue; |
||
| 161 | case '#': |
||
| 162 | flags |= FL_ALT; |
||
| 163 | continue; |
||
| 164 | } |
||
| 165 | } |
||
| 166 | |||
| 167 | if (flags < FL_LONG) { |
||
| 168 | if (c >= '0' && c <= '9') { |
||
| 169 | c -= '0'; |
||
| 170 | if (flags & FL_PREC) { |
||
| 171 | prec = 10*prec + c; |
||
| 172 | continue; |
||
| 173 | } |
||
| 174 | width = 10*width + c; |
||
| 175 | flags |= FL_WIDTH; |
||
| 176 | continue; |
||
| 177 | } |
||
| 178 | if (c == '.') { |
||
| 179 | if (flags & FL_PREC) |
||
| 180 | return; |
||
| 181 | flags |= FL_PREC; |
||
| 182 | continue; |
||
| 183 | } |
||
| 184 | if (c == 'l') { |
||
| 185 | flags |= FL_LONG; |
||
| 186 | continue; |
||
| 187 | } |
||
| 188 | if (c == 'h') |
||
| 189 | continue; |
||
| 190 | } |
||
| 191 | |||
| 192 | break; |
||
| 193 | } while ( (c = GETBYTE (in_progmem, 1, fmt)) != 0); |
||
| 194 | |||
| 195 | /* |
||
| 196 | * Handle floating-point formats E, F, G, e, f, g. |
||
| 197 | */ |
||
| 198 | if (c >= 'E' && c <= 'G') { |
||
| 199 | flags |= FL_FLTUPP; |
||
| 200 | c += 'e' - 'E'; |
||
| 201 | goto flt_oper; |
||
| 202 | |||
| 203 | } else if (c >= 'e' && c <= 'g') { |
||
| 204 | |||
| 205 | int exp; /* exponent of master decimal digit */ |
||
| 206 | int n; |
||
| 207 | unsigned char vtype; /* result of float value parse */ |
||
| 208 | unsigned char sign; /* sign character (or 0) */ |
||
| 209 | unsigned char ndigs; |
||
| 210 | |||
| 211 | flags &= ~FL_FLTUPP; |
||
| 212 | |||
| 213 | flt_oper: |
||
| 214 | if (!(flags & FL_PREC)) |
||
| 215 | prec = 6; |
||
| 216 | flags &= ~(FL_FLTEXP | FL_FLTFIX); |
||
| 217 | if (c == 'e') |
||
| 218 | flags |= FL_FLTEXP; |
||
| 219 | else if (c == 'f') |
||
| 220 | flags |= FL_FLTFIX; |
||
| 221 | else if (prec > 0) |
||
| 222 | prec -= 1; |
||
| 223 | |||
| 224 | if (flags & FL_FLTFIX) { |
||
| 225 | vtype = 7; /* 'prec' arg for 'ftoa_engine' */ |
||
| 226 | ndigs = prec < 60 ? prec + 1 : 60; |
||
| 227 | } else { |
||
| 228 | if (prec > 7) prec = 7; |
||
| 229 | vtype = prec; |
||
| 230 | ndigs = 0; |
||
| 231 | } |
||
| 232 | exp = __ftoa_engine (va_arg(ap,double), (char *)buf, vtype, ndigs); |
||
| 233 | vtype = buf[0]; |
||
| 234 | |||
| 235 | sign = 0; |
||
| 236 | if ((vtype & FTOA_MINUS) && !(vtype & FTOA_NAN)) |
||
| 237 | sign = '-'; |
||
| 238 | else if (flags & FL_PLUS) |
||
| 239 | sign = '+'; |
||
| 240 | else if (flags & FL_SPACE) |
||
| 241 | sign = ' '; |
||
| 242 | |||
| 243 | if (vtype & (FTOA_NAN | FTOA_INF)) { |
||
| 244 | const char *p; |
||
| 245 | ndigs = sign ? 4 : 3; |
||
| 246 | if (width > ndigs) { |
||
| 247 | width -= ndigs; |
||
| 248 | if (!(flags & FL_LPAD)) { |
||
| 249 | do { |
||
| 250 | write(' '); |
||
| 251 | } while (--width); |
||
| 252 | } |
||
| 253 | } else { |
||
| 254 | width = 0; |
||
| 255 | } |
||
| 256 | if (sign) |
||
| 257 | write(sign); |
||
| 258 | p = PSTR("inf"); |
||
| 259 | if (vtype & FTOA_NAN) |
||
| 260 | p = PSTR("nan"); |
||
| 261 | while ( (ndigs = pgm_read_byte((const prog_char *)p)) != 0) { |
||
| 262 | if (flags & FL_FLTUPP) |
||
| 263 | ndigs += 'I' - 'i'; |
||
| 264 | write(ndigs); |
||
| 265 | p++; |
||
| 266 | } |
||
| 267 | goto tail; |
||
| 268 | } |
||
| 269 | |||
| 270 | /* Output format adjustment, number of decimal digits in buf[] */ |
||
| 271 | if (flags & FL_FLTFIX) { |
||
| 272 | ndigs += exp; |
||
| 273 | if ((vtype & FTOA_CARRY) && buf[1] == '1') |
||
| 274 | ndigs -= 1; |
||
| 275 | if ((signed char)ndigs < 1) |
||
| 276 | ndigs = 1; |
||
| 277 | else if (ndigs > 8) |
||
| 278 | ndigs = 8; |
||
| 279 | } else if (!(flags & FL_FLTEXP)) { /* 'g(G)' format */ |
||
| 280 | if (exp <= prec && exp >= -4) |
||
| 281 | flags |= FL_FLTFIX; |
||
| 282 | while (prec && buf[1+prec] == '0') |
||
| 283 | prec--; |
||
| 284 | if (flags & FL_FLTFIX) { |
||
| 285 | ndigs = prec + 1; /* number of digits in buf */ |
||
| 286 | prec = prec > exp |
||
| 287 | ? prec - exp : 0; /* fractional part length */ |
||
| 288 | } |
||
| 289 | } |
||
| 290 | |||
| 291 | /* Conversion result length, width := free space length */ |
||
| 292 | if (flags & FL_FLTFIX) |
||
| 293 | n = (exp>0 ? exp+1 : 1); |
||
| 294 | else |
||
| 295 | n = 5; /* 1e+00 */ |
||
| 296 | if (sign) n += 1; |
||
| 297 | if (prec) n += prec + 1; |
||
| 298 | width = width > n ? width - n : 0; |
||
| 299 | |||
| 300 | /* Output before first digit */ |
||
| 301 | if (!(flags & (FL_LPAD | FL_ZFILL))) { |
||
| 302 | while (width) { |
||
| 303 | write(' '); |
||
| 304 | width--; |
||
| 305 | } |
||
| 306 | } |
||
| 307 | if (sign) write(sign); |
||
| 308 | if (!(flags & FL_LPAD)) { |
||
| 309 | while (width) { |
||
| 310 | write('0'); |
||
| 311 | width--; |
||
| 312 | } |
||
| 313 | } |
||
| 314 | |||
| 315 | if (flags & FL_FLTFIX) { /* 'f' format */ |
||
| 316 | |||
| 317 | n = exp > 0 ? exp : 0; /* exponent of left digit */ |
||
| 318 | do { |
||
| 319 | if (n == -1) |
||
| 320 | write('.'); |
||
| 321 | flags = (n <= exp && n > exp - ndigs) |
||
| 322 | ? buf[exp - n + 1] : '0'; |
||
| 323 | if (--n < -prec) |
||
| 324 | break; |
||
| 325 | write(flags); |
||
| 326 | } while (1); |
||
| 327 | if (n == exp |
||
| 328 | && (buf[1] > '5' |
||
| 329 | || (buf[1] == '5' && !(vtype & FTOA_CARRY))) ) |
||
| 330 | { |
||
| 331 | flags = '1'; |
||
| 332 | } |
||
| 333 | write(flags); |
||
| 334 | |||
| 335 | } else { /* 'e(E)' format */ |
||
| 336 | |||
| 337 | /* mantissa */ |
||
| 338 | if (buf[1] != '1') |
||
| 339 | vtype &= ~FTOA_CARRY; |
||
| 340 | write(buf[1]); |
||
| 341 | if (prec) { |
||
| 342 | write('.'); |
||
| 343 | sign = 2; |
||
| 344 | do { |
||
| 345 | write(buf[sign++]); |
||
| 346 | } while (--prec); |
||
| 347 | } |
||
| 348 | |||
| 349 | /* exponent */ |
||
| 350 | write(flags & FL_FLTUPP ? 'E' : 'e'); |
||
| 351 | ndigs = '+'; |
||
| 352 | if (exp < 0 || (exp == 0 && (vtype & FTOA_CARRY) != 0)) { |
||
| 353 | exp = -exp; |
||
| 354 | ndigs = '-'; |
||
| 355 | } |
||
| 356 | write(ndigs); |
||
| 357 | for (ndigs = '0'; exp >= 10; exp -= 10) |
||
| 358 | ndigs += 1; |
||
| 359 | write(ndigs); |
||
| 360 | write('0' + exp); |
||
| 361 | } |
||
| 362 | |||
| 363 | goto tail; |
||
| 364 | } |
||
| 365 | |||
| 366 | /* |
||
| 367 | * Handle string formats c, s, S. |
||
| 368 | */ |
||
| 369 | { |
||
| 370 | const char * pnt; |
||
| 371 | size_t size; |
||
| 372 | |||
| 373 | switch (c) { |
||
| 374 | |||
| 375 | case 'c': |
||
| 376 | buf[0] = va_arg (ap, int); |
||
| 377 | pnt = (char *)buf; |
||
| 378 | size = 1; |
||
| 379 | goto no_pgmstring; |
||
| 380 | |||
| 381 | case 's': |
||
| 382 | pnt = va_arg (ap, char *); |
||
| 383 | size = strnlen (pnt, (flags & FL_PREC) ? prec : ~0); |
||
| 384 | no_pgmstring: |
||
| 385 | flags &= ~FL_PGMSTRING; |
||
| 386 | goto str_lpad; |
||
| 387 | |||
| 388 | case 'S': |
||
| 389 | // pgmstring: // not yet used |
||
| 390 | pnt = va_arg (ap, char *); |
||
| 391 | size = strnlen_P (pnt, (flags & FL_PREC) ? prec : ~0); |
||
| 392 | flags |= FL_PGMSTRING; |
||
| 393 | |||
| 394 | str_lpad: |
||
| 395 | if (!(flags & FL_LPAD)) { |
||
| 396 | while (size < width) { |
||
| 397 | write(' '); |
||
| 398 | width--; |
||
| 399 | } |
||
| 400 | } |
||
| 401 | while (size) { |
||
| 402 | write(GETBYTE (flags, FL_PGMSTRING, pnt)); |
||
| 403 | if (width) width -= 1; |
||
| 404 | size -= 1; |
||
| 405 | } |
||
| 406 | goto tail; |
||
| 407 | } |
||
| 408 | } |
||
| 409 | |||
| 410 | /* |
||
| 411 | * Handle integer formats variations for d/i, u, o, p, x, X. |
||
| 412 | */ |
||
| 413 | if (c == 'd' || c == 'i') { |
||
| 414 | long x = (flags & FL_LONG) ? va_arg(ap,long) : va_arg(ap,int); |
||
| 415 | flags &= ~(FL_NEGATIVE | FL_ALT); |
||
| 416 | if (x < 0) { |
||
| 417 | x = -x; |
||
| 418 | flags |= FL_NEGATIVE; |
||
| 419 | } |
||
| 420 | c = __ultoa_invert (x, (char *)buf, 10) - (char *)buf; |
||
| 421 | |||
| 422 | } else { |
||
| 423 | int base; |
||
| 424 | |||
| 425 | if (c == 'u') { |
||
| 426 | flags &= ~FL_ALT; |
||
| 427 | base = 10; |
||
| 428 | goto ultoa; |
||
| 429 | } |
||
| 430 | |||
| 431 | flags &= ~(FL_PLUS | FL_SPACE); |
||
| 432 | |||
| 433 | switch (c) { |
||
| 434 | case 'o': |
||
| 435 | base = 8; |
||
| 436 | goto ultoa; |
||
| 437 | case 'p': |
||
| 438 | flags |= FL_ALT; |
||
| 439 | /* no break */ |
||
| 440 | case 'x': |
||
| 441 | if (flags & FL_ALT) |
||
| 442 | flags |= FL_ALTHEX; |
||
| 443 | base = 16; |
||
| 444 | goto ultoa; |
||
| 445 | case 'X': |
||
| 446 | if (flags & FL_ALT) |
||
| 447 | flags |= (FL_ALTHEX | FL_ALTUPP); |
||
| 448 | base = 16 | XTOA_UPPER; |
||
| 449 | ultoa: |
||
| 450 | c = __ultoa_invert ((flags & FL_LONG) |
||
| 451 | ? va_arg(ap, unsigned long) |
||
| 452 | : va_arg(ap, unsigned int), |
||
| 453 | (char *)buf, base) - (char *)buf; |
||
| 454 | flags &= ~FL_NEGATIVE; |
||
| 455 | break; |
||
| 456 | |||
| 457 | default: |
||
| 458 | return; |
||
| 459 | } |
||
| 460 | } |
||
| 461 | |||
| 462 | /* |
||
| 463 | * Format integers. |
||
| 464 | */ |
||
| 465 | { |
||
| 466 | unsigned char len; |
||
| 467 | |||
| 468 | len = c; |
||
| 469 | if (flags & FL_PREC) { |
||
| 470 | flags &= ~FL_ZFILL; |
||
| 471 | if (len < prec) { |
||
| 472 | len = prec; |
||
| 473 | if ((flags & FL_ALT) && !(flags & FL_ALTHEX)) |
||
| 474 | flags &= ~FL_ALT; |
||
| 475 | } |
||
| 476 | } |
||
| 477 | if (flags & FL_ALT) { |
||
| 478 | if (buf[c-1] == '0') { |
||
| 479 | flags &= ~(FL_ALT | FL_ALTHEX | FL_ALTUPP); |
||
| 480 | } else { |
||
| 481 | len += 1; |
||
| 482 | if (flags & FL_ALTHEX) |
||
| 483 | len += 1; |
||
| 484 | } |
||
| 485 | } else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) { |
||
| 486 | len += 1; |
||
| 487 | } |
||
| 488 | |||
| 489 | if (!(flags & FL_LPAD)) { |
||
| 490 | if (flags & FL_ZFILL) { |
||
| 491 | prec = c; |
||
| 492 | if (len < width) { |
||
| 493 | prec += width - len; |
||
| 494 | len = width; |
||
| 495 | } |
||
| 496 | } |
||
| 497 | while (len < width) { |
||
| 498 | write(' '); |
||
| 499 | len++; |
||
| 500 | } |
||
| 501 | } |
||
| 502 | |||
| 503 | width = (len < width) ? width - len : 0; |
||
| 504 | |||
| 505 | if (flags & FL_ALT) { |
||
| 506 | write('0'); |
||
| 507 | if (flags & FL_ALTHEX) |
||
| 508 | write(flags & FL_ALTUPP ? 'X' : 'x'); |
||
| 509 | } else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) { |
||
| 510 | unsigned char z = ' '; |
||
| 511 | if (flags & FL_PLUS) z = '+'; |
||
| 512 | if (flags & FL_NEGATIVE) z = '-'; |
||
| 513 | write(z); |
||
| 514 | } |
||
| 515 | |||
| 516 | while (prec > c) { |
||
| 517 | write('0'); |
||
| 518 | prec--; |
||
| 519 | } |
||
| 520 | |||
| 521 | do { |
||
| 522 | write(buf[--c]); |
||
| 523 | } while (c); |
||
| 524 | } |
||
| 525 | |||
| 526 | tail: |
||
| 527 | /* Tail is possible. */ |
||
| 528 | while (width) { |
||
| 529 | write(' '); |
||
| 530 | width--; |
||
| 531 | } |
||
| 532 | } /* for (;;) */ |
||
| 533 | } |
||
| 534 | #endif // DESKTOP_BUILD |