From a48d0863baa5288621baaefa208b5710d12414fa Mon Sep 17 00:00:00 2001 From: Giovanni Di Sirio Date: Fri, 6 Feb 2015 13:47:47 +0000 Subject: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@7667 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- os/hal/lib/streams/chprintf.c | 337 ++++++++++++++++++++++++++++++++++++++++ os/hal/lib/streams/chprintf.h | 82 ++++++++++ os/hal/lib/streams/memstreams.c | 113 ++++++++++++++ os/hal/lib/streams/memstreams.h | 95 +++++++++++ 4 files changed, 627 insertions(+) create mode 100644 os/hal/lib/streams/chprintf.c create mode 100644 os/hal/lib/streams/chprintf.h create mode 100644 os/hal/lib/streams/memstreams.c create mode 100644 os/hal/lib/streams/memstreams.h (limited to 'os/hal/lib') 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: + * - x hexadecimal integer. + * - X hexadecimal long. + * - o octal integer. + * - O octal long. + * - d decimal signed integer. + * - D decimal signed long. + * - u decimal unsigned integer. + * - U decimal unsigned long. + * - c character. + * - s 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: + * - x hexadecimal integer. + * - X hexadecimal long. + * - o octal integer. + * - O octal long. + * - d decimal signed integer. + * - D decimal signed long. + * - u decimal unsigned integer. + * - U decimal unsigned long. + * - c character. + * - s 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 + +/** + * @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: + * - x hexadecimal integer. + * - X hexadecimal long. + * - o octal integer. + * - O octal long. + * - d decimal signed integer. + * - D decimal signed long. + * - u decimal unsigned integer. + * - U decimal unsigned long. + * - c character. + * - s 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 + +#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_ */ + +/** @} */ -- cgit v1.2.3