MNRS-BM-BSP/libwrap/sys/printf.c

249 lines
5.5 KiB
C
Raw Normal View History

2018-09-25 18:31:29 +02:00
/* The functions in this file are only meant to support Dhrystone on an
* embedded RV32 system and are obviously incorrect in general. */
#include <stdarg.h>
#include <stddef.h>
#include <stdint.h>
2024-05-31 09:21:57 +02:00
#include <stdio.h>
2018-09-25 18:31:29 +02:00
#include <string.h>
#include <unistd.h>
#undef putchar
2024-05-31 09:21:57 +02:00
int putchar(int ch) { return write(STDOUT_FILENO, &ch, 1) == 1 ? ch : -1; }
2018-09-25 18:31:29 +02:00
2024-05-31 09:21:57 +02:00
size_t strnlen(const char *str, size_t n) {
2023-08-20 15:00:51 +02:00
const char *start = str;
2024-05-31 09:21:57 +02:00
while (n-- > 0 && *str)
str++;
2023-08-20 15:00:51 +02:00
return str - start;
}
2024-05-31 09:21:57 +02:00
static void fprintf_putch(int ch, void **data) { putchar(ch); }
static void sprintf_putch(int ch, void **data) {
char **pstr = (char **)data;
2018-09-25 18:31:29 +02:00
**pstr = ch;
(*pstr)++;
}
2024-05-31 09:21:57 +02:00
static unsigned long getuint(va_list *ap, int lflag) {
2018-09-25 18:31:29 +02:00
if (lflag)
return va_arg(*ap, unsigned long);
else
return va_arg(*ap, unsigned int);
}
2024-05-31 09:21:57 +02:00
static long getint(va_list *ap, int lflag) {
2018-09-25 18:31:29 +02:00
if (lflag)
return va_arg(*ap, long);
else
return va_arg(*ap, int);
}
2024-05-31 09:21:57 +02:00
static inline void printnum(void (*putch)(int, void **), void **putdat,
unsigned long num, unsigned base, int width,
int padc) {
unsigned digs[sizeof(num) * 8];
2018-09-25 18:31:29 +02:00
int pos = 0;
2024-05-31 09:21:57 +02:00
while (1) {
2018-09-25 18:31:29 +02:00
digs[pos++] = num % base;
if (num < base)
break;
num /= base;
}
while (width-- > pos)
putch(padc, putdat);
while (pos-- > 0)
putch(digs[pos] + (digs[pos] >= 10 ? 'a' - 10 : '0'), putdat);
}
2024-05-31 09:21:57 +02:00
static inline void print_double(void (*putch)(int, void **), void **putdat,
double num, int width, int prec) {
2018-09-25 18:31:29 +02:00
union {
double d;
uint64_t u;
} u;
u.d = num;
if (u.u & (1ULL << 63)) {
putch('-', putdat);
u.u &= ~(1ULL << 63);
}
for (int i = 0; i < prec; i++)
u.d *= 10;
char buf[32], *pbuf = buf;
2024-05-31 09:21:57 +02:00
printnum(sprintf_putch, (void **)&pbuf, (unsigned long)u.d, 10, 0, 0);
2018-09-25 18:31:29 +02:00
if (prec > 0) {
for (int i = 0; i < prec; i++) {
2024-05-31 09:21:57 +02:00
pbuf[-i] = pbuf[-i - 1];
2018-09-25 18:31:29 +02:00
}
pbuf[-prec] = '.';
pbuf++;
}
2024-05-31 09:21:57 +02:00
for (char *p = buf; p < pbuf; p++)
2018-09-25 18:31:29 +02:00
putch(*p, putdat);
}
2024-05-31 09:21:57 +02:00
static void vprintfmt(void (*putch)(int, void **), void **putdat,
const char *fmt, va_list ap) {
register const char *p;
const char *last_fmt;
2024-01-13 23:06:01 +01:00
register int ch;
2018-09-25 18:31:29 +02:00
unsigned long num;
2024-01-13 23:06:01 +01:00
int base, lflag, width, precision;
2018-09-25 18:31:29 +02:00
char padc;
while (1) {
2024-05-31 09:21:57 +02:00
while ((ch = *(const char *)fmt) != '%') {
2018-09-25 18:31:29 +02:00
if (ch == '\0')
return;
fmt++;
putch(ch, putdat);
}
fmt++;
// Process a %-escape sequence
last_fmt = fmt;
padc = ' ';
width = -1;
precision = -1;
lflag = 0;
reswitch:
2024-05-31 09:21:57 +02:00
switch (ch = *(const char *)fmt++) {
2018-09-25 18:31:29 +02:00
// flag to pad on the right
case '-':
padc = '-';
goto reswitch;
2024-05-31 09:21:57 +02:00
2018-09-25 18:31:29 +02:00
// flag to pad with 0's instead of spaces
case '0':
padc = '0';
goto reswitch;
// width field
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
2024-05-31 09:21:57 +02:00
for (precision = 0;; ++fmt) {
2018-09-25 18:31:29 +02:00
precision = precision * 10 + ch - '0';
ch = *fmt;
if (ch < '0' || ch > '9')
break;
}
goto process_precision;
case '*':
precision = va_arg(ap, int);
goto process_precision;
case '.':
if (width < 0)
width = 0;
goto reswitch;
case '#':
goto reswitch;
process_precision:
if (width < 0)
width = precision, precision = -1;
goto reswitch;
2024-05-31 09:21:57 +02:00
case 'l': // long flag
2018-09-25 18:31:29 +02:00
if (lflag)
goto bad;
goto reswitch;
2024-05-31 09:21:57 +02:00
case 'c': // character
2018-09-25 18:31:29 +02:00
putch(va_arg(ap, int), putdat);
break;
2024-05-31 09:21:57 +02:00
case 'f': // double
2018-09-25 18:31:29 +02:00
print_double(putch, putdat, va_arg(ap, double), width, precision);
break;
2024-05-31 09:21:57 +02:00
case 's': // string
2018-09-25 18:31:29 +02:00
if ((p = va_arg(ap, char *)) == NULL)
p = "(null)";
if (width > 0 && padc != '-')
for (width -= strnlen(p, precision); width > 0; width--)
putch(padc, putdat);
2024-05-31 09:21:57 +02:00
for (; (ch = *p) != '\0' && (precision < 0 || --precision >= 0);
width--) {
2018-09-25 18:31:29 +02:00
putch(ch, putdat);
p++;
}
for (; width > 0; width--)
putch(' ', putdat);
break;
2024-05-31 09:21:57 +02:00
case 'd': // (signed) decimal
2018-09-25 18:31:29 +02:00
num = getint(&ap, lflag);
2024-05-31 09:21:57 +02:00
if ((long)num < 0) {
2018-09-25 18:31:29 +02:00
putch('-', putdat);
2024-05-31 09:21:57 +02:00
num = -(long)num;
2018-09-25 18:31:29 +02:00
}
base = 10;
goto signed_number;
2024-05-31 09:21:57 +02:00
case 'u': // unsigned decimal
2018-09-25 18:31:29 +02:00
base = 10;
goto unsigned_number;
2024-01-13 23:06:01 +01:00
case 'o': // (unsigned) octal
2018-09-25 18:31:29 +02:00
// should do something with padding so it's always 3 octits
base = 8;
goto unsigned_number;
2024-05-31 09:21:57 +02:00
case 'p': // pointer
2018-09-25 18:31:29 +02:00
lflag = 1;
putch('0', putdat);
putch('x', putdat);
/* fall through to 'x' */
2024-01-13 23:06:01 +01:00
__attribute__((fallthrough));
2024-05-31 09:21:57 +02:00
case 'x': // (unsigned) hexadecimal
2018-09-25 18:31:29 +02:00
base = 16;
2024-05-31 09:21:57 +02:00
unsigned_number:
2018-09-25 18:31:29 +02:00
num = getuint(&ap, lflag);
2024-05-31 09:21:57 +02:00
signed_number:
2018-09-25 18:31:29 +02:00
printnum(putch, putdat, num, base, width, padc);
break;
2024-05-31 09:21:57 +02:00
case '%': // escaped '%' character
2018-09-25 18:31:29 +02:00
putch(ch, putdat);
break;
2024-05-31 09:21:57 +02:00
default: // unrecognized escape sequence - just print it literally
2018-09-25 18:31:29 +02:00
bad:
putch('%', putdat);
fmt = last_fmt;
break;
}
}
}
2024-05-31 09:21:57 +02:00
int __wrap_printf(const char *fmt, ...) {
2018-09-25 18:31:29 +02:00
va_list ap;
va_start(ap, fmt);
2024-01-13 23:06:01 +01:00
vprintfmt(fprintf_putch, 0, fmt, ap);
2018-09-25 18:31:29 +02:00
va_end(ap);
return 0; // incorrect return value, but who cares, anyway?
}
2024-05-31 09:21:57 +02:00
int __wrap_sprintf(char *str, const char *fmt, ...) {
2018-09-25 18:31:29 +02:00
va_list ap;
2024-05-31 09:21:57 +02:00
char *str0 = str;
2018-09-25 18:31:29 +02:00
va_start(ap, fmt);
2024-05-31 09:21:57 +02:00
vprintfmt(sprintf_putch, (void **)&str, fmt, ap);
2018-09-25 18:31:29 +02:00
*str = 0;
va_end(ap);
return str - str0;
}