git.sur5r.net Git - freertos/blob - FreeRTOS/Demo/CORTEX_MPU_M33F_NXP_LPC55S69_MCUXpresso/NXP_Code/utilities/fsl_str.c

   1 /*\r
   2  * Copyright 2017 NXP\r
   3  * All rights reserved.\r
   4  *\r
   5  *\r
   6  * SPDX-License-Identifier: BSD-3-Clause\r
   7  *\r
   8  */\r
   9 #include <math.h>\r
  10 #include <stdarg.h>\r
  11 #include <stdlib.h>\r
  12 #include "fsl_str.h"\r
  13 #include "fsl_debug_console_conf.h"\r
  14 \r
  15 /*******************************************************************************\r
  16  * Definitions\r
  17  ******************************************************************************/\r
  18 \r
  19 /*! @brief The overflow value.*/\r
  20 #ifndef HUGE_VAL\r
  21 #define HUGE_VAL (99.e99)\r
  22 #endif /* HUGE_VAL */\r
  23 \r
  24 #if PRINTF_ADVANCED_ENABLE\r
  25 /*! @brief Specification modifier flags for printf. */\r
  26 enum _debugconsole_printf_flag\r
  27 {\r
  28     kPRINTF_Minus             = 0x01U,  /*!< Minus FLag. */\r
  29     kPRINTF_Plus              = 0x02U,  /*!< Plus Flag. */\r
  30     kPRINTF_Space             = 0x04U,  /*!< Space Flag. */\r
  31     kPRINTF_Zero              = 0x08U,  /*!< Zero Flag. */\r
  32     kPRINTF_Pound             = 0x10U,  /*!< Pound Flag. */\r
  33     kPRINTF_LengthChar        = 0x20U,  /*!< Length: Char Flag. */\r
  34     kPRINTF_LengthShortInt    = 0x40U,  /*!< Length: Short Int Flag. */\r
  35     kPRINTF_LengthLongInt     = 0x80U,  /*!< Length: Long Int Flag. */\r
  36     kPRINTF_LengthLongLongInt = 0x100U, /*!< Length: Long Long Int Flag. */\r
  37 };\r
  38 #endif /* PRINTF_ADVANCED_ENABLE */\r
  39 \r
  40 /*! @brief Specification modifier flags for scanf. */\r
  41 enum _debugconsole_scanf_flag\r
  42 {\r
  43     kSCANF_Suppress   = 0x2U,    /*!< Suppress Flag. */\r
  44     kSCANF_DestMask   = 0x7cU,   /*!< Destination Mask. */\r
  45     kSCANF_DestChar   = 0x4U,    /*!< Destination Char Flag. */\r
  46     kSCANF_DestString = 0x8U,    /*!< Destination String FLag. */\r
  47     kSCANF_DestSet    = 0x10U,   /*!< Destination Set Flag. */\r
  48     kSCANF_DestInt    = 0x20U,   /*!< Destination Int Flag. */\r
  49     kSCANF_DestFloat  = 0x30U,   /*!< Destination Float Flag. */\r
  50     kSCANF_LengthMask = 0x1f00U, /*!< Length Mask Flag. */\r
  51 #if SCANF_ADVANCED_ENABLE\r
  52     kSCANF_LengthChar        = 0x100U, /*!< Length Char Flag. */\r
  53     kSCANF_LengthShortInt    = 0x200U, /*!< Length ShortInt Flag. */\r
  54     kSCANF_LengthLongInt     = 0x400U, /*!< Length LongInt Flag. */\r
  55     kSCANF_LengthLongLongInt = 0x800U, /*!< Length LongLongInt Flag. */\r
  56 #endif                                 /* SCANF_ADVANCED_ENABLE */\r
  57 #if SCANF_FLOAT_ENABLE\r
  58     kSCANF_LengthLongLongDouble = 0x1000U, /*!< Length LongLongDuoble Flag. */\r
  59 #endif                                     /*PRINTF_FLOAT_ENABLE */\r
  60     kSCANF_TypeSinged = 0x2000U,           /*!< TypeSinged Flag. */\r
  61 };\r
  62 \r
  63 /*! @brief Keil: suppress ellipsis warning in va_arg usage below. */\r
  64 #if defined(__CC_ARM)\r
  65 #pragma diag_suppress 1256\r
  66 #endif /* __CC_ARM */\r
  67 \r
  68 /*******************************************************************************\r
  69  * Prototypes\r
  70  ******************************************************************************/\r
  71 /*!\r
  72  * @brief Scanline function which ignores white spaces.\r
  73  *\r
  74  * @param[in]   s The address of the string pointer to update.\r
  75  * @return      String without white spaces.\r
  76  */\r
  77 static uint32_t ScanIgnoreWhiteSpace(const char **s);\r
  78 \r
  79 /*!\r
  80  * @brief Converts a radix number to a string and return its length.\r
  81  *\r
  82  * @param[in] numstr    Converted string of the number.\r
  83  * @param[in] nump      Pointer to the number.\r
  84  * @param[in] neg       Polarity of the number.\r
  85  * @param[in] radix     The radix to be converted to.\r
  86  * @param[in] use_caps  Used to identify %x/X output format.\r
  87 \r
  88  * @return Length of the converted string.\r
  89  */\r
  90 static int32_t ConvertRadixNumToString(char *numstr, void *nump, int32_t neg, int32_t radix, bool use_caps);\r
  91 \r
  92 #if PRINTF_FLOAT_ENABLE\r
  93 /*!\r
  94  * @brief Converts a floating radix number to a string and return its length.\r
  95  *\r
  96  * @param[in] numstr            Converted string of the number.\r
  97  * @param[in] nump              Pointer to the number.\r
  98  * @param[in] radix             The radix to be converted to.\r
  99  * @param[in] precision_width   Specify the precision width.\r
 100 \r
 101  * @return Length of the converted string.\r
 102  */\r
 103 static int32_t ConvertFloatRadixNumToString(char *numstr, void *nump, int32_t radix, uint32_t precision_width);\r
 104 #endif /* PRINTF_FLOAT_ENABLE */\r
 105 \r
 106 /*!\r
 107  *\r
 108  */\r
 109 double modf(double input_dbl, double *intpart_ptr);\r
 110 \r
 111 /*************Code for process formatted data*******************************/\r
 112 \r
 113 static uint32_t ScanIgnoreWhiteSpace(const char **s)\r
 114 {\r
 115     uint8_t count = 0;\r
 116     uint8_t c;\r
 117 \r
 118     c = **s;\r
 119     while ((c == ' ') || (c == '\t') || (c == '\n') || (c == '\r') || (c == '\v') || (c == '\f'))\r
 120     {\r
 121         count++;\r
 122         (*s)++;\r
 123         c = **s;\r
 124     }\r
 125     return count;\r
 126 }\r
 127 \r
 128 static int32_t ConvertRadixNumToString(char *numstr, void *nump, int32_t neg, int32_t radix, bool use_caps)\r
 129 {\r
 130 #if PRINTF_ADVANCED_ENABLE\r
 131     int64_t a;\r
 132     int64_t b;\r
 133     int64_t c;\r
 134 \r
 135     uint64_t ua;\r
 136     uint64_t ub;\r
 137     uint64_t uc;\r
 138 #else\r
 139     int32_t a;\r
 140     int32_t b;\r
 141     int32_t c;\r
 142 \r
 143     uint32_t ua;\r
 144     uint32_t ub;\r
 145     uint32_t uc;\r
 146 #endif /* PRINTF_ADVANCED_ENABLE */\r
 147 \r
 148     int32_t nlen;\r
 149     char *nstrp;\r
 150 \r
 151     nlen     = 0;\r
 152     nstrp    = numstr;\r
 153     *nstrp++ = '\0';\r
 154 \r
 155     if (neg)\r
 156     {\r
 157 #if PRINTF_ADVANCED_ENABLE\r
 158         a = *(int64_t *)nump;\r
 159 #else\r
 160         a = *(int32_t *)nump;\r
 161 #endif /* PRINTF_ADVANCED_ENABLE */\r
 162         if (a == 0)\r
 163         {\r
 164             *nstrp = '0';\r
 165             ++nlen;\r
 166             return nlen;\r
 167         }\r
 168         while (a != 0)\r
 169         {\r
 170 #if PRINTF_ADVANCED_ENABLE\r
 171             b = (int64_t)a / (int64_t)radix;\r
 172             c = (int64_t)a - ((int64_t)b * (int64_t)radix);\r
 173             if (c < 0)\r
 174             {\r
 175                 uc = (uint64_t)c;\r
 176                 c  = (int64_t)(~uc) + 1 + '0';\r
 177             }\r
 178 #else\r
 179             b = a / radix;\r
 180             c = a - (b * radix);\r
 181             if (c < 0)\r
 182             {\r
 183                 uc = (uint32_t)c;\r
 184                 c  = (uint32_t)(~uc) + 1 + '0';\r
 185             }\r
 186 #endif /* PRINTF_ADVANCED_ENABLE */\r
 187             else\r
 188             {\r
 189                 c = c + '0';\r
 190             }\r
 191             a        = b;\r
 192             *nstrp++ = (char)c;\r
 193             ++nlen;\r
 194         }\r
 195     }\r
 196     else\r
 197     {\r
 198 #if PRINTF_ADVANCED_ENABLE\r
 199         ua = *(uint64_t *)nump;\r
 200 #else\r
 201         ua = *(uint32_t *)nump;\r
 202 #endif /* PRINTF_ADVANCED_ENABLE */\r
 203         if (ua == 0)\r
 204         {\r
 205             *nstrp = '0';\r
 206             ++nlen;\r
 207             return nlen;\r
 208         }\r
 209         while (ua != 0)\r
 210         {\r
 211 #if PRINTF_ADVANCED_ENABLE\r
 212             ub = (uint64_t)ua / (uint64_t)radix;\r
 213             uc = (uint64_t)ua - ((uint64_t)ub * (uint64_t)radix);\r
 214 #else\r
 215             ub = ua / (uint32_t)radix;\r
 216             uc = ua - (ub * (uint32_t)radix);\r
 217 #endif /* PRINTF_ADVANCED_ENABLE */\r
 218 \r
 219             if (uc < 10)\r
 220             {\r
 221                 uc = uc + '0';\r
 222             }\r
 223             else\r
 224             {\r
 225                 uc = uc - 10 + (use_caps ? 'A' : 'a');\r
 226             }\r
 227             ua       = ub;\r
 228             *nstrp++ = (char)uc;\r
 229             ++nlen;\r
 230         }\r
 231     }\r
 232     return nlen;\r
 233 }\r
 234 \r
 235 #if PRINTF_FLOAT_ENABLE\r
 236 static int32_t ConvertFloatRadixNumToString(char *numstr, void *nump, int32_t radix, uint32_t precision_width)\r
 237 {\r
 238     int32_t a;\r
 239     int32_t b;\r
 240     int32_t c;\r
 241     int32_t i;\r
 242     uint32_t uc;\r
 243     double fa;\r
 244     double dc;\r
 245     double fb;\r
 246     double r;\r
 247     double fractpart;\r
 248     double intpart;\r
 249 \r
 250     int32_t nlen;\r
 251     char *nstrp;\r
 252     nlen     = 0;\r
 253     nstrp    = numstr;\r
 254     *nstrp++ = '\0';\r
 255     r        = *(double *)nump;\r
 256     if (!r)\r
 257     {\r
 258         *nstrp = '0';\r
 259         ++nlen;\r
 260         return nlen;\r
 261     }\r
 262     fractpart = modf((double)r, (double *)&intpart);\r
 263     /* Process fractional part. */\r
 264     for (i = 0; i < precision_width; i++)\r
 265     {\r
 266         fractpart *= radix;\r
 267     }\r
 268     if (r >= 0)\r
 269     {\r
 270         fa = fractpart + (double)0.5;\r
 271         if (fa >= pow(10, precision_width))\r
 272         {\r
 273             intpart++;\r
 274         }\r
 275     }\r
 276     else\r
 277     {\r
 278         fa = fractpart - (double)0.5;\r
 279         if (fa <= -pow(10, precision_width))\r
 280         {\r
 281             intpart--;\r
 282         }\r
 283     }\r
 284     for (i = 0; i < precision_width; i++)\r
 285     {\r
 286         fb = fa / (int32_t)radix;\r
 287         dc = (fa - (int64_t)fb * (int32_t)radix);\r
 288         c  = (int32_t)dc;\r
 289         if (c < 0)\r
 290         {\r
 291             uc = (uint32_t)c;\r
 292             c  = (int32_t)(~uc) + 1 + '0';\r
 293         }\r
 294         else\r
 295         {\r
 296             c = c + '0';\r
 297         }\r
 298         fa       = fb;\r
 299         *nstrp++ = (char)c;\r
 300         ++nlen;\r
 301     }\r
 302     *nstrp++ = (char)'.';\r
 303     ++nlen;\r
 304     a = (int32_t)intpart;\r
 305     if (a == 0)\r
 306     {\r
 307         *nstrp++ = '0';\r
 308         ++nlen;\r
 309     }\r
 310     else\r
 311     {\r
 312         while (a != 0)\r
 313         {\r
 314             b = (int32_t)a / (int32_t)radix;\r
 315             c = (int32_t)a - ((int32_t)b * (int32_t)radix);\r
 316             if (c < 0)\r
 317             {\r
 318                 uc = (uint32_t)c;\r
 319                 c  = (int32_t)(~uc) + 1 + '0';\r
 320             }\r
 321             else\r
 322             {\r
 323                 c = c + '0';\r
 324             }\r
 325             a        = b;\r
 326             *nstrp++ = (char)c;\r
 327             ++nlen;\r
 328         }\r
 329     }\r
 330     return nlen;\r
 331 }\r
 332 #endif /* PRINTF_FLOAT_ENABLE */\r
 333 \r
 334 /*!\r
 335  * brief This function outputs its parameters according to a formatted string.\r
 336  *\r
 337  * note I/O is performed by calling given function pointer using following\r
 338  * (*func_ptr)(c);\r
 339  *\r
 340  * param[in] fmt_ptr   Format string for printf.\r
 341  * param[in] args_ptr  Arguments to printf.\r
 342  * param[in] buf  pointer to the buffer\r
 343  * param cb print callback function pointer\r
 344  *\r
 345  * return Number of characters to be print\r
 346  */\r
 347 int StrFormatPrintf(const char *fmt, va_list ap, char *buf, printfCb cb)\r
 348 {\r
 349     /* va_list ap; */\r
 350     char *p;\r
 351     int32_t c;\r
 352 \r
 353     char vstr[33];\r
 354     char *vstrp  = NULL;\r
 355     int32_t vlen = 0;\r
 356 \r
 357     int32_t done;\r
 358     int32_t count = 0;\r
 359 \r
 360     uint32_t field_width;\r
 361     uint32_t precision_width;\r
 362     char *sval;\r
 363     int32_t cval;\r
 364     bool use_caps;\r
 365     uint8_t radix = 0;\r
 366 \r
 367 #if PRINTF_ADVANCED_ENABLE\r
 368     uint32_t flags_used;\r
 369     int32_t schar, dschar;\r
 370     int64_t ival;\r
 371     uint64_t uval = 0;\r
 372     bool valid_precision_width;\r
 373 #else\r
 374     int32_t ival;\r
 375     uint32_t uval = 0;\r
 376 #endif /* PRINTF_ADVANCED_ENABLE */\r
 377 \r
 378 #if PRINTF_FLOAT_ENABLE\r
 379     double fval;\r
 380 #endif /* PRINTF_FLOAT_ENABLE */\r
 381 \r
 382     /* Start parsing apart the format string and display appropriate formats and data. */\r
 383     for (p = (char *)fmt; (c = *p) != 0; p++)\r
 384     {\r
 385         /*\r
 386          * All formats begin with a '%' marker.  Special chars like\r
 387          * '\n' or '\t' are normally converted to the appropriate\r
 388          * character by the __compiler__.  Thus, no need for this\r
 389          * routine to account for the '\' character.\r
 390          */\r
 391         if (c != '%')\r
 392         {\r
 393             cb(buf, &count, c, 1);\r
 394             /* By using 'continue', the next iteration of the loop is used, skipping the code that follows. */\r
 395             continue;\r
 396         }\r
 397 \r
 398         use_caps = true;\r
 399 \r
 400 #if PRINTF_ADVANCED_ENABLE\r
 401         /* First check for specification modifier flags. */\r
 402         flags_used = 0;\r
 403         done       = false;\r
 404         while (!done)\r
 405         {\r
 406             switch (*++p)\r
 407             {\r
 408                 case '-':\r
 409                     flags_used |= kPRINTF_Minus;\r
 410                     break;\r
 411                 case '+':\r
 412                     flags_used |= kPRINTF_Plus;\r
 413                     break;\r
 414                 case ' ':\r
 415                     flags_used |= kPRINTF_Space;\r
 416                     break;\r
 417                 case '0':\r
 418                     flags_used |= kPRINTF_Zero;\r
 419                     break;\r
 420                 case '#':\r
 421                     flags_used |= kPRINTF_Pound;\r
 422                     break;\r
 423                 default:\r
 424                     /* We've gone one char too far. */\r
 425                     --p;\r
 426                     done = true;\r
 427                     break;\r
 428             }\r
 429         }\r
 430 #endif /* PRINTF_ADVANCED_ENABLE */\r
 431 \r
 432         /* Next check for minimum field width. */\r
 433         field_width = 0;\r
 434         done        = false;\r
 435         while (!done)\r
 436         {\r
 437             c = *++p;\r
 438             if ((c >= '0') && (c <= '9'))\r
 439             {\r
 440                 field_width = (field_width * 10) + (c - '0');\r
 441             }\r
 442 #if PRINTF_ADVANCED_ENABLE\r
 443             else if (c == '*')\r
 444             {\r
 445                 field_width = (uint32_t)va_arg(ap, uint32_t);\r
 446             }\r
 447 #endif /* PRINTF_ADVANCED_ENABLE */\r
 448             else\r
 449             {\r
 450                 /* We've gone one char too far. */\r
 451                 --p;\r
 452                 done = true;\r
 453             }\r
 454         }\r
 455         /* Next check for the width and precision field separator. */\r
 456         precision_width = 6;\r
 457 #if PRINTF_ADVANCED_ENABLE\r
 458         valid_precision_width = false;\r
 459 #endif /* PRINTF_ADVANCED_ENABLE */\r
 460         if (*++p == '.')\r
 461         {\r
 462             /* Must get precision field width, if present. */\r
 463             precision_width = 0;\r
 464             done            = false;\r
 465             while (!done)\r
 466             {\r
 467                 c = *++p;\r
 468                 if ((c >= '0') && (c <= '9'))\r
 469                 {\r
 470                     precision_width = (precision_width * 10) + (c - '0');\r
 471 #if PRINTF_ADVANCED_ENABLE\r
 472                     valid_precision_width = true;\r
 473 #endif /* PRINTF_ADVANCED_ENABLE */\r
 474                 }\r
 475 #if PRINTF_ADVANCED_ENABLE\r
 476                 else if (c == '*')\r
 477                 {\r
 478                     precision_width       = (uint32_t)va_arg(ap, uint32_t);\r
 479                     valid_precision_width = true;\r
 480                 }\r
 481 #endif /* PRINTF_ADVANCED_ENABLE */\r
 482                 else\r
 483                 {\r
 484                     /* We've gone one char too far. */\r
 485                     --p;\r
 486                     done = true;\r
 487                 }\r
 488             }\r
 489         }\r
 490         else\r
 491         {\r
 492             /* We've gone one char too far. */\r
 493             --p;\r
 494         }\r
 495 #if PRINTF_ADVANCED_ENABLE\r
 496         /*\r
 497          * Check for the length modifier.\r
 498          */\r
 499         switch (/* c = */ *++p)\r
 500         {\r
 501             case 'h':\r
 502                 if (*++p != 'h')\r
 503                 {\r
 504                     flags_used |= kPRINTF_LengthShortInt;\r
 505                     --p;\r
 506                 }\r
 507                 else\r
 508                 {\r
 509                     flags_used |= kPRINTF_LengthChar;\r
 510                 }\r
 511                 break;\r
 512             case 'l':\r
 513                 if (*++p != 'l')\r
 514                 {\r
 515                     flags_used |= kPRINTF_LengthLongInt;\r
 516                     --p;\r
 517                 }\r
 518                 else\r
 519                 {\r
 520                     flags_used |= kPRINTF_LengthLongLongInt;\r
 521                 }\r
 522                 break;\r
 523             default:\r
 524                 /* we've gone one char too far */\r
 525                 --p;\r
 526                 break;\r
 527         }\r
 528 #endif /* PRINTF_ADVANCED_ENABLE */\r
 529         /* Now we're ready to examine the format. */\r
 530         c = *++p;\r
 531         {\r
 532             if ((c == 'd') || (c == 'i') || (c == 'f') || (c == 'F') || (c == 'x') || (c == 'X') || (c == 'o') ||\r
 533                 (c == 'b') || (c == 'p') || (c == 'u'))\r
 534             {\r
 535                 if ((c == 'd') || (c == 'i'))\r
 536                 {\r
 537 #if PRINTF_ADVANCED_ENABLE\r
 538                     if (flags_used & kPRINTF_LengthLongLongInt)\r
 539                     {\r
 540                         ival = (int64_t)va_arg(ap, int64_t);\r
 541                     }\r
 542                     else\r
 543 #endif /* PRINTF_ADVANCED_ENABLE */\r
 544                     {\r
 545                         ival = (int32_t)va_arg(ap, int32_t);\r
 546                     }\r
 547                     vlen  = ConvertRadixNumToString(vstr, &ival, true, 10, use_caps);\r
 548                     vstrp = &vstr[vlen];\r
 549 #if PRINTF_ADVANCED_ENABLE\r
 550                     if (ival < 0)\r
 551                     {\r
 552                         schar = '-';\r
 553                         ++vlen;\r
 554                     }\r
 555                     else\r
 556                     {\r
 557                         if (flags_used & kPRINTF_Plus)\r
 558                         {\r
 559                             schar = '+';\r
 560                             ++vlen;\r
 561                         }\r
 562                         else\r
 563                         {\r
 564                             if (flags_used & kPRINTF_Space)\r
 565                             {\r
 566                                 schar = ' ';\r
 567                                 ++vlen;\r
 568                             }\r
 569                             else\r
 570                             {\r
 571                                 schar = 0;\r
 572                             }\r
 573                         }\r
 574                     }\r
 575                     dschar = false;\r
 576                     /* Do the ZERO pad. */\r
 577                     if (flags_used & kPRINTF_Zero)\r
 578                     {\r
 579                         if (schar)\r
 580                         {\r
 581                             cb(buf, &count, schar, 1);\r
 582                         }\r
 583                         dschar = true;\r
 584 \r
 585                         cb(buf, &count, '0', field_width - vlen);\r
 586                         vlen = field_width;\r
 587                     }\r
 588                     else\r
 589                     {\r
 590                         if (!(flags_used & kPRINTF_Minus))\r
 591                         {\r
 592                             cb(buf, &count, ' ', field_width - vlen);\r
 593                             if (schar)\r
 594                             {\r
 595                                 cb(buf, &count, schar, 1);\r
 596                             }\r
 597                             dschar = true;\r
 598                         }\r
 599                     }\r
 600                     /* The string was built in reverse order, now display in correct order. */\r
 601                     if ((!dschar) && schar)\r
 602                     {\r
 603                         cb(buf, &count, schar, 1);\r
 604                     }\r
 605 #endif /* PRINTF_ADVANCED_ENABLE */\r
 606                 }\r
 607 \r
 608 #if PRINTF_FLOAT_ENABLE\r
 609                 if ((c == 'f') || (c == 'F'))\r
 610                 {\r
 611                     fval  = (double)va_arg(ap, double);\r
 612                     vlen  = ConvertFloatRadixNumToString(vstr, &fval, 10, precision_width);\r
 613                     vstrp = &vstr[vlen];\r
 614 \r
 615 #if PRINTF_ADVANCED_ENABLE\r
 616                     if (fval < 0)\r
 617                     {\r
 618                         schar = '-';\r
 619                         ++vlen;\r
 620                     }\r
 621                     else\r
 622                     {\r
 623                         if (flags_used & kPRINTF_Plus)\r
 624                         {\r
 625                             schar = '+';\r
 626                             ++vlen;\r
 627                         }\r
 628                         else\r
 629                         {\r
 630                             if (flags_used & kPRINTF_Space)\r
 631                             {\r
 632                                 schar = ' ';\r
 633                                 ++vlen;\r
 634                             }\r
 635                             else\r
 636                             {\r
 637                                 schar = 0;\r
 638                             }\r
 639                         }\r
 640                     }\r
 641                     dschar = false;\r
 642                     if (flags_used & kPRINTF_Zero)\r
 643                     {\r
 644                         if (schar)\r
 645                         {\r
 646                             cb(buf, &count, schar, 1);\r
 647                         }\r
 648                         dschar = true;\r
 649                         cb(buf, &count, '0', field_width - vlen);\r
 650                         vlen = field_width;\r
 651                     }\r
 652                     else\r
 653                     {\r
 654                         if (!(flags_used & kPRINTF_Minus))\r
 655                         {\r
 656                             cb(buf, &count, ' ', field_width - vlen);\r
 657                             if (schar)\r
 658                             {\r
 659                                 cb(buf, &count, schar, 1);\r
 660                             }\r
 661                             dschar = true;\r
 662                         }\r
 663                     }\r
 664                     if ((!dschar) && schar)\r
 665                     {\r
 666                         cb(buf, &count, schar, 1);\r
 667                     }\r
 668 #endif /* PRINTF_ADVANCED_ENABLE */\r
 669                 }\r
 670 #endif /* PRINTF_FLOAT_ENABLE */\r
 671                 if ((c == 'X') || (c == 'x'))\r
 672                 {\r
 673                     if (c == 'x')\r
 674                     {\r
 675                         use_caps = false;\r
 676                     }\r
 677 #if PRINTF_ADVANCED_ENABLE\r
 678                     if (flags_used & kPRINTF_LengthLongLongInt)\r
 679                     {\r
 680                         uval = (uint64_t)va_arg(ap, uint64_t);\r
 681                     }\r
 682                     else\r
 683 #endif /* PRINTF_ADVANCED_ENABLE */\r
 684                     {\r
 685                         uval = (uint32_t)va_arg(ap, uint32_t);\r
 686                     }\r
 687                     vlen  = ConvertRadixNumToString(vstr, &uval, false, 16, use_caps);\r
 688                     vstrp = &vstr[vlen];\r
 689 \r
 690 #if PRINTF_ADVANCED_ENABLE\r
 691                     dschar = false;\r
 692                     if (flags_used & kPRINTF_Zero)\r
 693                     {\r
 694                         if (flags_used & kPRINTF_Pound)\r
 695                         {\r
 696                             cb(buf, &count, '0', 1);\r
 697                             cb(buf, &count, (use_caps ? 'X' : 'x'), 1);\r
 698                             dschar = true;\r
 699                         }\r
 700                         cb(buf, &count, '0', field_width - vlen);\r
 701                         vlen = field_width;\r
 702                     }\r
 703                     else\r
 704                     {\r
 705                         if (!(flags_used & kPRINTF_Minus))\r
 706                         {\r
 707                             if (flags_used & kPRINTF_Pound)\r
 708                             {\r
 709                                 vlen += 2;\r
 710                             }\r
 711                             cb(buf, &count, ' ', field_width - vlen);\r
 712                             if (flags_used & kPRINTF_Pound)\r
 713                             {\r
 714                                 cb(buf, &count, '0', 1);\r
 715                                 cb(buf, &count, (use_caps ? 'X' : 'x'), 1);\r
 716                                 dschar = true;\r
 717                             }\r
 718                         }\r
 719                     }\r
 720 \r
 721                     if ((flags_used & kPRINTF_Pound) && (!dschar))\r
 722                     {\r
 723                         cb(buf, &count, '0', 1);\r
 724                         cb(buf, &count, (use_caps ? 'X' : 'x'), 1);\r
 725                         vlen += 2;\r
 726                     }\r
 727 #endif /* PRINTF_ADVANCED_ENABLE */\r
 728                 }\r
 729                 if ((c == 'o') || (c == 'b') || (c == 'p') || (c == 'u'))\r
 730                 {\r
 731 #if PRINTF_ADVANCED_ENABLE\r
 732                     if (flags_used & kPRINTF_LengthLongLongInt)\r
 733                     {\r
 734                         uval = (uint64_t)va_arg(ap, uint64_t);\r
 735                     }\r
 736                     else\r
 737 #endif /* PRINTF_ADVANCED_ENABLE */\r
 738                     {\r
 739                         uval = (uint32_t)va_arg(ap, uint32_t);\r
 740                     }\r
 741 \r
 742                     if (c == 'o')\r
 743                     {\r
 744                         radix = 8;\r
 745                     }\r
 746                     else if (c == 'b')\r
 747                     {\r
 748                         radix = 2;\r
 749                     }\r
 750                     else if (c == 'p')\r
 751                     {\r
 752                         radix = 16;\r
 753                     }\r
 754                     else\r
 755                     {\r
 756                         radix = 10;\r
 757                     }\r
 758 \r
 759                     vlen  = ConvertRadixNumToString(vstr, &uval, false, radix, use_caps);\r
 760                     vstrp = &vstr[vlen];\r
 761 #if PRINTF_ADVANCED_ENABLE\r
 762                     if (flags_used & kPRINTF_Zero)\r
 763                     {\r
 764                         cb(buf, &count, '0', field_width - vlen);\r
 765                         vlen = field_width;\r
 766                     }\r
 767                     else\r
 768                     {\r
 769                         if (!(flags_used & kPRINTF_Minus))\r
 770                         {\r
 771                             cb(buf, &count, ' ', field_width - vlen);\r
 772                         }\r
 773                     }\r
 774 #endif /* PRINTF_ADVANCED_ENABLE */\r
 775                 }\r
 776 #if !PRINTF_ADVANCED_ENABLE\r
 777                 cb(buf, &count, ' ', field_width - vlen);\r
 778 #endif /* !PRINTF_ADVANCED_ENABLE */\r
 779                 if (vstrp != NULL)\r
 780                 {\r
 781                     while (*vstrp)\r
 782                     {\r
 783                         cb(buf, &count, *vstrp--, 1);\r
 784                     }\r
 785                 }\r
 786 #if PRINTF_ADVANCED_ENABLE\r
 787                 if (flags_used & kPRINTF_Minus)\r
 788                 {\r
 789                     cb(buf, &count, ' ', field_width - vlen);\r
 790                 }\r
 791 #endif /* PRINTF_ADVANCED_ENABLE */\r
 792             }\r
 793             else if (c == 'c')\r
 794             {\r
 795                 cval = (char)va_arg(ap, uint32_t);\r
 796                 cb(buf, &count, cval, 1);\r
 797             }\r
 798             else if (c == 's')\r
 799             {\r
 800                 sval = (char *)va_arg(ap, char *);\r
 801                 if (sval)\r
 802                 {\r
 803 #if PRINTF_ADVANCED_ENABLE\r
 804                     if (valid_precision_width)\r
 805                     {\r
 806                         vlen = precision_width;\r
 807                     }\r
 808                     else\r
 809                     {\r
 810                         vlen = strlen(sval);\r
 811                     }\r
 812 #else\r
 813                     vlen = strlen(sval);\r
 814 #endif /* PRINTF_ADVANCED_ENABLE */\r
 815 #if PRINTF_ADVANCED_ENABLE\r
 816                     if (!(flags_used & kPRINTF_Minus))\r
 817 #endif /* PRINTF_ADVANCED_ENABLE */\r
 818                     {\r
 819                         cb(buf, &count, ' ', field_width - vlen);\r
 820                     }\r
 821 \r
 822 #if PRINTF_ADVANCED_ENABLE\r
 823                     if (valid_precision_width)\r
 824                     {\r
 825                         while ((*sval) && (vlen > 0))\r
 826                         {\r
 827                             cb(buf, &count, *sval++, 1);\r
 828                             vlen--;\r
 829                         }\r
 830                         /* In case that vlen sval is shorter than vlen */\r
 831                         vlen = precision_width - vlen;\r
 832                     }\r
 833                     else\r
 834                     {\r
 835 #endif /* PRINTF_ADVANCED_ENABLE */\r
 836                         while (*sval)\r
 837                         {\r
 838                             cb(buf, &count, *sval++, 1);\r
 839                         }\r
 840 #if PRINTF_ADVANCED_ENABLE\r
 841                     }\r
 842 #endif /* PRINTF_ADVANCED_ENABLE */\r
 843 \r
 844 #if PRINTF_ADVANCED_ENABLE\r
 845                     if (flags_used & kPRINTF_Minus)\r
 846                     {\r
 847                         cb(buf, &count, ' ', field_width - vlen);\r
 848                     }\r
 849 #endif /* PRINTF_ADVANCED_ENABLE */\r
 850                 }\r
 851             }\r
 852             else\r
 853             {\r
 854                 cb(buf, &count, c, 1);\r
 855             }\r
 856         }\r
 857     }\r
 858 \r
 859     return count;\r
 860 }\r
 861 \r
 862 /*!\r
 863  * brief Converts an input line of ASCII characters based upon a provided\r
 864  * string format.\r
 865  *\r
 866  * param[in] line_ptr The input line of ASCII data.\r
 867  * param[in] format   Format first points to the format string.\r
 868  * param[in] args_ptr The list of parameters.\r
 869  *\r
 870  * return Number of input items converted and assigned.\r
 871  * retval IO_EOF When line_ptr is empty string "".\r
 872  */\r
 873 int StrFormatScanf(const char *line_ptr, char *format, va_list args_ptr)\r
 874 {\r
 875     uint8_t base;\r
 876     int8_t neg;\r
 877     /* Identifier for the format string. */\r
 878     char *c = format;\r
 879     char temp;\r
 880     char *buf;\r
 881     /* Flag telling the conversion specification. */\r
 882     uint32_t flag = 0;\r
 883     /* Filed width for the matching input streams. */\r
 884     uint32_t field_width;\r
 885     /* How many arguments are assigned except the suppress. */\r
 886     uint32_t nassigned = 0;\r
 887     /* How many characters are read from the input streams. */\r
 888     uint32_t n_decode = 0;\r
 889 \r
 890     int32_t val;\r
 891 \r
 892     const char *s;\r
 893     /* Identifier for the input string. */\r
 894     const char *p = line_ptr;\r
 895 \r
 896 #if SCANF_FLOAT_ENABLE\r
 897     double fnum = 0.0;\r
 898 #endif /* SCANF_FLOAT_ENABLE */\r
 899     /* Return EOF error before any conversion. */\r
 900     if (*p == '\0')\r
 901     {\r
 902         return -1;\r
 903     }\r
 904 \r
 905     /* Decode directives. */\r
 906     while ((*c) && (*p))\r
 907     {\r
 908         /* Ignore all white-spaces in the format strings. */\r
 909         if (ScanIgnoreWhiteSpace((const char **)&c))\r
 910         {\r
 911             n_decode += ScanIgnoreWhiteSpace(&p);\r
 912         }\r
 913         else if ((*c != '%') || ((*c == '%') && (*(c + 1) == '%')))\r
 914         {\r
 915             /* Ordinary characters. */\r
 916             c++;\r
 917             if (*p == *c)\r
 918             {\r
 919                 n_decode++;\r
 920                 p++;\r
 921                 c++;\r
 922             }\r
 923             else\r
 924             {\r
 925                 /* Match failure. Misalignment with C99, the unmatched characters need to be pushed back to stream.\r
 926                  * However, it is deserted now. */\r
 927                 break;\r
 928             }\r
 929         }\r
 930         else\r
 931         {\r
 932             /* convernsion specification */\r
 933             c++;\r
 934             /* Reset. */\r
 935             flag        = 0;\r
 936             field_width = 0;\r
 937             base        = 0;\r
 938 \r
 939             /* Loop to get full conversion specification. */\r
 940             while ((*c) && (!(flag & kSCANF_DestMask)))\r
 941             {\r
 942                 switch (*c)\r
 943                 {\r
 944 #if SCANF_ADVANCED_ENABLE\r
 945                     case '*':\r
 946                         if (flag & kSCANF_Suppress)\r
 947                         {\r
 948                             /* Match failure. */\r
 949                             return nassigned;\r
 950                         }\r
 951                         flag |= kSCANF_Suppress;\r
 952                         c++;\r
 953                         break;\r
 954                     case 'h':\r
 955                         if (flag & kSCANF_LengthMask)\r
 956                         {\r
 957                             /* Match failure. */\r
 958                             return nassigned;\r
 959                         }\r
 960 \r
 961                         if (c[1] == 'h')\r
 962                         {\r
 963                             flag |= kSCANF_LengthChar;\r
 964                             c++;\r
 965                         }\r
 966                         else\r
 967                         {\r
 968                             flag |= kSCANF_LengthShortInt;\r
 969                         }\r
 970                         c++;\r
 971                         break;\r
 972                     case 'l':\r
 973                         if (flag & kSCANF_LengthMask)\r
 974                         {\r
 975                             /* Match failure. */\r
 976                             return nassigned;\r
 977                         }\r
 978 \r
 979                         if (c[1] == 'l')\r
 980                         {\r
 981                             flag |= kSCANF_LengthLongLongInt;\r
 982                             c++;\r
 983                         }\r
 984                         else\r
 985                         {\r
 986                             flag |= kSCANF_LengthLongInt;\r
 987                         }\r
 988                         c++;\r
 989                         break;\r
 990 #endif /* SCANF_ADVANCED_ENABLE */\r
 991 #if SCANF_FLOAT_ENABLE\r
 992                     case 'L':\r
 993                         if (flag & kSCANF_LengthMask)\r
 994                         {\r
 995                             /* Match failure. */\r
 996                             return nassigned;\r
 997                         }\r
 998                         flag |= kSCANF_LengthLongLongDouble;\r
 999                         c++;\r
1000                         break;\r
1001 #endif /* SCANF_FLOAT_ENABLE */\r
1002                     case '0':\r
1003                     case '1':\r
1004                     case '2':\r
1005                     case '3':\r
1006                     case '4':\r
1007                     case '5':\r
1008                     case '6':\r
1009                     case '7':\r
1010                     case '8':\r
1011                     case '9':\r
1012                         if (field_width)\r
1013                         {\r
1014                             /* Match failure. */\r
1015                             return nassigned;\r
1016                         }\r
1017                         do\r
1018                         {\r
1019                             field_width = field_width * 10 + *c - '0';\r
1020                             c++;\r
1021                         } while ((*c >= '0') && (*c <= '9'));\r
1022                         break;\r
1023                     case 'd':\r
1024                         base = 10;\r
1025                         flag |= kSCANF_TypeSinged;\r
1026                         flag |= kSCANF_DestInt;\r
1027                         c++;\r
1028                         break;\r
1029                     case 'u':\r
1030                         base = 10;\r
1031                         flag |= kSCANF_DestInt;\r
1032                         c++;\r
1033                         break;\r
1034                     case 'o':\r
1035                         base = 8;\r
1036                         flag |= kSCANF_DestInt;\r
1037                         c++;\r
1038                         break;\r
1039                     case 'x':\r
1040                     case 'X':\r
1041                         base = 16;\r
1042                         flag |= kSCANF_DestInt;\r
1043                         c++;\r
1044                         break;\r
1045                     case 'i':\r
1046                         base = 0;\r
1047                         flag |= kSCANF_DestInt;\r
1048                         c++;\r
1049                         break;\r
1050 #if SCANF_FLOAT_ENABLE\r
1051                     case 'a':\r
1052                     case 'A':\r
1053                     case 'e':\r
1054                     case 'E':\r
1055                     case 'f':\r
1056                     case 'F':\r
1057                     case 'g':\r
1058                     case 'G':\r
1059                         flag |= kSCANF_DestFloat;\r
1060                         c++;\r
1061                         break;\r
1062 #endif /* SCANF_FLOAT_ENABLE */\r
1063                     case 'c':\r
1064                         flag |= kSCANF_DestChar;\r
1065                         if (!field_width)\r
1066                         {\r
1067                             field_width = 1;\r
1068                         }\r
1069                         c++;\r
1070                         break;\r
1071                     case 's':\r
1072                         flag |= kSCANF_DestString;\r
1073                         c++;\r
1074                         break;\r
1075                     default:\r
1076                         return nassigned;\r
1077                 }\r
1078             }\r
1079 \r
1080             if (!(flag & kSCANF_DestMask))\r
1081             {\r
1082                 /* Format strings are exhausted. */\r
1083                 return nassigned;\r
1084             }\r
1085 \r
1086             if (!field_width)\r
1087             {\r
1088                 /* Large than length of a line. */\r
1089                 field_width = 99;\r
1090             }\r
1091 \r
1092             /* Matching strings in input streams and assign to argument. */\r
1093             switch (flag & kSCANF_DestMask)\r
1094             {\r
1095                 case kSCANF_DestChar:\r
1096                     s   = (const char *)p;\r
1097                     buf = va_arg(args_ptr, char *);\r
1098                     while ((field_width--) && (*p))\r
1099                     {\r
1100                         if (!(flag & kSCANF_Suppress))\r
1101                         {\r
1102                             *buf++ = *p++;\r
1103                         }\r
1104                         else\r
1105                         {\r
1106                             p++;\r
1107                         }\r
1108                         n_decode++;\r
1109                     }\r
1110 \r
1111                     if ((!(flag & kSCANF_Suppress)) && (s != p))\r
1112                     {\r
1113                         nassigned++;\r
1114                     }\r
1115                     break;\r
1116                 case kSCANF_DestString:\r
1117                     n_decode += ScanIgnoreWhiteSpace(&p);\r
1118                     s   = p;\r
1119                     buf = va_arg(args_ptr, char *);\r
1120                     while ((field_width--) && (*p != '\0') && (*p != ' ') && (*p != '\t') && (*p != '\n') &&\r
1121                            (*p != '\r') && (*p != '\v') && (*p != '\f'))\r
1122                     {\r
1123                         if (flag & kSCANF_Suppress)\r
1124                         {\r
1125                             p++;\r
1126                         }\r
1127                         else\r
1128                         {\r
1129                             *buf++ = *p++;\r
1130                         }\r
1131                         n_decode++;\r
1132                     }\r
1133 \r
1134                     if ((!(flag & kSCANF_Suppress)) && (s != p))\r
1135                     {\r
1136                         /* Add NULL to end of string. */\r
1137                         *buf = '\0';\r
1138                         nassigned++;\r
1139                     }\r
1140                     break;\r
1141                 case kSCANF_DestInt:\r
1142                     n_decode += ScanIgnoreWhiteSpace(&p);\r
1143                     s   = p;\r
1144                     val = 0;\r
1145                     if ((base == 0) || (base == 16))\r
1146                     {\r
1147                         if ((s[0] == '0') && ((s[1] == 'x') || (s[1] == 'X')))\r
1148                         {\r
1149                             base = 16;\r
1150                             if (field_width >= 1)\r
1151                             {\r
1152                                 p += 2;\r
1153                                 n_decode += 2;\r
1154                                 field_width -= 2;\r
1155                             }\r
1156                         }\r
1157                     }\r
1158 \r
1159                     if (base == 0)\r
1160                     {\r
1161                         if (s[0] == '0')\r
1162                         {\r
1163                             base = 8;\r
1164                         }\r
1165                         else\r
1166                         {\r
1167                             base = 10;\r
1168                         }\r
1169                     }\r
1170 \r
1171                     neg = 1;\r
1172                     switch (*p)\r
1173                     {\r
1174                         case '-':\r
1175                             neg = -1;\r
1176                             n_decode++;\r
1177                             p++;\r
1178                             field_width--;\r
1179                             break;\r
1180                         case '+':\r
1181                             neg = 1;\r
1182                             n_decode++;\r
1183                             p++;\r
1184                             field_width--;\r
1185                             break;\r
1186                         default:\r
1187                             break;\r
1188                     }\r
1189 \r
1190                     while ((*p) && (field_width--))\r
1191                     {\r
1192                         if ((*p <= '9') && (*p >= '0'))\r
1193                         {\r
1194                             temp = *p - '0';\r
1195                         }\r
1196                         else if ((*p <= 'f') && (*p >= 'a'))\r
1197                         {\r
1198                             temp = *p - 'a' + 10;\r
1199                         }\r
1200                         else if ((*p <= 'F') && (*p >= 'A'))\r
1201                         {\r
1202                             temp = *p - 'A' + 10;\r
1203                         }\r
1204                         else\r
1205                         {\r
1206                             temp = base;\r
1207                         }\r
1208 \r
1209                         if (temp >= base)\r
1210                         {\r
1211                             break;\r
1212                         }\r
1213                         else\r
1214                         {\r
1215                             val = base * val + temp;\r
1216                         }\r
1217                         p++;\r
1218                         n_decode++;\r
1219                     }\r
1220                     val *= neg;\r
1221                     if (!(flag & kSCANF_Suppress))\r
1222                     {\r
1223 #if SCANF_ADVANCED_ENABLE\r
1224                         switch (flag & kSCANF_LengthMask)\r
1225                         {\r
1226                             case kSCANF_LengthChar:\r
1227                                 if (flag & kSCANF_TypeSinged)\r
1228                                 {\r
1229                                     *va_arg(args_ptr, signed char *) = (signed char)val;\r
1230                                 }\r
1231                                 else\r
1232                                 {\r
1233                                     *va_arg(args_ptr, unsigned char *) = (unsigned char)val;\r
1234                                 }\r
1235                                 break;\r
1236                             case kSCANF_LengthShortInt:\r
1237                                 if (flag & kSCANF_TypeSinged)\r
1238                                 {\r
1239                                     *va_arg(args_ptr, signed short *) = (signed short)val;\r
1240                                 }\r
1241                                 else\r
1242                                 {\r
1243                                     *va_arg(args_ptr, unsigned short *) = (unsigned short)val;\r
1244                                 }\r
1245                                 break;\r
1246                             case kSCANF_LengthLongInt:\r
1247                                 if (flag & kSCANF_TypeSinged)\r
1248                                 {\r
1249                                     *va_arg(args_ptr, signed long int *) = (signed long int)val;\r
1250                                 }\r
1251                                 else\r
1252                                 {\r
1253                                     *va_arg(args_ptr, unsigned long int *) = (unsigned long int)val;\r
1254                                 }\r
1255                                 break;\r
1256                             case kSCANF_LengthLongLongInt:\r
1257                                 if (flag & kSCANF_TypeSinged)\r
1258                                 {\r
1259                                     *va_arg(args_ptr, signed long long int *) = (signed long long int)val;\r
1260                                 }\r
1261                                 else\r
1262                                 {\r
1263                                     *va_arg(args_ptr, unsigned long long int *) = (unsigned long long int)val;\r
1264                                 }\r
1265                                 break;\r
1266                             default:\r
1267                                 /* The default type is the type int. */\r
1268                                 if (flag & kSCANF_TypeSinged)\r
1269                                 {\r
1270                                     *va_arg(args_ptr, signed int *) = (signed int)val;\r
1271                                 }\r
1272                                 else\r
1273                                 {\r
1274                                     *va_arg(args_ptr, unsigned int *) = (unsigned int)val;\r
1275                                 }\r
1276                                 break;\r
1277                         }\r
1278 #else\r
1279                         /* The default type is the type int. */\r
1280                         if (flag & kSCANF_TypeSinged)\r
1281                         {\r
1282                             *va_arg(args_ptr, signed int *) = (signed int)val;\r
1283                         }\r
1284                         else\r
1285                         {\r
1286                             *va_arg(args_ptr, unsigned int *) = (unsigned int)val;\r
1287                         }\r
1288 #endif /* SCANF_ADVANCED_ENABLE */\r
1289                         nassigned++;\r
1290                     }\r
1291                     break;\r
1292 #if SCANF_FLOAT_ENABLE\r
1293                 case kSCANF_DestFloat:\r
1294                     n_decode += ScanIgnoreWhiteSpace(&p);\r
1295                     fnum = strtod(p, (char **)&s);\r
1296 \r
1297                     if ((fnum >= HUGE_VAL) || (fnum <= -HUGE_VAL))\r
1298                     {\r
1299                         break;\r
1300                     }\r
1301 \r
1302                     n_decode += (int)(s) - (int)(p);\r
1303                     p = s;\r
1304                     if (!(flag & kSCANF_Suppress))\r
1305                     {\r
1306                         if (flag & kSCANF_LengthLongLongDouble)\r
1307                         {\r
1308                             *va_arg(args_ptr, double *) = fnum;\r
1309                         }\r
1310                         else\r
1311                         {\r
1312                             *va_arg(args_ptr, float *) = (float)fnum;\r
1313                         }\r
1314                         nassigned++;\r
1315                     }\r
1316                     break;\r
1317 #endif /* SCANF_FLOAT_ENABLE */\r
1318                 default:\r
1319                     return nassigned;\r
1320             }\r
1321         }\r
1322     }\r
1323     return nassigned;\r
1324 }\r