firmware/common/format.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2002 by Gary Czvitkovicz
  11  *
  12  * This program is free software; you can redistribute it and/or
  13  * modify it under the terms of the GNU General Public License
  14  * as published by the Free Software Foundation; either version 2
  15  * of the License, or (at your option) any later version.
  16  *
  17  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  18  * KIND, either express or implied.
  19  *
  20  ****************************************************************************/
  21
  22
  23 #include <stdarg.h>
  24 #include <stdbool.h>
  25 #include <limits.h>
  26 #include <string.h>
  27 #include "file.h"
  28
  29 static const char hexdigit[] = "0123456789ABCDEF";
  30
  31 int format(
  32     /* call 'push()' for each output letter */
  33     int (*push)(void *userp, unsigned char data),
  34     void *userp,
  35     const char *fmt,
  36     va_list ap)
  37 {
  38     char *str;
  39     char tmpbuf[12], pad;
  40     int ch, width, val, sign, precision;
  41     long lval, lsign;
  42     unsigned int uval;
  43     unsigned long ulval;
  44     bool ok = true;
  45
  46     tmpbuf[sizeof tmpbuf - 1] = '\0';
  47
  48     while ((ch = *fmt++) != '\0' && ok)
  49     {
  50     if (ch == '%')
  51     {
  52         ch = *fmt++;
  53         pad = ' ';
  54         if (ch == '0')
  55         pad = '0';
  56
  57         width = 0;
  58         while (ch >= '0' && ch <= '9')
  59         {
  60         width = 10*width + ch - '0';
  61         ch = *fmt++;
  62         }
  63
  64         precision = 0;
  65         if(ch == '.')
  66         {
  67             ch = *fmt++;
  68             while (ch >= '0' && ch <= '9')
  69             {
  70                 precision = 10*precision + ch - '0';
  71                 ch = *fmt++;
  72             }
  73         } else {
  74             precision = INT_MAX;
  75         }
  76
  77         str = tmpbuf + sizeof tmpbuf - 1;
  78         switch (ch)
  79         {
  80         case 'c':
  81             *--str = va_arg (ap, int);
  82             break;
  83
  84         case 's':
  85             str = va_arg (ap, char*);
  86             break;
  87
  88         case 'd':
  89             val = sign = va_arg (ap, int);
  90             if (val < 0)
  91             val = -val;
  92             do
  93             {
  94             *--str = (val % 10) + '0';
  95             val /= 10;
  96             }
  97             while (val > 0);
  98             if (sign < 0)
  99             *--str = '-';
 100             break;
 101
 102         case 'u':
 103             uval = va_arg(ap, unsigned int);
 104             do
 105             {
 106             *--str = (uval % 10) + '0';
 107             uval /= 10;
 108             }
 109             while (uval > 0);
 110             break;
 111
 112         case 'x':
 113         case 'X':
 114             pad='0';
 115             uval = va_arg (ap, int);
 116             do
 117             {
 118             *--str = hexdigit[uval & 0xf];
 119             uval >>= 4;
 120             }
 121             while (uval);
 122             break;
 123
 124         case 'l':
 125         case 'z': /* assume sizeof(size_t) == sizeof(long) */
 126             ch = *fmt++;
 127             switch(ch) {
 128                 case 'x':
 129                 case 'X':
 130                     pad='0';
 131                     ulval = va_arg (ap, long);
 132                     do
 133                     {
 134                         *--str = hexdigit[ulval & 0xf];
 135                         ulval >>= 4;
 136                     }
 137                     while (ulval);
 138                     break;
 139                 case 'd':
 140                     lval = lsign = va_arg (ap, long);
 141                     if (lval < 0)
 142                         lval = -lval;
 143                     do
 144                     {
 145                         *--str = (lval % 10) + '0';
 146                         lval /= 10;
 147                     }
 148                     while (lval > 0);
 149                     if (lsign < 0)
 150                         *--str = '-';
 151                     break;
 152
 153                 case 'u':
 154                     ulval = va_arg(ap, unsigned long);
 155                     do
 156                     {
 157                         *--str = (ulval % 10) + '0';
 158                         ulval /= 10;
 159                     }
 160                     while (ulval > 0);
 161                     break;
 162
 163                 default:
 164                     *--str = 'l';
 165                     *--str = ch;
 166             }
 167
 168             break;
 169
 170         default:
 171             *--str = ch;
 172             break;
 173         }
 174
 175         if (width > 0)
 176         {
 177         width -= strlen (str);
 178         while (width-- > 0 && ok)
 179             ok=push(userp, pad);
 180         }
 181         while (*str != '\0' && ok && precision--)
 182                 ok=push(userp, *str++);
 183     }
 184     else
 185         ok=push(userp, ch);
 186     }
 187     return ok; /* true means good */
 188 }
 189
 190 struct for_fprintf {
 191     int fd;    /* where to store it */
 192     int bytes; /* amount stored */
 193 };
 194
 195 static int fprfunc(void *pr, unsigned char letter)
 196 {
 197     struct for_fprintf *fpr  = (struct for_fprintf *)pr;
 198     int rc = write(fpr->fd, &letter, 1);
 199
 200     if(rc > 0) {
 201         fpr->bytes++; /* count them */
 202         return true;  /* we are ok */
 203     }
 204
 205     return false; /* failure */
 206 }
 207
 208
 209 int fdprintf(int fd, const char *fmt, ...)
 210 {
 211     bool ok;
 212     va_list ap;
 213     struct for_fprintf fpr;
 214
 215     fpr.fd=fd;
 216     fpr.bytes=0;
 217
 218     va_start(ap, fmt);
 219     ok = format(fprfunc, &fpr, fmt, ap);
 220     va_end(ap);
 221
 222     return fpr.bytes; /* return 0 on error */
 223 }
 224
 225 int vuprintf(int (*push)(void *userp, unsigned char data), void *userp, const char *fmt, va_list ap)
 226 {
 227     return format(push, userp, fmt, ap);
 228 }