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 #include "format.h"
  29
  30 static const char hexdigit[] = "0123456789ABCDEF";
  31
  32 void format(
  33     /* call 'push()' for each output letter */
  34     int (*push)(void *userp, unsigned char data),
  35     void *userp,
  36     const char *fmt,
  37     va_list ap)
  38 {
  39     char *str;
  40     char tmpbuf[12], pad;
  41     int ch, width, val, sign, precision;
  42     long lval, lsign;
  43     unsigned int uval;
  44     unsigned long ulval;
  45     size_t uszval;
  46     ssize_t szval, szsign;
  47     bool ok = true;
  48
  49     tmpbuf[sizeof tmpbuf - 1] = '\0';
  50
  51     while ((ch = *fmt++) != '\0' && ok)
  52     {
  53     if (ch == '%')
  54     {
  55         ch = *fmt++;
  56         pad = ' ';
  57         if (ch == '0')
  58         pad = '0';
  59
  60         width = 0;
  61         while (ch >= '0' && ch <= '9')
  62         {
  63         width = 10*width + ch - '0';
  64         ch = *fmt++;
  65         }
  66
  67         precision = 0;
  68         if(ch == '.')
  69         {
  70             ch = *fmt++;
  71             while (ch >= '0' && ch <= '9')
  72             {
  73                 precision = 10*precision + ch - '0';
  74                 ch = *fmt++;
  75             }
  76         } else {
  77             precision = INT_MAX;
  78         }
  79
  80         str = tmpbuf + sizeof tmpbuf - 1;
  81         switch (ch)
  82         {
  83         case 'c':
  84             *--str = va_arg (ap, int);
  85             break;
  86
  87         case 's':
  88             str = va_arg (ap, char*);
  89             break;
  90
  91         case 'd':
  92             val = sign = va_arg (ap, int);
  93             if (val < 0)
  94             val = -val;
  95             do
  96             {
  97             *--str = (val % 10) + '0';
  98             val /= 10;
  99             }
 100             while (val > 0);
 101             if (sign < 0)
 102             *--str = '-';
 103             break;
 104
 105         case 'u':
 106             uval = va_arg(ap, unsigned int);
 107             do
 108             {
 109             *--str = (uval % 10) + '0';
 110             uval /= 10;
 111             }
 112             while (uval > 0);
 113             break;
 114
 115         case 'x':
 116         case 'X':
 117             pad='0';
 118             uval = va_arg (ap, int);
 119             do
 120             {
 121             *--str = hexdigit[uval & 0xf];
 122             uval >>= 4;
 123             }
 124             while (uval);
 125             break;
 126
 127         case 'l':
 128             ch = *fmt++;
 129             switch(ch) {
 130                 case 'x':
 131                 case 'X':
 132                     pad='0';
 133                     ulval = va_arg (ap, long);
 134                     do
 135                     {
 136                         *--str = hexdigit[ulval & 0xf];
 137                         ulval >>= 4;
 138                     }
 139                     while (ulval);
 140                     break;
 141                 case 'd':
 142                     lval = lsign = va_arg (ap, long);
 143                     if (lval < 0)
 144                         lval = -lval;
 145                     do
 146                     {
 147                         *--str = (lval % 10) + '0';
 148                         lval /= 10;
 149                     }
 150                     while (lval > 0);
 151                     if (lsign < 0)
 152                         *--str = '-';
 153                     break;
 154
 155                 case 'u':
 156                     ulval = va_arg(ap, unsigned long);
 157                     do
 158                     {
 159                         *--str = (ulval % 10) + '0';
 160                         ulval /= 10;
 161                     }
 162                     while (ulval > 0);
 163                     break;
 164
 165                 default:
 166                     *--str = 'l';
 167                     *--str = ch;
 168             }
 169
 170             break;
 171
 172         case 'z':
 173             ch = *fmt++;
 174             switch(ch) {
 175                 case 'd':
 176                     szval = szsign = va_arg (ap, ssize_t);
 177                     if (szval < 0)
 178                         szval = -szval;
 179                     do
 180                     {
 181                         *--str = (szval % 10) + '0';
 182                         szval /= 10;
 183                     }
 184                     while (szval > 0);
 185                     if (szsign < 0)
 186                         *--str = '-';
 187                     break;
 188
 189                 case 'u':
 190                     uszval = va_arg(ap, size_t);
 191                     do
 192                     {
 193                         *--str = (uszval % 10) + '0';
 194                         uszval /= 10;
 195                     }
 196                     while (uszval > 0);
 197                     break;
 198
 199                 default:
 200                     *--str = 'z';
 201                     *--str = ch;
 202             }
 203
 204             break;
 205
 206         default:
 207             *--str = ch;
 208             break;
 209         }
 210
 211         if (width > 0)
 212         {
 213         width -= strlen (str);
 214         while (width-- > 0 && ok)
 215             ok=push(userp, pad);
 216         }
 217         while (*str != '\0' && ok && precision--)
 218                 ok=push(userp, *str++);
 219     }
 220     else
 221         ok=push(userp, ch);
 222     }
 223 }
 224
 225 struct for_fprintf {
 226     int fd;    /* where to store it */
 227     int bytes; /* amount stored */
 228 };
 229
 230 static int fprfunc(void *pr, unsigned char letter)
 231 {
 232     struct for_fprintf *fpr  = (struct for_fprintf *)pr;
 233     int rc = write(fpr->fd, &letter, 1);
 234
 235     if(rc > 0) {
 236         fpr->bytes++; /* count them */
 237         return true;  /* we are ok */
 238     }
 239
 240     return false; /* failure */
 241 }
 242
 243
 244 int fdprintf(int fd, const char *fmt, ...)
 245 {
 246     va_list ap;
 247     struct for_fprintf fpr;
 248
 249     fpr.fd=fd;
 250     fpr.bytes=0;
 251
 252     va_start(ap, fmt);
 253     format(fprfunc, &fpr, fmt, ap);
 254     va_end(ap);
 255
 256     return fpr.bytes; /* return 0 on error */
 257 }
 258
 259 void vuprintf(int (*push)(void *userp, unsigned char data), void *userp, const char *fmt, va_list ap)
 260 {
 261     format(push, userp, fmt, ap);
 262 }