• Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

vprintf.cpp

Go to the documentation of this file.
00001 // -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
00002 /*
00003    Adapted from the avr-libc vfprintf:
00004 
00005    Copyright (c) 2002, Alexander Popov (sasho@vip.bg)
00006    Copyright (c) 2002,2004,2005 Joerg Wunsch
00007    Copyright (c) 2005, Helmut Wallner
00008    Copyright (c) 2007, Dmitry Xmelkov
00009    All rights reserved.
00010 
00011    Redistribution and use in source and binary forms, with or without
00012    modification, are permitted provided that the following conditions are met:
00013 
00014    * Redistributions of source code must retain the above copyright
00015    notice, this list of conditions and the following disclaimer.
00016    * Redistributions in binary form must reproduce the above copyright
00017    notice, this list of conditions and the following disclaimer in
00018    the documentation and/or other materials provided with the
00019    distribution.
00020    * Neither the name of the copyright holders nor the names of
00021    contributors may be used to endorse or promote products derived
00022    from this software without specific prior written permission.
00023 
00024    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00025    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00026    IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00027    ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00028    LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00029    CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00030    SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00031    INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00032    CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00033    ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034    POSSIBILITY OF SUCH DAMAGE.
00035 */
00036 
00037 /* From: Id: printf_p_new.c,v 1.1.1.9 2002/10/15 20:10:28 joerg_wunsch Exp */
00038 /* $Id: vfprintf.c,v 1.18.2.1 2009/04/01 23:12:06 arcanum Exp $ */
00039 
00040 #include "BetterStream.h"
00041 
00042 #include <avr/pgmspace.h>
00043 #include <stdarg.h>
00044 #include <string.h>
00045 extern "C" {
00046 #include "ftoa_engine.h"
00047 #include "ntz.h"
00048 #include "xtoa_fast.h"
00049 }
00050 
00051 // workaround for GCC bug c++/34734
00052 #undef PROGMEM 
00053 #define PROGMEM __attribute__(( section(".progmem.data") )) 
00054 #undef PSTR 
00055 #define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) 
00056 
00057 #define GETBYTE(flag, mask, pnt)        ({                              \
00058                         unsigned char __c;                              \
00059                         asm (                                           \
00060                              "sbrc      %2,%3   \n\t"                   \
00061                              "lpm       %0,Z+   \n\t"                   \
00062                              "sbrs      %2,%3   \n\t"                   \
00063                              "ld        %0,Z+   "                       \
00064                              : "=r" (__c),                              \
00065                                "+z" (pnt)                               \
00066                              : "r" (flag),                              \
00067                                "I" (ntz(mask))                          \
00068                         );                                              \
00069                         __c;                                            \
00070                 })
00071 /*
00072 #define GETBYTE(flag, mask, pnt)        ({      \
00073        unsigned char __c;                       \
00074        __c = ((flag) & (mask))                  \
00075              ? pgm_read_byte(pnt) : *pnt;       \
00076        pnt++;                                   \
00077        __c;                                     \
00078 })
00079 */
00080 
00081 #define FL_ZFILL        0x01
00082 #define FL_PLUS         0x02
00083 #define FL_SPACE        0x04
00084 #define FL_LPAD         0x08
00085 #define FL_ALT          0x10
00086 #define FL_WIDTH        0x20
00087 #define FL_PREC         0x40
00088 #define FL_LONG         0x80
00089 
00090 #define FL_PGMSTRING    FL_LONG
00091 #define FL_NEGATIVE     FL_LONG
00092 
00093 #define FL_ALTUPP       FL_PLUS
00094 #define FL_ALTHEX       FL_SPACE
00095 
00096 #define FL_FLTUPP       FL_ALT
00097 #define FL_FLTEXP       FL_PREC
00098 #define FL_FLTFIX       FL_LONG
00099 
00100 void
00101 BetterStream::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap)
00102 {
00103         unsigned char c;        /* holds a char from the format string */
00104         unsigned char flags;
00105         unsigned char width;
00106         unsigned char prec;
00107         unsigned char buf[11];  /* size for -1 in octal, without '\0'   */
00108 
00109         for (;;) {
00110 
00111                 /*
00112                  * Process non-format characters
00113                  */
00114                 for (;;) {
00115                         c = GETBYTE (in_progmem, 1, fmt);
00116                         if (!c) return;
00117                         if (c == '%') {
00118                                 c = GETBYTE (in_progmem, 1, fmt);
00119                                 if (c != '%') break;
00120                         }
00121                         /* emit cr before lf to make most terminals happy */
00122                         if (c == '\n')
00123                                 write('\r');
00124                         write(c);
00125                 }
00126 
00127                 flags = 0;
00128                 width = 0;
00129                 prec = 0;
00130                 
00131                 /*
00132                  * Process format adjustment characters, precision, width.
00133                  */
00134                 do {
00135                         if (flags < FL_WIDTH) {
00136                                 switch (c) {
00137                                 case '0':
00138                                         flags |= FL_ZFILL;
00139                                         continue;
00140                                 case '+':
00141                                         flags |= FL_PLUS;
00142                                         /* FALLTHROUGH */
00143                                 case ' ':
00144                                         flags |= FL_SPACE;
00145                                         continue;
00146                                 case '-':
00147                                         flags |= FL_LPAD;
00148                                         continue;
00149                                 case '#':
00150                                         flags |= FL_ALT;
00151                                         continue;
00152                                 }
00153                         }
00154 
00155                         if (flags < FL_LONG) {
00156                                 if (c >= '0' && c <= '9') {
00157                                         c -= '0';
00158                                         if (flags & FL_PREC) {
00159                                                 prec = 10*prec + c;
00160                                                 continue;
00161                                         }
00162                                         width = 10*width + c;
00163                                         flags |= FL_WIDTH;
00164                                         continue;
00165                                 }
00166                                 if (c == '.') {
00167                                         if (flags & FL_PREC)
00168                                                 return;
00169                                         flags |= FL_PREC;
00170                                         continue;
00171                                 }
00172                                 if (c == 'l') {
00173                                         flags |= FL_LONG;
00174                                         continue;
00175                                 }
00176                                 if (c == 'h')
00177                                         continue;
00178                         }
00179             
00180                         break;
00181                 } while ( (c = GETBYTE (in_progmem, 1, fmt)) != 0);
00182 
00183                 /*
00184                  * Handle floating-point formats E, F, G, e, f, g.
00185                  */
00186                 if (c >= 'E' && c <= 'G') {
00187                         flags |= FL_FLTUPP;
00188                         c += 'e' - 'E';
00189                         goto flt_oper;
00190 
00191                 } else if (c >= 'e' && c <= 'g') {
00192 
00193                         int exp;                /* exponent of master decimal digit     */
00194                         int n;
00195                         unsigned char vtype;    /* result of float value parse  */
00196                         unsigned char sign;     /* sign character (or 0)        */
00197                         unsigned char ndigs;
00198 
00199                         flags &= ~FL_FLTUPP;
00200 
00201                 flt_oper:
00202                         if (!(flags & FL_PREC))
00203                                 prec = 6;
00204                         flags &= ~(FL_FLTEXP | FL_FLTFIX);
00205                         if (c == 'e')
00206                                 flags |= FL_FLTEXP;
00207                         else if (c == 'f')
00208                                 flags |= FL_FLTFIX;
00209                         else if (prec > 0)
00210                                 prec -= 1;
00211 
00212                         if (flags & FL_FLTFIX) {
00213                                 vtype = 7;              /* 'prec' arg for 'ftoa_engine' */
00214                                 ndigs = prec < 60 ? prec + 1 : 60;
00215                         } else {
00216                                 if (prec > 7) prec = 7;
00217                                 vtype = prec;
00218                                 ndigs = 0;
00219                         }
00220                         exp = __ftoa_engine (va_arg(ap,double), (char *)buf, vtype, ndigs);
00221                         vtype = buf[0];
00222     
00223                         sign = 0;
00224                         if ((vtype & FTOA_MINUS) && !(vtype & FTOA_NAN))
00225                                 sign = '-';
00226                         else if (flags & FL_PLUS)
00227                                 sign = '+';
00228                         else if (flags & FL_SPACE)
00229                                 sign = ' ';
00230 
00231                         if (vtype & (FTOA_NAN | FTOA_INF)) {
00232                                 const char *p;
00233                                 ndigs = sign ? 4 : 3;
00234                                 if (width > ndigs) {
00235                                         width -= ndigs;
00236                                         if (!(flags & FL_LPAD)) {
00237                                                 do {
00238                                                         write(' ');
00239                                                 } while (--width);
00240                                         }
00241                                 } else {
00242                                         width = 0;
00243                                 }
00244                                 if (sign)
00245                                         write(sign);
00246                                 p = PSTR("inf");
00247                                 if (vtype & FTOA_NAN)
00248                                         p = PSTR("nan");
00249                                 while ( (ndigs = pgm_read_byte(p)) != 0) {
00250                                         if (flags & FL_FLTUPP)
00251                                                 ndigs += 'I' - 'i';
00252                                         write(ndigs);
00253                                         p++;
00254                                 }
00255                                 goto tail;
00256                         }
00257 
00258                         /* Output format adjustment, number of decimal digits in buf[] */
00259                         if (flags & FL_FLTFIX) {
00260                                 ndigs += exp;
00261                                 if ((vtype & FTOA_CARRY) && buf[1] == '1')
00262                                         ndigs -= 1;
00263                                 if ((signed char)ndigs < 1)
00264                                         ndigs = 1;
00265                                 else if (ndigs > 8)
00266                                         ndigs = 8;
00267                         } else if (!(flags & FL_FLTEXP)) {              /* 'g(G)' format */
00268                                 if (exp <= prec && exp >= -4)
00269                                         flags |= FL_FLTFIX;
00270                                 while (prec && buf[1+prec] == '0')
00271                                         prec--;
00272                                 if (flags & FL_FLTFIX) {
00273                                         ndigs = prec + 1;               /* number of digits in buf */
00274                                         prec = prec > exp
00275                                                 ? prec - exp : 0;       /* fractional part length  */
00276                                 }
00277                         }
00278     
00279                         /* Conversion result length, width := free space length */
00280                         if (flags & FL_FLTFIX)
00281                                 n = (exp>0 ? exp+1 : 1);
00282                         else
00283                                 n = 5;          /* 1e+00 */
00284                         if (sign) n += 1;
00285                         if (prec) n += prec + 1;
00286                         width = width > n ? width - n : 0;
00287     
00288                         /* Output before first digit    */
00289                         if (!(flags & (FL_LPAD | FL_ZFILL))) {
00290                                 while (width) {
00291                                         write(' ');
00292                                         width--;
00293                                 }
00294                         }
00295                         if (sign) write(sign);
00296                         if (!(flags & FL_LPAD)) {
00297                                 while (width) {
00298                                         write('0');
00299                                         width--;
00300                                 }
00301                         }
00302     
00303                         if (flags & FL_FLTFIX) {                /* 'f' format           */
00304 
00305                                 n = exp > 0 ? exp : 0;          /* exponent of left digit */
00306                                 do {
00307                                         if (n == -1)
00308                                                 write('.');
00309                                         flags = (n <= exp && n > exp - ndigs)
00310                                                 ? buf[exp - n + 1] : '0';
00311                                         if (--n < -prec)
00312                                                 break;
00313                                         write(flags);
00314                                 } while (1);
00315                                 if (n == exp
00316                                     && (buf[1] > '5'
00317                                         || (buf[1] == '5' && !(vtype & FTOA_CARRY))) )
00318                                         {
00319                                                 flags = '1';
00320                                         }
00321                                 write(flags);
00322         
00323                         } else {                                /* 'e(E)' format        */
00324 
00325                                 /* mantissa     */
00326                                 if (buf[1] != '1')
00327                                         vtype &= ~FTOA_CARRY;
00328                                 write(buf[1]);
00329                                 if (prec) {
00330                                         write('.');
00331                                         sign = 2;
00332                                         do {
00333                                                 write(buf[sign++]);
00334                                         } while (--prec);
00335                                 }
00336 
00337                                 /* exponent     */
00338                                 write(flags & FL_FLTUPP ? 'E' : 'e');
00339                                 ndigs = '+';
00340                                 if (exp < 0 || (exp == 0 && (vtype & FTOA_CARRY) != 0)) {
00341                                         exp = -exp;
00342                                         ndigs = '-';
00343                                 }
00344                                 write(ndigs);
00345                                 for (ndigs = '0'; exp >= 10; exp -= 10)
00346                                         ndigs += 1;
00347                                 write(ndigs);
00348                                 write('0' + exp);
00349                         }
00350 
00351                         goto tail;
00352                 }
00353 
00354                 /*
00355                  * Handle string formats c, s, S.
00356                  */
00357                 {
00358                         const char * pnt;
00359                         size_t size;
00360 
00361                         switch (c) {
00362 
00363                         case 'c':
00364                                 buf[0] = va_arg (ap, int);
00365                                 pnt = (char *)buf;
00366                                 size = 1;
00367                                 goto no_pgmstring;
00368 
00369                         case 's':
00370                                 pnt = va_arg (ap, char *);
00371                                 size = strnlen (pnt, (flags & FL_PREC) ? prec : ~0);
00372                         no_pgmstring:
00373                                 flags &= ~FL_PGMSTRING;
00374                                 goto str_lpad;
00375 
00376                         case 'S':
00377                         pgmstring:
00378                                 pnt = va_arg (ap, char *);
00379                                 size = strnlen_P (pnt, (flags & FL_PREC) ? prec : ~0);
00380                                 flags |= FL_PGMSTRING;
00381 
00382                         str_lpad:
00383                                 if (!(flags & FL_LPAD)) {
00384                                         while (size < width) {
00385                                                 write(' ');
00386                                                 width--;
00387                                         }
00388                                 }
00389                                 while (size) {
00390                                         write(GETBYTE (flags, FL_PGMSTRING, pnt));
00391                                         if (width) width -= 1;
00392                                         size -= 1;
00393                                 }
00394                                 goto tail;
00395                         }
00396                 }
00397 
00398                 /*
00399                  * Handle integer formats variations for d/i, u, o, p, x, X.
00400                  */
00401                 if (c == 'd' || c == 'i') {
00402                         long x = (flags & FL_LONG) ? va_arg(ap,long) : va_arg(ap,int);
00403                         flags &= ~(FL_NEGATIVE | FL_ALT);
00404                         if (x < 0) {
00405                                 x = -x;
00406                                 flags |= FL_NEGATIVE;
00407                         }
00408                         c = __ultoa_invert (x, (char *)buf, 10) - (char *)buf;
00409 
00410                 } else {
00411                         int base;
00412 
00413                         if (c == 'u') {
00414                                 flags &= ~FL_ALT;
00415                                 base = 10;
00416                                 goto ultoa;
00417                         }
00418 
00419                         flags &= ~(FL_PLUS | FL_SPACE);
00420 
00421                         switch (c) {
00422                         case 'o':
00423                                 base = 8;
00424                                 goto ultoa;
00425                         case 'p':
00426                                 flags |= FL_ALT;
00427                                 /* no break */
00428                         case 'x':
00429                                 if (flags & FL_ALT)
00430                                         flags |= FL_ALTHEX;
00431                                 base = 16;
00432                                 goto ultoa;
00433                         case 'X':
00434                                 if (flags & FL_ALT)
00435                                         flags |= (FL_ALTHEX | FL_ALTUPP);
00436                                 base = 16 | XTOA_UPPER;
00437                         ultoa:
00438                                 c = __ultoa_invert ((flags & FL_LONG)
00439                                                     ? va_arg(ap, unsigned long)
00440                                                     : va_arg(ap, unsigned int),
00441                                                     (char *)buf, base)  -  (char *)buf;
00442                                 flags &= ~FL_NEGATIVE;
00443                                 break;
00444 
00445                         default:
00446                                 return;
00447                         }
00448                 }
00449 
00450                 /*
00451                  * Format integers.
00452                  */
00453                 {
00454                         unsigned char len;
00455 
00456                         len = c;
00457                         if (flags & FL_PREC) {
00458                                 flags &= ~FL_ZFILL;
00459                                 if (len < prec) {
00460                                         len = prec;
00461                                         if ((flags & FL_ALT) && !(flags & FL_ALTHEX))
00462                                                 flags &= ~FL_ALT;
00463                                 }
00464                         }
00465                         if (flags & FL_ALT) {
00466                                 if (buf[c-1] == '0') {
00467                                         flags &= ~(FL_ALT | FL_ALTHEX | FL_ALTUPP);
00468                                 } else {
00469                                         len += 1;
00470                                         if (flags & FL_ALTHEX)
00471                                                 len += 1;
00472                                 }
00473                         } else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) {
00474                                 len += 1;
00475                         }
00476 
00477                         if (!(flags & FL_LPAD)) {
00478                                 if (flags & FL_ZFILL) {
00479                                         prec = c;
00480                                         if (len < width) {
00481                                                 prec += width - len;
00482                                                 len = width;
00483                                         }
00484                                 }
00485                                 while (len < width) {
00486                                         write(' ');
00487                                         len++;
00488                                 }
00489                         }
00490         
00491                         width =  (len < width) ? width - len : 0;
00492 
00493                         if (flags & FL_ALT) {
00494                                 write('0');
00495                                 if (flags & FL_ALTHEX)
00496                                         write(flags & FL_ALTUPP ? 'X' : 'x');
00497                         } else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) {
00498                                 unsigned char z = ' ';
00499                                 if (flags & FL_PLUS) z = '+';
00500                                 if (flags & FL_NEGATIVE) z = '-';
00501                                 write(z);
00502                         }
00503                 
00504                         while (prec > c) {
00505                                 write('0');
00506                                 prec--;
00507                         }
00508         
00509                         do {
00510                                 write(buf[--c]);
00511                         } while (c);
00512                 }
00513         
00514         tail:
00515                 /* Tail is possible.    */
00516                 while (width) {
00517                         write(' ');
00518                         width--;
00519                 }
00520         } /* for (;;) */
00521 }

Generated for ArduPilot Libraries by doxygen