blob: 27041bb4cc759d6690951ef84b350e2237cdf771 [file] [log] [blame]
/*
* libc printf and friends
*
* This code is free software; you can redistribute it and/or modify it
* under the terms of the GNU Library General Public License version 2.
*/
#include "libcflat.h"
#include "ctype.h"
#define BUFSZ 2000
typedef struct pstream {
char *buffer;
int remain;
int added;
} pstream_t;
typedef struct strprops {
char pad;
int npad;
bool alternate;
int precision;
} strprops_t;
static void addchar(pstream_t *p, char c)
{
if (p->remain) {
*p->buffer++ = c;
--p->remain;
}
++p->added;
}
static void print_str(pstream_t *p, const char *s, strprops_t props)
{
const char *s_orig = s;
int npad = props.npad;
if (npad > 0) {
npad -= strlen(s_orig);
while (npad > 0) {
addchar(p, props.pad);
--npad;
}
}
while (*s && props.precision--)
addchar(p, *s++);
if (npad < 0) {
props.pad = ' '; /* ignore '0' flag with '-' flag */
npad += strlen(s_orig);
while (npad < 0) {
addchar(p, props.pad);
++npad;
}
}
}
/*
* Adapted from drivers/firmware/efi/libstub/vsprintf.c
*/
static u32 utf16_to_utf32(const u16 **s16)
{
u16 c0, c1;
c0 = *(*s16)++;
/* not a surrogate */
if ((c0 & 0xf800) != 0xd800)
return c0;
/* invalid: low surrogate instead of high */
if (c0 & 0x0400)
return 0xfffd;
c1 = **s16;
/* invalid: missing low surrogate */
if ((c1 & 0xfc00) != 0xdc00)
return 0xfffd;
/* valid surrogate pair */
++(*s16);
return (0x10000 - (0xd800 << 10) - 0xdc00) + (c0 << 10) + c1;
}
/*
* Adapted from drivers/firmware/efi/libstub/vsprintf.c
*/
static size_t utf16s_utf8nlen(const u16 *s16, size_t maxlen)
{
size_t len, clen;
for (len = 0; len < maxlen && *s16; len += clen) {
u16 c0 = *s16++;
/* First, get the length for a BMP character */
clen = 1 + (c0 >= 0x80) + (c0 >= 0x800);
if (len + clen > maxlen)
break;
/*
* If this is a high surrogate, and we're already at maxlen, we
* can't include the character if it's a valid surrogate pair.
* Avoid accessing one extra word just to check if it's valid
* or not.
*/
if ((c0 & 0xfc00) == 0xd800) {
if (len + clen == maxlen)
break;
if ((*s16 & 0xfc00) == 0xdc00) {
++s16;
++clen;
}
}
}
return len;
}
/*
* Adapted from drivers/firmware/efi/libstub/vsprintf.c
*/
static void print_wstring(pstream_t *p, const u16 *s, strprops_t props)
{
const u16 *ws = (const u16 *)s;
size_t pos = 0, size = p->remain + 1, len = utf16s_utf8nlen(ws, props.precision);
while (len-- > 0) {
u32 c32 = utf16_to_utf32(&ws);
u8 *s8;
size_t clen;
if (c32 < 0x80) {
addchar(p, c32);
continue;
}
/* Number of trailing octets */
clen = 1 + (c32 >= 0x800) + (c32 >= 0x10000);
len -= clen;
s8 = (u8 *)(p->buffer - p->added + pos);
/* Avoid writing partial character */
addchar(p, '\0');
pos += clen;
if (pos >= size)
continue;
/* Set high bits of leading octet */
*s8 = (0xf00 >> 1) >> clen;
/* Write trailing octets in reverse order */
for (s8 += clen; clen; --clen, c32 >>= 6)
*s8-- = 0x80 | (c32 & 0x3f);
/* Set low bits of leading octet */
*s8 |= c32;
}
}
static char digits[16] = "0123456789abcdef";
static void print_int(pstream_t *ps, long long n, int base, strprops_t props)
{
char buf[sizeof(long) * 3 + 2], *p = buf;
int s = 0, i;
if (n < 0) {
n = -n;
s = 1;
}
while (n) {
*p++ = digits[n % base];
n /= base;
}
while (p == buf || (p - buf < props.precision))
*p++ = '0';
props.precision = -1;
if (s)
*p++ = '-';
for (i = 0; i < (p - buf) / 2; ++i) {
char tmp;
tmp = buf[i];
buf[i] = p[-1 - i];
p[-1 - i] = tmp;
}
*p = 0;
print_str(ps, buf, props);
}
static void print_unsigned(pstream_t *ps, unsigned long long n, int base,
strprops_t props)
{
char buf[sizeof(long) * 3 + 3], *p = buf;
int i;
while (n) {
*p++ = digits[n % base];
n /= base;
}
if (p == buf)
props.alternate = false;
while (p == buf || (p - buf < props.precision))
*p++ = '0';
props.precision = -1;
if (props.alternate && base == 16) {
if (props.pad == '0') {
addchar(ps, '0');
addchar(ps, 'x');
if (props.npad > 0)
props.npad = MAX(props.npad - 2, 0);
} else {
*p++ = 'x';
*p++ = '0';
}
}
for (i = 0; i < (p - buf) / 2; ++i) {
char tmp;
tmp = buf[i];
buf[i] = p[-1 - i];
p[-1 - i] = tmp;
}
*p = 0;
print_str(ps, buf, props);
}
static int fmtnum(const char **fmt)
{
const char *f = *fmt;
int len = 0, num;
if (*f == '-')
++f, ++len;
while (*f >= '0' && *f <= '9')
++f, ++len;
num = atol(*fmt);
*fmt += len;
return num;
}
/*
* Adapted from drivers/firmware/efi/libstub/vsprintf.c
*/
static int skip_atoi(const char **s)
{
int i = 0;
do {
i = i*10 + *((*s)++) - '0';
} while (isdigit(**s));
return i;
}
/*
* Adapted from drivers/firmware/efi/libstub/vsprintf.c
*/
static int get_int(const char **fmt, va_list *ap)
{
if (isdigit(**fmt))
return skip_atoi(fmt);
if (**fmt == '*') {
++(*fmt);
/* it's the next argument */
return va_arg(*ap, int);
}
return 0;
}
int vsnprintf(char *buf, int size, const char *fmt, va_list va)
{
pstream_t s;
va_list args;
/*
* We want to pass our input va_list to helper functions by reference,
* but there's an annoying edge case. If va_list was originally passed
* to us by value, we could just pass &ap down to the helpers. This is
* the case on, for example, X86_32.
* However, on X86_64 (and possibly others), va_list is actually a
* size-1 array containing a structure. Our function parameter ap has
* decayed from T[1] to T*, and &ap has type T** rather than T(*)[1],
* which is what will be expected by a function taking a va_list *
* parameter.
* One standard way to solve this mess is by creating a copy in a local
* variable of type va_list and then passing a pointer to that local
* copy instead, which is what we do here.
*/
va_copy(args, va);
s.buffer = buf;
s.remain = size - 1;
s.added = 0;
while (*fmt) {
char f = *fmt++;
int nlong = 0;
strprops_t props;
memset(&props, 0, sizeof(props));
props.pad = ' ';
props.precision = -1;
if (f != '%') {
addchar(&s, f);
continue;
}
morefmt:
f = *fmt++;
switch (f) {
case '%':
addchar(&s, '%');
break;
case 'c':
addchar(&s, va_arg(args, int));
break;
case '\0':
--fmt;
break;
case '.':
props.pad = ' ';
props.precision = get_int(&fmt, &args);
goto morefmt;
case '#':
props.alternate = true;
goto morefmt;
case '0':
props.pad = '0';
++fmt;
/* fall through */
case '1' ... '9':
case '-':
--fmt;
props.npad = fmtnum(&fmt);
goto morefmt;
case 'l':
++nlong;
goto morefmt;
case 't':
case 'z':
/* Here we only care that sizeof(size_t) == sizeof(long).
* On a 32-bit platform it doesn't matter that size_t is
* typedef'ed to int or long; va_arg will work either way.
* Same for ptrdiff_t (%td).
*/
nlong = 1;
goto morefmt;
case 'd':
switch (nlong) {
case 0:
print_int(&s, va_arg(args, int), 10, props);
break;
case 1:
print_int(&s, va_arg(args, long), 10, props);
break;
default:
print_int(&s, va_arg(args, long long), 10, props);
break;
}
break;
case 'u':
switch (nlong) {
case 0:
print_unsigned(&s, va_arg(args, unsigned int), 10, props);
break;
case 1:
print_unsigned(&s, va_arg(args, unsigned long), 10, props);
break;
default:
print_unsigned(&s, va_arg(args, unsigned long long), 10, props);
break;
}
break;
case 'x':
switch (nlong) {
case 0:
print_unsigned(&s, va_arg(args, unsigned int), 16, props);
break;
case 1:
print_unsigned(&s, va_arg(args, unsigned long), 16, props);
break;
default:
print_unsigned(&s, va_arg(args, unsigned long long), 16, props);
break;
}
break;
case 'p':
props.alternate = true;
print_unsigned(&s, (unsigned long)va_arg(args, void *), 16, props);
break;
case 's':
if (nlong)
print_wstring(&s, va_arg(args, const u16 *), props);
else
print_str(&s, va_arg(args, const char *), props);
break;
default:
addchar(&s, f);
break;
}
}
va_end(args);
*s.buffer = 0;
return s.added;
}
int snprintf(char *buf, int size, const char *fmt, ...)
{
va_list va;
int r;
va_start(va, fmt);
r = vsnprintf(buf, size, fmt, va);
va_end(va);
return r;
}
int vprintf(const char *fmt, va_list va)
{
char buf[BUFSZ];
int r;
r = vsnprintf(buf, sizeof(buf), fmt, va);
puts(buf);
return r;
}
int printf(const char *fmt, ...)
{
va_list va;
char buf[BUFSZ];
int r;
va_start(va, fmt);
r = vsnprintf(buf, sizeof buf, fmt, va);
va_end(va);
puts(buf);
return r;
}
void binstr(unsigned long x, char out[BINSTR_SZ])
{
int i;
char *c;
int n;
n = sizeof(unsigned long) * 8;
i = 0;
c = &out[0];
for (;;) {
*c++ = (x & (1ul << (n - i - 1))) ? '1' : '0';
i++;
if (i == n) {
*c = '\0';
break;
}
if (i % 4 == 0)
*c++ = '\'';
}
assert(c + 1 - &out[0] == BINSTR_SZ);
}
void print_binstr(unsigned long x)
{
char out[BINSTR_SZ];
binstr(x, out);
printf("%s", out);
}