1/*
2 * Copyright 2014, General Dynamics C4 Systems
3 *
4 * This software may be distributed and modified according to the terms of
5 * the GNU General Public License version 2. Note that NO WARRANTY is provided.
6 * See "LICENSE_GPLv2.txt" for details.
7 *
8 * @TAG(GD_GPL)
9 */
10
11#include <config.h>
12#include <machine/io.h>
13
14#ifdef CONFIG_PRINTING
15
16#include <stdarg.h>
17
18void
19putchar(char c)
20{
21    putConsoleChar(c);
22    if (c == '\n') {
23        putConsoleChar('\r');
24    }
25}
26
27static unsigned int
28print_spaces(int n)
29{
30    for (int i = 0; i < n; i++) {
31        kernel_putchar(' ');
32    }
33
34    return n;
35}
36
37static unsigned int
38print_string(const char *s)
39{
40    unsigned int n;
41
42    for (n = 0; *s; s++, n++) {
43        kernel_putchar(*s);
44    }
45
46    return n;
47}
48
49static unsigned long
50xdiv(unsigned long x, unsigned int denom)
51{
52    switch (denom) {
53    case 16:
54        return x / 16;
55    case 10:
56        return x / 10;
57    default:
58        return 0;
59    }
60}
61
62static unsigned long
63xmod(unsigned long x, unsigned int denom)
64{
65    switch (denom) {
66    case 16:
67        return x % 16;
68    case 10:
69        return x % 10;
70    default:
71        return 0;
72    }
73}
74
75word_t
76print_unsigned_long(unsigned long x, word_t ui_base)
77{
78    char out[sizeof(unsigned long) * 2 + 3];
79    word_t i, j;
80    unsigned int d;
81
82    /*
83     * Only base 10 and 16 supported for now. We want to avoid invoking the
84     * compiler's support libraries through doing arbitrary divisions.
85     */
86    if (ui_base != 10 && ui_base != 16) {
87        return 0;
88    }
89
90    if (x == 0) {
91        kernel_putchar('0');
92        return 1;
93    }
94
95    for (i = 0; x; x = xdiv(x, ui_base), i++) {
96        d = xmod(x, ui_base);
97
98        if (d >= 10) {
99            out[i] = 'a' + d - 10;
100        } else {
101            out[i] = '0' + d;
102        }
103    }
104
105    for (j = i; j > 0; j--) {
106        kernel_putchar(out[j - 1]);
107    }
108
109    return i;
110}
111
112/* The print_unsigned_long_long function assumes that an unsinged int
113   is half the size of an unsigned long long */
114compile_assert(print_unsigned_long_long_sizes, sizeof(unsigned int) * 2 == sizeof(unsigned long long))
115
116static unsigned int
117print_unsigned_long_long(unsigned long long x, unsigned int ui_base)
118{
119    unsigned int upper, lower;
120    unsigned int n = 0;
121    unsigned int mask = 0xF0000000u;
122    unsigned int shifts = 0;
123
124    /* only implemented for hex, decimal is harder without 64 bit division */
125    if (ui_base != 16) {
126        return 0;
127    }
128
129    /* we can't do 64 bit division so break it up into two hex numbers */
130    upper = (unsigned int) (x >> 32llu);
131    lower = (unsigned int) x & 0xffffffff;
132
133    /* print first 32 bits if they exist */
134    if (upper > 0) {
135        n += print_unsigned_long(upper, ui_base);
136        /* print leading 0s */
137        while (!(mask & lower)) {
138            kernel_putchar('0');
139            n++;
140            mask = mask >> 4;
141            shifts++;
142            if (shifts == 8) {
143                break;
144            }
145        }
146    }
147    /* print last 32 bits */
148    n += print_unsigned_long(lower, ui_base);
149
150    return n;
151}
152
153static inline bool_t
154isdigit(char c)
155{
156    return c >= '0' &&
157           c <= '9';
158}
159
160static inline int
161atoi(char c)
162{
163    return c - '0';
164}
165
166static int
167vprintf(const char *format, va_list ap)
168{
169    unsigned int n;
170    unsigned int formatting;
171    int nspaces = 0;
172
173    if (!format) {
174        return 0;
175    }
176
177    n = 0;
178    formatting = 0;
179    while (*format) {
180        if (formatting) {
181            while (isdigit(*format)) {
182                nspaces = nspaces * 10 + atoi(*format);
183                format++;
184                if (format == NULL) {
185                    break;
186                }
187            }
188            switch (*format) {
189            case '%':
190                kernel_putchar('%');
191                n++;
192                format++;
193                break;
194
195            case 'd': {
196                int x = va_arg(ap, int);
197
198                if (x < 0) {
199                    kernel_putchar('-');
200                    n++;
201                    x = -x;
202                }
203
204                n += print_unsigned_long(x, 10);
205                format++;
206                break;
207            }
208
209            case 'u':
210                n += print_unsigned_long(va_arg(ap, unsigned int), 10);
211                format++;
212                break;
213
214            case 'x':
215                n += print_unsigned_long(va_arg(ap, unsigned int), 16);
216                format++;
217                break;
218
219            case 'p': {
220                unsigned long p = va_arg(ap, unsigned long);
221                if (p == 0) {
222                    n += print_string("(nil)");
223                } else {
224                    n += print_string("0x");
225                    n += print_unsigned_long(p, 16);
226                }
227                format++;
228                break;
229            }
230
231            case 's':
232                n += print_string(va_arg(ap, char *));
233                format++;
234                break;
235
236            case 'l':
237                format++;
238                switch (*format) {
239                case 'd': {
240                    long x = va_arg(ap, long);
241
242                    if (x < 0) {
243                        kernel_putchar('-');
244                        n++;
245                        x = -x;
246                    }
247
248                    n += print_unsigned_long((unsigned long)x, 10);
249                    format++;
250                }
251                break;
252                case 'l':
253                    if (*(format + 1) == 'x') {
254                        n += print_unsigned_long_long(va_arg(ap, unsigned long long), 16);
255                    }
256                    format += 2;
257                    break;
258                case 'u':
259                    n += print_unsigned_long(va_arg(ap, unsigned long), 10);
260                    format++;
261                    break;
262                case 'x':
263                    n += print_unsigned_long(va_arg(ap, unsigned long), 16);
264                    format++;
265                    break;
266
267                default:
268                    /* format not supported */
269                    return -1;
270                }
271                break;
272            default:
273                /* format not supported */
274                return -1;
275            }
276
277            n += print_spaces(nspaces - n);
278            nspaces = 0;
279            formatting = 0;
280        } else {
281            switch (*format) {
282            case '%':
283                formatting = 1;
284                format++;
285                break;
286
287            default:
288                kernel_putchar(*format);
289                n++;
290                format++;
291                break;
292            }
293        }
294    }
295
296    return n;
297}
298
299word_t puts(const char *s)
300{
301    for (; *s; s++) {
302        kernel_putchar(*s);
303    }
304    kernel_putchar('\n');
305    return 0;
306}
307
308word_t
309kprintf(const char *format, ...)
310{
311    va_list args;
312    word_t i;
313
314    va_start(args, format);
315    i = vprintf(format, args);
316    va_end(args);
317    return i;
318}
319
320#endif /* CONFIG_PRINTING */
321