325 lines
7.6 KiB
C
325 lines
7.6 KiB
C
/***************************************************************************
|
|
* __________ __ ___.
|
|
* Open \______ \ ____ ____ | | _\_ |__ _______ ___
|
|
* Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
|
|
* Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
|
|
* Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
|
|
* \/ \/ \/ \/ \/
|
|
* $Id$
|
|
*
|
|
* Copyright (C) 2002 by Gary Czvitkovicz
|
|
* Copyright (C) 2017 by William Wilgus
|
|
*
|
|
* This program is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU General Public License
|
|
* as published by the Free Software Foundation; either version 2
|
|
* of the License, or (at your option) any later version.
|
|
*
|
|
* This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
|
|
* KIND, either express or implied.
|
|
*
|
|
****************************************************************************/
|
|
|
|
|
|
#include <stdarg.h>
|
|
#include <stdbool.h>
|
|
#include <limits.h>
|
|
#include <string.h>
|
|
#include "file.h"
|
|
#include "format.h"
|
|
|
|
static const char hexdigit[] = "0123456789ABCDEF";
|
|
|
|
/* smaller compressed binary without inline but 18% slower */
|
|
#define FMT_DECL static inline
|
|
|
|
FMT_DECL int fmt_width_precision(int *ch, const char **fmt, char **str, va_list *ap)
|
|
{
|
|
int value = 0;
|
|
(void) str;
|
|
(void) ap;
|
|
|
|
while (*ch >= '0' && *ch <= '9')
|
|
{
|
|
value = 10 * value + (*ch - '0');
|
|
*ch = *(*fmt)++;
|
|
}
|
|
return value;
|
|
}
|
|
|
|
FMT_DECL int fmt_integer_signed(int *ch, const char **fmt, char **str, va_list *ap)
|
|
{
|
|
int val, rem, sign;
|
|
(void) ch;
|
|
(void) fmt;
|
|
|
|
val = sign = va_arg(*ap, int);
|
|
if (val < 0)
|
|
val = -val;
|
|
do {
|
|
rem = val % 10;
|
|
val /= 10;
|
|
*--(*str) = rem + '0';
|
|
|
|
} while (val > 0);
|
|
|
|
if (sign < 0)
|
|
*--(*str) = '-';
|
|
return 0;
|
|
}
|
|
|
|
FMT_DECL int fmt_integer_unsigned(int *ch, const char **fmt, char **str, va_list *ap)
|
|
{
|
|
unsigned int uval, urem;
|
|
(void) ch;
|
|
(void) fmt;
|
|
|
|
uval = va_arg(*ap, unsigned int);
|
|
do {
|
|
urem = uval % 10;
|
|
uval /= 10;
|
|
*--(*str) = urem + '0';
|
|
} while (uval > 0);
|
|
return 0;
|
|
}
|
|
|
|
FMT_DECL int fmt_long(int *ch, const char **fmt, char **str, va_list *ap)
|
|
{
|
|
int pad = 0;
|
|
long lval, lrem, lsign = 0;
|
|
unsigned long ulval, ulrem;
|
|
char ch_l = *ch;
|
|
|
|
*ch = *(*fmt)++;
|
|
if (*ch == 'd') {
|
|
lval = lsign = va_arg(*ap, long);
|
|
|
|
if (lval < 0)
|
|
lval = -lval;
|
|
do {
|
|
lrem = lval % 10;
|
|
lval /= 10;
|
|
*--(*str) = lrem + '0';
|
|
} while (lval > 0);
|
|
|
|
if (lsign < 0)
|
|
*--(*str) = '-';
|
|
}
|
|
else if (*ch == 'u') {
|
|
ulval = va_arg(*ap, unsigned long);
|
|
do {
|
|
ulrem = ulval % 10;
|
|
ulval /= 10;
|
|
*--(*str) = ulrem + '0';
|
|
} while (ulval > 0);
|
|
}
|
|
else if (*ch == 'x' || *ch == 'X') {
|
|
pad++;
|
|
ulval = va_arg(*ap, long);
|
|
do {
|
|
*--(*str) = hexdigit[ulval & 0xf];
|
|
ulval >>= 4;
|
|
} while (ulval > 0);
|
|
}
|
|
else {
|
|
*--(*str) = ch_l;
|
|
*--(*str) = *ch;
|
|
}
|
|
return pad;
|
|
}
|
|
|
|
FMT_DECL int fmt_character(int *ch, const char **fmt, char **str, va_list *ap)
|
|
{
|
|
(void) ch;
|
|
(void) fmt;
|
|
|
|
*--(*str) = va_arg(*ap, int);
|
|
return 0;
|
|
}
|
|
|
|
FMT_DECL int fmt_string(int *ch, const char **fmt, char **str, va_list *ap)
|
|
{
|
|
(void) ch;
|
|
(void) fmt;
|
|
|
|
*str = va_arg (*ap, char*);
|
|
return 0;
|
|
}
|
|
|
|
FMT_DECL int fmt_hex_unsigned(int *ch, const char **fmt, char **str, va_list *ap)
|
|
{
|
|
unsigned int uval;
|
|
(void) ch;
|
|
(void) fmt;
|
|
|
|
uval = va_arg(*ap, int);
|
|
do {
|
|
*--(*str) = hexdigit[uval & 0xf];
|
|
uval >>= 4;
|
|
} while (uval > 0);
|
|
return 1;
|
|
}
|
|
|
|
FMT_DECL int fmt_pointer(int *ch, const char **fmt, char **str, va_list *ap)
|
|
{
|
|
int pad = fmt_hex_unsigned(ch, fmt, str, ap);
|
|
/* for pointers prepend 0x and act like 'X' */
|
|
*--(*str) = 'x';
|
|
*--(*str) = '0';
|
|
return pad;
|
|
}
|
|
|
|
FMT_DECL int fmt_sizet(int *ch, const char **fmt, char **str, va_list *ap)
|
|
{
|
|
size_t uszval, uszrem;
|
|
ssize_t szval, szrem, szsign;
|
|
char ch_z = *ch;
|
|
*ch = *(*fmt)++;
|
|
|
|
if (*ch == 'd') {
|
|
szval = szsign = va_arg(*ap, ssize_t);
|
|
if (szval < 0)
|
|
szval = -szval;
|
|
do {
|
|
szrem = szval % 10;
|
|
szval /= 10;
|
|
*--(*str) = szrem + '0';
|
|
} while (szval > 0);
|
|
|
|
if (szsign < 0)
|
|
*--(*str) = '-';
|
|
}
|
|
else if (*ch == 'u') {
|
|
uszval = va_arg(*ap, size_t);
|
|
do {
|
|
uszrem = uszval % 10;
|
|
uszval /= 10;
|
|
*--(*str) = uszrem + '0';
|
|
} while (uszval > 0);
|
|
}
|
|
else {
|
|
*--(*str) = ch_z;
|
|
*--(*str) = *ch;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static inline int fmt_next_char(int *ch, const char **fmt, char **str, va_list *ap)
|
|
{
|
|
(void) fmt;
|
|
(void) ap;
|
|
|
|
*--(*str) = *ch;
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
void format(
|
|
/* call 'push()' for each output letter */
|
|
int (*push)(void *userp, unsigned char data),
|
|
void *userp,
|
|
const char *fmt,
|
|
va_list ap)
|
|
{
|
|
bool ok = true;
|
|
char *str;
|
|
char tmpbuf[12], pad;
|
|
int ch, width, precision, padded;
|
|
|
|
|
|
ch = *fmt++;
|
|
tmpbuf[sizeof tmpbuf - 1] = '\0';
|
|
|
|
do
|
|
{
|
|
if (ch == '%')
|
|
{
|
|
str = tmpbuf + sizeof tmpbuf - 1;
|
|
ch = *fmt++;
|
|
padded = (ch == '0' ? 1 : 0);
|
|
width = fmt_width_precision(&ch, &fmt, &str, &ap);
|
|
|
|
precision = INT_MAX;
|
|
if(ch == '.')
|
|
{
|
|
ch = *fmt++;
|
|
precision = fmt_width_precision(&ch, &fmt, &str, &ap);
|
|
}
|
|
|
|
if (ch == 'd')
|
|
fmt_integer_signed(&ch, &fmt, &str, &ap);
|
|
else if (ch == 'u')
|
|
fmt_integer_unsigned(&ch, &fmt, &str, &ap);
|
|
else if (ch == 'l')
|
|
padded += fmt_long(&ch, &fmt, &str, &ap);
|
|
else if (ch == 'c')
|
|
fmt_character(&ch, &fmt, &str, &ap);
|
|
else if (ch == 's')
|
|
fmt_string(&ch, &fmt, &str, &ap);
|
|
else if (ch == 'x' || ch == 'X')
|
|
padded += fmt_hex_unsigned(&ch, &fmt, &str, &ap);
|
|
else if (ch == 'p' || ch == 'P')
|
|
padded += fmt_pointer(&ch, &fmt, &str, &ap);
|
|
#if 0
|
|
else if (ch == 'z')
|
|
fmt_sizet(&ch, &fmt, &str, &ap);
|
|
#endif
|
|
else
|
|
fmt_next_char(&ch, &fmt, &str, &ap);
|
|
|
|
width -= strlen (str);
|
|
if (width > 0)
|
|
{
|
|
pad = (padded ? '0' : ' ');
|
|
while (width-- > 0 && ok)
|
|
ok=push(userp, pad);
|
|
}
|
|
while(*str != '\0' && ok && precision--)
|
|
ok=push(userp, *str++);
|
|
}
|
|
else
|
|
ok=push(userp, ch);
|
|
|
|
} while ((ch = *fmt++) != '\0' && ok);
|
|
}
|
|
|
|
struct for_fprintf {
|
|
int fd; /* where to store it */
|
|
int bytes; /* amount stored */
|
|
};
|
|
|
|
static int fprfunc(void *pr, unsigned char letter)
|
|
{
|
|
struct for_fprintf *fpr = (struct for_fprintf *)pr;
|
|
int rc = write(fpr->fd, &letter, 1);
|
|
|
|
if(rc > 0) {
|
|
fpr->bytes++; /* count them */
|
|
return true; /* we are ok */
|
|
}
|
|
|
|
return false; /* failure */
|
|
}
|
|
|
|
|
|
int fdprintf(int fd, const char *fmt, ...)
|
|
{
|
|
va_list ap;
|
|
struct for_fprintf fpr;
|
|
|
|
fpr.fd=fd;
|
|
fpr.bytes=0;
|
|
|
|
va_start(ap, fmt);
|
|
format(fprfunc, &fpr, fmt, ap);
|
|
va_end(ap);
|
|
|
|
return fpr.bytes; /* return 0 on error */
|
|
}
|
|
|
|
void vuprintf(int (*push)(void *userp, unsigned char data), void *userp, const char *fmt, va_list ap)
|
|
{
|
|
format(push, userp, fmt, ap);
|
|
}
|