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 |