git.sur5r.net Git - freertos/blob - FreeRTOS-Plus/Source/FreeRTOS-Plus-Trace(streaming)/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-2014 SEGGER Microcontroller GmbH & Co. KG          *\r
   7 *                                                                    *\r
   8 *       Internet: www.segger.com Support: support@segger.com         *\r
   9 *                                                                    *\r
  10 **********************************************************************\r
  11 ----------------------------------------------------------------------\r
  12 File    : SEGGER_RTT_printf.c\r
  13 Date    : 17 Dec 2014\r
  14 Purpose : Replacement for printf to write formatted data via RTT\r
  15 ---------------------------END-OF-HEADER------------------------------\r
  16 */\r
  17 #include "SEGGER_RTT.h"\r
  18 #include "SEGGER_RTT_Conf.h"\r
  19 \r
  20 /*********************************************************************\r
  21 *\r
  22 *       Defines, configurable\r
  23 *\r
  24 **********************************************************************\r
  25 */\r
  26 \r
  27 #ifndef SEGGER_RTT_PRINTF_BUFFER_SIZE\r
  28   #define SEGGER_RTT_PRINTF_BUFFER_SIZE (64)\r
  29 #endif\r
  30 \r
  31 #include <stdlib.h>\r
  32 #include <stdarg.h>\r
  33 \r
  34 \r
  35 #define FORMAT_FLAG_LEFT_JUSTIFY   (1 << 0)\r
  36 #define FORMAT_FLAG_PAD_ZERO       (1 << 1)\r
  37 #define FORMAT_FLAG_PRINT_SIGN     (1 << 2)\r
  38 #define FORMAT_FLAG_ALTERNATE      (1 << 3)\r
  39 \r
  40 /*********************************************************************\r
  41 *\r
  42 *       Types\r
  43 *\r
  44 **********************************************************************\r
  45 */\r
  46 \r
  47 typedef struct {\r
  48   char* pBuffer;\r
  49   int   BufferSize;\r
  50   int   Cnt;\r
  51 \r
  52   int   ReturnValue;\r
  53 \r
  54   unsigned RTTBufferIndex;\r
  55 } SEGGER_RTT_PRINTF_DESC;\r
  56 \r
  57 /*********************************************************************\r
  58 *\r
  59 *       Function prototypes\r
  60 *\r
  61 **********************************************************************\r
  62 */\r
  63 int SEGGER_RTT_vprintf(unsigned BufferIndex, const char * sFormat, va_list * pParamList);\r
  64 \r
  65 /*********************************************************************\r
  66 *\r
  67 *       Static code\r
  68 *\r
  69 **********************************************************************\r
  70 */\r
  71 /*********************************************************************\r
  72 *\r
  73 *       _StoreChar\r
  74 */\r
  75 static void _StoreChar(SEGGER_RTT_PRINTF_DESC * p, char c) {\r
  76   int Cnt;\r
  77 \r
  78   Cnt = p->Cnt;\r
  79   if ((Cnt + 1) <= p->BufferSize) {\r
  80     *(p->pBuffer + Cnt) = c;\r
  81     p->Cnt = Cnt + 1;\r
  82     p->ReturnValue++;\r
  83   }\r
  84   //\r
  85   // Write part of string, when the buffer is full\r
  86   //\r
  87   if (p->Cnt == p->BufferSize) {\r
  88     if (SEGGER_RTT_Write(p->RTTBufferIndex, p->pBuffer, p->Cnt) != p->Cnt) {\r
  89       p->ReturnValue = -1;\r
  90     } else {\r
  91       p->Cnt = 0;\r
  92     }\r
  93   }\r
  94 }\r
  95 \r
  96 /*********************************************************************\r
  97 *\r
  98 *       _PrintUnsigned\r
  99 */\r
 100 static void _PrintUnsigned(SEGGER_RTT_PRINTF_DESC * pBufferDesc, unsigned v, unsigned Base, int NumDigits, unsigned FieldWidth, unsigned FormatFlags) {\r
 101   static const char _aV2C[16] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };\r
 102   unsigned Div;\r
 103   unsigned Digit = 1;\r
 104   unsigned Number;\r
 105   unsigned Width;\r
 106   char c;\r
 107 \r
 108   Number = v;\r
 109 \r
 110   //\r
 111   // Get actual field width\r
 112   //\r
 113   Width = 1;\r
 114   while (Number >= Base) {\r
 115     Number = (Number / Base);\r
 116     Width++;\r
 117   }\r
 118   if ((unsigned)NumDigits > Width) {\r
 119     Width = NumDigits;\r
 120   }\r
 121   //\r
 122   // Print leading chars if necessary\r
 123   //\r
 124   if ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0) {\r
 125     if (FieldWidth != 0) {\r
 126       if (((FormatFlags & FORMAT_FLAG_PAD_ZERO) == FORMAT_FLAG_PAD_ZERO) && (NumDigits == 0)) {\r
 127         c = '0';\r
 128       } else {\r
 129         c = ' ';\r
 130       }\r
 131       while ((FieldWidth != 0) && (Width < FieldWidth--)) {\r
 132         _StoreChar(pBufferDesc, c);\r
 133         if (pBufferDesc->ReturnValue < 0) {\r
 134           return;\r
 135         }\r
 136       }\r
 137     }\r
 138   }\r
 139   //\r
 140   // Count how many digits are required by precision\r
 141   //\r
 142   while (((v / Digit) >= Base) | (NumDigits-- > 1)) {\r
 143     Digit *= Base;\r
 144   }\r
 145   //\r
 146   // Output digits\r
 147   //\r
 148   do {\r
 149     Div = v / Digit;\r
 150     v -= Div * Digit;\r
 151     _StoreChar(pBufferDesc, _aV2C[Div]);\r
 152     if (pBufferDesc->ReturnValue < 0) {\r
 153       break;\r
 154     }\r
 155     Digit /= Base;\r
 156   } while (Digit);\r
 157   //\r
 158   // Print trailing spaces if necessary\r
 159   //\r
 160   if ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == FORMAT_FLAG_LEFT_JUSTIFY) {\r
 161     if (FieldWidth != 0) {\r
 162       while ((FieldWidth != 0) && (Width < FieldWidth--)) {\r
 163         _StoreChar(pBufferDesc, ' ');\r
 164         if (pBufferDesc->ReturnValue < 0) {\r
 165           return;\r
 166         }\r
 167       }\r
 168     }\r
 169   }\r
 170 }\r
 171 \r
 172 /*********************************************************************\r
 173 *\r
 174 *       _PrintInt\r
 175 */\r
 176 static void _PrintInt(SEGGER_RTT_PRINTF_DESC * pBufferDesc, int v, unsigned Base, unsigned NumDigits, unsigned FieldWidth, unsigned FormatFlags) {\r
 177   unsigned Width;\r
 178   unsigned Number;\r
 179 \r
 180   Number = (v < 0) ? -v : v;\r
 181 \r
 182   //\r
 183   // Get actual field width\r
 184   //\r
 185   Width = 1;\r
 186   while (Number >= Base) {\r
 187     Number = (Number / Base);\r
 188     Width++;\r
 189   }\r
 190   if (NumDigits > Width) {\r
 191     Width = NumDigits;\r
 192   }\r
 193   if ((FieldWidth > 0) && ((v < 0) || ((FormatFlags & FORMAT_FLAG_PRINT_SIGN) == FORMAT_FLAG_PRINT_SIGN))) {\r
 194     FieldWidth--;\r
 195   }\r
 196 \r
 197   //\r
 198   // Print leading spaces if necessary\r
 199   //\r
 200   if ((((FormatFlags & FORMAT_FLAG_PAD_ZERO) == 0) || (NumDigits != 0)) && ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0)) {\r
 201     if (FieldWidth != 0) {\r
 202       while ((FieldWidth != 0) && (Width < FieldWidth--)) {\r
 203         _StoreChar(pBufferDesc, ' ');\r
 204         if (pBufferDesc->ReturnValue < 0) {\r
 205           return;\r
 206         }\r
 207       }\r
 208     }\r
 209   }\r
 210   //\r
 211   // Print sign if necessary\r
 212   //\r
 213   if (v < 0) {\r
 214     v = -v;\r
 215     _StoreChar(pBufferDesc, '-');\r
 216     if (pBufferDesc->ReturnValue < 0) {\r
 217       return;\r
 218     }\r
 219   } else if ((FormatFlags & FORMAT_FLAG_PRINT_SIGN) == FORMAT_FLAG_PRINT_SIGN) {\r
 220     _StoreChar(pBufferDesc, '+');\r
 221     if (pBufferDesc->ReturnValue < 0) {\r
 222       return;\r
 223     }\r
 224   }\r
 225   //\r
 226   // Print leading zeros if necessary\r
 227   //\r
 228   if (((FormatFlags & FORMAT_FLAG_PAD_ZERO) == FORMAT_FLAG_PAD_ZERO) && ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0) && (NumDigits == 0)) {\r
 229     if (FieldWidth != 0) {\r
 230       while ((FieldWidth != 0) && (Width < FieldWidth--)) {\r
 231         _StoreChar(pBufferDesc, '0');\r
 232         if (pBufferDesc->ReturnValue < 0) {\r
 233           return;\r
 234         }\r
 235       }\r
 236     }\r
 237   }\r
 238 \r
 239   //\r
 240   // Print number without sign\r
 241   //\r
 242   _PrintUnsigned(pBufferDesc, v, Base, NumDigits, FieldWidth, FormatFlags);\r
 243 }\r
 244 \r
 245 /*********************************************************************\r
 246 *\r
 247 *       Public code\r
 248 *\r
 249 **********************************************************************\r
 250 */\r
 251 /*********************************************************************\r
 252 *\r
 253 *       SEGGER_RTT_vprintf\r
 254 *\r
 255 *  Function description\r
 256 *    Stores a formatted string in SEGGER RTT control block.\r
 257 *    This data is read by the host.\r
 258 *\r
 259 *  Parameters\r
 260 *    BufferIndex  Index of "Up"-buffer to be used. (e.g. 0 for "Terminal")\r
 261 *    sFormat      Pointer to format string\r
 262 *    pParamList   Pointer to the list of arguments for the format string\r
 263 *\r
 264 *  Return values\r
 265 *    >= 0:  Number of bytes which have been stored in the "Up"-buffer.\r
 266 *     < 0:  Error\r
 267 */\r
 268 int SEGGER_RTT_vprintf(unsigned BufferIndex, const char * sFormat, va_list * pParamList) {\r
 269   char c;\r
 270   SEGGER_RTT_PRINTF_DESC BufferDesc;\r
 271   int v;\r
 272   unsigned NumDigits;\r
 273   unsigned FormatFlags;\r
 274   unsigned FieldWidth;\r
 275   char acBuffer[SEGGER_RTT_PRINTF_BUFFER_SIZE];\r
 276 \r
 277   BufferDesc.pBuffer        = acBuffer;\r
 278   BufferDesc.BufferSize     = SEGGER_RTT_PRINTF_BUFFER_SIZE;\r
 279   BufferDesc.Cnt            = 0;\r
 280   BufferDesc.RTTBufferIndex = BufferIndex;\r
 281   BufferDesc.ReturnValue    = 0;\r
 282 \r
 283   do {\r
 284     c = *sFormat++;\r
 285     if (c == 0) {\r
 286       break;\r
 287     }\r
 288     if (c == '%') {\r
 289       //\r
 290       // Filter out flags\r
 291       //\r
 292       FormatFlags = 0;\r
 293       do {\r
 294         c = *sFormat;\r
 295         switch (c) {\r
 296         case '-': FormatFlags |= FORMAT_FLAG_LEFT_JUSTIFY; sFormat++; break;\r
 297         case '0': FormatFlags |= FORMAT_FLAG_PAD_ZERO;     sFormat++; break;\r
 298         case '+': FormatFlags |= FORMAT_FLAG_PRINT_SIGN;   sFormat++; break;\r
 299         case '#': FormatFlags |= FORMAT_FLAG_ALTERNATE;    sFormat++; break;\r
 300         default:  goto FilterFieldWidth;                   break;\r
 301         }\r
 302       } while (1);\r
 303       //\r
 304       // filter out field with\r
 305       //\r
 306 FilterFieldWidth:\r
 307       FieldWidth = 0;\r
 308       do {\r
 309         c = *sFormat;\r
 310         if (c < '0' || c > '9') {\r
 311           break;\r
 312         }\r
 313         sFormat++;\r
 314         FieldWidth = FieldWidth * 10 + (c - '0');\r
 315       } while (1);\r
 316 \r
 317       //\r
 318       // Filter out precision (number of digits to display)\r
 319       //\r
 320       NumDigits = 0;\r
 321       c = *sFormat;\r
 322       if (c == '.') {\r
 323         sFormat++;\r
 324         do {\r
 325           c = *sFormat;\r
 326           if (c < '0' || c > '9') {\r
 327             break;\r
 328           }\r
 329           sFormat++;\r
 330           NumDigits = NumDigits * 10 + (c - '0');\r
 331         } while (1);\r
 332       }\r
 333       //\r
 334       // Filter out length modifier\r
 335       //\r
 336       c = *sFormat;\r
 337       do {\r
 338         if (c == 'l' || c == 'h') {\r
 339           c = *sFormat++;\r
 340           continue;\r
 341         }\r
 342         break;\r
 343       } while (1);\r
 344       //\r
 345       // Handle specifiers\r
 346       //\r
 347       switch (c) {\r
 348       case 'c': {\r
 349         char c0;\r
 350         v = va_arg(*pParamList, int);\r
 351         c0 = (char)v;\r
 352         _StoreChar(&BufferDesc, c0);\r
 353         break;\r
 354       }\r
 355       case 'd':\r
 356         v = va_arg(*pParamList, int);\r
 357         _PrintInt(&BufferDesc, v, 10, NumDigits, FieldWidth, FormatFlags);\r
 358         break;\r
 359       case 'u':\r
 360         v = va_arg(*pParamList, int);\r
 361         _PrintUnsigned(&BufferDesc, v, 10, NumDigits, FieldWidth, FormatFlags);\r
 362         break;\r
 363       case 'x':\r
 364       case 'X':\r
 365         v = va_arg(*pParamList, int);\r
 366         _PrintUnsigned(&BufferDesc, v, 16, NumDigits, FieldWidth, FormatFlags);\r
 367         break;\r
 368       case 's':\r
 369         {\r
 370           const char * s = va_arg(*pParamList, const char *);\r
 371           do {\r
 372             c = *s++;\r
 373             if (c == 0) {\r
 374               break;\r
 375             }\r
 376            _StoreChar(&BufferDesc, c);\r
 377           } while (BufferDesc.ReturnValue >= 0);\r
 378         }\r
 379         break;\r
 380       case 'p':\r
 381         v = va_arg(*pParamList, int);\r
 382         _PrintUnsigned(&BufferDesc, v, 16, 8, 8, 0);\r
 383         break;\r
 384       case '%':\r
 385         _StoreChar(&BufferDesc, '%');\r
 386         break;\r
 387       }\r
 388       sFormat++;\r
 389     } else {\r
 390       _StoreChar(&BufferDesc, c);\r
 391     }\r
 392   } while (BufferDesc.ReturnValue >= 0);\r
 393 \r
 394   if (BufferDesc.ReturnValue > 0) {\r
 395     //\r
 396     // Write remaining data, if any\r
 397     //\r
 398     if (BufferDesc.Cnt != 0) {\r
 399       SEGGER_RTT_Write(BufferIndex, acBuffer, BufferDesc.Cnt);\r
 400     }\r
 401     BufferDesc.ReturnValue += BufferDesc.Cnt;\r
 402   }\r
 403   return BufferDesc.ReturnValue;\r
 404 }\r
 405 \r
 406 /*********************************************************************\r
 407 *\r
 408 *       SEGGER_RTT_printf\r
 409 *\r
 410 *  Function description\r
 411 *    Stores a formatted string in SEGGER RTT control block.\r
 412 *    This data is read by the host.\r
 413 *\r
 414 *  Parameters\r
 415 *    BufferIndex  Index of "Up"-buffer to be used. (e.g. 0 for "Terminal")\r
 416 *    sFormat      Pointer to format string, followed by the arguments for conversion\r
 417 *\r
 418 *  Return values\r
 419 *    >= 0:  Number of bytes which have been stored in the "Up"-buffer.\r
 420 *     < 0:  Error\r
 421 *\r
 422 *  Notes\r
 423 *    (1) Conversion specifications have following syntax:\r
 424 *          %[flags][FieldWidth][.Precision]ConversionSpecifier\r
 425 *    (2) Supported flags:\r
 426 *          -: Left justify within the field width\r
 427 *          +: Always print sign extension for signed conversions\r
 428 *          0: Pad with 0 instead of spaces. Ignored when using '-'-flag or precision\r
 429 *        Supported conversion specifiers:\r
 430 *          c: Print the argument as one char\r
 431 *          d: Print the argument as a signed integer\r
 432 *          u: Print the argument as an unsigned integer\r
 433 *          x: Print the argument as an hexadecimal integer\r
 434 *          s: Print the string pointed to by the argument\r
 435 *          p: Print the argument as an 8-digit hexadecimal integer. (Argument shall be a pointer to void.)\r
 436 */\r
 437 int SEGGER_RTT_printf(unsigned BufferIndex, const char * sFormat, ...) {\r
 438   va_list ParamList;\r
 439 \r
 440   va_start(ParamList, sFormat);\r
 441   return SEGGER_RTT_vprintf(BufferIndex, sFormat, &ParamList);\r
 442 }\r
 443 /*************************** End of file ****************************/\r