You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
534 lines
22 KiB
534 lines
22 KiB
// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- |
|
/* |
|
Adapted from the avr-libc vfprintf: |
|
|
|
Copyright (c) 2002, Alexander Popov (sasho@vip.bg) |
|
Copyright (c) 2002,2004,2005 Joerg Wunsch |
|
Copyright (c) 2005, Helmut Wallner |
|
Copyright (c) 2007, Dmitry Xmelkov |
|
All rights reserved. |
|
|
|
Redistribution and use in source and binary forms, with or without |
|
modification, are permitted provided that the following conditions are met: |
|
|
|
* Redistributions of source code must retain the above copyright |
|
notice, this list of conditions and the following disclaimer. |
|
* Redistributions in binary form must reproduce the above copyright |
|
notice, this list of conditions and the following disclaimer in |
|
the documentation and/or other materials provided with the |
|
distribution. |
|
* Neither the name of the copyright holders nor the names of |
|
contributors may be used to endorse or promote products derived |
|
from this software without specific prior written permission. |
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
|
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
|
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
|
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
|
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
|
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
|
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
|
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
|
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
|
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
|
POSSIBILITY OF SUCH DAMAGE. |
|
*/ |
|
|
|
/* From: Id: printf_p_new.c,v 1.1.1.9 2002/10/15 20:10:28 joerg_wunsch Exp */ |
|
/* $Id: vfprintf.c,v 1.18.2.1 2009/04/01 23:12:06 arcanum Exp $ */ |
|
|
|
#include "BetterStream.h" |
|
|
|
#include <avr/pgmspace.h> |
|
#include <stdarg.h> |
|
#include <string.h> |
|
extern "C" { |
|
#include "ftoa_engine.h" |
|
#include "ntz.h" |
|
#include "xtoa_fast.h" |
|
} |
|
|
|
// workaround for GCC bug c++/34734 |
|
#undef PROGMEM |
|
#define PROGMEM __attribute__(( section(".progmem.data") )) |
|
#undef PSTR |
|
#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) |
|
|
|
#define GETBYTE(flag, mask, pnt) ({ \ |
|
unsigned char __c; \ |
|
asm ( \ |
|
"sbrc %2,%3 \n\t" \ |
|
"lpm %0,Z+ \n\t" \ |
|
"sbrs %2,%3 \n\t" \ |
|
"ld %0,Z+ " \ |
|
: "=r" (__c), \ |
|
"+z" (pnt) \ |
|
: "r" (flag), \ |
|
"I" (ntz(mask)) \ |
|
); \ |
|
__c; \ |
|
}) |
|
|
|
#define FL_ZFILL 0x01 |
|
#define FL_PLUS 0x02 |
|
#define FL_SPACE 0x04 |
|
#define FL_LPAD 0x08 |
|
#define FL_ALT 0x10 |
|
#define FL_WIDTH 0x20 |
|
#define FL_PREC 0x40 |
|
#define FL_LONG 0x80 |
|
|
|
#define FL_PGMSTRING FL_LONG |
|
#define FL_NEGATIVE FL_LONG |
|
|
|
#define FL_ALTUPP FL_PLUS |
|
#define FL_ALTHEX FL_SPACE |
|
|
|
#define FL_FLTUPP FL_ALT |
|
#define FL_FLTEXP FL_PREC |
|
#define FL_FLTFIX FL_LONG |
|
|
|
#ifdef DESKTOP_BUILD |
|
void |
|
BetterStream::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap) |
|
{ |
|
char *str = NULL; |
|
int i; |
|
char *fmt2 = strdup(fmt); |
|
for (i=0; fmt2[i]; i++) { |
|
// cope with %S |
|
if (fmt2[i] == '%' && fmt2[i+1] == 'S') { |
|
fmt2[i+1] = 's'; |
|
} |
|
} |
|
vasprintf(&str, fmt2, ap); |
|
for (i=0; str[i]; i++) { |
|
write(str[i]); |
|
} |
|
free(str); |
|
free(fmt2); |
|
} |
|
#else |
|
void |
|
BetterStream::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap) |
|
{ |
|
unsigned char c; /* holds a char from the format string */ |
|
unsigned char flags; |
|
unsigned char width; |
|
unsigned char prec; |
|
unsigned char buf[11]; /* size for -1 in octal, without '\0' */ |
|
|
|
for (;;) { |
|
|
|
/* |
|
* Process non-format characters |
|
*/ |
|
for (;;) { |
|
c = GETBYTE (in_progmem, 1, fmt); |
|
if (!c) return; |
|
if (c == '%') { |
|
c = GETBYTE (in_progmem, 1, fmt); |
|
if (c != '%') break; |
|
} |
|
/* emit cr before lf to make most terminals happy */ |
|
if (c == '\n') |
|
write('\r'); |
|
write(c); |
|
} |
|
|
|
flags = 0; |
|
width = 0; |
|
prec = 0; |
|
|
|
/* |
|
* Process format adjustment characters, precision, width. |
|
*/ |
|
do { |
|
if (flags < FL_WIDTH) { |
|
switch (c) { |
|
case '0': |
|
flags |= FL_ZFILL; |
|
continue; |
|
case '+': |
|
flags |= FL_PLUS; |
|
/* FALLTHROUGH */ |
|
case ' ': |
|
flags |= FL_SPACE; |
|
continue; |
|
case '-': |
|
flags |= FL_LPAD; |
|
continue; |
|
case '#': |
|
flags |= FL_ALT; |
|
continue; |
|
} |
|
} |
|
|
|
if (flags < FL_LONG) { |
|
if (c >= '0' && c <= '9') { |
|
c -= '0'; |
|
if (flags & FL_PREC) { |
|
prec = 10*prec + c; |
|
continue; |
|
} |
|
width = 10*width + c; |
|
flags |= FL_WIDTH; |
|
continue; |
|
} |
|
if (c == '.') { |
|
if (flags & FL_PREC) |
|
return; |
|
flags |= FL_PREC; |
|
continue; |
|
} |
|
if (c == 'l') { |
|
flags |= FL_LONG; |
|
continue; |
|
} |
|
if (c == 'h') |
|
continue; |
|
} |
|
|
|
break; |
|
} while ( (c = GETBYTE (in_progmem, 1, fmt)) != 0); |
|
|
|
/* |
|
* Handle floating-point formats E, F, G, e, f, g. |
|
*/ |
|
if (c >= 'E' && c <= 'G') { |
|
flags |= FL_FLTUPP; |
|
c += 'e' - 'E'; |
|
goto flt_oper; |
|
|
|
} else if (c >= 'e' && c <= 'g') { |
|
|
|
int exp; /* exponent of master decimal digit */ |
|
int n; |
|
unsigned char vtype; /* result of float value parse */ |
|
unsigned char sign; /* sign character (or 0) */ |
|
unsigned char ndigs; |
|
|
|
flags &= ~FL_FLTUPP; |
|
|
|
flt_oper: |
|
if (!(flags & FL_PREC)) |
|
prec = 6; |
|
flags &= ~(FL_FLTEXP | FL_FLTFIX); |
|
if (c == 'e') |
|
flags |= FL_FLTEXP; |
|
else if (c == 'f') |
|
flags |= FL_FLTFIX; |
|
else if (prec > 0) |
|
prec -= 1; |
|
|
|
if (flags & FL_FLTFIX) { |
|
vtype = 7; /* 'prec' arg for 'ftoa_engine' */ |
|
ndigs = prec < 60 ? prec + 1 : 60; |
|
} else { |
|
if (prec > 7) prec = 7; |
|
vtype = prec; |
|
ndigs = 0; |
|
} |
|
exp = __ftoa_engine (va_arg(ap,double), (char *)buf, vtype, ndigs); |
|
vtype = buf[0]; |
|
|
|
sign = 0; |
|
if ((vtype & FTOA_MINUS) && !(vtype & FTOA_NAN)) |
|
sign = '-'; |
|
else if (flags & FL_PLUS) |
|
sign = '+'; |
|
else if (flags & FL_SPACE) |
|
sign = ' '; |
|
|
|
if (vtype & (FTOA_NAN | FTOA_INF)) { |
|
const char *p; |
|
ndigs = sign ? 4 : 3; |
|
if (width > ndigs) { |
|
width -= ndigs; |
|
if (!(flags & FL_LPAD)) { |
|
do { |
|
write(' '); |
|
} while (--width); |
|
} |
|
} else { |
|
width = 0; |
|
} |
|
if (sign) |
|
write(sign); |
|
p = PSTR("inf"); |
|
if (vtype & FTOA_NAN) |
|
p = PSTR("nan"); |
|
while ( (ndigs = pgm_read_byte((const prog_char *)p)) != 0) { |
|
if (flags & FL_FLTUPP) |
|
ndigs += 'I' - 'i'; |
|
write(ndigs); |
|
p++; |
|
} |
|
goto tail; |
|
} |
|
|
|
/* Output format adjustment, number of decimal digits in buf[] */ |
|
if (flags & FL_FLTFIX) { |
|
ndigs += exp; |
|
if ((vtype & FTOA_CARRY) && buf[1] == '1') |
|
ndigs -= 1; |
|
if ((signed char)ndigs < 1) |
|
ndigs = 1; |
|
else if (ndigs > 8) |
|
ndigs = 8; |
|
} else if (!(flags & FL_FLTEXP)) { /* 'g(G)' format */ |
|
if (exp <= prec && exp >= -4) |
|
flags |= FL_FLTFIX; |
|
while (prec && buf[1+prec] == '0') |
|
prec--; |
|
if (flags & FL_FLTFIX) { |
|
ndigs = prec + 1; /* number of digits in buf */ |
|
prec = prec > exp |
|
? prec - exp : 0; /* fractional part length */ |
|
} |
|
} |
|
|
|
/* Conversion result length, width := free space length */ |
|
if (flags & FL_FLTFIX) |
|
n = (exp>0 ? exp+1 : 1); |
|
else |
|
n = 5; /* 1e+00 */ |
|
if (sign) n += 1; |
|
if (prec) n += prec + 1; |
|
width = width > n ? width - n : 0; |
|
|
|
/* Output before first digit */ |
|
if (!(flags & (FL_LPAD | FL_ZFILL))) { |
|
while (width) { |
|
write(' '); |
|
width--; |
|
} |
|
} |
|
if (sign) write(sign); |
|
if (!(flags & FL_LPAD)) { |
|
while (width) { |
|
write('0'); |
|
width--; |
|
} |
|
} |
|
|
|
if (flags & FL_FLTFIX) { /* 'f' format */ |
|
|
|
n = exp > 0 ? exp : 0; /* exponent of left digit */ |
|
do { |
|
if (n == -1) |
|
write('.'); |
|
flags = (n <= exp && n > exp - ndigs) |
|
? buf[exp - n + 1] : '0'; |
|
if (--n < -prec) |
|
break; |
|
write(flags); |
|
} while (1); |
|
if (n == exp |
|
&& (buf[1] > '5' |
|
|| (buf[1] == '5' && !(vtype & FTOA_CARRY))) ) |
|
{ |
|
flags = '1'; |
|
} |
|
write(flags); |
|
|
|
} else { /* 'e(E)' format */ |
|
|
|
/* mantissa */ |
|
if (buf[1] != '1') |
|
vtype &= ~FTOA_CARRY; |
|
write(buf[1]); |
|
if (prec) { |
|
write('.'); |
|
sign = 2; |
|
do { |
|
write(buf[sign++]); |
|
} while (--prec); |
|
} |
|
|
|
/* exponent */ |
|
write(flags & FL_FLTUPP ? 'E' : 'e'); |
|
ndigs = '+'; |
|
if (exp < 0 || (exp == 0 && (vtype & FTOA_CARRY) != 0)) { |
|
exp = -exp; |
|
ndigs = '-'; |
|
} |
|
write(ndigs); |
|
for (ndigs = '0'; exp >= 10; exp -= 10) |
|
ndigs += 1; |
|
write(ndigs); |
|
write('0' + exp); |
|
} |
|
|
|
goto tail; |
|
} |
|
|
|
/* |
|
* Handle string formats c, s, S. |
|
*/ |
|
{ |
|
const char * pnt; |
|
size_t size; |
|
|
|
switch (c) { |
|
|
|
case 'c': |
|
buf[0] = va_arg (ap, int); |
|
pnt = (char *)buf; |
|
size = 1; |
|
goto no_pgmstring; |
|
|
|
case 's': |
|
pnt = va_arg (ap, char *); |
|
size = strnlen (pnt, (flags & FL_PREC) ? prec : ~0); |
|
no_pgmstring: |
|
flags &= ~FL_PGMSTRING; |
|
goto str_lpad; |
|
|
|
case 'S': |
|
// pgmstring: // not yet used |
|
pnt = va_arg (ap, char *); |
|
size = strnlen_P (pnt, (flags & FL_PREC) ? prec : ~0); |
|
flags |= FL_PGMSTRING; |
|
|
|
str_lpad: |
|
if (!(flags & FL_LPAD)) { |
|
while (size < width) { |
|
write(' '); |
|
width--; |
|
} |
|
} |
|
while (size) { |
|
write(GETBYTE (flags, FL_PGMSTRING, pnt)); |
|
if (width) width -= 1; |
|
size -= 1; |
|
} |
|
goto tail; |
|
} |
|
} |
|
|
|
/* |
|
* Handle integer formats variations for d/i, u, o, p, x, X. |
|
*/ |
|
if (c == 'd' || c == 'i') { |
|
long x = (flags & FL_LONG) ? va_arg(ap,long) : va_arg(ap,int); |
|
flags &= ~(FL_NEGATIVE | FL_ALT); |
|
if (x < 0) { |
|
x = -x; |
|
flags |= FL_NEGATIVE; |
|
} |
|
c = __ultoa_invert (x, (char *)buf, 10) - (char *)buf; |
|
|
|
} else { |
|
int base; |
|
|
|
if (c == 'u') { |
|
flags &= ~FL_ALT; |
|
base = 10; |
|
goto ultoa; |
|
} |
|
|
|
flags &= ~(FL_PLUS | FL_SPACE); |
|
|
|
switch (c) { |
|
case 'o': |
|
base = 8; |
|
goto ultoa; |
|
case 'p': |
|
flags |= FL_ALT; |
|
/* no break */ |
|
case 'x': |
|
if (flags & FL_ALT) |
|
flags |= FL_ALTHEX; |
|
base = 16; |
|
goto ultoa; |
|
case 'X': |
|
if (flags & FL_ALT) |
|
flags |= (FL_ALTHEX | FL_ALTUPP); |
|
base = 16 | XTOA_UPPER; |
|
ultoa: |
|
c = __ultoa_invert ((flags & FL_LONG) |
|
? va_arg(ap, unsigned long) |
|
: va_arg(ap, unsigned int), |
|
(char *)buf, base) - (char *)buf; |
|
flags &= ~FL_NEGATIVE; |
|
break; |
|
|
|
default: |
|
return; |
|
} |
|
} |
|
|
|
/* |
|
* Format integers. |
|
*/ |
|
{ |
|
unsigned char len; |
|
|
|
len = c; |
|
if (flags & FL_PREC) { |
|
flags &= ~FL_ZFILL; |
|
if (len < prec) { |
|
len = prec; |
|
if ((flags & FL_ALT) && !(flags & FL_ALTHEX)) |
|
flags &= ~FL_ALT; |
|
} |
|
} |
|
if (flags & FL_ALT) { |
|
if (buf[c-1] == '0') { |
|
flags &= ~(FL_ALT | FL_ALTHEX | FL_ALTUPP); |
|
} else { |
|
len += 1; |
|
if (flags & FL_ALTHEX) |
|
len += 1; |
|
} |
|
} else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) { |
|
len += 1; |
|
} |
|
|
|
if (!(flags & FL_LPAD)) { |
|
if (flags & FL_ZFILL) { |
|
prec = c; |
|
if (len < width) { |
|
prec += width - len; |
|
len = width; |
|
} |
|
} |
|
while (len < width) { |
|
write(' '); |
|
len++; |
|
} |
|
} |
|
|
|
width = (len < width) ? width - len : 0; |
|
|
|
if (flags & FL_ALT) { |
|
write('0'); |
|
if (flags & FL_ALTHEX) |
|
write(flags & FL_ALTUPP ? 'X' : 'x'); |
|
} else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) { |
|
unsigned char z = ' '; |
|
if (flags & FL_PLUS) z = '+'; |
|
if (flags & FL_NEGATIVE) z = '-'; |
|
write(z); |
|
} |
|
|
|
while (prec > c) { |
|
write('0'); |
|
prec--; |
|
} |
|
|
|
do { |
|
write(buf[--c]); |
|
} while (c); |
|
} |
|
|
|
tail: |
|
/* Tail is possible. */ |
|
while (width) { |
|
write(' '); |
|
width--; |
|
} |
|
} /* for (;;) */ |
|
} |
|
#endif // DESKTOP_BUILD
|
|
|