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         case 'p':
 115         case 'P':
 116             /* for pointers prepend 0x and act like 'X' */
 117             push(userp, '0');
 118             push(userp, 'x');
 119             /* fall through */
 120         case 'x':
 121         case 'X':
 122             pad='0';
 123             uval = va_arg (ap, int);
 124             do
 125             {
 126             *--str = hexdigit[uval & 0xf];
 127             uval >>= 4;
 128             }
 129             while (uval);
 130             break;
 131
 132         case 'l':
 133             ch = *fmt++;
 134             switch(ch) {
 135                 case 'x':
 136                 case 'X':
 137                     pad='0';
 138                     ulval = va_arg (ap, long);
 139                     do
 140                     {
 141                         *--str = hexdigit[ulval & 0xf];
 142                         ulval >>= 4;
 143                     }
 144                     while (ulval);
 145                     break;
 146                 case 'd':
 147                     lval = lsign = va_arg (ap, long);
 148                     if (lval < 0)
 149                         lval = -lval;
 150                     do
 151                     {
 152                         *--str = (lval % 10) + '0';
 153                         lval /= 10;
 154                     }
 155                     while (lval > 0);
 156                     if (lsign < 0)
 157                         *--str = '-';
 158                     break;
 159
 160                 case 'u':
 161                     ulval = va_arg(ap, unsigned long);
 162                     do
 163                     {
 164                         *--str = (ulval % 10) + '0';
 165                         ulval /= 10;
 166                     }
 167                     while (ulval > 0);
 168                     break;
 169
 170                 default:
 171                     *--str = 'l';
 172                     *--str = ch;
 173             }
 174
 175             break;
 176
 177         case 'z':
 178             ch = *fmt++;
 179             switch(ch) {
 180                 case 'd':
 181                     szval = szsign = va_arg (ap, ssize_t);
 182                     if (szval < 0)
 183                         szval = -szval;
 184                     do
 185                     {
 186                         *--str = (szval % 10) + '0';
 187                         szval /= 10;
 188                     }
 189                     while (szval > 0);
 190                     if (szsign < 0)
 191                         *--str = '-';
 192                     break;
 193
 194                 case 'u':
 195                     uszval = va_arg(ap, size_t);
 196                     do
 197                     {
 198                         *--str = (uszval % 10) + '0';
 199                         uszval /= 10;
 200                     }
 201                     while (uszval > 0);
 202                     break;
 203
 204                 default:
 205                     *--str = 'z';
 206                     *--str = ch;
 207             }
 208
 209             break;
 210
 211         default:
 212             *--str = ch;
 213             break;
 214         }
 215
 216         if (width > 0)
 217         {
 218         width -= strlen (str);
 219         while (width-- > 0 && ok)
 220             ok=push(userp, pad);
 221         }
 222         while (*str != '\0' && ok && precision--)
 223                 ok=push(userp, *str++);
 224     }
 225     else
 226         ok=push(userp, ch);
 227     }
 228 }
 229
 230 struct for_fprintf {
 231     int fd;    /* where to store it */
 232     int bytes; /* amount stored */
 233 };
 234
 235 static int fprfunc(void *pr, unsigned char letter)
 236 {
 237     struct for_fprintf *fpr  = (struct for_fprintf *)pr;
 238     int rc = write(fpr->fd, &letter, 1);
 239
 240     if(rc > 0) {
 241         fpr->bytes++; /* count them */
 242         return true;  /* we are ok */
 243     }
 244
 245     return false; /* failure */
 246 }
 247
 248
 249 int fdprintf(int fd, const char *fmt, ...)
 250 {
 251     va_list ap;
 252     struct for_fprintf fpr;
 253
 254     fpr.fd=fd;
 255     fpr.bytes=0;
 256
 257     va_start(ap, fmt);
 258     format(fprfunc, &fpr, fmt, ap);
 259     va_end(ap);
 260
 261     return fpr.bytes; /* return 0 on error */
 262 }
 263
 264 void vuprintf(int (*push)(void *userp, unsigned char data), void *userp, const char *fmt, va_list ap)
 265 {
 266     format(push, userp, fmt, ap);
 267 }