git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@7667 35acf78f-673a-0410-8e92-d51de3d6d3f4

4 files changed, 627 insertions, 0 deletions

diff --git a/os/hal/lib/streams/chprintf.c b/os/hal/lib/streams/chprintf.c
new file mode 100644
index 000000000..336a86e17
--- /dev/null
+++ b/os/hal/lib/streams/chprintf.c
@@ -0,0 +1,337 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 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"

+

+#define MAX_FILLER 11

+#define FLOAT_PRECISION 9

+

+static char *long_to_string_with_divisor(char *p,

+                                         long num,

+                                         unsigned radix,

+                                         long divisor) {

+  int i;

+  char *q;

+  long l, ll;

+

+  l = num;

+  if (divisor == 0) {

+    ll = num;

+  } else {

+    ll = divisor;

+  }

+

+  q = p + MAX_FILLER;

+  do {

+    i = (int)(l % radix);

+    i += '0';

+    if (i > '9')

+      i += 'A' - '0' - 10;

+    *--q = i;

+    l /= radix;

+  } while ((ll /= radix) != 0);

+

+  i = (int)(p + MAX_FILLER - q);

+  do

+    *p++ = *q++;

+  while (--i);

+

+  return p;

+}

+

+static char *ch_ltoa(char *p, long num, unsigned radix) {

+

+  return long_to_string_with_divisor(p, num, radix, 0);

+}

+

+#if CHPRINTF_USE_FLOAT

+static const long pow10[FLOAT_PRECISION] = {

+    10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000

+};

+

+static char *ftoa(char *p, double num, unsigned long precision) {

+  long l;

+

+  if ((precision == 0) || (precision > FLOAT_PRECISION))

+    precision = FLOAT_PRECISION;

+  precision = pow10[precision - 1];

+

+  l = (long)num;

+  p = long_to_string_with_divisor(p, l, 10, 0);

+  *p++ = '.';

+  l = (long)((num - l) * precision);

+  return long_to_string_with_divisor(p, l, 10, precision / 10);

+}

+#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

+ */

+int chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap) {

+  char *p, *s, c, filler;

+  int i, precision, width;

+  int n = 0;

+  bool is_long, left_align;

+  long l;

+#if CHPRINTF_USE_FLOAT

+  float f;

+  char tmpbuf[2*MAX_FILLER + 1];

+#else

+  char tmpbuf[MAX_FILLER + 1];

+#endif

+

+  while (TRUE) {

+    c = *fmt++;

+    if (c == 0)

+      return n;

+    if (c != '%') {

+      chSequentialStreamPut(chp, (uint8_t)c);

+      n++;

+      continue;

+    }

+    p = tmpbuf;

+    s = tmpbuf;

+    left_align = FALSE;

+    if (*fmt == '-') {

+      fmt++;

+      left_align = TRUE;

+    }

+    filler = ' ';

+    if (*fmt == '0') {

+      fmt++;

+      filler = '0';

+    }

+    width = 0;

+    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;

+    }

+    precision = 0;

+    if (c == '.') {

+      while (TRUE) {

+        c = *fmt++;

+        if (c >= '0' && c <= '9')

+          c -= '0';

+        else if (c == '*')

+          c = va_arg(ap, int);

+        else

+          break;

+        precision *= 10;

+        precision += c;

+      }

+    }

+    /* Long modifier.*/

+    if (c == 'l' || c == 'L') {

+      is_long = TRUE;

+      if (*fmt)

+        c = *fmt++;

+    }

+    else

+      is_long = (c >= 'A') && (c <= 'Z');

+

+    /* Command decoding.*/

+    switch (c) {

+    case 'c':

+      filler = ' ';

+      *p++ = va_arg(ap, int);

+      break;

+    case 's':

+      filler = ' ';

+      if ((s = va_arg(ap, char *)) == 0)

+        s = "(null)";

+      if (precision == 0)

+        precision = 32767;

+      for (p = s; *p && (--precision >= 0); p++)

+        ;

+      break;

+    case 'D':

+    case 'd':

+    case 'I':

+    case 'i':

+      if (is_long)

+        l = va_arg(ap, long);

+      else

+        l = va_arg(ap, int);

+      if (l < 0) {

+        *p++ = '-';

+        l = -l;

+      }

+      p = ch_ltoa(p, l, 10);

+      break;

+#if CHPRINTF_USE_FLOAT

+    case 'f':

+      f = (float) va_arg(ap, double);

+      if (f < 0) {

+        *p++ = '-';

+        f = -f;

+      }

+      p = ftoa(p, f, 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 (is_long)

+        l = va_arg(ap, unsigned long);

+      else

+        l = va_arg(ap, unsigned int);

+      p = ch_ltoa(p, l, c);

+      break;

+    default:

+      *p++ = c;

+      break;

+    }

+    i = (int)(p - s);

+    if ((width -= i) < 0)

+      width = 0;

+    if (left_align == FALSE)

+      width = -width;

+    if (width < 0) {

+      if (*s == '-' && filler == '0') {

+        chSequentialStreamPut(chp, (uint8_t)*s++);

+        n++;

+        i--;

+      }

+      do {

+        chSequentialStreamPut(chp, (uint8_t)filler);

+        n++;

+      } while (++width != 0);

+    }

+    while (--i >= 0) {

+      chSequentialStreamPut(chp, (uint8_t)*s++);

+      n++;

+    }

+

+    while (width) {

+      chSequentialStreamPut(chp, (uint8_t)filler);

+      n++;

+      width--;

+    }

+  }

+}

+

+/**

+ * @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

+ */

+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 *)&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;

+}

+

+/** @} */

diff --git a/os/hal/lib/streams/chprintf.h b/os/hal/lib/streams/chprintf.h
new file mode 100644
index 000000000..030640aa5
--- /dev/null
+++ b/os/hal/lib/streams/chprintf.h
@@ -0,0 +1,82 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 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.

+*/

+

+/**

+ * @file    chprintf.h

+ * @brief   Mini printf-like functionality.

+ *

+ * @addtogroup chprintf

+ * @{

+ */

+

+#ifndef _CHPRINTF_H_

+#define _CHPRINTF_H_

+

+#include <stdarg.h>

+

+/**

+ * @brief   Float type support.

+ */

+#if !defined(CHPRINTF_USE_FLOAT) || defined(__DOXYGEN__)

+#define CHPRINTF_USE_FLOAT          FALSE

+#endif

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+  int chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap);

+  int chsnprintf(char *str, size_t size, const char *fmt, ...);

+#ifdef __cplusplus

+}

+#endif

+

+/**

+ * @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

+ */

+static inline 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 /* _CHPRINTF_H_ */

+

+/** @} */

diff --git a/os/hal/lib/streams/memstreams.c b/os/hal/lib/streams/memstreams.c
new file mode 100644
index 000000000..71401f43e
--- /dev/null
+++ b/os/hal/lib/streams/memstreams.c
@@ -0,0 +1,113 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 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.

+*/

+

+/**

+ * @file    memstreams.c

+ * @brief   Memory streams code.

+ *

+ * @addtogroup memory_streams

+ * @{

+ */

+

+#include <string.h>

+

+#include "hal.h"

+#include "memstreams.h"

+

+/*===========================================================================*/

+/* Driver local definitions.                                                 */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver exported variables.                                                */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver local variables.                                                   */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver local functions.                                                   */

+/*===========================================================================*/

+

+static size_t writes(void *ip, const uint8_t *bp, size_t n) {

+  MemoryStream *msp = ip;

+

+  if (msp->size - msp->eos < n)

+    n = msp->size - msp->eos;

+  memcpy(msp->buffer + msp->eos, bp, n);

+  msp->eos += n;

+  return n;

+}

+

+static size_t reads(void *ip, uint8_t *bp, size_t n) {

+  MemoryStream *msp = ip;

+

+  if (msp->eos - msp->offset < n)

+    n = msp->eos - msp->offset;

+  memcpy(bp, msp->buffer + msp->offset, n);

+  msp->offset += n;

+  return n;

+}

+

+static msg_t put(void *ip, uint8_t b) {

+  MemoryStream *msp = ip;

+

+  if (msp->size - msp->eos <= 0)

+    return MSG_RESET;

+  *(msp->buffer + msp->eos) = b;

+  msp->eos += 1;

+  return MSG_OK;

+}

+

+static msg_t get(void *ip) {

+  uint8_t b;

+  MemoryStream *msp = ip;

+

+  if (msp->eos - msp->offset <= 0)

+    return MSG_RESET;

+  b = *(msp->buffer + msp->offset);

+  msp->offset += 1;

+  return b;

+}

+

+static const struct MemStreamVMT vmt = {writes, reads, put, get};

+

+/*===========================================================================*/

+/* Driver exported functions.                                                */

+/*===========================================================================*/

+

+/**

+ * @brief   Memory stream object initialization.

+ *

+ * @param[out] msp      pointer to the @p MemoryStream object to be initialized

+ * @param[in] buffer    pointer to the memory buffer for the memory stream

+ * @param[in] size      total size of the memory stream buffer

+ * @param[in] eos       initial End Of Stream offset. Normally you need to

+ *                      put this to zero for RAM buffers or equal to @p size

+ *                      for ROM streams.

+ */

+void msObjectInit(MemoryStream *msp, uint8_t *buffer,

+                  size_t size, size_t eos) {

+

+  msp->vmt    = &vmt;

+  msp->buffer = buffer;

+  msp->size   = size;

+  msp->eos    = eos;

+  msp->offset = 0;

+}

+

+/** @} */

diff --git a/os/hal/lib/streams/memstreams.h b/os/hal/lib/streams/memstreams.h
new file mode 100644
index 000000000..5d0072e2e
--- /dev/null
+++ b/os/hal/lib/streams/memstreams.h
@@ -0,0 +1,95 @@
+/*

+    ChibiOS - Copyright (C) 2006..2015 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.

+*/

+

+/**

+ * @file    memstreams.h

+ * @brief   Memory streams structures and macros.

+ 

+ * @addtogroup memory_streams

+ * @{

+ */

+

+#ifndef _MEMSTREAMS_H_

+#define _MEMSTREAMS_H_

+

+/*===========================================================================*/

+/* Driver constants.                                                         */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver pre-compile time settings.                                         */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Derived constants and error checks.                                       */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver data structures and types.                                         */

+/*===========================================================================*/

+

+/**

+ * @brief   @p MemStream specific data.

+ */

+#define _memory_stream_data                                                 \

+  _base_sequential_stream_data                                              \

+  /* Pointer to the stream buffer.*/                                        \

+  uint8_t               *buffer;                                            \

+  /* Size of the stream.*/                                                  \

+  size_t                size;                                               \

+  /* Current end of stream.*/                                               \

+  size_t                eos;                                                \

+  /* Current read offset.*/                                                 \

+  size_t                offset;

+

+/**

+ * @brief   @p MemStream virtual methods table.

+ */

+struct MemStreamVMT {

+  _base_sequential_stream_methods

+};

+

+/**

+ * @extends BaseSequentialStream

+ *

+ * @brief Memory stream object.

+ */

+typedef struct {

+  /** @brief Virtual Methods Table.*/

+  const struct MemStreamVMT *vmt;

+  _memory_stream_data

+} MemoryStream;

+

+/*===========================================================================*/

+/* Driver macros.                                                            */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* External declarations.                                                    */

+/*===========================================================================*/

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+  void msObjectInit(MemoryStream *msp, uint8_t *buffer,

+                    size_t size, size_t eos);

+#ifdef __cplusplus

+}

+#endif

+

+#endif /* _MEMSTREAMS_H_ */

+

+/** @} */