diff options
Diffstat (limited to 'src/libc/src/format.c')
| -rw-r--r-- | src/libc/src/format.c | 467 |
1 files changed, 467 insertions, 0 deletions
diff --git a/src/libc/src/format.c b/src/libc/src/format.c new file mode 100644 index 0000000..f62d5dc --- /dev/null +++ b/src/libc/src/format.c @@ -0,0 +1,467 @@ +/* + * <format.c> + * + * Open Hack'Ware BIOS: formated output functions + * + * Copyright (c) 2004-2005 Jocelyn Mayer + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License V2 + * as published by the Free Software Foundation + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/* functions prototypes are here */ +#include <stdio.h> +/* va_list is defined here */ +#include <stdarg.h> +/* size_t is defined here */ +#include <stddef.h> +/* NULL is defined here */ +#include <stdlib.h> +/* write is defined here */ +#include <unistd.h> +/* memcpy is defined here */ +/* memset is defined here */ +/* strlen is defined here */ +#include <string.h> + +#define unused __attribute__ (( unused )) +int console_write (const void *buffer, int len); +/* XXX: this is a hack to be fixed */ +int serial_write (const void *buffer, int len); +#define debug_write serial_write + +/* Low level output fonctions */ +typedef size_t (*outf_t)(void *private, const unsigned char *buf, size_t len); + +/* output to fd */ +#if defined (__USE__vprintf__) +size_t outfd (void *private, const unsigned char *buf, size_t len) +{ + int *fd = private; + + if (*fd == 1 || *fd == 2) + return console_write(buf, len); + + return write(*fd, buf, len); +} + +size_t outf_dbg (void *private, const unsigned char *buf, size_t len) +{ + int *fd = private; + + if (*fd == 1 || *fd == 2) + return debug_write(buf, len); + + return write(*fd, buf, len); +} + +/* output to buffer */ +size_t outbuf (void *private, const unsigned char *buf, size_t len) +{ + unsigned char **dst = private; + + memcpy(*dst, buf, len); + (*dst) += len; + (*dst)[0] = '\0'; + + return len; +} + +/* misc formatted output functions */ +/* out one character */ +static size_t outc (outf_t outf, void *private, + unsigned int value, size_t maxlen) +{ + unsigned char buffer; + + if (maxlen < 1) + return 0; + buffer = value; + if ((*outf)(private, &buffer, 1) == (size_t)-1) + return -1; + + return 1; +} + +/* out one int in decimal */ +static size_t outdecs (outf_t outf, void *private, + int value, size_t fill, size_t maxlen) +{ + unsigned char buffer[12]; + size_t pos, len; + int sign; + + buffer[11] = '\0'; + pos = 10; + if (value == 0) { + sign = 0; + buffer[pos--] = '0'; + } else { + if (value < 0) { + sign = -1; + value = -value; + } else { + sign = 1; + } + for (; value != 0; pos--) { + buffer[pos] = (value % 10) + '0'; + value = value / 10; + } + } + if (fill != 0) + fill -= pos - 10; + for (; fill != 0 && pos != 0; fill--) { + buffer[pos--] = '0'; + } + if (sign == -1) + buffer[pos--] = '-'; + len = 10 - pos; + if (len > maxlen) + len = maxlen; + if ((*outf)(private, buffer + pos + 1, len) == (size_t)-1) + return -1; + + return len; +} + +/* out one unsigned int as decimal */ +static size_t outdecu (outf_t outf, void *private, + unsigned int value, size_t fill, size_t maxlen) +{ + unsigned char buffer[11]; + size_t pos, len; + + buffer[10] = '\0'; + pos = 9; + if (value == 0) { + buffer[pos--] = '0'; + } else { + for (; value != 0; pos--) { + buffer[pos] = (value % 10) + '0'; + value = value / 10; + } + } + if (fill != 0) + fill -= pos - 9; + for (; fill != 0 && pos != (size_t)-1; fill--) { + buffer[pos--] = '0'; + } + len = 9 - pos; + if (len > maxlen) + len = maxlen; + if ((*outf)(private, buffer + pos + 1, len) == (size_t)-1) + return -1; + + return len; +} + +/* out one unsigned int as hexadecimal */ +static size_t outhex (outf_t outf, void *private, + unsigned int value, size_t fill, size_t maxlen) +{ + unsigned char buffer[9]; + size_t pos, len; + int d; + + buffer[8] = '\0'; + pos = 7; + if (value == 0) { + buffer[pos--] = '0'; + } else { + for (; value != 0; pos--) { + d = value & 0xF; + if (d > 9) + d += 'a' - '0' - 10; + buffer[pos] = d + '0'; + value = value >> 4; + } + } + if (fill > 0) + fill -= pos - 7; + for (; fill != 0 && pos != (size_t)-1; fill--) { + buffer[pos--] = '0'; + } + len = 7 - pos; + if (len > maxlen) + len = maxlen; + if ((*outf)(private, buffer + pos + 1, len) == (size_t)-1) + return -1; + + return len; +} + +static size_t outstr (outf_t outf, void *private, + const unsigned char *str, unused size_t fill, + size_t maxlen) +{ +#define TMPBUF_LEN 256 +#if 0 + unsigned char tmpbuf[TMPBUF_LEN]; + size_t len, totlen, tmp; +#else + size_t len, totlen; +#endif + + if (str == NULL) { + /* Avoid crash if given a NULL string */ + str = "<null>"; + } + len = strlen(str); + totlen = 0; +#if 0 + if (len < fill) { + memset(tmpbuf, ' ', TMPBUF_LEN); + fill -= len; + for (; fill > 0; fill -= tmp) { + tmp = fill; + if (tmp > TMPBUF_LEN) + tmp = TMPBUF_LEN; + totlen += tmp; + if (totlen > maxlen) { + tmp = maxlen - totlen; + totlen = maxlen; + } + (*outf)(private, tmpbuf, tmp); + } + } +#endif + totlen += len; + if (totlen > maxlen) { + len = maxlen - totlen; + totlen = maxlen; + } + if ((*outf)(private, str, len) == (size_t)-1) + return -1; + + return totlen; +} + +int _vprintf(outf_t outf, void *private, size_t maxlen, + const unsigned char *format, va_list ap) +{ + const unsigned char *p, *str; + size_t maxfill, totlen, len, tmp; + int cur; + + cur = 0; + str = format; + for (totlen = 0; totlen != maxlen;) { + for (p = str; (*p != '%' || cur > 6) && *p != '\0'; p++) + continue; + len = p - str; + if (len + totlen > maxlen) + len = maxlen - totlen; + tmp = (*outf)(private, str, p - str); + if (tmp == (size_t)-1) + return -1; + totlen += tmp; + if (*p == '\0') + break; + maxfill = -2; + str = p; + next: + p++; + switch (*p) { + case '\0': + /* Invalid format */ + goto invalid; + case '0': + if (maxfill >= (size_t)-2) { + maxfill = -1; + goto next; + } + /* No break here */ + case '1' ... '9': + switch (maxfill) { + case -2: + /* Invalid format */ + goto invalid; + case -1: + maxfill = *p - '0'; + break; + default: + maxfill = (maxfill * 10) + *p - '0'; + break; + } + goto next; + case 'l': + /* Ignore it */ + goto next; + case 'h': + /* Ignore it */ + goto next; + case 'd': + if (maxfill == (size_t)-2 || maxfill == (size_t)(-1)) + maxfill = 0; + tmp = outdecs(outf, private, + va_arg(ap, int), maxfill, maxlen - totlen); + break; + case 'u': + if (maxfill == (size_t)-2 || maxfill == (size_t)(-1)) + maxfill = 0; + tmp = outdecu(outf, private, + va_arg(ap, unsigned int), maxfill, maxlen - totlen); + break; + case 'x': + if (maxfill == (size_t)-2 || maxfill == (size_t)(-1)) + maxfill = 0; + tmp = outhex(outf, private, + va_arg(ap, unsigned int), maxfill, maxlen - totlen); + break; + case 'p': + if (p != str + 1) { + /* Invalid format */ + goto invalid; + } else { + if (maxfill == (size_t)-2 || maxfill == (size_t)(-1)) + maxfill = 0; + tmp = outhex(outf, private, va_arg(ap, unsigned int), + maxfill, maxlen - totlen); + } + break; + case 'c': + if (p != str + 1) { + /* Invalid format */ + goto invalid; + } else { + tmp = outc(outf, private, + va_arg(ap, int), maxlen - totlen); + } + break; + case 's': + if (maxfill == (size_t)-2 || maxfill == (size_t)(-1)) + maxfill = 0; + str = va_arg(ap, const unsigned char *); + tmp = outstr(outf, private, str, maxfill, maxlen - totlen); + break; + case '%': + if (p != str + 1) { + /* Invalid format */ + goto invalid; + } else { + tmp = outc(outf, private, '%', maxlen - totlen); + } + default: + invalid: + /* Invalid format : display the raw string */ + len = p - str + 1; + if (len + totlen > maxlen) + len = maxlen - totlen; + tmp = (*outf)(private, str, len); + break; + } + if (tmp == (size_t)-1) + return -1; + totlen += tmp; + str = p + 1; + } + + return 0; +} +#else /* defined (__USE__vprintf__) */ +size_t outfd (void *private, const unsigned char *buf, size_t len); +size_t outf_dbg (void *private, const unsigned char *buf, size_t len); +size_t outbuf (void *private, const unsigned char *buf, size_t len); +int _vprintf(outf_t outf, void *private, size_t maxlen, + const unsigned char *format, va_list ap); +#endif /* defined (__USE__vprintf__) */ + +#if defined (__USE_printf__) +int printf (const char *format, ...) +{ + va_list ap; + int fd = 1; + int ret; + + va_start(ap, format); + ret = _vprintf(&outfd, &fd, -1, format, ap); + va_end(ap); + + return ret; +} +#endif /* defined (__USE_printf__) */ + +#if defined (__USE_dprintf__) +int dprintf (const char *format, ...) +{ + va_list ap; + int fd = 1; + int ret; + + va_start(ap, format); + ret = _vprintf(&outf_dbg, &fd, -1, format, ap); + va_end(ap); + + return ret; +} +#endif /* defined (__USE_dprintf__) */ + +#if defined (__USE_sprintf__) +int sprintf (char *str, const char *format, ...) +{ + va_list ap; + int ret; + + va_start(ap, format); + ret = _vprintf(&outbuf, &str, -1, format, ap); + va_end(ap); + + return ret; +} +#endif /* defined (__USE_sprintf__) */ + +#if defined (__USE_snprintf__) +int snprintf (char *str, size_t size, const char *format, ...) +{ + va_list ap; + int ret; + + va_start(ap, format); + ret = _vprintf(&outbuf, &str, size, format, ap); + va_end(ap); + + return ret; +} +#endif /* defined (__USE_snprintf__) */ + +#if defined (__USE_vprintf__) +int vprintf (const char *format, va_list ap) +{ + int fd = 1; + + return _vprintf(&outfd, &fd, -1, format, ap); +} +#endif /* defined (__USE_vprintf__) */ + +#if defined (__USE_vdprintf__) +int vdprintf (const char *format, va_list ap) +{ + int fd = 1; + + return _vprintf(&outf_dbg, &fd, -1, format, ap); +} +#endif /* defined (__USE_vdprintf__) */ + +#if defined (__USE_vsprintf__) +int vsprintf (char *str, const char *format, va_list ap) +{ + return _vprintf(&outbuf, &str, -1, format, ap); +} +#endif /* defined (__USE_vsprintf__) */ + +#if defined (__USE_vsnprintf__) +int vsnprintf (char *str, size_t size, const char *format, va_list ap) +{ + return _vprintf(&outbuf, &str, size, format, ap); +} +#endif /* defined (__USE_vsnprintf__) */ |