1 /*
2 * Copyright (C) 2018-2022 Intel Corporation.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6 #include <types.h>
7 #include <rtl.h>
8 #include <util.h>
9 #include <sprintf.h>
10
11 #ifndef NULL
12 #define NULL ((void *) 0)
13 #endif
14
15 #define PRINT_STRING_MAX_LEN 4096U
16
17 #define HEX_DIGITS_LEN 17U
18
19 /** Use upper case letters for hexadecimal format. */
20 #define PRINT_FLAG_UPPER 0x00000001U
21
22 /** Use alternate form. */
23 #define PRINT_FLAG_ALTERNATE_FORM 0x00000002U
24
25 /** Use '0' instead of ' ' for padding. */
26 #define PRINT_FLAG_PAD_ZERO 0x00000004U
27
28 /** Use left instead of right justification. */
29 #define PRINT_FLAG_LEFT_JUSTIFY 0x00000008U
30
31 /** Always use the sign as prefix. */
32 #define PRINT_FLAG_SIGN 0x00000010U
33
34 /** Use ' ' as prefix if no sign is used. */
35 #define PRINT_FLAG_SPACE 0x00000020U
36
37 /** The original value was a (unsigned) char. */
38 #define PRINT_FLAG_CHAR 0x00000040U
39
40 /** The original value was a (unsigned) short. */
41 #define PRINT_FLAG_SHORT 0x00000080U
42
43 /** The original value was a (unsigned) long. 64bit on ACRN also */
44 #define PRINT_FLAG_LONG 0x00000200U
45
46 /** The original value was a (unsigned) long long. */
47 #define PRINT_FLAG_LONG_LONG 0x00000200U
48
49 /** The value is interpreted as unsigned. */
50 #define PRINT_FLAG_UINT32 0x00000400U
51
52 /** The characters to use for upper case hexadecimal conversion.
53 *
54 * Note that this array is 17 bytes long. The first 16 characters
55 * are used to convert a 4 bit number to a printable character.
56 * The last character is used to determine the prefix for the
57 * alternate form.
58 */
59
60 static const char upper_hex_digits[] = {
61 '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
62 'A', 'B', 'C', 'D', 'E', 'F', 'X'
63 };
64
65 /** The characters to use for lower case hexadecimal conversion.
66 *
67 * Note that this array is 17 bytes long. The first 16 characters
68 * are used to convert a 4 bit number to a printable character.
69 * The last character is used to determine the prefix for the
70 * alternate form.
71 */
72
73 static const char lower_hex_digits[] = {
74 '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
75 'a', 'b', 'c', 'd', 'e', 'f', 'x'
76 };
77
get_param(const char * s_arg,uint32_t * x)78 static const char *get_param(const char *s_arg, uint32_t *x)
79 {
80 const char *s = s_arg;
81 *x = 0U;
82
83 /* ignore '-' for negative numbers, it will be handled in flags*/
84 if (*s == '-') {
85 ++s;
86 }
87
88 /* parse uint32_teger */
89 while ((*s >= '0') && (*s <= '9')) {
90 char delta = *s - '0';
91 *x = ((*x) * 10U) + (uint32_t)delta;
92 s++;
93 }
94
95 return s;
96 }
97
get_flags(const char * s_arg,uint32_t * flags)98 static const char *get_flags(const char *s_arg, uint32_t *flags)
99 {
100 const char *s = s_arg;
101 /* contains the flag characters */
102 static const char flagchars[5] = "#0- +";
103 /* contains the numeric flags for the characters above */
104 static const uint32_t fl[sizeof(flagchars)] = {
105 PRINT_FLAG_ALTERNATE_FORM, /* # */
106 PRINT_FLAG_PAD_ZERO, /* 0 */
107 PRINT_FLAG_LEFT_JUSTIFY, /* - */
108 PRINT_FLAG_SPACE, /* ' ' */
109 PRINT_FLAG_SIGN /* + */
110 };
111 uint32_t i;
112 bool found;
113
114 /* parse multiple flags */
115 while ((*s) != '\0') {
116 /*
117 * Get index of flag.
118 * Terminate loop if no flag character was found.
119 */
120 found = false;
121 for (i = 0U; i < sizeof(flagchars); i++) {
122 if (*s == flagchars[i]) {
123 found = true;
124 break;
125 }
126 }
127 if (!found) {
128 break;
129 }
130
131 /* apply matching flags and continue with the next character */
132 ++s;
133 *flags |= fl[i];
134 }
135
136 /* Spec says that '-' has a higher priority than '0' */
137 if ((*flags & PRINT_FLAG_LEFT_JUSTIFY) != 0U) {
138 *flags &= ~PRINT_FLAG_PAD_ZERO;
139 }
140
141 /* Spec says that '+' has a higher priority than ' ' */
142 if ((*flags & PRINT_FLAG_SIGN) != 0U) {
143 *flags &= ~PRINT_FLAG_SPACE;
144 }
145
146 return s;
147 }
148
get_length_modifier(const char * s_arg,uint32_t * flags,uint64_t * mask)149 static const char *get_length_modifier(const char *s_arg,
150 uint32_t *flags, uint64_t *mask)
151 {
152 const char *s = s_arg;
153 if (*s == 'h') {
154 /* check for h[h] (char/short) */
155 s++;
156 if (*s == 'h') {
157 *flags |= PRINT_FLAG_CHAR;
158 *mask = 0x000000FFU;
159 ++s;
160 } else {
161 *flags |= PRINT_FLAG_SHORT;
162 *mask = 0x0000FFFFU;
163 }
164 } else if (*s == 'l') {
165 /* check for l[l] (long/long long) */
166 s++;
167 if (*s == 'l') {
168 *flags |= PRINT_FLAG_LONG_LONG;
169 ++s;
170 } else {
171 *flags |= PRINT_FLAG_LONG;
172 }
173 } else {
174 /* No length modifiers found. */
175 }
176
177 return s;
178 }
179
format_number(struct print_param * param)180 static void format_number(struct print_param *param)
181 {
182 /* contains the character used for padding */
183 char pad;
184 /* effective width of the result */
185 uint32_t width;
186 /* number of characters to insert for width (w) and precision (p) */
187 uint32_t p = 0U, w = 0U;
188
189 /* initialize variables */
190 width = param->vars.valuelen + param->vars.prefixlen;
191
192 /* calculate additional characters for precision */
193 if (param->vars.precision > width) {
194 p = param->vars.precision - width;
195 }
196
197 /* calculate additional characters for width */
198 if (param->vars.width > (width + p)) {
199 w = param->vars.width - (width + p);
200 }
201
202 /* handle case of right justification */
203 if ((param->vars.flags & PRINT_FLAG_LEFT_JUSTIFY) == 0U) {
204 /* assume ' ' as padding character */
205 pad = ' ';
206
207 /*
208 * if padding with 0 is used, we have to emit the prefix (if any
209 * ) first to achieve the expected result. However, if a blank is
210 * used for padding, the prefix is emitted after the padding.
211 */
212
213 if ((param->vars.flags & PRINT_FLAG_PAD_ZERO) != 0U) {
214 /* use '0' for padding */
215 pad = '0';
216
217 /* emit prefix, return early if an error occurred */
218 param->emit(PRINT_CMD_COPY, param->vars.prefix,
219 param->vars.prefixlen, param->data);
220
221 /* invalidate prefix */
222 param->vars.prefix = NULL;
223 param->vars.prefixlen = 0U;
224 }
225
226 /* fill the width with the padding character, return early if
227 * an error occurred
228 */
229 param->emit(PRINT_CMD_FILL, &pad, w, param->data);
230 }
231
232 /* emit prefix (if any), return early in case of an error */
233 param->emit(PRINT_CMD_COPY, param->vars.prefix,
234 param->vars.prefixlen, param->data);
235
236 /* insert additional 0's for precision, return early if an error
237 * occurred
238 */
239 param->emit(PRINT_CMD_FILL, "0", p, param->data);
240
241 /* emit the pre-calculated result, return early in case of an error */
242 param->emit(PRINT_CMD_COPY, param->vars.value,
243 param->vars.valuelen, param->data);
244
245 /* handle left justification */
246 if ((param->vars.flags & PRINT_FLAG_LEFT_JUSTIFY) != 0U) {
247 /* emit trailing blanks, return early in case of an error */
248 param->emit(PRINT_CMD_FILL, " ", w, param->data);
249 }
250
251 }
252
print_pow2(struct print_param * param,uint64_t v_arg,uint32_t shift)253 static void print_pow2(struct print_param *param,
254 uint64_t v_arg, uint32_t shift)
255 {
256 uint64_t v = v_arg;
257 /* max buffer required for octal representation of uint64_t long */
258 char digitbuff[22];
259 /* Insert position for the next character+1 */
260 char *pos = digitbuff + sizeof(digitbuff);
261 /* buffer for the 0/0x/0X prefix */
262 char prefix[2];
263 /* pointer to the digits translation table */
264 const char (*digits)[HEX_DIGITS_LEN];
265 /* mask to extract next character */
266 uint64_t mask;
267
268 /* calculate mask */
269 mask = (1UL << shift) - 1UL;
270
271 /* determine digit translation table */
272 digits = ((param->vars.flags & PRINT_FLAG_UPPER) != 0U) ? &upper_hex_digits : &lower_hex_digits;
273
274 /* apply mask for short/char */
275 v &= param->vars.mask;
276
277 /* determine prefix for alternate form */
278 if ((v == 0UL) &&
279 ((param->vars.flags & PRINT_FLAG_ALTERNATE_FORM) != 0U)) {
280 prefix[0] = '0';
281 param->vars.prefix = prefix;
282 param->vars.prefixlen = 1U;
283
284 if (shift == 4U) {
285 param->vars.prefixlen = 2U;
286 prefix[1] = (*digits)[16];
287 }
288 }
289
290 /* determine digits from right to left */
291 do {
292 pos--;
293 *pos = (*digits)[(v & mask)];
294 v >>= shift;
295 } while (v != 0UL);
296
297 /* assign parameter and apply width and precision */
298 param->vars.value = pos;
299 param->vars.valuelen = (digitbuff + sizeof(digitbuff)) - pos;
300
301 format_number(param);
302
303 param->vars.value = NULL;
304 param->vars.valuelen = 0U;
305
306 }
307
print_decimal(struct print_param * param,int64_t value)308 static void print_decimal(struct print_param *param, int64_t value)
309 {
310 /* max. required buffer for uint64_t long in decimal format */
311 char digitbuff[20];
312 /* pointer to the next character position (+1) */
313 char *pos = digitbuff + sizeof(digitbuff);
314 /* current value in 32/64 bit */
315 union u_qword v;
316 /* next value in 32/64 bit */
317 union u_qword nv;
318
319 /* assume an unsigned 64 bit value */
320 v.qword = ((uint64_t)value) & param->vars.mask;
321
322 /*
323 * assign sign and correct value if value is negative and
324 * value must be interpreted as signed
325 */
326 if (((param->vars.flags & PRINT_FLAG_UINT32) == 0U) && (value < 0)) {
327 v.qword = (uint64_t)-value;
328 param->vars.prefix = "-";
329 param->vars.prefixlen = 1U;
330 }
331
332 /* determine sign if explicit requested in the format string */
333 if (param->vars.prefix == NULL) {
334 if ((param->vars.flags & PRINT_FLAG_SIGN) != 0U) {
335 param->vars.prefix = "+";
336 param->vars.prefixlen = 1U;
337 } else if ((param->vars.flags & PRINT_FLAG_SPACE) != 0U) {
338 param->vars.prefix = " ";
339 param->vars.prefixlen = 1U;
340 } else {
341 /* No prefix specified. */
342 }
343 }
344
345 /* process 64 bit value as long as needed */
346 while (v.dwords.high != 0U) {
347 /* determine digits from right to left */
348 pos--;
349 *pos = (char)(v.qword % 10UL) + '0';
350 v.qword = v.qword / 10UL;
351 }
352
353 nv.dwords.low = v.dwords.low;
354 /* process 32 bit (or reduced 64 bit) value */
355 do {
356 /* determine digits from right to left. The compiler should be
357 * able to handle a division and multiplication by the constant
358 * 10.
359 */
360 pos--;
361 *pos = (char)(nv.dwords.low % 10U) + '0';
362 nv.dwords.low = nv.dwords.low / 10U;
363 } while (nv.dwords.low != 0U);
364
365 /* assign parameter and apply width and precision */
366 param->vars.value = pos;
367 param->vars.valuelen = (digitbuff + sizeof(digitbuff)) - pos;
368
369 format_number(param);
370
371 param->vars.value = NULL;
372 param->vars.valuelen = 0U;
373
374 }
375
print_string(const struct print_param * param,const char * s)376 static void print_string(const struct print_param *param, const char *s)
377 {
378 /* the length of the string (-1) if unknown */
379 uint32_t len;
380 /* the number of additional characters to insert to reach the required
381 * width
382 */
383 uint32_t w = 0U;
384
385 len = strnlen_s(s, PRINT_STRING_MAX_LEN);
386
387 /* precision gives the max. number of characters to emit. */
388 if ((param->vars.precision != 0U) && (len > param->vars.precision)) {
389 len = param->vars.precision;
390 }
391
392 /* calculate the number of additional characters to get the required
393 * width
394 */
395 if ((param->vars.width > 0U) && (param->vars.width > len)) {
396 w = param->vars.width - len;
397 }
398
399 /* emit additional characters for width, return early if an error
400 * occurred
401 */
402 if ((param->vars.flags & PRINT_FLAG_LEFT_JUSTIFY) == 0U) {
403 param->emit(PRINT_CMD_FILL, " ", w, param->data);
404 }
405
406 param->emit(PRINT_CMD_COPY, s, len, param->data);
407
408 /* emit additional characters on the right, return early if an error
409 * occurred
410 */
411 if ((param->vars.flags & PRINT_FLAG_LEFT_JUSTIFY) != 0U) {
412 param->emit(PRINT_CMD_FILL, " ", w, param->data);
413 }
414
415 }
416
do_print(const char * fmt_arg,struct print_param * param,__builtin_va_list args)417 void do_print(const char *fmt_arg, struct print_param *param,
418 __builtin_va_list args)
419 {
420 const char *fmt = fmt_arg;
421
422 /* temp. storage for the next character */
423 char ch;
424 /* temp. pointer to the start of an analysed character sequence */
425 const char *start;
426
427 /* main loop: analyse until there are no more characters */
428 while ((*fmt) != '\0') {
429 /* mark the current position and search the next '%' */
430 start = fmt;
431
432 while (((*fmt) != '\0') && (*fmt != '%')) {
433 fmt++;
434 }
435
436 /*
437 * pass all characters until the next '%' to the emit function.
438 * Return early if the function fails
439 */
440 param->emit(PRINT_CMD_COPY, start, fmt - start,
441 param->data);
442
443 /* continue only if the '%' character was found */
444 if (*fmt == '%') {
445 /* mark current position in the format string */
446 start = fmt;
447 fmt++;
448
449 /* initialize the variables for the next argument */
450 (void)memset(&(param->vars), 0U, sizeof(param->vars));
451 param->vars.mask = 0xFFFFFFFFFFFFFFFFUL;
452
453 /*
454 * analyze the format specification:
455 * - get the flags
456 * - get the width
457 * - get the precision
458 * - get the length modifier
459 */
460 fmt = get_flags(fmt, &(param->vars.flags));
461 fmt = get_param(fmt, &(param->vars.width));
462
463 if (*fmt == '.') {
464 fmt++;
465 fmt = get_param(fmt, &(param->vars.precision));
466 }
467
468 fmt = get_length_modifier(fmt, &(param->vars.flags),
469 &(param->vars.mask));
470 ch = *fmt;
471 fmt++;
472
473 /* a single '%'? => print out a single '%' */
474 if (ch == '%') {
475 param->emit(PRINT_CMD_COPY, &ch, 1U,
476 param->data);
477 } else if ((ch == 'd') || (ch == 'i')) {
478 /* decimal number */
479 if ((param->vars.flags &
480 PRINT_FLAG_LONG_LONG) != 0U) {
481 print_decimal(param,
482 __builtin_va_arg(args, int64_t));
483 } else {
484 print_decimal(param,
485 __builtin_va_arg(args, int32_t));
486 }
487 }
488 /* unsigned decimal number */
489 else if (ch == 'u') {
490 param->vars.flags |= PRINT_FLAG_UINT32;
491 if ((param->vars.flags &
492 PRINT_FLAG_LONG_LONG) != 0U) {
493 print_decimal(param,
494 (int64_t)__builtin_va_arg(args,
495 uint64_t));
496 } else {
497 print_decimal(param,
498 (int64_t)__builtin_va_arg(args,
499 uint32_t));
500 }
501 }
502 /* hexadecimal number */
503 else if ((ch == 'X') || (ch == 'x')) {
504 if (ch == 'X') {
505 param->vars.flags |= PRINT_FLAG_UPPER;
506 }
507 if ((param->vars.flags &
508 PRINT_FLAG_LONG_LONG) != 0U) {
509 print_pow2(param,
510 __builtin_va_arg(args,
511 uint64_t), 4U);
512 } else {
513 print_pow2(param,
514 __builtin_va_arg(args,
515 uint32_t), 4U);
516 }
517 }
518 /* string argument */
519 else if (ch == 's') {
520 const char *s = __builtin_va_arg(args, char *);
521
522 if (s == NULL) {
523 s = "(null)";
524 }
525 print_string(param, s);
526 }
527 /* single character argument */
528 else if (ch == 'c') {
529 char c[2];
530
531 c[0] = (char)__builtin_va_arg(args, int32_t);
532 c[1] = 0;
533 print_string(param, c);
534 }
535 /* default: print the format specifier as it is */
536 else {
537 param->emit(PRINT_CMD_COPY, start,
538 fmt - start, param->data);
539 }
540 }
541 }
542
543 }
544
545 static void
charmem(size_t cmd,const char * s_arg,uint32_t sz,struct snprint_param * param)546 charmem(size_t cmd, const char *s_arg, uint32_t sz, struct snprint_param *param)
547 {
548 const char *s = s_arg;
549 /* pointer to the destination */
550 char *p = param->dst + param->wrtn;
551 /* characters actually written */
552 uint32_t n = 0U;
553
554 /* copy mode ? */
555 if (cmd == PRINT_CMD_COPY) {
556 if (sz > 0U) {
557 while (((*s) != '\0') && (n < sz)) {
558 if (n < (param->sz - param->wrtn)) {
559 *p = *s;
560 }
561 p++;
562 s++;
563 n++;
564 }
565 }
566
567 param->wrtn += n;
568 }
569 /* fill mode */
570 else {
571 n = (sz < (param->sz - param->wrtn)) ? sz : 0U;
572 param->wrtn += sz;
573 (void)memset(p, (uint8_t)*s, n);
574 }
575
576 }
577
vsnprintf(char * dst_arg,size_t sz_arg,const char * fmt,va_list args)578 size_t vsnprintf(char *dst_arg, size_t sz_arg, const char *fmt, va_list args)
579 {
580 char *dst = dst_arg;
581 uint32_t sz = sz_arg;
582 size_t res = 0U;
583
584
585 /* struct to store all necessary parameters */
586 struct print_param param;
587
588 /* struct to store snprintf specific parameters */
589 struct snprint_param snparam;
590
591 /* initialize parameters */
592 (void)memset(&snparam, 0U, sizeof(snparam));
593 snparam.dst = dst;
594 snparam.sz = sz;
595 (void)memset(¶m, 0U, sizeof(param));
596 param.emit = charmem;
597 param.data = &snparam;
598
599 /* execute the printf()*/
600 do_print(fmt, ¶m, args);
601
602 /* ensure the written string is NULL terminated */
603 if (snparam.wrtn < sz) {
604 snparam.dst[snparam.wrtn] = '\0';
605 }
606 else {
607 snparam.dst[sz - 1] = '\0';
608 }
609
610 /* return the number of chars which would be written */
611 res = snparam.wrtn;
612
613 /* done */
614 return res;
615 }
616
snprintf(char * dest,size_t sz,const char * fmt,...)617 size_t snprintf(char *dest, size_t sz, const char *fmt, ...)
618 {
619 /* variable argument list needed for do_print() */
620 va_list args;
621 /* the result of this function */
622 size_t res;
623
624 va_start(args, fmt);
625
626 /* execute the printf() */
627 res = vsnprintf(dest, sz, fmt, args);
628
629 /* destroy parameter list */
630 va_end(args);
631
632 /* done */
633 return res;
634 }
635