1/*
2 * Copyright 2014, General Dynamics C4 Systems
3 *
4 * SPDX-License-Identifier: GPL-2.0-only
5 *
6 * Portions derived from musl:
7 *
8 * Copyright �� 2005-2020 Rich Felker, et al.
9 *
10 * SPDX-License-Identifier: MIT
11 */
12
13#include <config.h>
14#include <machine/io.h>
15
16#ifdef CONFIG_PRINTING
17
18#include <stdarg.h>
19#include <stdint.h>
20
21/*
22 * a handle defining how to output a character
23 */
24typedef void (*out_fn)(char character, char *buf, word_t idx);
25
26/*
27 * structure to allow a generic vprintf
28 * a out_fn handle and a buf to work on
29 */
30typedef struct {
31    out_fn putchar;
32    char *buf;
33    word_t idx;
34    word_t maxlen;
35} out_wrap_t;
36
37/*
38 * putchar would then just call the handle with its buf
39 * and current idx and then increment idx
40 */
41static void putchar_wrap(out_wrap_t *out, char c)
42{
43    if (out->maxlen < 0 || out->idx < out->maxlen) {
44        out->putchar(c, out->buf, out->idx);
45        out->idx++;
46    }
47}
48
49
50void putchar(char c)
51{
52    if (c == '\n') {
53        putDebugChar('\r');
54    }
55    putDebugChar(c);
56}
57
58static inline bool_t isdigit(char c)
59{
60    return c >= '0' &&
61           c <= '9';
62}
63
64/* Convenient bit representation for modifier flags, which all fall
65 * within 31 codepoints of the space character. */
66
67#define MASK_TYPE(a) (1U<<( a -' '))
68
69#define ALT_FORM     (1U<<('#'-' '))
70#define ZERO_PAD     (1U<<('0'-' '))
71#define LEFT_ADJ     (1U<<('-'-' '))
72#define PAD_POS      (1U<<(' '-' '))
73#define MARK_POS     (1U<<('+'-' '))
74#define GROUPED      (1U<<('\''-' '))
75
76#define FLAGMASK (ALT_FORM|ZERO_PAD|LEFT_ADJ|PAD_POS|MARK_POS|GROUPED)
77
78#define INTMAX_MAX INT32_MAX
79#define INT_MAX  0x7fffffff
80
81#define ULONG_MAX ((unsigned long)(-1))
82
83/* State machine to accept length modifiers + conversion specifiers.
84 * Result is 0 on failure, or an argument type to pop on success. */
85
86enum {
87    BARE, LPRE, LLPRE, HPRE, HHPRE, BIGLPRE,
88    ZTPRE, JPRE,
89    STOP,
90    PTR, INT, UINT, ULLONG,
91    LONG, ULONG,
92    SHORT, USHORT, CHAR, UCHAR,
93    WORDT, LLONG,
94#define IMAX LLONG
95#define UMAX ULLONG
96#define PDIFF LONG
97#define UIPTR ULONG
98    NOARG,
99    MAXSTATE
100};
101
102#define S(x) [(x)-'A']
103
104static const unsigned char states[]['z' - 'A' + 1] = {
105    { /* 0: bare types */
106        S('d') = INT, S('i') = INT,
107        S('o') = UINT, S('u') = UINT, S('x') = UINT, S('X') = UINT,
108        S('c') = CHAR,
109        S('s') = PTR, S('p') = UIPTR, S('n') = PTR,
110        S('l') = LPRE, S('h') = HPRE,
111        S('z') = ZTPRE, S('j') = JPRE, S('t') = ZTPRE,
112    }, { /* 1: l-prefixed */
113        S('d') = LONG, S('i') = LONG,
114        S('o') = ULONG, S('u') = ULONG, S('x') = ULONG, S('X') = ULONG,
115        S('n') = PTR,
116        S('l') = LLPRE,
117    }, { /* 2: ll-prefixed */
118        S('d') = LLONG, S('i') = LLONG,
119        S('o') = ULLONG, S('u') = ULLONG,
120        S('x') = ULLONG, S('X') = ULLONG,
121        S('n') = PTR,
122    }, { /* 3: h-prefixed */
123        S('d') = SHORT, S('i') = SHORT,
124        S('o') = USHORT, S('u') = USHORT,
125        S('x') = USHORT, S('X') = USHORT,
126        S('n') = PTR,
127        S('h') = HHPRE,
128    }, { /* 4: hh-prefixed */
129        S('d') = CHAR, S('i') = CHAR,
130        S('o') = UCHAR, S('u') = UCHAR,
131        S('x') = UCHAR, S('X') = UCHAR,
132        S('n') = PTR,
133    }, { /* 5: L-prefixed not supported */
134    }, { /* 6: z- or t-prefixed (assumed to be same size) */
135        S('d') = PDIFF, S('i') = PDIFF,
136        S('o') = WORDT, S('u') = WORDT,
137        S('x') = WORDT, S('X') = WORDT,
138        S('n') = PTR,
139    }, { /* 7: j-prefixed */
140        S('d') = IMAX, S('i') = IMAX,
141        S('o') = UMAX, S('u') = UMAX,
142        S('x') = UMAX, S('X') = UMAX,
143        S('n') = PTR,
144    }
145};
146
147#define OOB(x) ((unsigned)(x)-'A' > 'z'-'A')
148#define DIGIT(c) (c - '0')
149
150union arg {
151    word_t i;
152    long double f;
153    void *p;
154};
155
156static void pop_arg(union arg *arg, int type, va_list *ap)
157{
158    switch (type) {
159    case PTR:
160        arg->p = va_arg(*ap, void *);
161        break;
162    case INT:
163        arg->i = va_arg(*ap, int);
164        break;
165    case UINT:
166        arg->i = va_arg(*ap, unsigned int);
167        break;
168    case LONG:
169        arg->i = va_arg(*ap, long);
170        break;
171    case ULONG:
172        arg->i = va_arg(*ap, unsigned long);
173        break;
174    case LLONG:
175        arg->i = va_arg(*ap, long long);
176        break;
177    case ULLONG:
178        arg->i = va_arg(*ap, unsigned long long);
179        break;
180    case SHORT:
181        arg->i = (short)va_arg(*ap, int);
182        break;
183    case USHORT:
184        arg->i = (unsigned short)va_arg(*ap, int);
185        break;
186    case CHAR:
187        arg->i = (signed char)va_arg(*ap, int);
188        break;
189    case UCHAR:
190        arg->i = (unsigned char)va_arg(*ap, int);
191        break;
192    case WORDT:
193        arg->i = va_arg(*ap, word_t);
194    }
195}
196
197static void out(out_wrap_t *f, const char *s, word_t l)
198{
199    for (word_t i = 0; i < l; i++) {
200        putchar_wrap(f, s[i]);
201    }
202}
203
204static void pad(out_wrap_t *f, char c, int w, int l, int fl)
205{
206    char pad[256];
207    if (fl & (LEFT_ADJ | ZERO_PAD) || l >= w) {
208        return;
209    }
210    l = w - l;
211    memset(pad, c, l > sizeof pad ? sizeof pad : l);
212    for (; l >= sizeof pad; l -= sizeof pad) {
213        out(f, pad, sizeof pad);
214    }
215    out(f, pad, l);
216}
217
218static const char xdigits[16] = {
219    "0123456789ABCDEF"
220};
221
222static char *fmt_x(word_t x, char *s, int lower)
223{
224    for (; x; x >>= 4) {
225        *--s = xdigits[(x & 15)] | lower;
226    }
227    return s;
228}
229
230static char *fmt_o(word_t x, char *s)
231{
232    for (; x; x >>= 3) {
233        *--s = '0' + (x & 7);
234    }
235    return s;
236}
237
238static char *fmt_u(word_t x, char *s)
239{
240    unsigned long y;
241    for (; x > ULONG_MAX; x /= 10) {
242        *--s = '0' + x % 10;
243    }
244    for (y = x;           y; y /= 10) {
245        *--s = '0' + y % 10;
246    }
247    return s;
248}
249
250// Maximum buffer size taken to ensure correct adaptation
251// However, it could be reduced/removed if we could measure
252// the buf length under all code paths
253#define LDBL_MANT_DIG 113
254
255#define NL_ARGMAX 9
256
257static int getint(char **s)
258{
259    int i;
260    for (i = 0; isdigit(**s); (*s)++) {
261        if (i > INT_MAX / 10U || DIGIT(**s) > INT_MAX - 10 * i) {
262            i = -1;
263        } else {
264            i = 10 * i + DIGIT(**s);
265        }
266    }
267    return i;
268}
269
270static int printf_core(out_wrap_t *f, const char *fmt, va_list *ap, union arg *nl_arg, int *nl_type)
271{
272    char *a, *z, *s = (char *)fmt;
273    unsigned l10n = 0, fl;
274    int w, p, xp;
275    union arg arg;
276    int argpos;
277    unsigned st, ps;
278    int cnt = 0, l = 0;
279    word_t i;
280    char buf[sizeof(word_t) * 3 + 3 + LDBL_MANT_DIG / 4];
281    const char *prefix;
282    int t, pl;
283
284    for (;;) {
285        /* This error is only specified for snprintf, but since it's
286         * unspecified for other forms, do the same. Stop immediately
287         * on overflow; otherwise %n could produce wrong results. */
288        if (l > INT_MAX - cnt) {
289            goto overflow;
290        }
291
292        /* Update output count, end loop when fmt is exhausted */
293        cnt += l;
294        if (!*s) {
295            break;
296        }
297
298        /* Handle literal text and %% format specifiers */
299        for (a = s; *s && *s != '%'; s++);
300        for (z = s; s[0] == '%' && s[1] == '%'; z++, s += 2);
301        if (z - a > INT_MAX - cnt) {
302            goto overflow;
303        }
304        l = z - a;
305        if (f) {
306            out(f, a, l);
307        }
308        if (l) {
309            continue;
310        }
311
312        if (isdigit(s[1]) && s[2] == '$') {
313            l10n = 1;
314            argpos = DIGIT(s[1]);
315            s += 3;
316        } else {
317            argpos = -1;
318            s++;
319        }
320
321        /* Read modifier flags */
322        for (fl = 0; (unsigned)*s - ' ' < 32 && (FLAGMASK & MASK_TYPE(*s)); s++) {
323            fl |= MASK_TYPE(*s);
324        }
325
326        /* Read field width */
327        if (*s == '*') {
328            if (isdigit(s[1]) && s[2] == '$') {
329                l10n = 1;
330                nl_type[DIGIT(s[1])] = INT;
331                w = nl_arg[DIGIT(s[1])].i;
332                s += 3;
333            } else if (!l10n) {
334                w = f ? va_arg(*ap, int) : 0;
335                s++;
336            } else {
337                goto inval;
338            }
339            if (w < 0) {
340                fl |= LEFT_ADJ;
341                w = -w;
342            }
343        } else if ((w = getint(&s)) < 0) {
344            goto overflow;
345        }
346
347        /* Read precision */
348        if (*s == '.' && s[1] == '*') {
349            if (isdigit(s[2]) && s[3] == '$') {
350                nl_type[DIGIT(s[2])] = INT;
351                p = nl_arg[DIGIT(s[2])].i;
352                s += 4;
353            } else if (!l10n) {
354                p = f ? va_arg(*ap, int) : 0;
355                s += 2;
356            } else {
357                goto inval;
358            }
359            xp = (p >= 0);
360        } else if (*s == '.') {
361            s++;
362            p = getint(&s);
363            xp = 1;
364        } else {
365            p = -1;
366            xp = 0;
367        }
368
369        /* Format specifier state machine */
370        st = 0;
371        do {
372            if (OOB(*s)) {
373                goto inval;
374            }
375            ps = st;
376            st = states[st]S(*s++);
377        } while (st - 1 < STOP);
378        if (!st) {
379            goto inval;
380        }
381
382        /* Check validity of argument type (nl/normal) */
383        if (st == NOARG) {
384            if (argpos >= 0) {
385                goto inval;
386            }
387        } else {
388            if (argpos >= 0) {
389                nl_type[argpos] = st;
390                arg = nl_arg[argpos];
391            } else if (f) {
392                pop_arg(&arg, st, ap);
393            } else {
394                return 0;
395            }
396        }
397
398        if (!f) {
399            continue;
400        }
401
402        z = buf + sizeof(buf);
403        prefix = "-+   0X0x";
404        pl = 0;
405        t = s[-1];
406
407        /* - and 0 flags are mutually exclusive */
408        if (fl & LEFT_ADJ) {
409            fl &= ~ZERO_PAD;
410        }
411
412        if (t == 'n') {
413            if (!arg.p) {
414                continue;
415            }
416            switch (ps) {
417            case BARE:
418                *(int *)arg.p = cnt;
419                break;
420            case LPRE:
421                *(long *)arg.p = cnt;
422                break;
423            case LLPRE:
424                *(long long *)arg.p = cnt;
425                break;
426            case HPRE:
427                *(unsigned short *)arg.p = cnt;
428                break;
429            case HHPRE:
430                *(unsigned char *)arg.p = cnt;
431                break;
432            case ZTPRE:
433                *(word_t *)arg.p = cnt;
434                break;
435            case JPRE:
436                *(word_t *)arg.p = cnt;
437                break;
438            }
439            continue;
440        } else if (t == 'c') {
441            p = 1;
442            a = z - p;
443            *a = arg.i;
444            fl &= ~ZERO_PAD;
445        } else if (t == 's') {
446            a = arg.p ? arg.p : "(null)";
447            z = a + strnlen(a, p < 0 ? INT_MAX : p);
448            if (p < 0 && *z) {
449                goto overflow;
450            }
451            p = z - a;
452            fl &= ~ZERO_PAD;
453        } else {
454            switch (t) {
455            case 'p':
456                p = MAX(p, 2 * sizeof(void *));
457                t = 'x';
458                fl |= ALT_FORM;
459            case 'x':
460            case 'X':
461                a = fmt_x(arg.i, z, t & 32);
462                if (arg.i && (fl & ALT_FORM)) {
463                    prefix += (t >> 4);
464                    pl = 2;
465                }
466                break;
467            case 'o':
468                a = fmt_o(arg.i, z);
469                if ((fl & ALT_FORM) && p < (z - a + 1)) {
470                    p = z - a + 1;
471                }
472                break;
473            case 'd':
474            case 'i':
475                pl = 1;
476                if (arg.i > INTMAX_MAX) {
477                    arg.i = -arg.i;
478                } else if (fl & MARK_POS) {
479                    prefix++;
480                } else if (fl & PAD_POS) {
481                    prefix += 2;
482                } else {
483                    pl = 0;
484                }
485            case 'u':
486                a = fmt_u(arg.i, z);
487                break;
488            }
489            if (xp && p < 0) {
490                goto overflow;
491            }
492            if (xp) {
493                fl &= ~ZERO_PAD;
494            }
495            if (!arg.i && !p) {
496                a = z;
497            } else {
498                p = MAX(p, z - a + !arg.i);
499            }
500        }
501
502        if (p < z - a) {
503            p = z - a;
504        }
505        if (p > INT_MAX - pl) {
506            goto overflow;
507        }
508        if (w < pl + p) {
509            w = pl + p;
510        }
511        if (w > INT_MAX - cnt) {
512            goto overflow;
513        }
514
515        pad(f, ' ', w, pl + p, fl);
516        out(f, prefix, pl);
517        pad(f, '0', w, pl + p, fl ^ ZERO_PAD);
518        pad(f, '0', p, z - a, 0);
519        out(f, a, z - a);
520        pad(f, ' ', w, pl + p, fl ^ LEFT_ADJ);
521
522        l = w;
523    }
524
525    if (f) {
526        return cnt;
527    }
528    if (!l10n) {
529        return 0;
530    }
531
532    for (i = 1; i <= NL_ARGMAX && nl_type[i]; i++) {
533        pop_arg(nl_arg + i, nl_type[i], ap);
534    }
535    for (; i <= NL_ARGMAX && !nl_type[i]; i++);
536    if (i <= NL_ARGMAX) {
537        goto inval;
538    }
539    return 1;
540
541// goto for potential debug error support
542inval:
543overflow:
544    return -1;
545}
546
547// sprintf fills its buf with the given character
548static void buf_out_fn(char c, char *buf, word_t idx)
549{
550    buf[idx] = c;
551}
552
553// printf only needs to call kernel_putchar
554static void kernel_out_fn(char c, char *buf, word_t idx)
555{
556    kernel_putchar(c);
557}
558
559word_t puts(const char *s)
560{
561    for (; *s; s++) {
562        kernel_putchar(*s);
563    }
564    kernel_putchar('\n');
565    return 0;
566}
567
568static int vprintf(out_wrap_t *out, const char *fmt, va_list ap)
569{
570    va_list ap2;
571    int nl_type[NL_ARGMAX + 1] = {0};
572    union arg nl_arg[NL_ARGMAX + 1];
573    int ret;
574
575    // validate format string
576    va_copy(ap2, ap);
577    if (printf_core(0, fmt, &ap2, nl_arg, nl_type) < 0) {
578        va_end(ap2);
579        return -1;
580    }
581
582    ret = printf_core(out, fmt, &ap2, nl_arg, nl_type);
583    va_end(ap2);
584    return ret;
585}
586
587word_t kprintf(const char *format, ...)
588{
589    va_list args;
590    word_t ret;
591
592    out_wrap_t out = { kernel_out_fn, NULL, 0, -1 };
593
594    va_start(args, format);
595    ret = vprintf(&out, format, args);
596    va_end(args);
597    return ret;
598}
599
600word_t ksnprintf(char *str, word_t size, const char *format, ...)
601{
602    va_list args;
603    word_t i;
604
605    out_wrap_t out = { buf_out_fn, str, 0, size };
606
607    va_start(args, format);
608    i = vprintf(&out, format, args);
609    va_end(args);
610
611    // make sure there is space for a 0 byte
612    if (i >= size) {
613        i = size - 1;
614    }
615    str[i] = 0;
616
617    return i;
618}
619
620#endif /* CONFIG_PRINTING */
621