1 /*
2 * Copyright (c) 2010, 2013-2014 Wind River Systems, Inc.
3 * Copyright (c) 2020 Nordic Semiconductor ASA
4 * Copyright (c) 2021 BayLibre, SAS
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <stdarg.h>
10 #include <stdint.h>
11 #include <string.h>
12 #include <zephyr/sys/cbprintf.h>
13 #include <sys/types.h>
14 #include <zephyr/sys/util.h>
15
16 #ifdef CONFIG_CBPRINTF_FULL_INTEGRAL
17 typedef intmax_t int_value_type;
18 typedef uintmax_t uint_value_type;
19 #define DIGITS_BUFLEN 21
20 BUILD_ASSERT(sizeof(uint_value_type) <= 8U,
21 "DIGITS_BUFLEN may be incorrect");
22 #else
23 typedef int32_t int_value_type;
24 typedef uint32_t uint_value_type;
25 #define DIGITS_BUFLEN 10
26 #endif
27
28 #define ALPHA(fmt) (((fmt) & 0x60) - '0' - 10 + 1)
29
30 /* Convert value to string, storing characters downwards */
convert_value(uint_value_type num,unsigned int base,unsigned int alpha,char * buftop)31 static inline int convert_value(uint_value_type num, unsigned int base,
32 unsigned int alpha, char *buftop)
33 {
34 int i = 0;
35
36 do {
37 unsigned int c = num % base;
38 if (c >= 10) {
39 c += alpha;
40 }
41 buftop[i--] = c + '0';
42 num /= base;
43 } while (num);
44
45 return -i;
46 }
47
48 #define OUTC(_c) do { \
49 out((int)(_c), ctx); \
50 if (IS_ENABLED(CONFIG_CBPRINTF_LIBC_SUBSTS)) { \
51 ++count; \
52 } \
53 } while (false)
54
55 #define PAD_ZERO BIT(0)
56 #define PAD_TAIL BIT(1)
57
58 /* Skip over the argument tag if needed as it is not being used here. */
59 #define SKIP_TAG_IF_NEEDED(ap, tagged_ap) \
60 do { \
61 if (IS_ENABLED(CONFIG_CBPRINTF_PACKAGE_SUPPORT_TAGGED_ARGUMENTS) \
62 && tagged_ap) { \
63 (void)va_arg(ap, int); \
64 } \
65 } while (0)
66
67 /**
68 * @brief Printk internals
69 *
70 * See printk() for description.
71 * @param fmt Format string
72 * @param ap Variable parameters
73 *
74 * @return printed byte count if CONFIG_CBPRINTF_LIBC_SUBSTS is set
75 */
z_cbvprintf_impl(cbprintf_cb __out,void * ctx,const char * fmt,va_list ap,uint32_t flags)76 int z_cbvprintf_impl(cbprintf_cb __out, void *ctx, const char *fmt,
77 va_list ap, uint32_t flags)
78 {
79 size_t count = 0;
80 char buf[DIGITS_BUFLEN];
81 char *prefix, *data;
82 int min_width, precision, data_len;
83 char padding_mode, length_mod, special;
84 cbprintf_cb_local out = __out;
85
86 const bool tagged_ap = (flags & Z_CBVPRINTF_PROCESS_FLAG_TAGGED_ARGS)
87 == Z_CBVPRINTF_PROCESS_FLAG_TAGGED_ARGS;
88
89 /* we pre-increment in the loop afterwards */
90 fmt--;
91
92 start:
93 while (*++fmt != '%') {
94 if (*fmt == '\0') {
95 return count;
96 }
97 OUTC(*fmt);
98 }
99
100 min_width = -1;
101 precision = -1;
102 prefix = "";
103 padding_mode = 0;
104 length_mod = 0;
105 special = 0;
106
107 for (fmt++ ; ; fmt++) {
108 switch (*fmt) {
109 case 0:
110 return count;
111
112 case '%':
113 OUTC('%');
114 goto start;
115
116 case '-':
117 padding_mode = PAD_TAIL;
118 continue;
119
120 case '.':
121 precision = 0;
122 padding_mode &= (char)~PAD_ZERO;
123 continue;
124
125 case '0':
126 if (min_width < 0 && precision < 0 && !padding_mode) {
127 padding_mode = PAD_ZERO;
128 continue;
129 }
130 __fallthrough;
131
132 case '1':
133 case '2':
134 case '3':
135 case '4':
136 case '5':
137 case '6':
138 case '7':
139 case '8':
140 case '9':
141 if (precision >= 0) {
142 precision = 10 * precision + *fmt - '0';
143 } else {
144 if (min_width < 0) {
145 min_width = 0;
146 }
147 min_width = 10 * min_width + *fmt - '0';
148 }
149 continue;
150
151 case '*':
152 SKIP_TAG_IF_NEEDED(ap, tagged_ap);
153
154 if (precision >= 0) {
155 precision = va_arg(ap, int);
156 } else {
157 min_width = va_arg(ap, int);
158 if (min_width < 0) {
159 min_width = -min_width;
160 padding_mode = PAD_TAIL;
161 }
162 }
163 continue;
164
165 case '+':
166 case ' ':
167 case '#':
168 special = *fmt;
169 continue;
170
171 case 'h':
172 case 'l':
173 case 'z':
174 if (*fmt == 'h' && length_mod == 'h') {
175 length_mod = 'H';
176 } else if (*fmt == 'l' && length_mod == 'l') {
177 length_mod = 'L';
178 } else if (length_mod == '\0') {
179 length_mod = *fmt;
180 } else {
181 OUTC('%');
182 OUTC(*fmt);
183 goto start;
184 }
185 continue;
186
187 case 'd':
188 case 'i':
189 case 'u': {
190 uint_value_type d;
191
192 SKIP_TAG_IF_NEEDED(ap, tagged_ap);
193
194 if (length_mod == 'z') {
195 if (*fmt == 'u') {
196 d = va_arg(ap, size_t);
197 } else {
198 d = va_arg(ap, ssize_t);
199 }
200 } else if (length_mod == 'l') {
201 if (*fmt == 'u') {
202 d = va_arg(ap, unsigned long);
203 } else {
204 d = va_arg(ap, long);
205 }
206 } else if (length_mod == 'L') {
207 if (*fmt == 'u') {
208 unsigned long long llu =
209 va_arg(ap, unsigned long long);
210
211 if (llu != (uint_value_type) llu) {
212 data = "ERR";
213 data_len = 3;
214 precision = 0;
215 break;
216 }
217 d = (uint_value_type) llu;
218 } else {
219 long long lld = va_arg(ap, long long);
220
221 if (lld != (int_value_type) lld) {
222 data = "ERR";
223 data_len = 3;
224 precision = 0;
225 break;
226 }
227 d = (int_value_type) lld;
228 }
229 } else if (*fmt == 'u') {
230 d = va_arg(ap, unsigned int);
231 } else {
232 d = va_arg(ap, int);
233 }
234
235 if (*fmt != 'u' && (int_value_type)d < 0) {
236 d = -d;
237 prefix = "-";
238 min_width--;
239 } else if (special == ' ') {
240 prefix = " ";
241 min_width--;
242 } else if (special == '+') {
243 prefix = "+";
244 min_width--;
245 } else {
246 ;
247 }
248 data_len = convert_value(d, 10, 0, buf + sizeof(buf) - 1);
249 data = buf + sizeof(buf) - data_len;
250 break;
251 }
252
253 case 'p':
254 case 'x':
255 case 'X': {
256 uint_value_type x;
257
258 SKIP_TAG_IF_NEEDED(ap, tagged_ap);
259
260 if (*fmt == 'p') {
261 x = (uintptr_t)va_arg(ap, void *);
262 if (x == (uint_value_type)0) {
263 data = "(nil)";
264 data_len = 5;
265 precision = 0;
266 break;
267 }
268 special = '#';
269 } else if (length_mod == 'l') {
270 x = va_arg(ap, unsigned long);
271 } else if (length_mod == 'L') {
272 unsigned long long llx =
273 va_arg(ap, unsigned long long);
274
275 if (llx != (uint_value_type) llx) {
276 data = "ERR";
277 data_len = 3;
278 precision = 0;
279 break;
280 }
281 x = (uint_value_type) llx;
282 } else {
283 x = va_arg(ap, unsigned int);
284 }
285 if (special == '#') {
286 prefix = (*fmt & 0x20) ? "0x" : "0X";
287 min_width -= 2;
288 }
289 data_len = convert_value(x, 16, ALPHA(*fmt),
290 buf + sizeof(buf) - 1);
291 data = buf + sizeof(buf) - data_len;
292 break;
293 }
294
295 case 's': {
296 SKIP_TAG_IF_NEEDED(ap, tagged_ap);
297
298 data = va_arg(ap, char *);
299 data_len = strlen(data);
300 if (precision >= 0 && data_len > precision) {
301 data_len = precision;
302 }
303 precision = 0;
304 break;
305 }
306
307 case 'c': {
308 int c;
309
310 SKIP_TAG_IF_NEEDED(ap, tagged_ap);
311
312 c = va_arg(ap, int);
313
314 buf[0] = c;
315 data = buf;
316 data_len = 1;
317 precision = 0;
318 break;
319 }
320
321 default:
322 OUTC('%');
323 OUTC(*fmt);
324 goto start;
325 }
326
327 if (precision < 0 && (padding_mode & PAD_ZERO)) {
328 precision = min_width;
329 }
330 min_width -= data_len;
331 precision -= data_len;
332 if (precision > 0) {
333 min_width -= precision;
334 }
335
336 if (!(padding_mode & PAD_TAIL)) {
337 while (--min_width >= 0) {
338 OUTC(' ');
339 }
340 }
341 while (*prefix) {
342 OUTC(*prefix++);
343 }
344 while (--precision >= 0) {
345 OUTC('0');
346 }
347 while (--data_len >= 0) {
348 OUTC(*data++);
349 }
350 while (--min_width >= 0) {
351 OUTC(' ');
352 }
353
354 goto start;
355 }
356 }
357
