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