| /* |
| * Copyright (c) 2016, Texas Instruments Incorporated |
| * All rights reserved. |
| * |
| * Redistribution and use in source and binary forms, with or without |
| * modification, are permitted provided that the following conditions |
| * are met: |
| * |
| * * Redistributions of source code must retain the above copyright |
| * notice, this list of conditions and the following disclaimer. |
| * |
| * * 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. |
| * |
| * * Neither the name of Texas Instruments Incorporated 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 OWNER 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. |
| */ |
| /* |
| * ======== SystemP_freertos.c ======== |
| */ |
| #include <ti/drivers/dpl/SystemP.h> |
| |
| #include <stddef.h> |
| #include <stdint.h> |
| #include <stdbool.h> |
| #include <stdlib.h> |
| #include <string.h> |
| #include <stdarg.h> |
| |
| /* ----------------------------------------------------------------------------- |
| * Constants and macros |
| * ----------------------------------------------------------------------------- |
| */ |
| #ifndef MIN |
| # define MIN(n, m) (((n) > (m)) ? (m) : (n)) |
| #endif |
| |
| /* |
| * ======== OUTMAX ======== |
| * The maximum length of the output of a base 8 number produced by formatNum |
| * plus 5 to accomodate the decimal point and 4 digits after the decimal |
| * point. |
| */ |
| #define OUTMAX ((32 + 2) / 3) + 5 |
| #define PTRZPAD 8 |
| #define MAXARGS 5 |
| |
| /* ---------------------------------------------------------------------------- |
| * Type definitions |
| * ---------------------------------------------------------------------------- |
| */ |
| /* ParseData */ |
| typedef struct ParseData { |
| int width; |
| bool lFlag; |
| bool lJust; |
| int precis; |
| int len; |
| int zpad; |
| char *end; |
| char *ptr; |
| } ParseData; |
| |
| /* |
| * Maximum sized (un)signed integer that we'll format. |
| */ |
| typedef uint32_t UNum; |
| typedef int32_t INum; |
| typedef UNum UIntMax; |
| typedef INum IntMax; |
| |
| static int doPrint(char *buf, size_t n, const char *fmt, va_list va); |
| static char *formatNum(char *ptr, UIntMax un, int zpad, int base); |
| static void putChar(char **bufp, char c, size_t *n); |
| |
| /* |
| * ======== SystemP_snprintf ======== |
| */ |
| int SystemP_snprintf(char *buf, size_t n, const char *format,...) |
| { |
| va_list args; |
| int ret; |
| |
| va_start(args, format); |
| ret = doPrint(buf, n, format, args); |
| va_end(args); |
| |
| return (ret); |
| } |
| |
| /* |
| * ======== SystemP_snprintf_va ======== |
| */ |
| int SystemP_vsnprintf(char *buf, size_t n, const char *format, |
| va_list va) |
| { |
| int ret; |
| |
| ret = doPrint(buf, n, format, va); |
| return (ret); |
| } |
| |
| /* |
| * ======== doPrint ======== |
| */ |
| static int doPrint(char *buf, size_t n, const char *fmt, va_list va) |
| { |
| ParseData parse; |
| int base; |
| char c; |
| int res = 0; |
| char outbuf[OUTMAX]; |
| |
| if (fmt == (char *)NULL) { |
| return (res); |
| } |
| |
| while ((c = *fmt++) != '\0') { |
| if (c != '%') { |
| putChar(&buf, c, &n); |
| res++; |
| } |
| else { |
| c = *fmt++; |
| /* check for - flag (pad on right) */ |
| if (c == '-') { |
| parse.lJust = true; |
| c = *fmt++; |
| } |
| else { |
| parse.lJust = false; |
| } |
| /* check for leading 0 pad */ |
| if (c == '0') { |
| parse.zpad = 1; |
| c = *fmt++; |
| } |
| else { |
| parse.zpad = 0; |
| } |
| |
| /* allow optional field width/precision specification */ |
| parse.width = 0; |
| parse.precis = -1; |
| |
| /* note: dont use isdigit (very large for C30) */ |
| if (c == '*') { |
| /* Width is specified in argument, not in format string */ |
| parse.width = (int)va_arg(va, int); |
| |
| c = *fmt++; |
| if (parse.width < 0) { |
| parse.lJust = true; |
| parse.width = -parse.width; |
| } |
| } |
| else { |
| while (c >= '0' && c <= '9') { |
| parse.width = parse.width * 10 + c - '0'; |
| c = *fmt++; |
| } |
| } |
| |
| /* allow optional field precision specification */ |
| if (c == '.') { |
| parse.precis = 0; |
| c = *fmt++; |
| if (c == '*') { |
| /* Width specified in argument, not in format string */ |
| parse.precis = (int)va_arg(va, int); |
| |
| if (parse.precis < 0) { |
| parse.precis = 0; |
| } |
| c = *fmt++; |
| } |
| else { |
| while (c >= '0' && c <= '9') { |
| parse.precis = parse.precis * 10 + c - '0'; |
| c = *fmt++; |
| } |
| } |
| } |
| |
| /* setup for leading zero padding */ |
| if (parse.zpad) { |
| parse.zpad = parse.width; |
| } |
| |
| /* check for presence of l flag (e.g., %ld) */ |
| if (c == 'l' || c == 'L') { |
| parse.lFlag = true; |
| c = *fmt++; |
| } |
| else { |
| parse.lFlag = false; |
| } |
| |
| parse.ptr = outbuf; |
| parse.end = outbuf + OUTMAX; |
| parse.len = 0; |
| |
| if (c == 'd' || c == 'i') { |
| /* signed decimal */ |
| IntMax val = (IntMax)va_arg(va, int32_t); |
| |
| if (parse.precis > parse.zpad) { |
| parse.zpad = parse.precis; |
| } |
| parse.ptr = formatNum(parse.end, val, parse.zpad, -10); |
| parse.len = parse.end - parse.ptr; |
| } |
| /* use comma operator to optimize code generation! */ |
| else if (((base = 10), (c == 'u')) || /* unsigned decimal */ |
| ((base = 16), (c == 'x')) || /* unsigned hex */ |
| ((base = 8), (c == 'o'))) { /* unsigned octal */ |
| |
| UIntMax val = (UIntMax)va_arg(va, uint32_t) ; |
| |
| if (parse.precis > parse.zpad) { |
| parse.zpad = parse.precis; |
| } |
| parse.ptr = formatNum(parse.end, val, parse.zpad, base); |
| parse.len = parse.end - parse.ptr; |
| } |
| else if ((base = 16), (c == 'p')) { |
| parse.zpad = PTRZPAD; /* ptrs are 0 padded */ |
| parse.ptr = formatNum( |
| parse.end, |
| (UIntMax)va_arg(va, uint32_t), |
| parse.zpad, base); |
| *(--parse.ptr) = '@'; |
| parse.len = parse.end - parse.ptr; |
| } |
| else if (c == 'c') { |
| /* character */ |
| *parse.ptr = (char)va_arg(va, int); |
| parse.len = 1; |
| } |
| else if (c == 's') { |
| /* string */ |
| parse.ptr = (char *)va_arg(va, void *); |
| |
| /* substitute (null) for NULL pointer */ |
| if (parse.ptr == (char *)NULL) { |
| parse.ptr = "(null)"; |
| } |
| parse.len = strlen(parse.ptr); |
| if (parse.precis != -1 && parse.precis < parse.len) { |
| parse.len = parse.precis; |
| } |
| } |
| else if (c == 'f') { |
| double d, tmp; |
| bool negative = false; |
| UNum fract; |
| |
| d = va_arg(va, double); |
| |
| if (d < 0.0) { |
| d = -d; |
| negative = true; |
| parse.zpad--; |
| } |
| |
| /* |
| * Assumes four digits after decimal point. We are using a |
| * temporary double variable to force double-precision |
| * computations without using --fp_mode=strict flag. |
| * See the description of that flag in the compiler's doc |
| * for a further explanation. |
| */ |
| tmp = (d - (INum)d) * 1e4; |
| fract = (UNum)tmp; |
| |
| parse.ptr = formatNum(parse.end, fract, 4, 10); |
| *(--parse.ptr) = '.'; |
| |
| parse.len = parse.end - parse.ptr; |
| /* format integer part (right to left!) */ |
| parse.ptr = formatNum(parse.ptr, (INum)d, |
| parse.zpad - parse.len, 10); |
| if (negative) { |
| *(--parse.ptr) = '-'; |
| } |
| |
| parse.len = parse.end - parse.ptr; |
| } |
| |
| /* compute number of characters left in field */ |
| parse.width -= parse.len; |
| |
| if (!parse.lJust) { |
| /* pad with blanks on left */ |
| while (--parse.width >= 0) { |
| putChar(&buf, ' ', &n); |
| res++; |
| } |
| } |
| |
| /* output number, character or string */ |
| while (parse.len--) { |
| putChar(&buf, *parse.ptr++, &n); |
| res++; |
| } |
| /* pad with blanks on right */ |
| if (parse.lJust) { |
| while (--parse.width >= 0) { |
| putChar(&buf, ' ', &n); |
| res++; |
| } |
| } |
| } /* if */ |
| } /* while */ |
| |
| if (buf) { |
| *buf = '\0'; |
| } |
| |
| return (res); |
| } |
| |
| |
| /* |
| * ======== formatNum ======== |
| * Internal function |
| * |
| * Format unsigned long number in specified base, returning pointer to |
| * converted output. |
| * |
| * Note: ptr points PAST end of the buffer, and is decremented as digits |
| * are converted from right to left! |
| * |
| * Note: base is negative if n is signed else n unsigned! |
| * |
| * ptr - Pointer to the end of the working buffer where the string version |
| * of the number will be placed. |
| * un - The unsigned number to be formated |
| * base - The base to format the number into. TODO - signed? |
| */ |
| static char *formatNum(char *ptr, UIntMax un, int zpad, int base) |
| { |
| int i = 0; |
| char sign = 0; |
| |
| UIntMax n; |
| n = un; |
| |
| if (base < 0) { |
| /* handle signed long case */ |
| base = -base; |
| if ((IntMax)n < 0) { |
| n = -(IntMax)n; |
| |
| /* account for sign '-': ok since zpad is signed */ |
| --zpad; |
| sign = '-'; |
| } |
| } |
| |
| /* compute digits in number from right to left */ |
| do { |
| *(--ptr) = "0123456789abcdef"[(int)(n % base)]; |
| n = n / base; |
| ++i; |
| } while (n); |
| |
| /* pad with leading 0s on left */ |
| while (i < zpad) { |
| *(--ptr) = '0'; |
| ++i; |
| } |
| |
| /* add sign indicator */ |
| if (sign) { |
| *(--ptr) = sign; |
| } |
| return (ptr); |
| } |
| |
| /* |
| * ======== putChar ======== |
| * Write character `c` to the buffer and the buffer pointer. |
| * |
| * Keeps track of the number of characters written into the buffer by |
| * modifying bufsize `n`. Atmost, `n` - 1 characters are written. |
| */ |
| static void putChar(char **bufp, char c, size_t *n) |
| { |
| /* if the size == 1, don't write so we can '\0' terminate buffer */ |
| if ((*n) <= 1) { |
| return; |
| } |
| |
| /* decrement n to keep track of the number of chars written */ |
| (*n)--; |
| *((*bufp)++) = c; |
| } |