chprintf.c

/*
    ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

/*
   Concepts and parts of this file have been contributed by Fabio Utzig,
   chvprintf() added by Brent Roman.
 */

/**
 * @file    chprintf.c
 * @brief   Mini printf-like functionality.
 *
 * @addtogroup chprintf
 * @{
 */

#include "hal.h"
#include "chprintf.h"
//#include "memstreams.h"
#include <math.h>

// Enable [flags], support:
// ' ' Prepends a space for positive signed-numeric types. positive = ' ', negative = '-'. This flag is ignored if the + flag exists.
//#define CHPRINTF_USE_SPACE_FLAG

// Force putting trailing zeros on float value
#define CHPRINTF_FORCE_TRAILING_ZEROS

#define MAX_FILLER 11
#define FLOAT_PRECISION 9

#pragma pack(push, 2)

static const uint32_t pow10[FLOAT_PRECISION+1] = {
    1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000
};
// Prefixes for values bigger then 1000.0
//                            1  1e3, 1e6, 1e9, 1e12, 1e15, 1e18, 1e21, 1e24
static char bigPrefix[] = {' ', 'k', 'M', 'G', 'T', 'P', 'E', 'Z', 'Y', 0};
// Prefixes for values less   then 1.0
//                          1e-3, 1e-6, 1e-9, 1e-12, 1e-15, 1e-18, 1e-21, 1e-24
static char smallPrefix[] = {'m', 0x1d, 'n', 'p', 'f', 'a', 'z', 'y', 0};

#pragma pack(pop)

static char *long_to_string_with_divisor(char *p,
                                         uint32_t num,
                                         uint32_t radix,
                                         uint32_t precision) {
  char *q = p + MAX_FILLER;
  char *b = q;
  // convert to string from end buffer to begin
  do {
    uint8_t c = num % radix;
    num /= radix;
    *--q = c + ((c > 9) ? ('A'-10) : '0');
  }while((precision && --precision) || num);
  // copy string at begin
  int i = (int)(b - q);
  do
    *p++ = *q++;
  while (--i);
  return p;
}

// default prescision = 13
// g.mmm kkk hhh
#define MAX_FREQ_PRESCISION 13
#define FREQ_PSET           1
#define FREQ_NO_SPACE       2
#define FREQ_PREFIX_SPACE   4

static char *
ulong_freq(char *p, uint32_t freq, uint32_t precision)
{
  uint8_t flag = FREQ_PSET;
  if (precision == 0)
    flag|=FREQ_PREFIX_SPACE;
  if (precision == 0 || precision > MAX_FREQ_PRESCISION)
    precision = MAX_FREQ_PRESCISION;
  char *q = p + MAX_FREQ_PRESCISION;
  char *b = q;
  // Prefix counter
  uint32_t s = 0;
  // Set format (every 3 digits add ' ' up to GHz)
  uint32_t format = 0b00100100100;
  do {
#if 0
    uint8_t c = freq % 10;
    freq/= 10;
#else
    // Fast and compact division uint32_t on 10, using shifts, result:
    // c = freq % 10
    // freq = freq / 10;
    uint32_t c = freq;
    freq >>= 1;
    freq += freq >> 1;
    freq += freq >> 4;
    freq += freq >> 8;
    freq += freq >> 16;  // freq = 858993459*freq/1073741824 = freq *
                         // 0,799999999813735485076904296875
    freq >>= 3;  // freq/=8; freq = freq * 0,09999999997671693563461303710938
    c -= freq * 10;  // freq*10 = (freq*4+freq)*2 = ((freq<<2)+freq)<<1
    while (c >= 10) {
      freq++;
      c -= 10;
    }
#endif
    *--q = c + '0';
    if (freq == 0) break;
    // Add spaces, calculate prefix
    if (format & 1) {
      *--q = ' ';
      s++;
    }
    format >>= 1;
  } while (1);
  s = bigPrefix[s];

  // Get string size
  uint32_t i = (b - q);
  // Limit string size, max size is - precision
  if (precision && i > precision) {
    i = precision;
    flag |= FREQ_NO_SPACE;
  }
  // copy string
  // Replace first ' ' by '.', remove ' ' if size too big
  do {
    char c = *q++;
    // replace first ' ' on '.'
    if (c == ' ') {
      if (flag & FREQ_PSET) {
        c = '.';
        flag &= ~FREQ_PSET;
      } else if (flag & FREQ_NO_SPACE)
        c = *q++;
    }
    *p++ = c;
  } while (--i);
  // Put pref (amd space before it if need)
  if (flag & FREQ_PREFIX_SPACE && s != ' ') 
    *p++ = ' ';
  *p++ = s;
  return p;
}

#if CHPRINTF_USE_FLOAT
static char *ftoa(char *p, float num, uint32_t precision) {
  // Check precision limit
  if (precision > FLOAT_PRECISION)
    precision = FLOAT_PRECISION;
  uint32_t multi = pow10[precision];
  uint32_t l = num;
  // Round value
  uint32_t k = ((num-l)*multi+0.5);
  // Fix rounding error if get
  if (k>=multi){k-=multi;l++;}
  p = long_to_string_with_divisor(p, l, 10, 0);
  if (precision) {
    *p++ = '.';
    p=long_to_string_with_divisor(p, k, 10, precision);
#ifndef CHPRINTF_FORCE_TRAILING_ZEROS
    // remove zeros at end
    while (p[-1]=='0') p--;
    if (p[-1]=='.') p--;
#endif
  }
  return p;
}

static char *ftoaS(char *p, float num, uint32_t precision) {
  char prefix=0;
  char *ptr;
  if (num > 1000.0){
    for (ptr = bigPrefix+1; *ptr && num > 1000.0; num/=1000, ptr++)
      ;
    prefix = ptr[-1];
  }
  else if (num < 1){
    for (ptr = smallPrefix; *ptr && num < 1.0; num*=1000, ptr++)
      ;
    prefix = num > 1e-3 ? ptr[-1] : 0;
  }
  // Auto set prescision
  uint32_t l = num;
  if (l < 10)
    precision+=2;
  else if (l < 100)
    precision+=1;
  p=ftoa(p, num, precision);
  if (prefix)
    *p++ = prefix;
  return p;
}
#endif

/**
 * @brief   System formatted output function.
 * @details This function implements a minimal @p vprintf()-like functionality
 *          with output on a @p BaseSequentialStream.
 *          The general parameters format is: %[-][width|*][.precision|*][l|L]p.
 *          The following parameter types (p) are supported:
 *          - <b>x</b> hexadecimal integer.
 *          - <b>X</b> hexadecimal long.
 *          - <b>o</b> octal integer.
 *          - <b>O</b> octal long.
 *          - <b>d</b> decimal signed integer.
 *          - <b>D</b> decimal signed long.
 *          - <b>u</b> decimal unsigned integer.
 *          - <b>U</b> decimal unsigned long.
 *          - <b>c</b> character.
 *          - <b>s</b> string.
 *          .
 *
 * @param[in] chp       pointer to a @p BaseSequentialStream implementing object
 * @param[in] fmt       formatting string
 * @param[in] ap        list of parameters
 * @return              The number of bytes that would have been
 *                      written to @p chp if no stream error occurs
 *
 * @api
 */
#define IS_LONG             1
#define LEFT_ALIGN          2
#define POSITIVE            4
#define NEGATIVE            8
#define PAD_ZERO            16
#define PLUS_SPACE          32
#define DEFAULT_PRESCISION  64

int chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap) {
  char *p, *s, c, filler=' ';
  int precision, width;
  int n = 0;
  uint32_t  state;
  union {
      uint32_t u;
      int32_t  l;
      float    f;
  }value;
#if CHPRINTF_USE_FLOAT
  char tmpbuf[2*MAX_FILLER + 1];
#else
  char tmpbuf[MAX_FREQ_PRESCISION + 1];
#endif

  while (true) {
    c = *fmt++;
    if (c == 0)
      return n;
    if (c != '%') {
      streamPut(chp, (uint8_t)c);
      n++;
      continue;
    }
    // Parse %[flags][width][.precision][length]type
    p = tmpbuf;
    s = tmpbuf;
    state = 0;
    width = 0;
    precision = 0;

    // Get [flags], support:
    // '-' Left-align the output of this placeholder. (The default is to right-align the output.)
    // '+' Prepends a  plus for positive signed-numeric types. positive = '+', negative = '-'.
    // ' ' Prepends a space for positive signed-numeric types. positive = ' ', negative = '-'. This flag is ignored if the + flag exists.
    // '0' When the 'width' option is specified, prepends zeros for numeric types. (The default prepends spaces.)
    while (true){
      if (*fmt == '-')
        state|=LEFT_ALIGN;
      else if (*fmt == '+')
        state|=POSITIVE;
      else if (*fmt == '0')
        state|=PAD_ZERO;
#ifdef CHPRINTF_USE_SPACE_FLAG
      else if (*fmt == ' ')
        state|=PLUS_SPACE;
#endif
      else
        break;
      fmt++;
    }
    // Get [width] - The Width field specifies a minimum number of characters to output
    // if set *, get width as argument
    while (true) {
      c = *fmt++;
      if (c >= '0' && c <= '9')
        c -= '0';
      else if (c == '*')
        c = va_arg(ap, int);
      else
        break;
      width = width * 10 + c;
    }
    // Get [.precision]
    if (c == '.') {
      while (true) {
        c = *fmt++;
        if (c >= '0' && c <= '9')
          c -= '0';
        else if (c == '*')
          c = va_arg(ap, int);
        else
           break;
        precision = precision * 10 + c;
      }
    }
    else
      state|=DEFAULT_PRESCISION;
    //Get [length]
    /*
    if (c == 'l' || c == 'L') {
      state|=IS_LONG;
      if (*fmt)
        c = *fmt++;
    }
    else if ((c >= 'A') && (c <= 'Z'))
        state|=IS_LONG;
    */
    // Parse type
    switch (c) {
    case 'c':
      state&=~PAD_ZERO;
      *p++ = va_arg(ap, int);
      break;
    case 's':
      state&=~PAD_ZERO;
      if ((s = va_arg(ap, char *)) == 0)
        s = "(null)";
      if (state&DEFAULT_PRESCISION)
        precision = 32767;
      for (p = s; *p && (--precision >= 0); p++)
        ;
      break;
    case 'D':
    case 'd':
    case 'I':
    case 'i':/*
      if (state & IS_LONG)
        value.l = va_arg(ap, long);
      else*/
        value.l = va_arg(ap, uint32_t);
      if (value.l < 0) {
        state|=NEGATIVE;
        *p++ = '-';
        value.l = -value.l;
      }
      else if (state & POSITIVE)
        *p++ = '+';
#ifdef CHPRINTF_USE_SPACE_FLAG
      else if (state & PLUS_SPACE)
        *p++ = ' ';
#endif
      p = long_to_string_with_divisor(p, value.l, 10, 0);
      break;
    case 'q':
      value.u = va_arg(ap, uint32_t);
      p=ulong_freq(p, value.u, precision);
      break;
#if CHPRINTF_USE_FLOAT
    case 'F':
    case 'f':
      value.f = va_arg(ap, double);
      if (value.f < 0) {
        state|=NEGATIVE;
        *p++ = '-';
        value.f = -value.f;
      }
      else if (state & POSITIVE)
        *p++ = '+';
#ifdef CHPRINTF_USE_SPACE_FLAG
      else if (state & PLUS_SPACE)
        *p++ = ' ';
#endif
      if (value.f == INFINITY){
        *p++ = 0x19;
        break;
      }
      p = (c=='F') ? ftoaS(p, value.f, precision) : ftoa(p, value.f, state&DEFAULT_PRESCISION ? FLOAT_PRECISION : precision);
      break;
#endif
    case 'X':
    case 'x':
      c = 16;
      goto unsigned_common;
    case 'U':
    case 'u':
      c = 10;
      goto unsigned_common;
    case 'O':
    case 'o':
      c = 8;
unsigned_common:/*
      if (state & IS_LONG)
        value.u = va_arg(ap, unsigned long);
      else*/
        value.u = va_arg(ap, uint32_t);
      p = long_to_string_with_divisor(p, value.u, c, 0);
      break;
    default:
      *p++ = c;
      break;
    }
    // Now need print buffer s[{sign}XXXXXXXXXXXX]p and align it on width
    // Check filler width (if buffer less then width) and prepare filler if need fill
    if ((width -=(int)(p - s)) < 0)
      width = 0;
    else
      filler = (state&PAD_ZERO) ? '0' : ' ';
    // if left align, put sign digit, and fill
    // [{sign}ffffffXXXXXXXXXXXX]
    if (!(state&LEFT_ALIGN)) {
      // Put '+' or '-' or ' ' first if need
      if ((state&(NEGATIVE|POSITIVE|PLUS_SPACE)) && (state&PAD_ZERO)) {
        streamPut(chp, (uint8_t)*s++);
        n++;
      }
      // fill from left
      while (width){
        streamPut(chp, (uint8_t)filler);
        n++;
        width--;
      }
    }
    // put data
    while (s < p) {
      streamPut(chp, (uint8_t)*s++);
      n++;
    }
    // Put filler from right (if need)
    while (width) {
      streamPut(chp, (uint8_t)filler);
      n++;
      width--;
    }
  }
}

/**
 * @brief   System formatted output function.
 * @details This function implements a minimal @p printf() like functionality
 *          with output on a @p BaseSequentialStream.
 *          The general parameters format is: %[-][width|*][.precision|*][l|L]p.
 *          The following parameter types (p) are supported:
 *          - <b>x</b> hexadecimal integer.
 *          - <b>X</b> hexadecimal long.
 *          - <b>o</b> octal integer.
 *          - <b>O</b> octal long.
 *          - <b>d</b> decimal signed integer.
 *          - <b>D</b> decimal signed long.
 *          - <b>u</b> decimal unsigned integer.
 *          - <b>U</b> decimal unsigned long.
 *          - <b>c</b> character.
 *          - <b>s</b> string.
 *          .
 *
 * @param[in] chp       pointer to a @p BaseSequentialStream implementing object
 * @param[in] fmt       formatting string
 *
 * @api
 */
#if 0
int chprintf(BaseSequentialStream *chp, const char *fmt, ...) {
  va_list ap;
  int formatted_bytes;

  va_start(ap, fmt);
  formatted_bytes = chvprintf(chp, fmt, ap);
  va_end(ap);

  return formatted_bytes;
}
#endif
/**
 * @brief   System formatted output function.
 * @details This function implements a minimal @p vprintf()-like functionality
 *          with output on a @p BaseSequentialStream.
 *          The general parameters format is: %[-][width|*][.precision|*][l|L]p.
 *          The following parameter types (p) are supported:
 *          - <b>x</b> hexadecimal integer.
 *          - <b>X</b> hexadecimal long.
 *          - <b>o</b> octal integer.
 *          - <b>O</b> octal long.
 *          - <b>d</b> decimal signed integer.
 *          - <b>D</b> decimal signed long.
 *          - <b>u</b> decimal unsigned integer.
 *          - <b>U</b> decimal unsigned long.
 *          - <b>c</b> character.
 *          - <b>s</b> string.
 *          .
 * @post    @p str is NUL-terminated, unless @p size is 0.
 *
 * @param[in] str       pointer to a buffer
 * @param[in] size      maximum size of the buffer
 * @param[in] fmt       formatting string
 * @return              The number of characters (excluding the
 *                      terminating NUL byte) that would have been
 *                      stored in @p str if there was room.
 *
 * @api
 */
#if 0
int chsnprintf(char *str, size_t size, const char *fmt, ...) {
  va_list ap;
  MemoryStream ms;
  BaseSequentialStream *chp;
  size_t size_wo_nul;
  int retval;

  if (size > 0)
    size_wo_nul = size - 1;
  else
    size_wo_nul = 0;

  /* Memory stream object to be used as a string writer, reserving one
     byte for the final zero.*/
  msObjectInit(&ms, (uint8_t *)str, size_wo_nul, 0);

  /* Performing the print operation using the common code.*/
  chp = (BaseSequentialStream *)(void *)&ms;
  va_start(ap, fmt);
  retval = chvprintf(chp, fmt, ap);
  va_end(ap);

  /* Terminate with a zero, unless size==0.*/
  if (ms.eos < size)
      str[ms.eos] = 0;

  /* Return number of bytes that would have been written.*/
  return retval;
}
#endif

//
// Small memory stream object, only put function
//
struct printStreamVMT {
  _base_sequential_stream_methods
};

typedef struct {
  const struct printStreamVMT *vmt;
  uint8_t  *buffer;
  uint16_t size;
} printStream;

static msg_t put(void *ip, uint8_t b) {
  printStream *ps = ip;
  if (ps->size > 1){
   *(ps->buffer++) = b;
   ps->size--;
  }
  return MSG_OK;
}

static const struct printStreamVMT vmt = {NULL, NULL, put, NULL};
void printObjectInit(printStream *ps, int size, uint8_t *buffer){
  ps->vmt    = &vmt;
  ps->buffer = buffer;
  ps->size   = size;
}
// Simple print in buffer function
int plot_printf(char *str, int size, const char *fmt, ...) {
  va_list ap;
  printStream ps;
  int retval;
  if (size <= 0) return 0;
  // Init small memory stream for print
  printObjectInit(&ps, size, (uint8_t *)str);
  // Performing the print operation using the common code.
  va_start(ap, fmt);
  retval = chvprintf((BaseSequentialStream *)(void *)&ps, fmt, ap);
  va_end(ap);
  *(ps.buffer)=0;
  if (retval > size-1) retval = size-1;
  // Return number of bytes that would have been written.
  return retval;
}

/** @} */