Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
43typedef struct {
44 union {
46 size_t size;
47 };
48 char *destBuf;
49 const char *fmt;
52
54
56
57static noreturn void serialPrint(void *arg)
58{
59
60 (void)arg;
61 chRegSetThreadName("serialPrint");
62
63 while (true) {
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 }
73 }
74}
75
76
77#if CHPRINTF_USE_FLOAT
78static 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
90static 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
125static char *ltoa(char *p, long num, unsigned radix)
126{
127
129}
130
131#if CHPRINTF_USE_FLOAT
132static 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
169static 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;
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;
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
367void directchvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap)
368{
369 _chvsnprintf(NULL, chp, 0, fmt, ap);
370}
371
372void chvsnprintf(char *buffer, size_t size, const char *fmt, va_list ap)
373{
374 _chvsnprintf(buffer, NULL, size, fmt, ap);
375}
376
377void 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
386void 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
395void chprintf(BaseSequentialStream *lchp, const char *fmt, ...)
396{
397 va_list ap;
398
399 if (printThreadPtr == NULL) {
401 }
402
403 va_start(ap, fmt);
405 .destBuf = NULL,
406 .fmt = fmt,
407 .ap = ap
408 };
409
411
412 va_end(ap);
413}
414
415void chvprintf(BaseSequentialStream *lchp, const char *fmt, va_list ap)
416{
417 if (printThreadPtr == NULL) {
419 }
420
422 .destBuf = NULL,
423 .fmt = fmt,
424 .ap = ap
425 };
426
428}
429
430
431
432void smchsnprintf(char *buffer, size_t size, const char *fmt, ...)
433{
434 va_list ap;
435
436 if (printThreadPtr == 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
448
449 va_end(ap);
450}
451
452
453void smchvsnprintf(char *buffer, size_t size, const char *fmt, va_list ap)
454{
455 if (printThreadPtr == NULL) {
457 }
458
459 synchronous_print_arg_t spat = {.size = size,
460 .destBuf = buffer,
461 .fmt = fmt,
462 .ap = ap
463 };
464
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
static char * ltoa(char *p, long num, unsigned radix)
Definition printf.c:125
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
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
uint16_t foo
Definition main_demo5.c:58
Mini printf-like functionality.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
float b
Definition wedgebug.c:202