firmware/common/sprintf.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2002 by Gary Czvitkovicz
  11  *
  12  * All files in this archive are subject to the GNU General Public License.
  13  * See the file COPYING in the source tree root for full license agreement.
  14  *
  15  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  16  * KIND, either express or implied.
  17  *
  18  ****************************************************************************/
  19
  20 /*
  21  * Minimal printf and snprintf formatting functions
  22  *
  23  * These support %c %s %d and %x
  24  * Field width and zero-padding flag only
  25  */
  26
  27 #include <stdarg.h>
  28 #include <string.h>
  29 #include <stdbool.h>
  30
  31 #include "file.h" /* for write(), used in fprintf() */
  32 #include "sprintf.h" /* to allow the simulator magic */
  33
  34 static const char hexdigit[] = "0123456789ABCDEF";
  35
  36 static int format(
  37     /* call 'push()' for each output letter */
  38     int (*push)(void *userp, unsigned char data),
  39     void *userp,
  40     const char *fmt,
  41     va_list ap)
  42 {
  43     char *str;
  44     char tmpbuf[12], pad;
  45     int ch, width, val, sign;
  46     long lval;
  47     unsigned int uval;
  48     unsigned long ulval;
  49     bool ok = true;
  50
  51     tmpbuf[sizeof tmpbuf - 1] = '\0';
  52
  53     while ((ch = *fmt++) != '\0' && ok)
  54     {
  55     if (ch == '%')
  56     {
  57         ch = *fmt++;
  58         pad = ' ';
  59         if (ch == '0')
  60         pad = '0';
  61
  62         width = 0;
  63         while (ch >= '0' && ch <= '9')
  64         {
  65         width = 10*width + ch - '0';
  66         ch = *fmt++;
  67         }
  68
  69         str = tmpbuf + sizeof tmpbuf - 1;
  70         switch (ch)
  71         {
  72         case 'c':
  73             *--str = va_arg (ap, int);
  74             break;
  75
  76         case 's':
  77             str = va_arg (ap, char*);
  78             break;
  79
  80         case 'd':
  81             val = sign = va_arg (ap, int);
  82             if (val < 0)
  83             val = -val;
  84             do
  85             {
  86             *--str = (val % 10) + '0';
  87             val /= 10;
  88             }
  89             while (val > 0);
  90             if (sign < 0)
  91             *--str = '-';
  92             break;
  93
  94         case 'x':
  95         case 'X':
  96             uval = va_arg (ap, int);
  97             do
  98             {
  99             *--str = hexdigit[uval & 0xf];
 100             uval >>= 4;
 101             }
 102             while (uval);
 103             break;
 104
 105         case 'l':
 106             ch = *fmt++;
 107             switch(ch) {
 108                 case 'x':
 109                 case 'X':
 110                     ulval = va_arg (ap, long);
 111                     do
 112                     {
 113                         *--str = hexdigit[ulval & 0xf];
 114                         ulval >>= 4;
 115                     }
 116                     while (ulval);
 117                     break;
 118                 case 'd':
 119                     lval = sign = va_arg (ap, long);
 120                     if (lval < 0)
 121                         lval = -lval;
 122                     do
 123                     {
 124                         *--str = (lval % 10) + '0';
 125                         lval /= 10;
 126                     }
 127                     while (lval > 0);
 128                     if (sign < 0)
 129                         *--str = '-';
 130                     break;
 131
 132                 default:
 133                     *--str = 'l';
 134                     *--str = ch;
 135             }
 136
 137             break;
 138
 139         default:
 140             *--str = ch;
 141             break;
 142         }
 143
 144         if (width > 0)
 145         {
 146         width -= strlen (str);
 147         while (width-- > 0 && ok)
 148             ok=push(userp, pad);
 149         }
 150         while (*str != '\0' && ok)
 151                 ok=push(userp, *str++);
 152     }
 153     else
 154         ok=push(userp, ch);
 155     }
 156     return ok; /* true means good */
 157 }
 158
 159 struct for_snprintf {
 160     unsigned char *ptr; /* where to store it */
 161     int bytes; /* amount already stored */
 162     int max;   /* max amount to store */
 163 };
 164
 165 static int sprfunc(void *ptr, unsigned char letter)
 166 {
 167     struct for_snprintf *pr = (struct for_snprintf *)ptr;
 168     if(pr->bytes < pr->max) {
 169         *pr->ptr = letter;
 170         pr->ptr++;
 171         pr->bytes++;
 172         return true;
 173     }
 174     return false; /* filled buffer */
 175 }
 176
 177
 178 int snprintf(char *buf, size_t size, const char *fmt, ...)
 179 {
 180     bool ok;
 181     va_list ap;
 182     struct for_snprintf pr;
 183
 184     pr.ptr = buf;
 185     pr.bytes = 0;
 186     pr.max = size;
 187
 188     va_start(ap, fmt);
 189     ok = format(sprfunc, &pr, fmt, ap);
 190     va_end(ap);
 191
 192     /* make sure it ends with a trailing zero */
 193     pr.ptr[(pr.bytes < pr.max) ? 0 : -1] = '\0';
 194
 195     return pr.bytes;
 196 }
 197
 198 int vsnprintf(char *buf, int size, const char *fmt, va_list ap)
 199 {
 200     bool ok;
 201     struct for_snprintf pr;
 202
 203     pr.ptr = buf;
 204     pr.bytes = 0;
 205     pr.max = size;
 206
 207     ok = format(sprfunc, &pr, fmt, ap);
 208
 209     /* make sure it ends with a trailing zero */
 210     pr.ptr[(pr.bytes < pr.max) ? 0 : -1] = '\0';
 211
 212     return pr.bytes;
 213 }
 214
 215 struct for_fprintf {
 216     int fd;    /* where to store it */
 217     int bytes; /* amount stored */
 218 };
 219
 220 static int fprfunc(void *pr, unsigned char letter)
 221 {
 222     struct for_fprintf *fpr  = (struct for_fprintf *)pr;
 223     int rc = write(fpr->fd, &letter, 1);
 224
 225     if(rc > 0) {
 226         fpr->bytes++; /* count them */
 227         return true;  /* we are ok */
 228     }
 229
 230     return false; /* failure */
 231 }
 232
 233
 234 int fprintf(int fd, const char *fmt, ...)
 235 {
 236     bool ok;
 237     va_list ap;
 238     struct for_fprintf fpr;
 239
 240     fpr.fd=fd;
 241     fpr.bytes=0;
 242
 243     va_start(ap, fmt);
 244     ok = format(fprfunc, &fpr, fmt, ap);
 245     va_end(ap);
 246
 247     return fpr.bytes; /* return 0 on error */
 248 }
 249