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