print.c

#include "print.h"


static void put_cnt_char(int c,int * cnt);
static int  strnlen(const char *s, unsigned int size);
static void vprintfmt(void (*putch)(int, int*), void *putdat, const char *fmt, va_list ap);

static void put_cnt_char(int c,int * cnt)
{
  uart_putchar(c);
  (*cnt) ++;
}

static int strnlen(const char *s, unsigned int size)
{
  int n;
  for (n = 0; size > 0 && *s != '\0'; s++, size--) n++;
  return n;
}

// Get an unsigned int of various possible sizes from a varargs list,
// depending on the lflag parameter.
static unsigned long long getuint(va_list *ap, int lflag)
{
  if (lflag >= 2)
    return va_arg(*ap, unsigned long long);
  else if (lflag)
    return va_arg(*ap, unsigned long);
  else
    return va_arg(*ap, unsigned int);
}


#if defined(__GNUC__) && defined(__GNUC_MINOR__)

static void printnum(void (*putch)(int, int*), void *putdat, unsigned long long num, unsigned base, int width, int padc)
{
  if (num >= 16)
  {
    printnum(putch, putdat, num >> 4, 16, width - 1, padc);
  }
  else
  {
    while (--width > 0) putch(padc, putdat);
  }

  putch("0123456789abcdef"[num & 0x00ffff], putdat);
}

//reduce uneccserssy functions
#else

#if defined( PRINT_ERROR_ENABLE )
/*
 * Space or zero padding and a field width are supported for the numeric
 * formats only.
 *
 * The special format %e takes an integer error code
 * and prints a string describing the error.
 * The integer may be positive or negative,
 * so that -E_NO_MEM and E_NO_MEM are equivalent.
 */

static const char * const error_string[MAXERROR] =
{
  "unspecified error",
  "bad environment",
  "invalid parameter",
  "out of memory",
  "out of environments",
  "segmentation fault",
  NULL
};

static void printfmt(void (*putch)(int, int*), void *putdat, const char *fmt, ...)
{
  va_list ap;
  va_start(ap, fmt);
  vprintfmt(putch, putdat, fmt, ap);
  va_end(ap);
}

#endif

/*
 * Print a number (base <= 16) in reverse order,
 * using specified putch function and associated pointer putdat.
 */
static void printnum(void (*putch)(int, int*), void *putdat, unsigned long long num, unsigned base, int width, int padc)
{
  // first recursively print all preceding (more significant) digits
  if (num >= base) {
    printnum(putch, putdat, num / base, base, width - 1, padc);
  } else {
    // print any needed pad characters before first digit
    while (--width > 0)
      putch(padc, putdat);
  }

  // then print this (the least significant) digit
  putch("0123456789abcdef"[num % base], putdat);
}

// Same as getuint but signed - can't use getuint
// because of sign extension
static long long getint(va_list *ap, int lflag)
{
  if (lflag >= 2)
    return va_arg(*ap, long long);
  else if (lflag)
    return va_arg(*ap, long);
  else
    return va_arg(*ap, int);
}

#endif


static void vprintfmt(void (*putch)(int, int*), void *putdat, const char *fmt, va_list ap)
{
  register const char *p;
  register int ch;

#if defined( PRINT_ERROR_ENABLE )
  register int err;
#endif

  unsigned long long num;
  int base, lflag, width, precision, altflag;
  char padc;

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

    // Process a %-escape sequence
    padc = ' ';
    width = -1;
    precision = -1;
    lflag = 0;
    altflag = 0;

    reswitch:
    switch (ch = *(unsigned 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;

      process_precision:
               if (width < 0) width = precision, precision = -1;
               goto reswitch;

      #if defined(__GNUC__) && defined(__GNUC_MINOR__)

      // reduce the uneccerssy format for the AVR chip
      #else 

      case '*':
               precision = va_arg(ap, int);
               goto process_precision;

      case '.':
               if (width < 0) width = 0;
               goto reswitch;
      case '#':
               altflag = 1;
               goto reswitch;

      #if defined( PRINT_ERROR_ENABLE )
      // error message
      case 'e':
               err = va_arg(ap, int);
               if (err < 0)  err = -err;
               if (err >= MAXERROR || (p = error_string[err]) == NULL)
                 printfmt(putch, putdat, "error %d", err);
               else
                 printfmt(putch, putdat, "%s", p);
               break;
      #endif

      // (signed) decimal
      case 'd':
               num = getint(&ap, lflag);
               if ((long long) num < 0) 
               {
                 putch('-', putdat);
                 num = -(long long) num;
               }
               base = 10;
               goto number;

      // unsigned decimal
      case 'u':
               num = getuint(&ap, lflag);
	       base = 10;
               goto number;

      // (unsigned) octal
      case 'o':
              num = getuint(&ap, lflag);
              base = 8;
              printnum(putch, putdat, num, base, width, padc);
              break;

      // long flag (doubled for long long)
      case 'l':
               lflag++;
               goto reswitch;

      #endif

      // character
      case 'c':
               putch(va_arg(ap, int), putdat);
               break;

      // string
      case 's':
              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--)
	      if (altflag && (ch < ' ' || ch > '~')) putch('?', putdat);
              else  putch(ch, putdat);
               
	      for (; width > 0; width--)  putch(' ', putdat);
           
	      break;

      // pointer
      case 'p':
             putch('0', putdat);
             putch('x', putdat);
             num = (unsigned long long) (uintptr_t) va_arg(ap, void *);
             base = 16;
             goto number;

      // (unsigned) hexadecimal
      case 'x':
             num = getuint(&ap, lflag);
             base = 16;
             number:
             printnum(putch, putdat, num, base, width, padc);
		         
             break;

      // escaped '%' character
      case '%':
             putch(ch, putdat);
             break;

      // unrecognized escape sequence - just print it literally
      default:
             putch('%', putdat);
             for (fmt--; fmt[-1] != '%'; fmt--) break;
    } 
  }
}



int print(const char * fmt, ...)
{	
  va_list ap;
  int cnt;
  cnt = 0;
  va_start(ap,fmt);	
  vprintfmt(put_cnt_char, &cnt, fmt, ap)  ;
  va_end(ap);
  return cnt;
}