--- /dev/null
+/*
+ * Copyright 2017 NXP
+ * All rights reserved.
+ *
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+#include <math.h>
+#include <stdarg.h>
+#include <stdlib.h>
+#include "fsl_str.h"
+#include "fsl_debug_console_conf.h"
+
+/*******************************************************************************
+ * Definitions
+ ******************************************************************************/
+
+/*! @brief The overflow value.*/
+#ifndef HUGE_VAL
+#define HUGE_VAL (99.e99)
+#endif /* HUGE_VAL */
+
+#if SCANF_FLOAT_ENABLE
+static double fnum = 0.0;
+#endif /* SCANF_FLOAT_ENABLE */
+
+#if PRINTF_ADVANCED_ENABLE
+/*! @brief Specification modifier flags for printf. */
+enum _debugconsole_printf_flag
+{
+ kPRINTF_Minus = 0x01U, /*!< Minus FLag. */
+ kPRINTF_Plus = 0x02U, /*!< Plus Flag. */
+ kPRINTF_Space = 0x04U, /*!< Space Flag. */
+ kPRINTF_Zero = 0x08U, /*!< Zero Flag. */
+ kPRINTF_Pound = 0x10U, /*!< Pound Flag. */
+ kPRINTF_LengthChar = 0x20U, /*!< Length: Char Flag. */
+ kPRINTF_LengthShortInt = 0x40U, /*!< Length: Short Int Flag. */
+ kPRINTF_LengthLongInt = 0x80U, /*!< Length: Long Int Flag. */
+ kPRINTF_LengthLongLongInt = 0x100U, /*!< Length: Long Long Int Flag. */
+};
+#endif /* PRINTF_ADVANCED_ENABLE */
+
+/*! @brief Specification modifier flags for scanf. */
+enum _debugconsole_scanf_flag
+{
+ kSCANF_Suppress = 0x2U, /*!< Suppress Flag. */
+ kSCANF_DestMask = 0x7cU, /*!< Destination Mask. */
+ kSCANF_DestChar = 0x4U, /*!< Destination Char Flag. */
+ kSCANF_DestString = 0x8U, /*!< Destination String FLag. */
+ kSCANF_DestSet = 0x10U, /*!< Destination Set Flag. */
+ kSCANF_DestInt = 0x20U, /*!< Destination Int Flag. */
+ kSCANF_DestFloat = 0x30U, /*!< Destination Float Flag. */
+ kSCANF_LengthMask = 0x1f00U, /*!< Length Mask Flag. */
+#if SCANF_ADVANCED_ENABLE
+ kSCANF_LengthChar = 0x100U, /*!< Length Char Flag. */
+ kSCANF_LengthShortInt = 0x200U, /*!< Length ShortInt Flag. */
+ kSCANF_LengthLongInt = 0x400U, /*!< Length LongInt Flag. */
+ kSCANF_LengthLongLongInt = 0x800U, /*!< Length LongLongInt Flag. */
+#endif /* SCANF_ADVANCED_ENABLE */
+#if PRINTF_FLOAT_ENABLE
+ kSCANF_LengthLongLongDouble = 0x1000U, /*!< Length LongLongDuoble Flag. */
+#endif /*PRINTF_FLOAT_ENABLE */
+ kSCANF_TypeSinged = 0x2000U, /*!< TypeSinged Flag. */
+};
+
+/*! @brief Keil: suppress ellipsis warning in va_arg usage below. */
+#if defined(__CC_ARM)
+#pragma diag_suppress 1256
+#endif /* __CC_ARM */
+
+/*******************************************************************************
+ * Prototypes
+ ******************************************************************************/
+/*!
+ * @brief Scanline function which ignores white spaces.
+ *
+ * @param[in] s The address of the string pointer to update.
+ * @return String without white spaces.
+ */
+static uint32_t ScanIgnoreWhiteSpace(const char **s);
+
+/*!
+ * @brief Converts a radix number to a string and return its length.
+ *
+ * @param[in] numstr Converted string of the number.
+ * @param[in] nump Pointer to the number.
+ * @param[in] neg Polarity of the number.
+ * @param[in] radix The radix to be converted to.
+ * @param[in] use_caps Used to identify %x/X output format.
+
+ * @return Length of the converted string.
+ */
+static int32_t ConvertRadixNumToString(char *numstr, void *nump, int32_t neg, int32_t radix, bool use_caps);
+
+#if PRINTF_FLOAT_ENABLE
+/*!
+ * @brief Converts a floating radix number to a string and return its length.
+ *
+ * @param[in] numstr Converted string of the number.
+ * @param[in] nump Pointer to the number.
+ * @param[in] radix The radix to be converted to.
+ * @param[in] precision_width Specify the precision width.
+
+ * @return Length of the converted string.
+ */
+static int32_t ConvertFloatRadixNumToString(char *numstr, void *nump, int32_t radix, uint32_t precision_width);
+#endif /* PRINTF_FLOAT_ENABLE */
+
+/*!
+*
+ */
+double modf(double input_dbl, double *intpart_ptr);
+
+/*************Code for process formatted data*******************************/
+
+static uint32_t ScanIgnoreWhiteSpace(const char **s)
+{
+ uint8_t count = 0;
+ uint8_t c;
+
+ c = **s;
+ while ((c == ' ') || (c == '\t') || (c == '\n') || (c == '\r') || (c == '\v') || (c == '\f'))
+ {
+ count++;
+ (*s)++;
+ c = **s;
+ }
+ return count;
+}
+
+static int32_t ConvertRadixNumToString(char *numstr, void *nump, int32_t neg, int32_t radix, bool use_caps)
+{
+#if PRINTF_ADVANCED_ENABLE
+ int64_t a;
+ int64_t b;
+ int64_t c;
+
+ uint64_t ua;
+ uint64_t ub;
+ uint64_t uc;
+#else
+ int32_t a;
+ int32_t b;
+ int32_t c;
+
+ uint32_t ua;
+ uint32_t ub;
+ uint32_t uc;
+#endif /* PRINTF_ADVANCED_ENABLE */
+
+ int32_t nlen;
+ char *nstrp;
+
+ nlen = 0;
+ nstrp = numstr;
+ *nstrp++ = '\0';
+
+ if (neg)
+ {
+#if PRINTF_ADVANCED_ENABLE
+ a = *(int64_t *)nump;
+#else
+ a = *(int32_t *)nump;
+#endif /* PRINTF_ADVANCED_ENABLE */
+ if (a == 0)
+ {
+ *nstrp = '0';
+ ++nlen;
+ return nlen;
+ }
+ while (a != 0)
+ {
+#if PRINTF_ADVANCED_ENABLE
+ b = (int64_t)a / (int64_t)radix;
+ c = (int64_t)a - ((int64_t)b * (int64_t)radix);
+ if (c < 0)
+ {
+ uc = (uint64_t)c;
+ c = (int64_t)(~uc) + 1 + '0';
+ }
+#else
+ b = a / radix;
+ c = a - (b * radix);
+ if (c < 0)
+ {
+ uc = (uint32_t)c;
+ c = (uint32_t)(~uc) + 1 + '0';
+ }
+#endif /* PRINTF_ADVANCED_ENABLE */
+ else
+ {
+ c = c + '0';
+ }
+ a = b;
+ *nstrp++ = (char)c;
+ ++nlen;
+ }
+ }
+ else
+ {
+#if PRINTF_ADVANCED_ENABLE
+ ua = *(uint64_t *)nump;
+#else
+ ua = *(uint32_t *)nump;
+#endif /* PRINTF_ADVANCED_ENABLE */
+ if (ua == 0)
+ {
+ *nstrp = '0';
+ ++nlen;
+ return nlen;
+ }
+ while (ua != 0)
+ {
+#if PRINTF_ADVANCED_ENABLE
+ ub = (uint64_t)ua / (uint64_t)radix;
+ uc = (uint64_t)ua - ((uint64_t)ub * (uint64_t)radix);
+#else
+ ub = ua / (uint32_t)radix;
+ uc = ua - (ub * (uint32_t)radix);
+#endif /* PRINTF_ADVANCED_ENABLE */
+
+ if (uc < 10)
+ {
+ uc = uc + '0';
+ }
+ else
+ {
+ uc = uc - 10 + (use_caps ? 'A' : 'a');
+ }
+ ua = ub;
+ *nstrp++ = (char)uc;
+ ++nlen;
+ }
+ }
+ return nlen;
+}
+
+#if PRINTF_FLOAT_ENABLE
+static int32_t ConvertFloatRadixNumToString(char *numstr, void *nump, int32_t radix, uint32_t precision_width)
+{
+ int32_t a;
+ int32_t b;
+ int32_t c;
+ int32_t i;
+ uint32_t uc;
+ double fa;
+ double dc;
+ double fb;
+ double r;
+ double fractpart;
+ double intpart;
+
+ int32_t nlen;
+ char *nstrp;
+ nlen = 0;
+ nstrp = numstr;
+ *nstrp++ = '\0';
+ r = *(double *)nump;
+ if (!r)
+ {
+ *nstrp = '0';
+ ++nlen;
+ return nlen;
+ }
+ fractpart = modf((double)r, (double *)&intpart);
+ /* Process fractional part. */
+ for (i = 0; i < precision_width; i++)
+ {
+ fractpart *= radix;
+ }
+ if (r >= 0)
+ {
+ fa = fractpart + (double)0.5;
+ if (fa >= pow(10, precision_width))
+ {
+ intpart++;
+ }
+ }
+ else
+ {
+ fa = fractpart - (double)0.5;
+ if (fa <= -pow(10, precision_width))
+ {
+ intpart--;
+ }
+ }
+ for (i = 0; i < precision_width; i++)
+ {
+ fb = fa / (int32_t)radix;
+ dc = (fa - (int64_t)fb * (int32_t)radix);
+ c = (int32_t)dc;
+ if (c < 0)
+ {
+ uc = (uint32_t)c;
+ c = (int32_t)(~uc) + 1 + '0';
+ }
+ else
+ {
+ c = c + '0';
+ }
+ fa = fb;
+ *nstrp++ = (char)c;
+ ++nlen;
+ }
+ *nstrp++ = (char)'.';
+ ++nlen;
+ a = (int32_t)intpart;
+ if (a == 0)
+ {
+ *nstrp++ = '0';
+ ++nlen;
+ }
+ else
+ {
+ while (a != 0)
+ {
+ b = (int32_t)a / (int32_t)radix;
+ c = (int32_t)a - ((int32_t)b * (int32_t)radix);
+ if (c < 0)
+ {
+ uc = (uint32_t)c;
+ c = (int32_t)(~uc) + 1 + '0';
+ }
+ else
+ {
+ c = c + '0';
+ }
+ a = b;
+ *nstrp++ = (char)c;
+ ++nlen;
+ }
+ }
+ return nlen;
+}
+#endif /* PRINTF_FLOAT_ENABLE */
+
+int StrFormatPrintf(const char *fmt, va_list ap, char *buf, printfCb cb)
+{
+ /* va_list ap; */
+ char *p;
+ int32_t c;
+
+ char vstr[33];
+ char *vstrp = NULL;
+ int32_t vlen = 0;
+
+ int32_t done;
+ int32_t count = 0;
+
+ uint32_t field_width;
+ uint32_t precision_width;
+ char *sval;
+ int32_t cval;
+ bool use_caps;
+ uint8_t radix = 0;
+
+#if PRINTF_ADVANCED_ENABLE
+ uint32_t flags_used;
+ int32_t schar, dschar;
+ int64_t ival;
+ uint64_t uval = 0;
+ bool valid_precision_width;
+#else
+ int32_t ival;
+ uint32_t uval = 0;
+#endif /* PRINTF_ADVANCED_ENABLE */
+
+#if PRINTF_FLOAT_ENABLE
+ double fval;
+#endif /* PRINTF_FLOAT_ENABLE */
+
+ /* Start parsing apart the format string and display appropriate formats and data. */
+ for (p = (char *)fmt; (c = *p) != 0; p++)
+ {
+ /*
+ * All formats begin with a '%' marker. Special chars like
+ * '\n' or '\t' are normally converted to the appropriate
+ * character by the __compiler__. Thus, no need for this
+ * routine to account for the '\' character.
+ */
+ if (c != '%')
+ {
+ cb(buf, &count, c, 1);
+ /* By using 'continue', the next iteration of the loop is used, skipping the code that follows. */
+ continue;
+ }
+
+ use_caps = true;
+
+#if PRINTF_ADVANCED_ENABLE
+ /* First check for specification modifier flags. */
+ flags_used = 0;
+ done = false;
+ while (!done)
+ {
+ switch (*++p)
+ {
+ case '-':
+ flags_used |= kPRINTF_Minus;
+ break;
+ case '+':
+ flags_used |= kPRINTF_Plus;
+ break;
+ case ' ':
+ flags_used |= kPRINTF_Space;
+ break;
+ case '0':
+ flags_used |= kPRINTF_Zero;
+ break;
+ case '#':
+ flags_used |= kPRINTF_Pound;
+ break;
+ default:
+ /* We've gone one char too far. */
+ --p;
+ done = true;
+ break;
+ }
+ }
+#endif /* PRINTF_ADVANCED_ENABLE */
+
+ /* Next check for minimum field width. */
+ field_width = 0;
+ done = false;
+ while (!done)
+ {
+ c = *++p;
+ if ((c >= '0') && (c <= '9'))
+ {
+ field_width = (field_width * 10) + (c - '0');
+ }
+#if PRINTF_ADVANCED_ENABLE
+ else if (c == '*')
+ {
+ field_width = (uint32_t)va_arg(ap, uint32_t);
+ }
+#endif /* PRINTF_ADVANCED_ENABLE */
+ else
+ {
+ /* We've gone one char too far. */
+ --p;
+ done = true;
+ }
+ }
+ /* Next check for the width and precision field separator. */
+ precision_width = 6;
+#if PRINTF_ADVANCED_ENABLE
+ valid_precision_width = false;
+#endif /* PRINTF_ADVANCED_ENABLE */
+ if (*++p == '.')
+ {
+ /* Must get precision field width, if present. */
+ precision_width = 0;
+ done = false;
+ while (!done)
+ {
+ c = *++p;
+ if ((c >= '0') && (c <= '9'))
+ {
+ precision_width = (precision_width * 10) + (c - '0');
+#if PRINTF_ADVANCED_ENABLE
+ valid_precision_width = true;
+#endif /* PRINTF_ADVANCED_ENABLE */
+ }
+#if PRINTF_ADVANCED_ENABLE
+ else if (c == '*')
+ {
+ precision_width = (uint32_t)va_arg(ap, uint32_t);
+ valid_precision_width = true;
+ }
+#endif /* PRINTF_ADVANCED_ENABLE */
+ else
+ {
+ /* We've gone one char too far. */
+ --p;
+ done = true;
+ }
+ }
+ }
+ else
+ {
+ /* We've gone one char too far. */
+ --p;
+ }
+#if PRINTF_ADVANCED_ENABLE
+ /*
+ * Check for the length modifier.
+ */
+ switch (/* c = */ *++p)
+ {
+ case 'h':
+ if (*++p != 'h')
+ {
+ flags_used |= kPRINTF_LengthShortInt;
+ --p;
+ }
+ else
+ {
+ flags_used |= kPRINTF_LengthChar;
+ }
+ break;
+ case 'l':
+ if (*++p != 'l')
+ {
+ flags_used |= kPRINTF_LengthLongInt;
+ --p;
+ }
+ else
+ {
+ flags_used |= kPRINTF_LengthLongLongInt;
+ }
+ break;
+ default:
+ /* we've gone one char too far */
+ --p;
+ break;
+ }
+#endif /* PRINTF_ADVANCED_ENABLE */
+ /* Now we're ready to examine the format. */
+ c = *++p;
+ {
+ if ((c == 'd') || (c == 'i') || (c == 'f') || (c == 'F') || (c == 'x') || (c == 'X') || (c == 'o') ||
+ (c == 'b') || (c == 'p') || (c == 'u'))
+ {
+ if ((c == 'd') || (c == 'i'))
+ {
+#if PRINTF_ADVANCED_ENABLE
+ if (flags_used & kPRINTF_LengthLongLongInt)
+ {
+ ival = (int64_t)va_arg(ap, int64_t);
+ }
+ else
+#endif /* PRINTF_ADVANCED_ENABLE */
+ {
+ ival = (int32_t)va_arg(ap, int32_t);
+ }
+ vlen = ConvertRadixNumToString(vstr, &ival, true, 10, use_caps);
+ vstrp = &vstr[vlen];
+#if PRINTF_ADVANCED_ENABLE
+ if (ival < 0)
+ {
+ schar = '-';
+ ++vlen;
+ }
+ else
+ {
+ if (flags_used & kPRINTF_Plus)
+ {
+ schar = '+';
+ ++vlen;
+ }
+ else
+ {
+ if (flags_used & kPRINTF_Space)
+ {
+ schar = ' ';
+ ++vlen;
+ }
+ else
+ {
+ schar = 0;
+ }
+ }
+ }
+ dschar = false;
+ /* Do the ZERO pad. */
+ if (flags_used & kPRINTF_Zero)
+ {
+ if (schar)
+ {
+ cb(buf, &count, schar, 1);
+ }
+ dschar = true;
+
+ cb(buf, &count, '0', field_width - vlen);
+ vlen = field_width;
+ }
+ else
+ {
+ if (!(flags_used & kPRINTF_Minus))
+ {
+ cb(buf, &count, ' ', field_width - vlen);
+ if (schar)
+ {
+ cb(buf, &count, schar, 1);
+ }
+ dschar = true;
+ }
+ }
+ /* The string was built in reverse order, now display in correct order. */
+ if ((!dschar) && schar)
+ {
+ cb(buf, &count, schar, 1);
+ }
+#endif /* PRINTF_ADVANCED_ENABLE */
+ }
+
+#if PRINTF_FLOAT_ENABLE
+ if ((c == 'f') || (c == 'F'))
+ {
+ fval = (double)va_arg(ap, double);
+ vlen = ConvertFloatRadixNumToString(vstr, &fval, 10, precision_width);
+ vstrp = &vstr[vlen];
+
+#if PRINTF_ADVANCED_ENABLE
+ if (fval < 0)
+ {
+ schar = '-';
+ ++vlen;
+ }
+ else
+ {
+ if (flags_used & kPRINTF_Plus)
+ {
+ schar = '+';
+ ++vlen;
+ }
+ else
+ {
+ if (flags_used & kPRINTF_Space)
+ {
+ schar = ' ';
+ ++vlen;
+ }
+ else
+ {
+ schar = 0;
+ }
+ }
+ }
+ dschar = false;
+ if (flags_used & kPRINTF_Zero)
+ {
+ if (schar)
+ {
+ cb(buf, &count, schar, 1);
+ }
+ dschar = true;
+ cb(buf, &count, '0', field_width - vlen);
+ vlen = field_width;
+ }
+ else
+ {
+ if (!(flags_used & kPRINTF_Minus))
+ {
+ cb(buf, &count, ' ', field_width - vlen);
+ if (schar)
+ {
+ cb(buf, &count, schar, 1);
+ }
+ dschar = true;
+ }
+ }
+ if ((!dschar) && schar)
+ {
+ cb(buf, &count, schar, 1);
+ }
+#endif /* PRINTF_ADVANCED_ENABLE */
+ }
+#endif /* PRINTF_FLOAT_ENABLE */
+ if ((c == 'X') || (c == 'x'))
+ {
+ if (c == 'x')
+ {
+ use_caps = false;
+ }
+#if PRINTF_ADVANCED_ENABLE
+ if (flags_used & kPRINTF_LengthLongLongInt)
+ {
+ uval = (uint64_t)va_arg(ap, uint64_t);
+ }
+ else
+#endif /* PRINTF_ADVANCED_ENABLE */
+ {
+ uval = (uint32_t)va_arg(ap, uint32_t);
+ }
+ vlen = ConvertRadixNumToString(vstr, &uval, false, 16, use_caps);
+ vstrp = &vstr[vlen];
+
+#if PRINTF_ADVANCED_ENABLE
+ dschar = false;
+ if (flags_used & kPRINTF_Zero)
+ {
+ if (flags_used & kPRINTF_Pound)
+ {
+ cb(buf, &count, '0', 1);
+ cb(buf, &count, (use_caps ? 'X' : 'x'), 1);
+ dschar = true;
+ }
+ cb(buf, &count, '0', field_width - vlen);
+ vlen = field_width;
+ }
+ else
+ {
+ if (!(flags_used & kPRINTF_Minus))
+ {
+ if (flags_used & kPRINTF_Pound)
+ {
+ vlen += 2;
+ }
+ cb(buf, &count, ' ', field_width - vlen);
+ if (flags_used & kPRINTF_Pound)
+ {
+ cb(buf, &count, '0', 1);
+ cb(buf, &count, (use_caps ? 'X' : 'x'), 1);
+ dschar = true;
+ }
+ }
+ }
+
+ if ((flags_used & kPRINTF_Pound) && (!dschar))
+ {
+ cb(buf, &count, '0', 1);
+ cb(buf, &count, (use_caps ? 'X' : 'x'), 1);
+ vlen += 2;
+ }
+#endif /* PRINTF_ADVANCED_ENABLE */
+ }
+ if ((c == 'o') || (c == 'b') || (c == 'p') || (c == 'u'))
+ {
+#if PRINTF_ADVANCED_ENABLE
+ if (flags_used & kPRINTF_LengthLongLongInt)
+ {
+ uval = (uint64_t)va_arg(ap, uint64_t);
+ }
+ else
+#endif /* PRINTF_ADVANCED_ENABLE */
+ {
+ uval = (uint32_t)va_arg(ap, uint32_t);
+ }
+ switch (c)
+ {
+ case 'o':
+ radix = 8;
+ break;
+ case 'b':
+ radix = 2;
+ break;
+ case 'p':
+ radix = 16;
+ break;
+ case 'u':
+ radix = 10;
+ break;
+ default:
+ break;
+ }
+ vlen = ConvertRadixNumToString(vstr, &uval, false, radix, use_caps);
+ vstrp = &vstr[vlen];
+#if PRINTF_ADVANCED_ENABLE
+ if (flags_used & kPRINTF_Zero)
+ {
+ cb(buf, &count, '0', field_width - vlen);
+ vlen = field_width;
+ }
+ else
+ {
+ if (!(flags_used & kPRINTF_Minus))
+ {
+ cb(buf, &count, ' ', field_width - vlen);
+ }
+ }
+#endif /* PRINTF_ADVANCED_ENABLE */
+ }
+#if !PRINTF_ADVANCED_ENABLE
+ cb(buf, &count, ' ', field_width - vlen);
+#endif /* !PRINTF_ADVANCED_ENABLE */
+ if (vstrp != NULL)
+ {
+ while (*vstrp)
+ {
+ cb(buf, &count, *vstrp--, 1);
+ }
+ }
+#if PRINTF_ADVANCED_ENABLE
+ if (flags_used & kPRINTF_Minus)
+ {
+ cb(buf, &count, ' ', field_width - vlen);
+ }
+#endif /* PRINTF_ADVANCED_ENABLE */
+ }
+ else if (c == 'c')
+ {
+ cval = (char)va_arg(ap, uint32_t);
+ cb(buf, &count, cval, 1);
+ }
+ else if (c == 's')
+ {
+ sval = (char *)va_arg(ap, char *);
+ if (sval)
+ {
+#if PRINTF_ADVANCED_ENABLE
+ if (valid_precision_width)
+ {
+ vlen = precision_width;
+ }
+ else
+ {
+ vlen = strlen(sval);
+ }
+#else
+ vlen = strlen(sval);
+#endif /* PRINTF_ADVANCED_ENABLE */
+#if PRINTF_ADVANCED_ENABLE
+ if (!(flags_used & kPRINTF_Minus))
+#endif /* PRINTF_ADVANCED_ENABLE */
+ {
+ cb(buf, &count, ' ', field_width - vlen);
+ }
+
+#if PRINTF_ADVANCED_ENABLE
+ if (valid_precision_width)
+ {
+ while ((*sval) && (vlen > 0))
+ {
+ cb(buf, &count, *sval++, 1);
+ vlen--;
+ }
+ /* In case that vlen sval is shorter than vlen */
+ vlen = precision_width - vlen;
+ }
+ else
+ {
+#endif /* PRINTF_ADVANCED_ENABLE */
+ while (*sval)
+ {
+ cb(buf, &count, *sval++, 1);
+ }
+#if PRINTF_ADVANCED_ENABLE
+ }
+#endif /* PRINTF_ADVANCED_ENABLE */
+
+#if PRINTF_ADVANCED_ENABLE
+ if (flags_used & kPRINTF_Minus)
+ {
+ cb(buf, &count, ' ', field_width - vlen);
+ }
+#endif /* PRINTF_ADVANCED_ENABLE */
+ }
+ }
+ else
+ {
+ cb(buf, &count, c, 1);
+ }
+ }
+ }
+
+ return count;
+}
+
+int StrFormatScanf(const char *line_ptr, char *format, va_list args_ptr)
+{
+ uint8_t base;
+ int8_t neg;
+ /* Identifier for the format string. */
+ char *c = format;
+ char temp;
+ char *buf;
+ /* Flag telling the conversion specification. */
+ uint32_t flag = 0;
+ /* Filed width for the matching input streams. */
+ uint32_t field_width;
+ /* How many arguments are assigned except the suppress. */
+ uint32_t nassigned = 0;
+ /* How many characters are read from the input streams. */
+ uint32_t n_decode = 0;
+
+ int32_t val;
+
+ const char *s;
+ /* Identifier for the input string. */
+ const char *p = line_ptr;
+
+ /* Return EOF error before any conversion. */
+ if (*p == '\0')
+ {
+ return -1;
+ }
+
+ /* Decode directives. */
+ while ((*c) && (*p))
+ {
+ /* Ignore all white-spaces in the format strings. */
+ if (ScanIgnoreWhiteSpace((const char **)&c))
+ {
+ n_decode += ScanIgnoreWhiteSpace(&p);
+ }
+ else if ((*c != '%') || ((*c == '%') && (*(c + 1) == '%')))
+ {
+ /* Ordinary characters. */
+ c++;
+ if (*p == *c)
+ {
+ n_decode++;
+ p++;
+ c++;
+ }
+ else
+ {
+ /* Match failure. Misalignment with C99, the unmatched characters need to be pushed back to stream.
+ * However, it is deserted now. */
+ break;
+ }
+ }
+ else
+ {
+ /* convernsion specification */
+ c++;
+ /* Reset. */
+ flag = 0;
+ field_width = 0;
+ base = 0;
+
+ /* Loop to get full conversion specification. */
+ while ((*c) && (!(flag & kSCANF_DestMask)))
+ {
+ switch (*c)
+ {
+#if SCANF_ADVANCED_ENABLE
+ case '*':
+ if (flag & kSCANF_Suppress)
+ {
+ /* Match failure. */
+ return nassigned;
+ }
+ flag |= kSCANF_Suppress;
+ c++;
+ break;
+ case 'h':
+ if (flag & kSCANF_LengthMask)
+ {
+ /* Match failure. */
+ return nassigned;
+ }
+
+ if (c[1] == 'h')
+ {
+ flag |= kSCANF_LengthChar;
+ c++;
+ }
+ else
+ {
+ flag |= kSCANF_LengthShortInt;
+ }
+ c++;
+ break;
+ case 'l':
+ if (flag & kSCANF_LengthMask)
+ {
+ /* Match failure. */
+ return nassigned;
+ }
+
+ if (c[1] == 'l')
+ {
+ flag |= kSCANF_LengthLongLongInt;
+ c++;
+ }
+ else
+ {
+ flag |= kSCANF_LengthLongInt;
+ }
+ c++;
+ break;
+#endif /* SCANF_ADVANCED_ENABLE */
+#if SCANF_FLOAT_ENABLE
+ case 'L':
+ if (flag & kSCANF_LengthMask)
+ {
+ /* Match failure. */
+ return nassigned;
+ }
+ flag |= kSCANF_LengthLongLongDouble;
+ c++;
+ break;
+#endif /* SCANF_FLOAT_ENABLE */
+ case '0':
+ case '1':
+ case '2':
+ case '3':
+ case '4':
+ case '5':
+ case '6':
+ case '7':
+ case '8':
+ case '9':
+ if (field_width)
+ {
+ /* Match failure. */
+ return nassigned;
+ }
+ do
+ {
+ field_width = field_width * 10 + *c - '0';
+ c++;
+ } while ((*c >= '0') && (*c <= '9'));
+ break;
+ case 'd':
+ base = 10;
+ flag |= kSCANF_TypeSinged;
+ flag |= kSCANF_DestInt;
+ c++;
+ break;
+ case 'u':
+ base = 10;
+ flag |= kSCANF_DestInt;
+ c++;
+ break;
+ case 'o':
+ base = 8;
+ flag |= kSCANF_DestInt;
+ c++;
+ break;
+ case 'x':
+ case 'X':
+ base = 16;
+ flag |= kSCANF_DestInt;
+ c++;
+ break;
+ case 'i':
+ base = 0;
+ flag |= kSCANF_DestInt;
+ c++;
+ break;
+#if SCANF_FLOAT_ENABLE
+ case 'a':
+ case 'A':
+ case 'e':
+ case 'E':
+ case 'f':
+ case 'F':
+ case 'g':
+ case 'G':
+ flag |= kSCANF_DestFloat;
+ c++;
+ break;
+#endif /* SCANF_FLOAT_ENABLE */
+ case 'c':
+ flag |= kSCANF_DestChar;
+ if (!field_width)
+ {
+ field_width = 1;
+ }
+ c++;
+ break;
+ case 's':
+ flag |= kSCANF_DestString;
+ c++;
+ break;
+ default:
+ return nassigned;
+ }
+ }
+
+ if (!(flag & kSCANF_DestMask))
+ {
+ /* Format strings are exhausted. */
+ return nassigned;
+ }
+
+ if (!field_width)
+ {
+ /* Large than length of a line. */
+ field_width = 99;
+ }
+
+ /* Matching strings in input streams and assign to argument. */
+ switch (flag & kSCANF_DestMask)
+ {
+ case kSCANF_DestChar:
+ s = (const char *)p;
+ buf = va_arg(args_ptr, char *);
+ while ((field_width--) && (*p))
+ {
+ if (!(flag & kSCANF_Suppress))
+ {
+ *buf++ = *p++;
+ }
+ else
+ {
+ p++;
+ }
+ n_decode++;
+ }
+
+ if ((!(flag & kSCANF_Suppress)) && (s != p))
+ {
+ nassigned++;
+ }
+ break;
+ case kSCANF_DestString:
+ n_decode += ScanIgnoreWhiteSpace(&p);
+ s = p;
+ buf = va_arg(args_ptr, char *);
+ while ((field_width--) && (*p != '\0') && (*p != ' ') && (*p != '\t') && (*p != '\n') &&
+ (*p != '\r') && (*p != '\v') && (*p != '\f'))
+ {
+ if (flag & kSCANF_Suppress)
+ {
+ p++;
+ }
+ else
+ {
+ *buf++ = *p++;
+ }
+ n_decode++;
+ }
+
+ if ((!(flag & kSCANF_Suppress)) && (s != p))
+ {
+ /* Add NULL to end of string. */
+ *buf = '\0';
+ nassigned++;
+ }
+ break;
+ case kSCANF_DestInt:
+ n_decode += ScanIgnoreWhiteSpace(&p);
+ s = p;
+ val = 0;
+ if ((base == 0) || (base == 16))
+ {
+ if ((s[0] == '0') && ((s[1] == 'x') || (s[1] == 'X')))
+ {
+ base = 16;
+ if (field_width >= 1)
+ {
+ p += 2;
+ n_decode += 2;
+ field_width -= 2;
+ }
+ }
+ }
+
+ if (base == 0)
+ {
+ if (s[0] == '0')
+ {
+ base = 8;
+ }
+ else
+ {
+ base = 10;
+ }
+ }
+
+ neg = 1;
+ switch (*p)
+ {
+ case '-':
+ neg = -1;
+ n_decode++;
+ p++;
+ field_width--;
+ break;
+ case '+':
+ neg = 1;
+ n_decode++;
+ p++;
+ field_width--;
+ break;
+ default:
+ break;
+ }
+
+ while ((*p) && (field_width--))
+ {
+ if ((*p <= '9') && (*p >= '0'))
+ {
+ temp = *p - '0';
+ }
+ else if ((*p <= 'f') && (*p >= 'a'))
+ {
+ temp = *p - 'a' + 10;
+ }
+ else if ((*p <= 'F') && (*p >= 'A'))
+ {
+ temp = *p - 'A' + 10;
+ }
+ else
+ {
+ temp = base;
+ }
+
+ if (temp >= base)
+ {
+ break;
+ }
+ else
+ {
+ val = base * val + temp;
+ }
+ p++;
+ n_decode++;
+ }
+ val *= neg;
+ if (!(flag & kSCANF_Suppress))
+ {
+#if SCANF_ADVANCED_ENABLE
+ switch (flag & kSCANF_LengthMask)
+ {
+ case kSCANF_LengthChar:
+ if (flag & kSCANF_TypeSinged)
+ {
+ *va_arg(args_ptr, signed char *) = (signed char)val;
+ }
+ else
+ {
+ *va_arg(args_ptr, unsigned char *) = (unsigned char)val;
+ }
+ break;
+ case kSCANF_LengthShortInt:
+ if (flag & kSCANF_TypeSinged)
+ {
+ *va_arg(args_ptr, signed short *) = (signed short)val;
+ }
+ else
+ {
+ *va_arg(args_ptr, unsigned short *) = (unsigned short)val;
+ }
+ break;
+ case kSCANF_LengthLongInt:
+ if (flag & kSCANF_TypeSinged)
+ {
+ *va_arg(args_ptr, signed long int *) = (signed long int)val;
+ }
+ else
+ {
+ *va_arg(args_ptr, unsigned long int *) = (unsigned long int)val;
+ }
+ break;
+ case kSCANF_LengthLongLongInt:
+ if (flag & kSCANF_TypeSinged)
+ {
+ *va_arg(args_ptr, signed long long int *) = (signed long long int)val;
+ }
+ else
+ {
+ *va_arg(args_ptr, unsigned long long int *) = (unsigned long long int)val;
+ }
+ break;
+ default:
+ /* The default type is the type int. */
+ if (flag & kSCANF_TypeSinged)
+ {
+ *va_arg(args_ptr, signed int *) = (signed int)val;
+ }
+ else
+ {
+ *va_arg(args_ptr, unsigned int *) = (unsigned int)val;
+ }
+ break;
+ }
+#else
+ /* The default type is the type int. */
+ if (flag & kSCANF_TypeSinged)
+ {
+ *va_arg(args_ptr, signed int *) = (signed int)val;
+ }
+ else
+ {
+ *va_arg(args_ptr, unsigned int *) = (unsigned int)val;
+ }
+#endif /* SCANF_ADVANCED_ENABLE */
+ nassigned++;
+ }
+ break;
+#if SCANF_FLOAT_ENABLE
+ case kSCANF_DestFloat:
+ n_decode += ScanIgnoreWhiteSpace(&p);
+ fnum = strtod(p, (char **)&s);
+
+ if ((fnum >= HUGE_VAL) || (fnum <= -HUGE_VAL))
+ {
+ break;
+ }
+
+ n_decode += (int)(s) - (int)(p);
+ p = s;
+ if (!(flag & kSCANF_Suppress))
+ {
+ if (flag & kSCANF_LengthLongLongDouble)
+ {
+ *va_arg(args_ptr, double *) = fnum;
+ }
+ else
+ {
+ *va_arg(args_ptr, float *) = (float)fnum;
+ }
+ nassigned++;
+ }
+ break;
+#endif /* SCANF_FLOAT_ENABLE */
+ default:
+ return nassigned;
+ }
+ }
+ }
+ return nassigned;
+}
projects / freertos / blobdiff