1 /*
2 * Copyright (c) 2006-2024, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Change Logs:
7 * Date Author Notes
8 * 2024-11-19 Meco Man the first version
9 */
10
11 #include <rtthread.h>
12
13 #define _ISDIGIT(c) ((unsigned)((c) - '0') < 10)
14
15 /**
16 * @brief This function will duplicate a string.
17 *
18 * @param n is the string to be duplicated.
19 *
20 * @param base is support divide instructions value.
21 *
22 * @return the duplicated string pointer.
23 */
24 #ifdef RT_KLIBC_USING_VSNPRINTF_LONGLONG
divide(unsigned long long * n,int base)25 rt_inline int divide(unsigned long long *n, int base)
26 #else
27 rt_inline int divide(unsigned long *n, int base)
28 #endif /* RT_KLIBC_USING_VSNPRINTF_LONGLONG */
29 {
30 int res;
31
32 /* optimized for processor which does not support divide instructions. */
33 #ifdef RT_KLIBC_USING_VSNPRINTF_LONGLONG
34 res = (int)((*n) % base);
35 *n = (long long)((*n) / base);
36 #else
37 res = (int)((*n) % base);
38 *n = (long)((*n) / base);
39 #endif
40
41 return res;
42 }
43
skip_atoi(const char ** s)44 rt_inline int skip_atoi(const char **s)
45 {
46 int i = 0;
47 while (_ISDIGIT(**s))
48 i = i * 10 + *((*s)++) - '0';
49
50 return i;
51 }
52
53 #define ZEROPAD (1 << 0) /* pad with zero */
54 #define SIGN (1 << 1) /* unsigned/signed long */
55 #define PLUS (1 << 2) /* show plus */
56 #define SPACE (1 << 3) /* space if plus */
57 #define LEFT (1 << 4) /* left justified */
58 #define SPECIAL (1 << 5) /* 0x */
59 #define LARGE (1 << 6) /* use 'ABCDEF' instead of 'abcdef' */
60
print_number(char * buf,char * end,unsigned long long num,int base,int qualifier,int s,int precision,int type)61 static char *print_number(char *buf,
62 char *end,
63 #ifdef RT_KLIBC_USING_VSNPRINTF_LONGLONG
64 unsigned long long num,
65 #else
66 unsigned long num,
67 #endif /* RT_KLIBC_USING_VSNPRINTF_LONGLONG */
68 int base,
69 int qualifier,
70 int s,
71 int precision,
72 int type)
73 {
74 char c = 0, sign = 0;
75 #ifdef RT_KLIBC_USING_VSNPRINTF_LONGLONG
76 char tmp[64] = {0};
77 #else
78 char tmp[32] = {0};
79 #endif /* RT_KLIBC_USING_VSNPRINTF_LONGLONG */
80 int precision_bak = precision;
81 const char *digits = RT_NULL;
82 static const char small_digits[] = "0123456789abcdef";
83 static const char large_digits[] = "0123456789ABCDEF";
84 int i = 0;
85 int size = 0;
86
87 size = s;
88
89 digits = (type & LARGE) ? large_digits : small_digits;
90 if (type & LEFT)
91 {
92 type &= ~ZEROPAD;
93 }
94
95 c = (type & ZEROPAD) ? '0' : ' ';
96
97 /* get sign */
98 sign = 0;
99 if (type & SIGN)
100 {
101 switch (qualifier)
102 {
103 case 'h':
104 if ((rt_int16_t)num < 0)
105 {
106 sign = '-';
107 num = (rt_uint16_t)-num;
108 }
109 break;
110 case 'L':
111 case 'l':
112 if ((long)num < 0)
113 {
114 sign = '-';
115 num = (unsigned long)-num;
116 }
117 break;
118 case 0:
119 default:
120 if ((rt_int32_t)num < 0)
121 {
122 sign = '-';
123 num = (rt_uint32_t)-num;
124 }
125 break;
126 }
127
128 if (sign != '-')
129 {
130 if (type & PLUS)
131 {
132 sign = '+';
133 }
134 else if (type & SPACE)
135 {
136 sign = ' ';
137 }
138 }
139 }
140
141 if (type & SPECIAL)
142 {
143 if (base == 2 || base == 16)
144 {
145 size -= 2;
146 }
147 else if (base == 8)
148 {
149 size--;
150 }
151 }
152
153 i = 0;
154 if (num == 0)
155 {
156 tmp[i++] = '0';
157 }
158 else
159 {
160 while (num != 0)
161 tmp[i++] = digits[divide(&num, base)];
162 }
163
164 if (i > precision)
165 {
166 precision = i;
167 }
168 size -= precision;
169
170 if (!(type & (ZEROPAD | LEFT)))
171 {
172 if ((sign) && (size > 0))
173 {
174 size--;
175 }
176
177 while (size-- > 0)
178 {
179 if (buf < end)
180 {
181 *buf = ' ';
182 }
183
184 ++ buf;
185 }
186 }
187
188 if (sign)
189 {
190 if (buf < end)
191 {
192 *buf = sign;
193 }
194 -- size;
195 ++ buf;
196 }
197
198 if (type & SPECIAL)
199 {
200 if (base == 2)
201 {
202 if (buf < end)
203 *buf = '0';
204 ++ buf;
205 if (buf < end)
206 *buf = 'b';
207 ++ buf;
208 }
209 else if (base == 8)
210 {
211 if (buf < end)
212 *buf = '0';
213 ++ buf;
214 }
215 else if (base == 16)
216 {
217 if (buf < end)
218 {
219 *buf = '0';
220 }
221
222 ++ buf;
223 if (buf < end)
224 {
225 *buf = type & LARGE ? 'X' : 'x';
226 }
227 ++ buf;
228 }
229 }
230
231 /* no align to the left */
232 if (!(type & LEFT))
233 {
234 while (size-- > 0)
235 {
236 if (buf < end)
237 {
238 *buf = c;
239 }
240
241 ++ buf;
242 }
243 }
244
245 while (i < precision--)
246 {
247 if (buf < end)
248 {
249 *buf = '0';
250 }
251
252 ++ buf;
253 }
254
255 /* put number in the temporary buffer */
256 while (i-- > 0 && (precision_bak != 0))
257 {
258 if (buf < end)
259 {
260 *buf = tmp[i];
261 }
262
263 ++ buf;
264 }
265
266 while (size-- > 0)
267 {
268 if (buf < end)
269 {
270 *buf = ' ';
271 }
272
273 ++ buf;
274 }
275
276 return buf;
277 }
278
279 #if (defined(__GNUC__) && !defined(__ARMCC_VERSION) /* GCC */) && (__GNUC__ >= 7)
280 /* Disable "-Wimplicit-fallthrough" below GNUC V7 */
281 #pragma GCC diagnostic push
282 /* ignore warning: this statement may fall through */
283 #pragma GCC diagnostic ignored "-Wimplicit-fallthrough"
284 #endif /* (defined(__GNUC__) && !defined(__ARMCC_VERSION)) && (__GNUC__ >= 7 */
285 /**
286 * @brief This function will fill a formatted string to buffer.
287 *
288 * @param buf is the buffer to save formatted string.
289 *
290 * @param size is the size of buffer.
291 *
292 * @param fmt is the format parameters.
293 *
294 * @param args is a list of variable parameters.
295 *
296 * @return The number of characters actually written to buffer.
297 */
rt_vsnprintf(char * buf,rt_size_t size,const char * fmt,va_list args)298 int rt_vsnprintf(char *buf, rt_size_t size, const char *fmt, va_list args)
299 {
300 #ifdef RT_KLIBC_USING_VSNPRINTF_LONGLONG
301 unsigned long long num = 0;
302 #else
303 unsigned long num = 0;
304 #endif /* RT_KLIBC_USING_VSNPRINTF_LONGLONG */
305 int i = 0, len = 0;
306 char *str = RT_NULL, *end = RT_NULL, c = 0;
307 const char *s = RT_NULL;
308
309 rt_uint8_t base = 0; /* the base of number */
310 rt_uint8_t flags = 0; /* flags to print number */
311 rt_uint8_t qualifier = 0; /* 'h', 'l', or 'L' for integer fields */
312 rt_int32_t field_width = 0; /* width of output field */
313 int precision = 0; /* min. # of digits for integers and max for a string */
314
315 str = buf;
316 end = buf + size;
317
318 /* Make sure end is always >= buf */
319 if (end < buf)
320 {
321 end = ((char *) - 1);
322 size = end - buf;
323 }
324
325 for (; *fmt ; ++fmt)
326 {
327 if (*fmt != '%')
328 {
329 if (str < end)
330 {
331 *str = *fmt;
332 }
333
334 ++ str;
335 continue;
336 }
337
338 /* process flags */
339 flags = 0;
340
341 while (1)
342 {
343 /* skips the first '%' also */
344 ++fmt;
345 if (*fmt == '-') flags |= LEFT;
346 else if (*fmt == '+') flags |= PLUS;
347 else if (*fmt == ' ') flags |= SPACE;
348 else if (*fmt == '#') flags |= SPECIAL;
349 else if (*fmt == '0') flags |= ZEROPAD;
350 else break;
351 }
352
353 /* get field width */
354 field_width = -1;
355 if (_ISDIGIT(*fmt))
356 {
357 field_width = skip_atoi(&fmt);
358 }
359 else if (*fmt == '*')
360 {
361 ++fmt;
362 /* it's the next argument */
363 field_width = va_arg(args, int);
364 if (field_width < 0)
365 {
366 field_width = -field_width;
367 flags |= LEFT;
368 }
369 }
370
371 /* get the precision */
372 precision = -1;
373 if (*fmt == '.')
374 {
375 ++fmt;
376 if (_ISDIGIT(*fmt))
377 {
378 precision = skip_atoi(&fmt);
379 }
380 else if (*fmt == '*')
381 {
382 ++fmt;
383 /* it's the next argument */
384 precision = va_arg(args, int);
385 }
386 if (precision < 0)
387 {
388 precision = 0;
389 }
390 }
391
392 qualifier = 0; /* get the conversion qualifier */
393
394 if (*fmt == 'h' || *fmt == 'l' ||
395 #ifdef RT_KLIBC_USING_VSNPRINTF_LONGLONG
396 *fmt == 'L' ||
397 #endif /* RT_KLIBC_USING_VSNPRINTF_LONGLONG */
398 *fmt == 'z')
399 {
400 qualifier = *fmt;
401 ++fmt;
402 #ifdef RT_KLIBC_USING_VSNPRINTF_LONGLONG
403 if (qualifier == 'l' && *fmt == 'l')
404 {
405 qualifier = 'L';
406 ++fmt;
407 }
408 #endif /* RT_KLIBC_USING_VSNPRINTF_LONGLONG */
409 if (qualifier == 'h' && *fmt == 'h')
410 {
411 qualifier = 'H';
412 ++fmt;
413 }
414 }
415
416 /* the default base */
417 base = 10;
418
419 switch (*fmt)
420 {
421 case 'c':
422 if (!(flags & LEFT))
423 {
424 while (--field_width > 0)
425 {
426 if (str < end) *str = ' ';
427 ++ str;
428 }
429 }
430
431 /* get character */
432 c = (rt_uint8_t)va_arg(args, int);
433 if (str < end)
434 {
435 *str = c;
436 }
437 ++ str;
438
439 /* put width */
440 while (--field_width > 0)
441 {
442 if (str < end) *str = ' ';
443 ++ str;
444 }
445 continue;
446
447 case 's':
448 s = va_arg(args, char *);
449 if (!s)
450 {
451 s = "(null)";
452 }
453
454 for (len = 0; (len != field_width) && (s[len] != '\0'); len++);
455
456 if (precision > 0 && len > precision)
457 {
458 len = precision;
459 }
460
461 if (!(flags & LEFT))
462 {
463 while (len < field_width--)
464 {
465 if (str < end) *str = ' ';
466 ++ str;
467 }
468 }
469
470 for (i = 0; i < len; ++i)
471 {
472 if (str < end) *str = *s;
473 ++ str;
474 ++ s;
475 }
476
477 while (len < field_width--)
478 {
479 if (str < end) *str = ' ';
480 ++ str;
481 }
482 continue;
483
484 case 'p':
485 if (field_width == -1)
486 {
487 field_width = sizeof(void *) << 1;
488 field_width += 2; /* `0x` prefix */
489 flags |= SPECIAL;
490 flags |= ZEROPAD;
491 }
492 str = print_number(str, end, (unsigned long)va_arg(args, void *),
493 16, qualifier, field_width, precision, flags);
494 continue;
495
496 case '%':
497 if (str < end)
498 {
499 *str = '%';
500 }
501 ++ str;
502 continue;
503
504 /* integer number formats - set up the flags and "break" */
505 case 'b':
506 base = 2;
507 break;
508 case 'o':
509 base = 8;
510 break;
511
512 case 'X':
513 flags |= LARGE;
514 case 'x':
515 base = 16;
516 break;
517
518 case 'd':
519 case 'i':
520 flags |= SIGN;
521 case 'u':
522 break;
523
524 case 'e':
525 case 'E':
526 case 'G':
527 case 'g':
528 case 'f':
529 case 'F':
530 va_arg(args, double);
531 default:
532 if (str < end)
533 {
534 *str = '%';
535 }
536 ++ str;
537
538 if (*fmt)
539 {
540 if (str < end)
541 {
542 *str = *fmt;
543 }
544 ++ str;
545 }
546 else
547 {
548 -- fmt;
549 }
550 continue;
551 }
552
553 if (qualifier == 'L')
554 {
555 num = va_arg(args, unsigned long long);
556 }
557 else if (qualifier == 'l')
558 {
559 num = va_arg(args, unsigned long);
560 }
561 else if (qualifier == 'H')
562 {
563 num = (rt_int8_t)va_arg(args, rt_int32_t);
564 if (flags & SIGN)
565 {
566 num = (rt_int8_t)num;
567 }
568 }
569 else if (qualifier == 'h')
570 {
571 num = (rt_uint16_t)va_arg(args, rt_int32_t);
572 if (flags & SIGN)
573 {
574 num = (rt_int16_t)num;
575 }
576 }
577 else if (qualifier == 'z')
578 {
579 num = va_arg(args, rt_size_t);
580 if (flags & SIGN)
581 {
582 num = (rt_ssize_t)num;
583 }
584 }
585 else
586 {
587 num = (rt_uint32_t)va_arg(args, unsigned long);
588 }
589 str = print_number(str, end, num, base, qualifier, field_width, precision, flags);
590 }
591
592 if (size > 0)
593 {
594 if (str < end)
595 {
596 *str = '\0';
597 }
598 else
599 {
600 end[-1] = '\0';
601 }
602 }
603
604 /* the trailing null byte doesn't count towards the total
605 * ++str;
606 */
607 return str - buf;
608 }
609 #if (defined(__GNUC__) && !defined(__ARMCC_VERSION) /* GCC */) && (__GNUC__ >= 7)
610 #pragma GCC diagnostic pop /* ignored "-Wimplicit-fallthrough" */
611 #endif /* (defined(__GNUC__) && !defined(__ARMCC_VERSION)) && (__GNUC__ >= 7 */
612