Subversion Repositories Projects

Rev

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