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

/* 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 <stdio.h>
#include <stdint.h>
#include <string.h>
#include <unistd.h>
//#include "semihosting.h"

#undef putchar
int putchar(int ch)
{
  return write(STDOUT_FILENO, &ch, 1) == 1 ? ch : -1;
}

size_t strnlen (const char *str, size_t n)
{
  const char *start = str;
  while (n-- > 0 && *str) str++;
  return str - start;
}

static void fprintf_putch(int ch, void** data)
{
  putchar(ch);
}
static void sprintf_putch(int ch, void** data)
{
  char** pstr = (char**)data;
  **pstr = ch;
  (*pstr)++;
}

static unsigned long getuint(va_list *ap, int lflag)
{
  if (lflag)
    return va_arg(*ap, unsigned long);
  else
    return va_arg(*ap, unsigned int);
}

static long getint(va_list *ap, int lflag)
{
  if (lflag)
    return va_arg(*ap, long);
  else
    return va_arg(*ap, int);
}

static inline void printnum(void (*putch)(int, void**), void **putdat,
                    unsigned long num, unsigned base, int width, int padc)
{
  unsigned digs[sizeof(num)*8];
  int pos = 0;

  while (1)
  {
    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);
}

static inline void print_double(void (*putch)(int, void**), void **putdat,
                                double num, int width, int prec)
{
  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;
  printnum(sprintf_putch, (void**)&pbuf, (unsigned long)u.d, 10, 0, 0);
  if (prec > 0) {
    for (int i = 0; i < prec; i++) {
      pbuf[-i] = pbuf[-i-1];
    }
    pbuf[-prec] = '.';
    pbuf++;
  }

  for (char* p = buf; p < pbuf; p++)
    putch(*p, putdat);
}

static void vprintfmt(void (*putch)(int, void**), void **putdat, const char *fmt, va_list ap)
{
  register const char* p;
  const char* last_fmt;
  register int ch;
  unsigned long num;
  int base, lflag, width, precision;
  char padc;

  while (1) {
    while ((ch = *(const char *) fmt) != '%') {
      if (ch == '\0')
        return;
      fmt++;
      putch(ch, putdat);
    }
    fmt++;

    // Process a %-escape sequence
    last_fmt = fmt;
    padc = ' ';
    width = -1;
    precision = -1;
    lflag = 0;
  reswitch:
    switch (ch = *(const char *) fmt++) {

    // flag to pad on the right
    case '-':
      padc = '-';
      goto reswitch;
      
    // 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':
      for (precision = 0; ; ++fmt) {
        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;

    case 'l':    // long flag
      if (lflag)
        goto bad;
      goto reswitch;
    case 'c':    // character
      putch(va_arg(ap, int), putdat);
      break;
    case 'f':    // double
      print_double(putch, putdat, va_arg(ap, double), width, precision);
      break;
    case 's':    // string
      if ((p = va_arg(ap, char *)) == NULL)
        p = "(null)";
      if (width > 0 && padc != '-')
        for (width -= strnlen(p, precision); width > 0; width--)
          putch(padc, putdat);
      for (; (ch = *p) != '\0' && (precision < 0 || --precision >= 0); width--) {
        putch(ch, putdat);
        p++;
      }
      for (; width > 0; width--)
        putch(' ', putdat);
      break;
    case 'd':    // (signed) decimal
      num = getint(&ap, lflag);
      if ((long) num < 0) {
        putch('-', putdat);
        num = -(long) num;
      }
      base = 10;
      goto signed_number;
    case 'u':    // unsigned decimal
      base = 10;
      goto unsigned_number;
    case 'o': // (unsigned) octal
      // should do something with padding so it's always 3 octits
      base = 8;
      goto unsigned_number;
    case 'p':// pointer
      lflag = 1;
      putch('0', putdat);
      putch('x', putdat);
      /* fall through to 'x' */
      __attribute__((fallthrough));
    case 'x':    // (unsigned) hexadecimal
      base = 16;
      unsigned_number:
      num = getuint(&ap, lflag);
      signed_number:
      printnum(putch, putdat, num, base, width, padc);
      break;
    case '%':    // escaped '%' character
      putch(ch, putdat);
      break;
    default:    // unrecognized escape sequence - just print it literally
    bad:
      putch('%', putdat);
      fmt = last_fmt;
      break;
    }
  }
}

int __wrap_printf(const char* fmt, ...)
{
  va_list ap;
  va_start(ap, fmt);

  vprintfmt(fprintf_putch, 0, fmt, ap);

  va_end(ap);
  return 0; // incorrect return value, but who cares, anyway?
}

int __wrap_sprintf(char* str, const char* fmt, ...)
{
  va_list ap;
  char* str0 = str;
  va_start(ap, fmt);

  vprintfmt(sprintf_putch, (void**)&str, fmt, ap);
  *str = 0;

  va_end(ap);
  return str - str0;
}