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 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 337 insertions(+) create mode 100644 os/hal/lib/streams/chprintf.c (limited to 'os/hal/lib/streams/chprintf.c') 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; +} + +/** @} */ -- cgit v1.2.3