git.sur5r.net Git - freertos/blob - FreeRTOS-Plus/Source/FreeRTOS-Plus-Trace/streamports/Jlink_RTT/SEGGER_RTT_Printf.c

   1 /*********************************************************************\r
   2 *               SEGGER MICROCONTROLLER GmbH & Co. KG                 *\r
   3 *       Solutions for real time microcontroller applications         *\r
   4 **********************************************************************\r
   5 *                                                                    *\r
   6 *       (c) 2014 - 2016  SEGGER Microcontroller GmbH & Co. KG        *\r
   7 *                                                                    *\r
   8 *       www.segger.com     Support: support@segger.com               *\r
   9 *                                                                    *\r
  10 **********************************************************************\r
  11 *                                                                    *\r
  12 *       SEGGER RTT * Real Time Transfer for embedded targets         *\r
  13 *                                                                    *\r
  14 **********************************************************************\r
  15 *                                                                    *\r
  16 * All rights reserved.                                               *\r
  17 *                                                                    *\r
  18 * * This software may in its unmodified form be freely redistributed *\r
  19 *   in source, linkable, or executable form.                         *\r
  20 * * The source code may be modified, provided the source code        *\r
  21 *   retains the above copyright notice, this list of conditions and  *\r
  22 *   the following disclaimer.                                        *\r
  23 * * Modified versions of this software in source, executable, or     *\r
  24 *   linkable form may not be distributed without prior consent of    *\r
  25 *   SEGGER.                                                          *\r
  26 * * This software may only be used for communication with SEGGER     *\r
  27 *   J-Link debug probes.                                             *\r
  28 *                                                                    *\r
  29 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND             *\r
  30 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,        *\r
  31 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF           *\r
  32 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE           *\r
  33 * DISCLAIMED. IN NO EVENT SHALL SEGGER Microcontroller BE LIABLE FOR *\r
  34 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR           *\r
  35 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT  *\r
  36 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;    *\r
  37 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF      *\r
  38 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT          *\r
  39 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE  *\r
  40 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH   *\r
  41 * DAMAGE.                                                            *\r
  42 *                                                                    *\r
  43 **********************************************************************\r
  44 *                                                                    *\r
  45 *       RTT version: 6.00e                                           *\r
  46 *                                                                    *\r
  47 **********************************************************************\r
  48 ---------------------------END-OF-HEADER------------------------------\r
  49 File    : SEGGER_RTT_printf.c\r
  50 Purpose : Replacement for printf to write formatted data via RTT\r
  51 Revision: $Rev: 3667 $\r
  52 ----------------------------------------------------------------------\r
  53 */\r
  54 #include "SEGGER_RTT.h"\r
  55 #include "SEGGER_RTT_Conf.h"\r
  56 \r
  57 /*********************************************************************\r
  58 *\r
  59 *       Defines, configurable\r
  60 *\r
  61 **********************************************************************\r
  62 */\r
  63 \r
  64 #ifndef SEGGER_RTT_PRINTF_BUFFER_SIZE\r
  65   #define SEGGER_RTT_PRINTF_BUFFER_SIZE (64)\r
  66 #endif\r
  67 \r
  68 #include <stdlib.h>\r
  69 #include <stdarg.h>\r
  70 \r
  71 \r
  72 #define FORMAT_FLAG_LEFT_JUSTIFY   (1u << 0)\r
  73 #define FORMAT_FLAG_PAD_ZERO       (1u << 1)\r
  74 #define FORMAT_FLAG_PRINT_SIGN     (1u << 2)\r
  75 #define FORMAT_FLAG_ALTERNATE      (1u << 3)\r
  76 \r
  77 /*********************************************************************\r
  78 *\r
  79 *       Types\r
  80 *\r
  81 **********************************************************************\r
  82 */\r
  83 \r
  84 typedef struct {\r
  85   char*     pBuffer;\r
  86   unsigned  BufferSize;\r
  87   unsigned  Cnt;\r
  88 \r
  89   int   ReturnValue;\r
  90 \r
  91   unsigned RTTBufferIndex;\r
  92 } SEGGER_RTT_PRINTF_DESC;\r
  93 \r
  94 /*********************************************************************\r
  95 *\r
  96 *       Function prototypes\r
  97 *\r
  98 **********************************************************************\r
  99 */\r
 100 int SEGGER_RTT_vprintf(unsigned BufferIndex, const char * sFormat, va_list * pParamList);\r
 101 \r
 102 /*********************************************************************\r
 103 *\r
 104 *       Static code\r
 105 *\r
 106 **********************************************************************\r
 107 */\r
 108 /*********************************************************************\r
 109 *\r
 110 *       _StoreChar\r
 111 */\r
 112 static void _StoreChar(SEGGER_RTT_PRINTF_DESC * p, char c) {\r
 113   unsigned Cnt;\r
 114 \r
 115   Cnt = p->Cnt;\r
 116   if ((Cnt + 1u) <= p->BufferSize) {\r
 117     *(p->pBuffer + Cnt) = c;\r
 118     p->Cnt = Cnt + 1u;\r
 119     p->ReturnValue++;\r
 120   }\r
 121   //\r
 122   // Write part of string, when the buffer is full\r
 123   //\r
 124   if (p->Cnt == p->BufferSize) {\r
 125     if (SEGGER_RTT_Write(p->RTTBufferIndex, p->pBuffer, p->Cnt) != p->Cnt) {\r
 126       p->ReturnValue = -1;\r
 127     } else {\r
 128       p->Cnt = 0u;\r
 129     }\r
 130   }\r
 131 }\r
 132 \r
 133 /*********************************************************************\r
 134 *\r
 135 *       _PrintUnsigned\r
 136 */\r
 137 static void _PrintUnsigned(SEGGER_RTT_PRINTF_DESC * pBufferDesc, unsigned v, unsigned Base, unsigned NumDigits, unsigned FieldWidth, unsigned FormatFlags) {\r
 138   static const char _aV2C[16] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };\r
 139   unsigned Div;\r
 140   unsigned Digit;\r
 141   unsigned Number;\r
 142   unsigned Width;\r
 143   char c;\r
 144 \r
 145   Number = v;\r
 146   Digit = 1u;\r
 147   //\r
 148   // Get actual field width\r
 149   //\r
 150   Width = 1u;\r
 151   while (Number >= Base) {\r
 152     Number = (Number / Base);\r
 153     Width++;\r
 154   }\r
 155   if (NumDigits > Width) {\r
 156     Width = NumDigits;\r
 157   }\r
 158   //\r
 159   // Print leading chars if necessary\r
 160   //\r
 161   if ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0u) {\r
 162     if (FieldWidth != 0u) {\r
 163       if (((FormatFlags & FORMAT_FLAG_PAD_ZERO) == FORMAT_FLAG_PAD_ZERO) && (NumDigits == 0u)) {\r
 164         c = '0';\r
 165       } else {\r
 166         c = ' ';\r
 167       }\r
 168       while ((FieldWidth != 0u) && (Width < FieldWidth)) {\r
 169         FieldWidth--;\r
 170         _StoreChar(pBufferDesc, c);\r
 171         if (pBufferDesc->ReturnValue < 0) {\r
 172           break;\r
 173         }\r
 174       }\r
 175     }\r
 176   }\r
 177   if (pBufferDesc->ReturnValue >= 0) {\r
 178     //\r
 179     // Compute Digit.\r
 180     // Loop until Digit has the value of the highest digit required.\r
 181     // Example: If the output is 345 (Base 10), loop 2 times until Digit is 100.\r
 182     //\r
 183     while (1) {\r
 184       if (NumDigits > 1u) {       // User specified a min number of digits to print? => Make sure we loop at least that often, before checking anything else (> 1 check avoids problems with NumDigits being signed / unsigned)\r
 185         NumDigits--;\r
 186       } else {\r
 187         Div = v / Digit;\r
 188         if (Div < Base) {        // Is our divider big enough to extract the highest digit from value? => Done\r
 189           break;\r
 190         }\r
 191       }\r
 192       Digit *= Base;\r
 193     }\r
 194     //\r
 195     // Output digits\r
 196     //\r
 197     do {\r
 198       Div = v / Digit;\r
 199       v -= Div * Digit;\r
 200       _StoreChar(pBufferDesc, _aV2C[Div]);\r
 201       if (pBufferDesc->ReturnValue < 0) {\r
 202         break;\r
 203       }\r
 204       Digit /= Base;\r
 205     } while (Digit);\r
 206     //\r
 207     // Print trailing spaces if necessary\r
 208     //\r
 209     if ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == FORMAT_FLAG_LEFT_JUSTIFY) {\r
 210       if (FieldWidth != 0u) {\r
 211         while ((FieldWidth != 0u) && (Width < FieldWidth)) {\r
 212           FieldWidth--;\r
 213           _StoreChar(pBufferDesc, ' ');\r
 214           if (pBufferDesc->ReturnValue < 0) {\r
 215             break;\r
 216           }\r
 217         }\r
 218       }\r
 219     }\r
 220   }\r
 221 }\r
 222 \r
 223 /*********************************************************************\r
 224 *\r
 225 *       _PrintInt\r
 226 */\r
 227 static void _PrintInt(SEGGER_RTT_PRINTF_DESC * pBufferDesc, int v, unsigned Base, unsigned NumDigits, unsigned FieldWidth, unsigned FormatFlags) {\r
 228   unsigned Width;\r
 229   int Number;\r
 230 \r
 231   Number = (v < 0) ? -v : v;\r
 232 \r
 233   //\r
 234   // Get actual field width\r
 235   //\r
 236   Width = 1u;\r
 237   while (Number >= (int)Base) {\r
 238     Number = (Number / (int)Base);\r
 239     Width++;\r
 240   }\r
 241   if (NumDigits > Width) {\r
 242     Width = NumDigits;\r
 243   }\r
 244   if ((FieldWidth > 0u) && ((v < 0) || ((FormatFlags & FORMAT_FLAG_PRINT_SIGN) == FORMAT_FLAG_PRINT_SIGN))) {\r
 245     FieldWidth--;\r
 246   }\r
 247 \r
 248   //\r
 249   // Print leading spaces if necessary\r
 250   //\r
 251   if ((((FormatFlags & FORMAT_FLAG_PAD_ZERO) == 0u) || (NumDigits != 0u)) && ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0u)) {\r
 252     if (FieldWidth != 0u) {\r
 253       while ((FieldWidth != 0u) && (Width < FieldWidth)) {\r
 254         FieldWidth--;\r
 255         _StoreChar(pBufferDesc, ' ');\r
 256         if (pBufferDesc->ReturnValue < 0) {\r
 257           break;\r
 258         }\r
 259       }\r
 260     }\r
 261   }\r
 262   //\r
 263   // Print sign if necessary\r
 264   //\r
 265   if (pBufferDesc->ReturnValue >= 0) {\r
 266     if (v < 0) {\r
 267       v = -v;\r
 268       _StoreChar(pBufferDesc, '-');\r
 269     } else if ((FormatFlags & FORMAT_FLAG_PRINT_SIGN) == FORMAT_FLAG_PRINT_SIGN) {\r
 270       _StoreChar(pBufferDesc, '+');\r
 271     } else {\r
 272 \r
 273     }\r
 274     if (pBufferDesc->ReturnValue >= 0) {\r
 275       //\r
 276       // Print leading zeros if necessary\r
 277       //\r
 278       if (((FormatFlags & FORMAT_FLAG_PAD_ZERO) == FORMAT_FLAG_PAD_ZERO) && ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0u) && (NumDigits == 0u)) {\r
 279         if (FieldWidth != 0u) {\r
 280           while ((FieldWidth != 0u) && (Width < FieldWidth)) {\r
 281             FieldWidth--;\r
 282             _StoreChar(pBufferDesc, '0');\r
 283             if (pBufferDesc->ReturnValue < 0) {\r
 284               break;\r
 285             }\r
 286           }\r
 287         }\r
 288       }\r
 289       if (pBufferDesc->ReturnValue >= 0) {\r
 290         //\r
 291         // Print number without sign\r
 292         //\r
 293         _PrintUnsigned(pBufferDesc, (unsigned)v, Base, NumDigits, FieldWidth, FormatFlags);\r
 294       }\r
 295     }\r
 296   }\r
 297 }\r
 298 \r
 299 /*********************************************************************\r
 300 *\r
 301 *       Public code\r
 302 *\r
 303 **********************************************************************\r
 304 */\r
 305 /*********************************************************************\r
 306 *\r
 307 *       SEGGER_RTT_vprintf\r
 308 *\r
 309 *  Function description\r
 310 *    Stores a formatted string in SEGGER RTT control block.\r
 311 *    This data is read by the host.\r
 312 *\r
 313 *  Parameters\r
 314 *    BufferIndex  Index of "Up"-buffer to be used. (e.g. 0 for "Terminal")\r
 315 *    sFormat      Pointer to format string\r
 316 *    pParamList   Pointer to the list of arguments for the format string\r
 317 *\r
 318 *  Return values\r
 319 *    >= 0:  Number of bytes which have been stored in the "Up"-buffer.\r
 320 *     < 0:  Error\r
 321 */\r
 322 int SEGGER_RTT_vprintf(unsigned BufferIndex, const char * sFormat, va_list * pParamList) {\r
 323   char c;\r
 324   SEGGER_RTT_PRINTF_DESC BufferDesc;\r
 325   int v;\r
 326   unsigned NumDigits;\r
 327   unsigned FormatFlags;\r
 328   unsigned FieldWidth;\r
 329   char acBuffer[SEGGER_RTT_PRINTF_BUFFER_SIZE];\r
 330 \r
 331   BufferDesc.pBuffer        = acBuffer;\r
 332   BufferDesc.BufferSize     = SEGGER_RTT_PRINTF_BUFFER_SIZE;\r
 333   BufferDesc.Cnt            = 0u;\r
 334   BufferDesc.RTTBufferIndex = BufferIndex;\r
 335   BufferDesc.ReturnValue    = 0;\r
 336 \r
 337   do {\r
 338     c = *sFormat;\r
 339     sFormat++;\r
 340     if (c == 0u) {\r
 341       break;\r
 342     }\r
 343     if (c == '%') {\r
 344       //\r
 345       // Filter out flags\r
 346       //\r
 347       FormatFlags = 0u;\r
 348       v = 1;\r
 349       do {\r
 350         c = *sFormat;\r
 351         switch (c) {\r
 352         case '-': FormatFlags |= FORMAT_FLAG_LEFT_JUSTIFY; sFormat++; break;\r
 353         case '0': FormatFlags |= FORMAT_FLAG_PAD_ZERO;     sFormat++; break;\r
 354         case '+': FormatFlags |= FORMAT_FLAG_PRINT_SIGN;   sFormat++; break;\r
 355         case '#': FormatFlags |= FORMAT_FLAG_ALTERNATE;    sFormat++; break;\r
 356         default:  v = 0; break;\r
 357         }\r
 358       } while (v);\r
 359       //\r
 360       // filter out field with\r
 361       //\r
 362       FieldWidth = 0u;\r
 363       do {\r
 364         c = *sFormat;\r
 365         if ((c < '0') || (c > '9')) {\r
 366           break;\r
 367         }\r
 368         sFormat++;\r
 369         FieldWidth = (FieldWidth * 10u) + ((unsigned)c - '0');\r
 370       } while (1);\r
 371 \r
 372       //\r
 373       // Filter out precision (number of digits to display)\r
 374       //\r
 375       NumDigits = 0u;\r
 376       c = *sFormat;\r
 377       if (c == '.') {\r
 378         sFormat++;\r
 379         do {\r
 380           c = *sFormat;\r
 381           if ((c < '0') || (c > '9')) {\r
 382             break;\r
 383           }\r
 384           sFormat++;\r
 385           NumDigits = NumDigits * 10u + ((unsigned)c - '0');\r
 386         } while (1);\r
 387       }\r
 388       //\r
 389       // Filter out length modifier\r
 390       //\r
 391       c = *sFormat;\r
 392       do {\r
 393         if ((c == 'l') || (c == 'h')) {\r
 394           sFormat++;\r
 395           c = *sFormat;\r
 396         } else {\r
 397           break;\r
 398         }\r
 399       } while (1);\r
 400       //\r
 401       // Handle specifiers\r
 402       //\r
 403       switch (c) {\r
 404       case 'c': {\r
 405         char c0;\r
 406         v = va_arg(*pParamList, int);\r
 407         c0 = (char)v;\r
 408         _StoreChar(&BufferDesc, c0);\r
 409         break;\r
 410       }\r
 411       case 'd':\r
 412         v = va_arg(*pParamList, int);\r
 413         _PrintInt(&BufferDesc, v, 10u, NumDigits, FieldWidth, FormatFlags);\r
 414         break;\r
 415       case 'u':\r
 416         v = va_arg(*pParamList, int);\r
 417         _PrintUnsigned(&BufferDesc, (unsigned)v, 10u, NumDigits, FieldWidth, FormatFlags);\r
 418         break;\r
 419       case 'x':\r
 420       case 'X':\r
 421         v = va_arg(*pParamList, int);\r
 422         _PrintUnsigned(&BufferDesc, (unsigned)v, 16u, NumDigits, FieldWidth, FormatFlags);\r
 423         break;\r
 424       case 's':\r
 425         {\r
 426           const char * s = va_arg(*pParamList, const char *);\r
 427           do {\r
 428             c = *s;\r
 429             s++;\r
 430             if (c == '\0') {\r
 431               break;\r
 432             }\r
 433            _StoreChar(&BufferDesc, c);\r
 434           } while (BufferDesc.ReturnValue >= 0);\r
 435         }\r
 436         break;\r
 437       case 'p':\r
 438         v = va_arg(*pParamList, int);\r
 439         _PrintUnsigned(&BufferDesc, (unsigned)v, 16u, 8u, 8u, 0u);\r
 440         break;\r
 441       case '%':\r
 442         _StoreChar(&BufferDesc, '%');\r
 443         break;\r
 444       default:\r
 445         break;\r
 446       }\r
 447       sFormat++;\r
 448     } else {\r
 449       _StoreChar(&BufferDesc, c);\r
 450     }\r
 451   } while (BufferDesc.ReturnValue >= 0);\r
 452 \r
 453   if (BufferDesc.ReturnValue > 0) {\r
 454     //\r
 455     // Write remaining data, if any\r
 456     //\r
 457     if (BufferDesc.Cnt != 0u) {\r
 458       SEGGER_RTT_Write(BufferIndex, acBuffer, BufferDesc.Cnt);\r
 459     }\r
 460     BufferDesc.ReturnValue += (int)BufferDesc.Cnt;\r
 461   }\r
 462   return BufferDesc.ReturnValue;\r
 463 }\r
 464 \r
 465 /*********************************************************************\r
 466 *\r
 467 *       SEGGER_RTT_printf\r
 468 *\r
 469 *  Function description\r
 470 *    Stores a formatted string in SEGGER RTT control block.\r
 471 *    This data is read by the host.\r
 472 *\r
 473 *  Parameters\r
 474 *    BufferIndex  Index of "Up"-buffer to be used. (e.g. 0 for "Terminal")\r
 475 *    sFormat      Pointer to format string, followed by the arguments for conversion\r
 476 *\r
 477 *  Return values\r
 478 *    >= 0:  Number of bytes which have been stored in the "Up"-buffer.\r
 479 *     < 0:  Error\r
 480 *\r
 481 *  Notes\r
 482 *    (1) Conversion specifications have following syntax:\r
 483 *          %[flags][FieldWidth][.Precision]ConversionSpecifier\r
 484 *    (2) Supported flags:\r
 485 *          -: Left justify within the field width\r
 486 *          +: Always print sign extension for signed conversions\r
 487 *          0: Pad with 0 instead of spaces. Ignored when using '-'-flag or precision\r
 488 *        Supported conversion specifiers:\r
 489 *          c: Print the argument as one char\r
 490 *          d: Print the argument as a signed integer\r
 491 *          u: Print the argument as an unsigned integer\r
 492 *          x: Print the argument as an hexadecimal integer\r
 493 *          s: Print the string pointed to by the argument\r
 494 *          p: Print the argument as an 8-digit hexadecimal integer. (Argument shall be a pointer to void.)\r
 495 */\r
 496 int SEGGER_RTT_printf(unsigned BufferIndex, const char * sFormat, ...) {\r
 497   int r;\r
 498   va_list ParamList;\r
 499 \r
 500   va_start(ParamList, sFormat);\r
 501   r = SEGGER_RTT_vprintf(BufferIndex, sFormat, &ParamList);\r
 502   va_end(ParamList);\r
 503   return r;\r
 504 }\r
 505 /*************************** End of file ****************************/\r