Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
printf.c
Go to the documentation of this file.
1 /*
2  ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
3  2011,2012 Giovanni Di Sirio.
4 
5  This file is part of ChibiOS/RT.
6 
7  ChibiOS/RT is free software; you can redistribute it and/or modify
8  it under the terms of the GNU General Public License as published by
9  the Free Software Foundation; either version 3 of the License, or
10  (at your option) any later version.
11 
12  ChibiOS/RT is distributed in the hope that it will be useful,
13  but WITHOUT ANY WARRANTY; without even the implied warranty of
14  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  GNU General Public License for more details.
16 
17  You should have received a copy of the GNU General Public License
18  along with this program. If not, see <http://www.gnu.org/licenses/>.
19 */
20 /*
21  Concepts and parts of this file have been contributed by Fabio Utzig.
22  */
23 
33 #include <stdarg.h>
34 
35 #include <ch.h>
36 #include "printf.h"
37 #include <stdnoreturn.h>
38 
39 
40 #define MAX_FILLER 11
41 #define FLOAT_PRECISION 100000
42 
43 typedef struct {
44  union {
45  BaseSequentialStream *chp;
46  size_t size;
47  };
48  char *destBuf;
49  const char *fmt;
50  va_list ap;
52 
53 static thread_t *printThreadPtr = NULL;
54 
55 static THD_WORKING_AREA(waSerialPrint, 512);
56 
57 static noreturn void serialPrint(void *arg)
58 {
59 
60  (void)arg;
61  chRegSetThreadName("serialPrint");
62 
63  while (true) {
64  thread_t *sender = chMsgWait();
65  synchronous_print_arg_t *spat = (synchronous_print_arg_t *) chMsgGet(sender);
66  // do the print
67  if (spat->destBuf == NULL) {
68  directchvprintf(spat->chp, spat->fmt, spat->ap);
69  } else {
70  chvsnprintf(spat->destBuf, spat->size, spat->fmt, spat->ap);
71  }
72  chMsgRelease(sender, MSG_OK);
73  }
74 }
75 
76 
77 #if CHPRINTF_USE_FLOAT
78 static int intPow(int a, int b)
79 {
80  uint32_t c = a;
81  for (uint32_t n = b; n > 1; n--) { c *= a; }
82  return c;
83 }
84 #endif
85 
86 
87 
88 
89 
90 static char *long_to_string_with_divisor(char *p,
91  long num,
92  unsigned radix,
93  long divisor)
94 {
95  int i;
96  char *q;
97  long l, ll;
98 
99  l = num;
100  if (divisor == 0) {
101  ll = num;
102  } else {
103  ll = divisor;
104  }
105 
106  q = p + MAX_FILLER;
107  do {
108  i = (int)(l % radix);
109  i += '0';
110  if (i > '9') {
111  i += 'A' - '0' - 10;
112  }
113  *--q = i;
114  l /= radix;
115  } while ((ll /= radix) != 0);
116 
117  i = (int)(p + MAX_FILLER - q);
118  do {
119  *p++ = *q++;
120  } while (--i);
121 
122  return p;
123 }
124 
125 static char *ltoa(char *p, long num, unsigned radix)
126 {
127 
128  return long_to_string_with_divisor(p, num, radix, 0);
129 }
130 
131 #if CHPRINTF_USE_FLOAT
132 static char *ftoa(char *p, double num, uint32_t precision)
133 {
134  long l;
135  // unsigned long precision = FLOAT_PRECISION;
136 
137  l = num;
138  p = long_to_string_with_divisor(p, l, 10, 0);
139  *p++ = '.';
140  l = (num - l) * precision;
141  return long_to_string_with_divisor(p, l, 10, precision / 10);
142 }
143 #endif
144 
169 static void _chvsnprintf(char *buffer, BaseSequentialStream *chp, size_t size, const char *fmt, va_list ap)
170 {
171  char *p, *s, c, filler;
172  int i, precision, width;
173  bool is_long, left_align, plus_on_float;
174  long l;
175 #if CHPRINTF_USE_FLOAT
176  int fprec = 0;
177  double d;
178  char tmpbuf[2 * MAX_FILLER + 1];
179 #else
180  char tmpbuf[MAX_FILLER + 1];
181 #endif
182 
183  // return true if space exhausted
184  bool _putChar(const char _c) {
185  if (buffer != NULL) {
186  if (size) {
187  *buffer = _c;
188  buffer++;
189  return (--size == 0);
190  } else {
191  return true;
192  }
193  } else if (chp != NULL) {
194  streamPut(chp, _c);
195  return false;
196  }
197  return false;
198  }
199 
200 
201 
202 
203  while (true) {
204  c = *fmt++;
205  if (c == 0) {
206  // only add end of string marker when filling buffer, not when outputing on I/O channel
207  if (buffer != NULL) {
208  _putChar(0);
209  }
210  return;
211  }
212  if (c != '%') {
213  if (_putChar(c)) { return; }
214  continue;
215  }
216  p = tmpbuf;
217  s = tmpbuf;
218  left_align = plus_on_float = false;
219  if (*fmt == '-') {
220  fmt++;
221  left_align = true;
222  } else if (*fmt == '+') {
223  fmt++;
224  plus_on_float = true;
225  }
226  filler = ' ';
227  if (*fmt == '.') {
228  fmt++;
229  filler = '0';
230 #if CHPRINTF_USE_FLOAT
231  fprec = intPow(10, (*fmt) - '0');
232 #endif
233  }
234  width = 0;
235  while (true) {
236  c = *fmt++;
237  if (c >= '0' && c <= '9') {
238  c -= '0';
239  } else if (c == '*') {
240  c = va_arg(ap, int);
241  } else {
242  break;
243  }
244  width = width * 10 + c;
245  }
246  precision = 0;
247  if (c == '.') {
248  while (true) {
249  c = *fmt++;
250  if (c >= '0' && c <= '9') {
251  c -= '0';
252 #if CHPRINTF_USE_FLOAT
253  fprec = intPow(10, c);
254 #endif
255  } else if (c == '*') {
256  c = va_arg(ap, int);
257  } else {
258  break;
259  }
260  precision *= 10;
261  precision += c;
262  }
263  }
264  /* Long modifier.*/
265  if (c == 'l' || c == 'L') {
266  is_long = true;
267  if (*fmt) {
268  c = *fmt++;
269  }
270  } else {
271  is_long = (c >= 'A') && (c <= 'Z');
272  }
273 
274  /* Command decoding.*/
275  switch (c) {
276  case 'c':
277  filler = ' ';
278  *p++ = va_arg(ap, int);
279  break;
280  case 's':
281  filler = ' ';
282  if ((s = va_arg(ap, char *)) == 0) {
283  s = "(null)";
284  }
285  if (precision == 0) {
286  precision = 32767;
287  }
288  for (p = s; *p && (--precision >= 0); p++)
289  ;
290  break;
291  case 'D':
292  case 'd':
293  if (is_long) {
294  l = va_arg(ap, long);
295  } else {
296  l = va_arg(ap, int);
297  }
298  if (l < 0) {
299  *p++ = '-';
300  l = -l;
301  }
302  p = ltoa(p, l, 10);
303  break;
304 #if CHPRINTF_USE_FLOAT
305  case 'f':
306  d = (double) va_arg(ap, double);
307  if (d < 0) {
308  *p++ = '-';
309  d = -d;
310  } else if (plus_on_float) {
311  *p++ = '+';
312  }
313  p = ftoa(p, d, fprec);
314  break;
315 #endif
316  case 'X':
317  case 'x':
318  c = 16;
319  goto unsigned_common;
320  case 'U':
321  case 'u':
322  c = 10;
323  goto unsigned_common;
324  case 'O':
325  case 'o':
326  c = 8;
327 unsigned_common:
328  if (is_long) {
329  l = va_arg(ap, long);
330  } else {
331  l = va_arg(ap, int);
332  }
333  p = ltoa(p, l, c);
334  break;
335  default:
336  *p++ = c;
337  break;
338  }
339  i = (int)(p - s);
340  if ((width -= i) < 0) {
341  width = 0;
342  }
343  if (left_align == false) {
344  width = -width;
345  }
346  if (width < 0) {
347  if (*s == '-' && filler == '0') {
348  if (_putChar((uint8_t)*s++)) { return; }
349  i--;
350  }
351  do
352  if (_putChar((uint8_t)filler)) { return; }
353  while (++width != 0);
354  }
355  while (--i >= 0)
356  if (_putChar((uint8_t)*s++)) { return; }
357 
358  while (width) {
359  if (_putChar((uint8_t)filler)) { return; }
360  width--;
361  }
362  }
363  _putChar(0) ;
364 }
365 
366 
367 void directchvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap)
368 {
369  _chvsnprintf(NULL, chp, 0, fmt, ap);
370 }
371 
372 void chvsnprintf(char *buffer, size_t size, const char *fmt, va_list ap)
373 {
374  _chvsnprintf(buffer, NULL, size, fmt, ap);
375 }
376 
377 void chsnprintf(char *buffer, size_t size, const char *fmt, ...)
378 {
379  va_list ap;
380 
381  va_start(ap, fmt);
382  _chvsnprintf(buffer, NULL, size, fmt, ap);
383  va_end(ap);
384 }
385 
386 void directchprintf(BaseSequentialStream *chp, const char *fmt, ...)
387 {
388  va_list ap;
389 
390  va_start(ap, fmt);
391  _chvsnprintf(NULL, chp, 0, fmt, ap);
392  va_end(ap);
393 }
394 
395 void chprintf(BaseSequentialStream *lchp, const char *fmt, ...)
396 {
397  va_list ap;
398 
399  if (printThreadPtr == NULL) {
400  printThreadPtr = chThdCreateStatic(waSerialPrint, sizeof(waSerialPrint), NORMALPRIO + 1, serialPrint, NULL);
401  }
402 
403  va_start(ap, fmt);
404  synchronous_print_arg_t spat = {.chp = lchp,
405  .destBuf = NULL,
406  .fmt = fmt,
407  .ap = ap
408  };
409 
410  chMsgSend(printThreadPtr, (msg_t) &spat);
411 
412  va_end(ap);
413 }
414 
415 void chvprintf(BaseSequentialStream *lchp, const char *fmt, va_list ap)
416 {
417  if (printThreadPtr == NULL) {
418  printThreadPtr = chThdCreateStatic(waSerialPrint, sizeof(waSerialPrint), NORMALPRIO + 1, serialPrint, NULL);
419  }
420 
421  synchronous_print_arg_t spat = {.chp = lchp,
422  .destBuf = NULL,
423  .fmt = fmt,
424  .ap = ap
425  };
426 
427  chMsgSend(printThreadPtr, (msg_t) &spat);
428 }
429 
430 
431 
432 void smchsnprintf(char *buffer, size_t size, const char *fmt, ...)
433 {
434  va_list ap;
435 
436  if (printThreadPtr == NULL) {
437  printThreadPtr = chThdCreateStatic(waSerialPrint, sizeof(waSerialPrint), NORMALPRIO + 1, serialPrint, NULL);
438  }
439 
440  va_start(ap, fmt);
441  synchronous_print_arg_t spat = {.size = size,
442  .destBuf = buffer,
443  .fmt = fmt,
444  .ap = ap
445  };
446 
447  chMsgSend(printThreadPtr, (msg_t) &spat);
448 
449  va_end(ap);
450 }
451 
452 
453 void smchvsnprintf(char *buffer, size_t size, const char *fmt, va_list ap)
454 {
455  if (printThreadPtr == NULL) {
456  printThreadPtr = chThdCreateStatic(waSerialPrint, sizeof(waSerialPrint), NORMALPRIO + 1, serialPrint, NULL);
457  }
458 
459  synchronous_print_arg_t spat = {.size = size,
460  .destBuf = buffer,
461  .fmt = fmt,
462  .ap = ap
463  };
464 
465  chMsgSend(printThreadPtr, (msg_t) &spat);
466 }
467 
468 
const char * fmt
Definition: printf.c:49
static thread_t * printThreadPtr
Definition: printf.c:53
static char * long_to_string_with_divisor(char *p, long num, unsigned radix, long divisor)
Definition: printf.c:90
void smchsnprintf(char *buffer, size_t size, const char *fmt,...)
Definition: printf.c:432
void chvsnprintf(char *buffer, size_t size, const char *fmt, va_list ap)
Definition: printf.c:372
void directchvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap)
Definition: printf.c:367
void directchprintf(BaseSequentialStream *chp, const char *fmt,...)
Definition: printf.c:386
#define MAX_FILLER
Definition: printf.c:40
void smchvsnprintf(char *buffer, size_t size, const char *fmt, va_list ap)
Definition: printf.c:453
static void _chvsnprintf(char *buffer, BaseSequentialStream *chp, size_t size, const char *fmt, va_list ap)
System formatted output function.
Definition: printf.c:169
static THD_WORKING_AREA(waSerialPrint, 512)
void chsnprintf(char *buffer, size_t size, const char *fmt,...)
Definition: printf.c:377
static char * ltoa(char *p, long num, unsigned radix)
Definition: printf.c:125
void chprintf(BaseSequentialStream *lchp, const char *fmt,...)
Definition: printf.c:395
void chvprintf(BaseSequentialStream *lchp, const char *fmt, va_list ap)
Definition: printf.c:415
static noreturn void serialPrint(void *arg)
Definition: printf.c:57
static float p[2][2]
static uint32_t s
Mini printf-like functionality.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float b
Definition: wedgebug.c:202