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     unsigned int uval;
  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             str = tmpbuf + sizeof tmpbuf - 1;
  68             switch (ch)
  69             {
  70                 case 'c':
  71                     *--str = va_arg (ap, int);
  72                     break;
  73
  74                 case 's':
  75                     str = va_arg (ap, char*);
  76                     break;
  77
  78                 case 'd':
  79                     val = sign = va_arg (ap, int);
  80                     if (val < 0)
  81                         val = -val;
  82                     do
  83                     {
  84                         *--str = (val % 10) + '0';
  85                         val /= 10;
  86                     }
  87                     while (val > 0);
  88                     if (sign < 0)
  89                         *--str = '-';
  90                     break;
  91
  92                 case 'x':
  93                 case 'X':
  94                     uval = va_arg (ap, int);
  95                     do
  96                     {
  97                         *--str = hexdigit[uval & 0xf];
  98                         uval >>= 4;
  99                     }
 100                     while (uval);
 101                     break;
 102
 103                 default:
 104                     *--str = ch;
 105                     break;
 106             }
 107
 108             if (width > 0)
 109             {
 110                 width -= strlen (str);
 111                 while (width-- > 0 && ok)
 112                     ok=push(userp, pad);
 113             }
 114             while (*str != '\0' && ok)
 115                 ok=push(userp, *str++);
 116         }
 117         else
 118             ok=push(userp, ch);
 119     }
 120     return ok; /* true means good */
 121 }
 122
 123 struct for_snprintf {
 124     unsigned char *ptr; /* where to store it */
 125     int bytes; /* amount already stored */
 126     int max;   /* max amount to store */
 127 };
 128
 129 static int sprfunc(void *ptr, unsigned char letter)
 130 {
 131     struct for_snprintf *pr = (struct for_snprintf *)ptr;
 132     if(pr->bytes < pr->max) {
 133         *pr->ptr = letter;
 134         pr->ptr++;
 135         pr->bytes++;
 136         return true;
 137     }
 138     return false; /* filled buffer */
 139 }
 140
 141
 142 int snprintf(char *buf, size_t size, const char *fmt, ...)
 143 {
 144     bool ok;
 145     va_list ap;
 146     struct for_snprintf pr;
 147
 148     pr.ptr = buf;
 149     pr.bytes = 0;
 150     pr.max = size;
 151
 152     va_start(ap, fmt);
 153     ok = format(sprfunc, &pr, fmt, ap);
 154     va_end(ap);
 155
 156     /* make sure it ends with a trailing zero */
 157     pr.ptr[(pr.bytes < pr.max) ? 0 : -1] = '\0';
 158
 159     return pr.bytes;
 160 }
 161
 162 int vsnprintf(char *buf, int size, const char *fmt, va_list ap)
 163 {
 164     bool ok;
 165     struct for_snprintf pr;
 166
 167     pr.ptr = buf;
 168     pr.bytes = 0;
 169     pr.max = size;
 170
 171     ok = format(sprfunc, &pr, fmt, ap);
 172
 173     /* make sure it ends with a trailing zero */
 174     pr.ptr[(pr.bytes < pr.max) ? 0 : -1] = '\0';
 175
 176     return pr.bytes;
 177 }
 178
 179 struct for_fprintf {
 180     int fd;    /* where to store it */
 181     int bytes; /* amount stored */
 182 };
 183
 184 static int fprfunc(void *pr, unsigned char letter)
 185 {
 186     struct for_fprintf *fpr  = (struct for_fprintf *)pr;
 187     int rc = write(fpr->fd, &letter, 1);
 188
 189     if(rc > 0) {
 190         fpr->bytes++; /* count them */
 191         return true;  /* we are ok */
 192     }
 193
 194     return false; /* failure */
 195 }
 196
 197
 198 int fprintf(int fd, const char *fmt, ...)
 199 {
 200     bool ok;
 201     va_list ap;
 202     struct for_fprintf fpr;
 203
 204     fpr.fd=fd;
 205     fpr.bytes=0;
 206
 207     va_start(ap, fmt);
 208     ok = format(fprfunc, &fpr, fmt, ap);
 209     va_end(ap);
 210
 211     return fpr.bytes; /* return 0 on error */
 212 }
 213