qemu/roms/openhackware/src/libc/src/format.c

   1 /*
   2  * <format.c>
   3  *
   4  * Open Hack'Ware BIOS: formated output functions
   5  *
   6  * Copyright (c) 2004-2005 Jocelyn Mayer
   7  *
   8  *   This program is free software; you can redistribute it and/or
   9  *   modify it under the terms of the GNU General Public License V2
  10  *   as published by the Free Software Foundation
  11  *
  12  *   This program is distributed in the hope that it will be useful,
  13  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  *   GNU General Public License for more details.
  16  *
  17  *   You should have received a copy of the GNU General Public License
  18  *   along with this program; if not, write to the Free Software
  19  *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  20  */
  21
  22 /* functions prototypes are here */
  23 #include <stdio.h>
  24 /* va_list is defined here */
  25 #include <stdarg.h>
  26 /* size_t is defined here */
  27 #include <stddef.h>
  28 /* NULL is defined here */
  29 #include <stdlib.h>
  30 /* write is defined here */
  31 #include <unistd.h>
  32 /* memcpy is defined here */
  33 /* memset is defined here */
  34 /* strlen is defined here */
  35 #include <string.h>
  36
  37 #define unused __attribute__ (( unused ))
  38 int console_write (const void *buffer, int len);
  39 /* XXX: this is a hack to be fixed */
  40 int serial_write (const void *buffer, int len);
  41 #define debug_write serial_write
  42
  43 /* Low level output fonctions */
  44 typedef size_t (*outf_t)(void *private, const unsigned char *buf, size_t len);
  45
  46 /* output to fd */
  47 #if defined (__USE__vprintf__)
  48 size_t outfd (void *private, const unsigned char *buf, size_t len)
  49 {
  50     int *fd = private;
  51
  52     if (*fd == 1 || *fd == 2)
  53         return console_write(buf, len);
  54
  55     return write(*fd, buf, len);
  56 }
  57
  58 size_t outf_dbg (void *private, const unsigned char *buf, size_t len)
  59 {
  60     int *fd = private;
  61
  62     if (*fd == 1 || *fd == 2)
  63         return debug_write(buf, len);
  64
  65     return write(*fd, buf, len);
  66 }
  67
  68 /* output to buffer */
  69 size_t outbuf (void *private, const unsigned char *buf, size_t len)
  70 {
  71     unsigned char **dst = private;
  72
  73     memcpy(*dst, buf, len);
  74     (*dst) += len;
  75     (*dst)[0] = '\0';
  76
  77     return len;
  78 }
  79
  80 /* misc formatted output functions */
  81 /* out one character */
  82 static size_t outc (outf_t outf, void *private,
  83                     unsigned int value, size_t maxlen)
  84 {
  85     unsigned char buffer;
  86
  87     if (maxlen < 1)
  88         return 0;
  89     buffer = value;
  90     if ((*outf)(private, &buffer, 1) == (size_t)-1)
  91         return -1;
  92
  93     return 1;
  94 }
  95
  96 /* out one int in decimal */
  97 static size_t outdecs (outf_t outf, void *private,
  98                        int value, size_t fill, size_t maxlen)
  99 {
 100     unsigned char buffer[12];
 101     size_t pos, len;
 102     int sign;
 103
 104     buffer[11] = '\0';
 105     pos = 10;
 106     if (value == 0) {
 107         sign = 0;
 108         buffer[pos--] = '0';
 109     } else {
 110         if (value < 0) {
 111             sign = -1;
 112             value = -value;
 113         } else {
 114             sign = 1;
 115         }
 116         for (; value != 0; pos--) {
 117             buffer[pos] = (value % 10) + '0';
 118             value = value / 10;
 119         }
 120     }
 121     if (fill != 0)
 122         fill -= pos - 10;
 123     for (; fill != 0 && pos != 0; fill--) {
 124         buffer[pos--] = '0';
 125     }
 126     if (sign == -1)
 127         buffer[pos--] = '-';
 128     len = 10 - pos;
 129     if (len > maxlen)
 130         len = maxlen;
 131     if ((*outf)(private, buffer + pos + 1, len) == (size_t)-1)
 132         return -1;
 133
 134     return len;
 135 }
 136
 137 /* out one unsigned int as decimal */
 138 static size_t outdecu (outf_t outf, void *private,
 139                        unsigned int value, size_t fill, size_t maxlen)
 140 {
 141     unsigned char buffer[11];
 142     size_t pos, len;
 143
 144     buffer[10] = '\0';
 145     pos = 9;
 146     if (value == 0) {
 147         buffer[pos--] = '0';
 148     } else {
 149         for (; value != 0; pos--) {
 150             buffer[pos] = (value % 10) + '0';
 151             value = value / 10;
 152         }
 153     }
 154     if (fill != 0)
 155         fill -= pos - 9;
 156     for (; fill != 0 && pos != (size_t)-1; fill--) {
 157         buffer[pos--] = '0';
 158     }
 159     len = 9 - pos;
 160     if (len > maxlen)
 161         len = maxlen;
 162     if ((*outf)(private, buffer + pos + 1, len) == (size_t)-1)
 163         return -1;
 164
 165     return len;
 166 }
 167
 168 /* out one unsigned int as hexadecimal */
 169 static size_t outhex (outf_t outf, void *private,
 170                       unsigned int value, size_t fill, size_t maxlen)
 171 {
 172     unsigned char buffer[9];
 173     size_t pos, len;
 174     int d;
 175
 176     buffer[8] = '\0';
 177     pos = 7;
 178     if (value == 0) {
 179         buffer[pos--] = '0';
 180     } else {
 181         for (; value != 0; pos--) {
 182             d = value & 0xF;
 183             if (d > 9)
 184             d += 'a' - '0' - 10;
 185             buffer[pos] = d + '0';
 186             value = value >> 4;
 187         }
 188     }
 189     if (fill > 0)
 190         fill -= pos - 7;
 191     for (; fill != 0 && pos != (size_t)-1; fill--) {
 192         buffer[pos--] = '0';
 193     }
 194     len = 7 - pos;
 195     if (len > maxlen)
 196         len = maxlen;
 197     if ((*outf)(private, buffer + pos + 1, len) == (size_t)-1)
 198         return -1;
 199
 200     return len;
 201 }
 202
 203 static size_t outstr (outf_t outf, void *private,
 204                       const unsigned char *str, unused size_t fill,
 205                       size_t maxlen)
 206 {
 207 #define TMPBUF_LEN 256
 208 #if 0
 209     unsigned char tmpbuf[TMPBUF_LEN];
 210     size_t len, totlen, tmp;
 211 #else
 212     size_t len, totlen;
 213 #endif
 214
 215     if (str == NULL) {
 216         /* Avoid crash if given a NULL string */
 217         str = "<null>";
 218     }
 219     len = strlen(str);
 220     totlen = 0;
 221 #if 0
 222     if (len < fill) {
 223         memset(tmpbuf, ' ', TMPBUF_LEN);
 224         fill -= len;
 225         for (; fill > 0; fill -= tmp) {
 226             tmp = fill;
 227             if (tmp > TMPBUF_LEN)
 228                 tmp = TMPBUF_LEN;
 229             totlen += tmp;
 230             if (totlen > maxlen) {
 231                 tmp = maxlen - totlen;
 232                 totlen = maxlen;
 233             }
 234             (*outf)(private, tmpbuf, tmp);
 235         }
 236     }
 237 #endif
 238     totlen += len;
 239     if (totlen > maxlen) {
 240         len = maxlen - totlen;
 241         totlen = maxlen;
 242     }
 243     if ((*outf)(private, str, len) == (size_t)-1)
 244         return -1;
 245
 246     return totlen;
 247 }
 248
 249 int _vprintf(outf_t outf, void *private, size_t maxlen,
 250              const unsigned char *format, va_list ap)
 251 {
 252     const unsigned char *p, *str;
 253     size_t maxfill, totlen, len, tmp;
 254     int cur;
 255
 256     cur = 0;
 257     str = format;
 258     for (totlen = 0; totlen != maxlen;) {
 259         for (p = str; (*p != '%' || cur > 6) && *p != '\0'; p++)
 260             continue;
 261         len = p - str;
 262         if (len + totlen > maxlen)
 263             len = maxlen - totlen;
 264         tmp = (*outf)(private, str, p - str);
 265         if (tmp == (size_t)-1)
 266             return -1;
 267         totlen += tmp;
 268         if (*p == '\0')
 269             break;
 270         maxfill = -2;
 271         str = p;
 272     next:
 273         p++;
 274         switch (*p) {
 275         case '\0':
 276             /* Invalid format */
 277             goto invalid;
 278         case '0':
 279             if (maxfill >= (size_t)-2) {
 280                 maxfill = -1;
 281                 goto next;
 282             }
 283             /* No break here */
 284         case '1' ... '9':
 285             switch (maxfill) {
 286             case -2:
 287                 /* Invalid format */
 288                 goto invalid;
 289             case -1:
 290                 maxfill = *p - '0';
 291                 break;
 292             default:
 293                 maxfill = (maxfill * 10) + *p - '0';
 294                 break;
 295             }
 296             goto next;
 297         case 'l':
 298             /* Ignore it */
 299             goto next;
 300         case 'h':
 301             /* Ignore it */
 302             goto next;
 303         case 'd':
 304             if (maxfill == (size_t)-2 || maxfill == (size_t)(-1))
 305                 maxfill = 0;
 306             tmp = outdecs(outf, private,
 307                           va_arg(ap, int), maxfill, maxlen - totlen);
 308             break;
 309         case 'u':
 310             if (maxfill == (size_t)-2 || maxfill == (size_t)(-1))
 311                 maxfill = 0;
 312             tmp = outdecu(outf, private,
 313                           va_arg(ap, unsigned int), maxfill, maxlen - totlen);
 314             break;
 315         case 'x':
 316             if (maxfill == (size_t)-2 || maxfill == (size_t)(-1))
 317                 maxfill = 0;
 318             tmp = outhex(outf, private,
 319                          va_arg(ap, unsigned int), maxfill, maxlen - totlen);
 320             break;
 321         case 'p':
 322             if (p != str + 1) {
 323                 /* Invalid format */
 324                 goto invalid;
 325             } else {
 326                 if (maxfill == (size_t)-2 || maxfill == (size_t)(-1))
 327                     maxfill = 0;
 328                 tmp = outhex(outf, private, va_arg(ap, unsigned int),
 329                              maxfill, maxlen - totlen);
 330             }
 331             break;
 332         case 'c':
 333             if (p != str + 1) {
 334                 /* Invalid format */
 335                 goto invalid;
 336             } else {
 337                 tmp = outc(outf, private,
 338                            va_arg(ap, int), maxlen - totlen);
 339             }
 340             break;
 341         case 's':
 342             if (maxfill == (size_t)-2 || maxfill == (size_t)(-1))
 343                 maxfill = 0;
 344             str = va_arg(ap, const unsigned char *);
 345             tmp = outstr(outf, private, str, maxfill, maxlen - totlen);
 346             break;
 347         case '%':
 348             if (p != str + 1) {
 349                 /* Invalid format */
 350                 goto invalid;
 351             } else {
 352                 tmp = outc(outf, private, '%', maxlen - totlen);
 353             }
 354         default:
 355         invalid:
 356             /* Invalid format : display the raw string */
 357             len = p - str + 1;
 358             if (len + totlen > maxlen)
 359                 len = maxlen - totlen;
 360             tmp = (*outf)(private, str, len);
 361             break;
 362         }
 363         if (tmp == (size_t)-1)
 364             return -1;
 365         totlen += tmp;
 366         str = p + 1;
 367     }
 368
 369     return 0;
 370 }
 371 #else /* defined (__USE__vprintf__) */
 372 size_t outfd (void *private, const unsigned char *buf, size_t len);
 373 size_t outf_dbg (void *private, const unsigned char *buf, size_t len);
 374 size_t outbuf (void *private, const unsigned char *buf, size_t len);
 375 int _vprintf(outf_t outf, void *private, size_t maxlen,
 376              const unsigned char *format, va_list ap);
 377 #endif /* defined (__USE__vprintf__) */
 378
 379 #if defined (__USE_printf__)
 380 int printf (const char *format, ...)
 381 {
 382     va_list ap;
 383     int fd = 1;
 384     int ret;
 385
 386     va_start(ap, format);
 387     ret = _vprintf(&outfd, &fd, -1, format, ap);
 388     va_end(ap);
 389
 390     return ret;
 391 }
 392 #endif /* defined (__USE_printf__) */
 393
 394 #if defined (__USE_dprintf__)
 395 int dprintf (const char *format, ...)
 396 {
 397     va_list ap;
 398     int fd = 1;
 399     int ret;
 400
 401     va_start(ap, format);
 402     ret = _vprintf(&outf_dbg, &fd, -1, format, ap);
 403     va_end(ap);
 404
 405     return ret;
 406 }
 407 #endif /* defined (__USE_dprintf__) */
 408
 409 #if defined (__USE_sprintf__)
 410 int sprintf (char *str, const char *format, ...)
 411 {
 412     va_list ap;
 413     int ret;
 414
 415     va_start(ap, format);
 416     ret = _vprintf(&outbuf, &str, -1, format, ap);
 417     va_end(ap);
 418
 419     return ret;
 420 }
 421 #endif /* defined (__USE_sprintf__) */
 422
 423 #if defined (__USE_snprintf__)
 424 int snprintf (char *str, size_t size, const char *format, ...)
 425 {
 426     va_list ap;
 427     int ret;
 428
 429     va_start(ap, format);
 430     ret = _vprintf(&outbuf, &str, size, format, ap);
 431     va_end(ap);
 432
 433     return ret;
 434 }
 435 #endif /* defined (__USE_snprintf__) */
 436
 437 #if defined (__USE_vprintf__)
 438 int vprintf (const char *format, va_list ap)
 439 {
 440     int fd = 1;
 441
 442     return _vprintf(&outfd, &fd, -1, format, ap);
 443 }
 444 #endif /* defined (__USE_vprintf__) */
 445
 446 #if defined (__USE_vdprintf__)
 447 int vdprintf (const char *format, va_list ap)
 448 {
 449     int fd = 1;
 450
 451     return _vprintf(&outf_dbg, &fd, -1, format, ap);
 452 }
 453 #endif /* defined (__USE_vdprintf__) */
 454
 455 #if defined (__USE_vsprintf__)
 456 int vsprintf (char *str, const char *format, va_list ap)
 457 {
 458     return _vprintf(&outbuf, &str, -1, format, ap);
 459 }
 460 #endif /* defined (__USE_vsprintf__) */
 461
 462 #if defined (__USE_vsnprintf__)
 463 int vsnprintf (char *str, size_t size, const char *format, va_list ap)
 464 {
 465     return _vprintf(&outbuf, &str, size, format, ap);
 466 }
 467 #endif /* defined (__USE_vsnprintf__) */