nes-proj/os/lib/dbg-io/strformat.c

/*
 * Copyright (c) 2009, Simon Berg
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE
 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
 */
/*---------------------------------------------------------------------------*/
#include "contiki.h"

#include <strformat.h>
/*---------------------------------------------------------------------------*/
#define HAVE_DOUBLE
#define HAVE_LONGLONG

#ifndef LARGEST_SIGNED
#ifdef HAVE_LONGLONG
#define LARGEST_SIGNED long long int
#else
#define LARGEST_UNSIGNED long int
#endif /* HAVE_LONGLONG */
#endif /* LARGEST_SIGNED */

#ifndef LARGEST_UNSIGNED
#ifdef HAVE_LONGLONG
#define LARGEST_UNSIGNED unsigned long long int
#else
#define LARGEST_UNSIGNED unsigned long int
#endif /* HAVE_LONGLONG */
#endif /* LARGEST_UNSIGNED */

#ifndef POINTER_INT
#define POINTER_INT unsigned long
#endif
/*---------------------------------------------------------------------------*/
typedef unsigned int FormatFlags;
/*---------------------------------------------------------------------------*/
#define MAKE_MASK(shift, size) (((1 << size) - 1) << (shift))
/*---------------------------------------------------------------------------*/
#define JUSTIFY_SHIFT   0
#define JUSTIFY_SIZE    1
#define JUSTIFY_RIGHT   0x0000
#define JUSTIFY_LEFT    0x0001
#define JUSTIFY_MASK    MAKE_MASK(JUSTIFY_SHIFT, JUSTIFY_SIZE)
/*---------------------------------------------------------------------------*/
/* How a positive number is prefixed */
#define POSITIVE_SHIFT  (JUSTIFY_SHIFT + JUSTIFY_SIZE)
#define POSITIVE_NONE   (0x0000 << POSITIVE_SHIFT)
#define POSITIVE_SPACE  (0x0001 << POSITIVE_SHIFT)
#define POSITIVE_PLUS   (0x0003 << POSITIVE_SHIFT)
#define POSITIVE_MASK   MAKE_MASK(POSITIVE_SHIFT, POSITIVE_SIZE)

#define POSITIVE_SIZE   2
/*---------------------------------------------------------------------------*/
#define ALTERNATE_FORM_SHIFT (POSITIVE_SHIFT + POSITIVE_SIZE)
#define ALTERNATE_FORM_SIZE  1
#define ALTERNATE_FORM       (0x0001 << ALTERNATE_FORM_SHIFT)
/*---------------------------------------------------------------------------*/
#define PAD_SHIFT (ALTERNATE_FORM_SHIFT + ALTERNATE_FORM_SIZE)
#define PAD_SIZE  1
#define PAD_SPACE (0x0000 << PAD_SHIFT)
#define PAD_ZERO  (0x0001 << PAD_SHIFT)
/*---------------------------------------------------------------------------*/
#define SIZE_SHIFT    (PAD_SHIFT + PAD_SIZE)
#define SIZE_SIZE     3
#define SIZE_CHAR     (0x0001 << SIZE_SHIFT)
#define SIZE_SHORT    (0x0002 << SIZE_SHIFT)
#define SIZE_INT      (0x0000 << SIZE_SHIFT)
#define SIZE_LONG     (0x0003 << SIZE_SHIFT)
#define SIZE_LONGLONG (0x0004 << SIZE_SHIFT)
#define SIZE_MASK     MAKE_MASK(SIZE_SHIFT, SIZE_SIZE)
/*---------------------------------------------------------------------------*/
#define CONV_SHIFT    (SIZE_SHIFT + SIZE_SIZE)
#define CONV_SIZE     3
#define CONV_INTEGER  (0x0001 << CONV_SHIFT)
#define CONV_FLOAT    (0x0002 << CONV_SHIFT)
#define CONV_POINTER  (0x0003 << CONV_SHIFT)
#define CONV_STRING   (0x0004 << CONV_SHIFT)
#define CONV_CHAR     (0x0005 << CONV_SHIFT)
#define CONV_PERCENT  (0x0006 << CONV_SHIFT)
#define CONV_WRITTEN  (0x0007 << CONV_SHIFT)
#define CONV_MASK     MAKE_MASK(CONV_SHIFT, CONV_SIZE)
/*---------------------------------------------------------------------------*/
#define RADIX_SHIFT   (CONV_SHIFT + CONV_SIZE)
#define RADIX_SIZE    2
#define RADIX_DECIMAL (0x0001 << RADIX_SHIFT)
#define RADIX_OCTAL   (0x0002 << RADIX_SHIFT)
#define RADIX_HEX     (0x0003 << RADIX_SHIFT)
#define RADIX_MASK    MAKE_MASK(RADIX_SHIFT, RADIX_SIZE)
/*---------------------------------------------------------------------------*/
#define SIGNED_SHIFT  (RADIX_SHIFT + RADIX_SIZE)
#define SIGNED_SIZE   1
#define SIGNED_NO     (0x0000 << SIGNED_SHIFT)
#define SIGNED_YES    (0x0001 << SIGNED_SHIFT)
#define SIGNED_MASK   MAKE_MASK(SIGNED_SHIFT, SIGNED_SIZE)
/*---------------------------------------------------------------------------*/
#define CAPS_SHIFT  (SIGNED_SHIFT + SIGNED_SIZE)
#define CAPS_SIZE 1
#define CAPS_NO     (0x0000 << CAPS_SHIFT)
#define CAPS_YES    (0x0001 << CAPS_SHIFT)
#define CAPS_MASK   MAKE_MASK(CAPS_SHIFT, CAPS_SIZE)
/*---------------------------------------------------------------------------*/
#define FLOAT_SHIFT     (CAPS_SHIFT + CAPS_SIZE)
#define FLOAT_SIZE      2
#define FLOAT_NORMAL    (0x0000 << FLOAT_SHIFT)
#define FLOAT_EXPONENT  (0x0001 << FLOAT_SHIFT)
#define FLOAT_DEPENDANT (0x0002 << FLOAT_SHIFT)
#define FLOAT_HEX       (0x0003 << FLOAT_SHIFT)
#define FLOAT_MASK      MAKE_MASK(FLOAT_SHIFT, FLOAT_SIZE)
/*---------------------------------------------------------------------------*/
#define CHECKCB(res) { if((res) != STRFORMAT_OK) { va_end(ap); return -1; } }
/*---------------------------------------------------------------------------*/
#define MAXCHARS_HEX ((sizeof(LARGEST_UNSIGNED) * 8) / 4)

/* Largest number of characters needed for converting an unsigned integer. */
#define MAXCHARS ((sizeof(LARGEST_UNSIGNED) * 8 + 2) / 3)
/*---------------------------------------------------------------------------*/
static FormatFlags
parse_flags(const char **posp)
{
  FormatFlags flags = 0;
  const char *pos = *posp;

  while(1) {
    switch(*pos) {
    case '-':
      flags |= JUSTIFY_LEFT;
      break;
    case '+':
      flags |= POSITIVE_PLUS;
      break;
    case ' ':
      flags |= POSITIVE_SPACE;
      break;
    case '#':
      flags |= ALTERNATE_FORM;
      break;
    case '0':
      flags |= PAD_ZERO;
      break;
    default:
      *posp = pos;
      return flags;
    }

    pos++;
  }
}
/*---------------------------------------------------------------------------*/
static unsigned int
parse_uint(const char **posp)
{
  unsigned v = 0;
  const char *pos = *posp;
  char ch;

  while((ch = *pos) >= '0' && ch <= '9') {
    v = v * 10 + (ch - '0');
    pos++;
  }

  *posp = pos;

  return v;
}
/*---------------------------------------------------------------------------*/
static unsigned int
output_uint_decimal(char **posp, LARGEST_UNSIGNED v)
{
  unsigned int len;
  char *pos = *posp;

  while(v > 0) {
    *--pos = (v % 10) + '0';
    v /= 10;
  }

  len = *posp - pos;
  *posp = pos;

  return len;
}
/*---------------------------------------------------------------------------*/
static unsigned int
output_uint_hex(char **posp, LARGEST_UNSIGNED v, unsigned int flags)
{
  unsigned int len;
  const char *hex = (flags & CAPS_YES) ? "0123456789ABCDEF" : "0123456789abcdef";
  char *pos = *posp;

  while(v > 0) {
    *--pos = hex[(v % 16)];
    v /= 16;
  }

  len = *posp - pos;
  *posp = pos;

  return len;
}
/*---------------------------------------------------------------------------*/
static unsigned int
output_uint_octal(char **posp, LARGEST_UNSIGNED v)
{
  unsigned int len;
  char *pos = *posp;

  while(v > 0) {
    *--pos = (v % 8) + '0';
    v /= 8;
  }

  len = *posp - pos;
  *posp = pos;

  return len;
}
/*---------------------------------------------------------------------------*/
static strformat_result
fill_space(const strformat_context_t *ctxt, unsigned int len)
{
  strformat_result res;
  static const char buffer[16] = "                ";

  while(len > 16) {
    res = ctxt->write_str(ctxt->user_data, buffer, 16);
    if(res != STRFORMAT_OK) {
      return res;
    }
    len -= 16;
  }

  if(len == 0) {
    return STRFORMAT_OK;
  }

  return ctxt->write_str(ctxt->user_data, buffer, len);
}
/*---------------------------------------------------------------------------*/
static strformat_result
fill_zero(const strformat_context_t *ctxt, unsigned int len)
{
  strformat_result res;
  static const char buffer[16] = "0000000000000000";

  while(len > 16) {
    res = ctxt->write_str(ctxt->user_data, buffer, 16);
    if(res != STRFORMAT_OK) {
      return res;
    }
    len -= 16;
  }

  if(len == 0) {
    return STRFORMAT_OK;
  }
  return ctxt->write_str(ctxt->user_data, buffer, len);
}
/*---------------------------------------------------------------------------*/
int
format_str(const strformat_context_t *ctxt, const char *format, ...)
{
  int ret;
  va_list ap;
  va_start(ap, format);
  ret = format_str_v(ctxt, format, ap);
  va_end(ap);
  return ret;
}
/*---------------------------------------------------------------------------*/
int
format_str_v(const strformat_context_t *ctxt, const char *format, va_list ap)
{
  unsigned int written = 0;
  const char *pos = format;

  while(*pos != '\0') {
    FormatFlags flags;
    unsigned int minwidth = 0;
    int precision = -1; /* Negative means no precision */
    char ch;
    const char *start = pos;

    while((ch = *pos) != '\0' && ch != '%') {
      pos++;
    }

    if(pos != start) {
      CHECKCB(ctxt->write_str(ctxt->user_data, start, pos - start));
      written += pos - start;
    }

    if(*pos == '\0') {
      va_end(ap);
      return written;
    }

    pos++;

    if(*pos == '\0') {
      va_end(ap);
      return written;
    }

    flags = parse_flags(&pos);

    /* parse width */
    if(*pos >= '1' && *pos <= '9') {
      minwidth = parse_uint(&pos);
    } else if(*pos == '*') {
      int w = va_arg(ap, int);

      if(w < 0) {
        flags |= JUSTIFY_LEFT;
        minwidth = w;
      } else {
        minwidth = w;
      }

      pos++;
    }

    /* parse precision */
    if(*pos == '.') {
      pos++;

      if(*pos >= '0' && *pos <= '9') {
        precision = parse_uint(&pos);
      } else if(*pos == '*') {
        pos++;
        precision = va_arg(ap, int);
      }
    }

    if(*pos == 'l') {
      pos++;

      if(*pos == 'l') {
        flags |= SIZE_LONGLONG;
        pos++;
      } else {
        flags |= SIZE_LONG;
      }
    } else if(*pos == 'h') {
      pos++;

      if(*pos == 'h') {
        flags |= SIZE_CHAR;
        pos++;
      } else {
        flags |= SIZE_SHORT;
      }
    }

    /* parse conversion specifier */
    switch(*pos) {
    case 'd':
    case 'i':
      flags |= CONV_INTEGER | RADIX_DECIMAL | SIGNED_YES;
      break;
    case 'u':
      flags |= CONV_INTEGER | RADIX_DECIMAL | SIGNED_NO;
      break;
    case 'o':
      flags |= CONV_INTEGER | RADIX_OCTAL | SIGNED_NO;
      break;
    case 'x':
      flags |= CONV_INTEGER | RADIX_HEX | SIGNED_NO;
      break;
    case 'X':
      flags |= CONV_INTEGER | RADIX_HEX | SIGNED_NO | CAPS_YES;
      break;
#ifdef HAVE_DOUBLE
    case 'f':
      flags |= CONV_FLOAT | FLOAT_NORMAL;
      break;
    case 'F':
      flags |= CONV_FLOAT | FLOAT_NORMAL | CAPS_YES;
      break;
    case 'e':
      flags |= CONV_FLOAT | FLOAT_EXPONENT;
      break;
    case 'E':
      flags |= CONV_FLOAT | FLOAT_EXPONENT | CAPS_YES;
      break;
    case 'g':
      flags |= CONV_FLOAT | FLOAT_DEPENDANT;
      break;
    case 'G':
      flags |= CONV_FLOAT | FLOAT_DEPENDANT | CAPS_YES;
      break;
    case 'a':
      flags |= CONV_FLOAT | FLOAT_HEX;
      break;
    case 'A':
      flags |= CONV_FLOAT | FLOAT_HEX | CAPS_YES;
      break;
#endif
    case 'c':
      flags |= CONV_CHAR;
      break;
    case 's':
      flags |= CONV_STRING;
      break;
    case 'p':
      flags |= CONV_POINTER;
      break;
    case 'n':
      flags |= CONV_WRITTEN;
      break;
    case '%':
      flags |= CONV_PERCENT;
      break;
    case '\0':
      va_end(ap);
      return written;
    }
    pos++;

    switch(flags & CONV_MASK) {
    case CONV_PERCENT:
      CHECKCB(ctxt->write_str(ctxt->user_data, "%", 1));
      written++;
      break;
    case CONV_INTEGER:
    {
      /* unsigned integers */
      char *prefix = 0; /* sign, "0x" or "0X" */
      unsigned int prefix_len = 0;
      char buffer[MAXCHARS];
      char *conv_pos = buffer + MAXCHARS;
      unsigned int conv_len = 0;
      unsigned int width = 0;
      unsigned int precision_fill;
      unsigned int field_fill;
      LARGEST_UNSIGNED uvalue = 0;
      int negative = 0;

      if(precision < 0) {
        precision = 1;
      } else {
        flags &= ~PAD_ZERO;
      }

      if(flags & SIGNED_YES) {
        /* signed integers */
        LARGEST_SIGNED value = 0;
        switch(flags & SIZE_MASK) {
        case SIZE_CHAR:
          value = (signed char)va_arg(ap, int);
          break;
        case SIZE_SHORT:
          value = (short)va_arg(ap, int);
          break;
        case SIZE_INT:
          value = va_arg(ap, int);
          break;
#ifndef HAVE_LONGLONG
        case SIZE_LONGLONG: /* Treat long long the same as long */
#endif
        case SIZE_LONG:
          value = va_arg(ap, long);
          break;
#ifdef HAVE_LONGLONG
        case SIZE_LONGLONG:
          value = va_arg(ap, long long);
          break;
#endif
        }
        if(value < 0) {
          uvalue = -value;
          negative = 1;
        } else {
          uvalue = value;
        }
      } else {

        switch(flags & SIZE_MASK) {
        case SIZE_CHAR:
          uvalue = (unsigned char)va_arg(ap, unsigned int);
          break;
        case SIZE_SHORT:
          uvalue = (unsigned short)va_arg(ap, unsigned int);
          break;
        case SIZE_INT:
          uvalue = va_arg(ap, unsigned int);
          break;
#ifndef HAVE_LONGLONG
        case SIZE_LONGLONG: /* Treat long long the same as long */
#endif
        case SIZE_LONG:
          uvalue = va_arg(ap, unsigned long);
          break;
#ifdef HAVE_LONGLONG
        case SIZE_LONGLONG:
          uvalue = va_arg(ap, unsigned long long);
          break;
#endif
        }
      }

      switch(flags & (RADIX_MASK)) {
      case RADIX_DECIMAL:
        conv_len = output_uint_decimal(&conv_pos, uvalue);
        break;
      case RADIX_OCTAL:
        conv_len = output_uint_octal(&conv_pos, uvalue);
        break;
      case RADIX_HEX:
        conv_len = output_uint_hex(&conv_pos, uvalue, flags);
        break;
      }

      width += conv_len;
      precision_fill = (precision > conv_len) ? precision - conv_len : 0;
      if((flags & (RADIX_MASK | ALTERNATE_FORM))
         == (RADIX_OCTAL | ALTERNATE_FORM)) {
        if(precision_fill < 1) {
          precision_fill = 1;
        }
      }

      width += precision_fill;

      if((flags & (RADIX_MASK | ALTERNATE_FORM))
         == (RADIX_HEX | ALTERNATE_FORM) && uvalue != 0) {
        prefix_len = 2;
        if(flags & CAPS_YES) {
          prefix = "0X";
        } else {
          prefix = "0x";
        }
      }

      if(flags & SIGNED_YES) {
        if(negative) {
          prefix = "-";
          prefix_len = 1;
        } else {
          switch(flags & POSITIVE_MASK) {
          case POSITIVE_SPACE:
            prefix = " ";
            prefix_len = 1;
            break;
          case POSITIVE_PLUS:
            prefix = "+";
            prefix_len = 1;
            break;
          }
        }
      }

      width += prefix_len;

      field_fill = (minwidth > width) ? minwidth - width : 0;

      if((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) {
        if(flags & PAD_ZERO) {
          precision_fill += field_fill;
          field_fill = 0; /* Do not double count padding */
        } else {
          CHECKCB(fill_space(ctxt, field_fill));
        }
      }

      if(prefix_len > 0) {
        CHECKCB(ctxt->write_str(ctxt->user_data, prefix, prefix_len));
      }
      written += prefix_len;

      CHECKCB(fill_zero(ctxt, precision_fill));
      written += precision_fill;

      CHECKCB(ctxt->write_str(ctxt->user_data, conv_pos, conv_len));
      written += conv_len;

      if((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) {
        CHECKCB(fill_space(ctxt, field_fill));
      }
      written += field_fill;
    }
    break;
    case CONV_STRING:
    {
      unsigned int field_fill;
      unsigned int len;
      char *str = va_arg(ap, char *);

      if(str) {
        char *pos = str;
        while(*pos != '\0') pos++;
        len = pos - str;
      } else {
        str = "(null)";
        len = 6;
      }

      if(precision >= 0 && precision < len) {
        len = precision;
      }

      field_fill = (minwidth > len) ? minwidth - len : 0;

      if((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) {
        CHECKCB(fill_space(ctxt, field_fill));
      }

      CHECKCB(ctxt->write_str(ctxt->user_data, str, len));
      written += len;

      if((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) {
        CHECKCB(fill_space(ctxt, field_fill));
      }
      written += field_fill;
    }
    break;
    case CONV_POINTER:
    {
      LARGEST_UNSIGNED uvalue =
        (LARGEST_UNSIGNED)(POINTER_INT)va_arg(ap, void *);
      char buffer[MAXCHARS_HEX + 3];
      char *conv_pos = buffer + MAXCHARS_HEX + 3;
      unsigned int conv_len;
      unsigned int field_fill;

      conv_len = output_uint_hex(&conv_pos, uvalue, flags);

      if(conv_len == 0) {
        *--conv_pos = '0';
        conv_len++;
      }

      *--conv_pos = 'x';
      *--conv_pos = '0';
      *--conv_pos = '#';
      conv_len += 3;

      field_fill = (minwidth > conv_len) ? minwidth - conv_len : 0;

      if((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) {
        CHECKCB(fill_space(ctxt, field_fill));
      }

      CHECKCB(ctxt->write_str(ctxt->user_data, conv_pos, conv_len));
      written += conv_len;

      if((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) {
        CHECKCB(fill_space(ctxt, field_fill));
      }

      written += field_fill;
    }
    break;
    case CONV_CHAR:
    {
      char ch = va_arg(ap, int);
      unsigned int field_fill = (minwidth > 1) ? minwidth - 1 : 0;

      if((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) {
        CHECKCB(fill_space(ctxt, field_fill));
        written += field_fill;
      }

      CHECKCB(ctxt->write_str(ctxt->user_data, &ch, 1));
      written++;

      if((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) {
        CHECKCB(fill_space(ctxt, field_fill));
      }
      written += field_fill;
    }
    break;
    case CONV_WRITTEN:
    {
      int *p = va_arg(ap, int *);
      *p = written;
    }
    break;
    }
  }

  return written;
}
/*---------------------------------------------------------------------------*/