1 /*
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2019 Western Digital Corporation or its affiliates.
5 *
6 * Authors:
7 * Anup Patel <anup.patel@wdc.com>
8 */
9
10 #include <sbi/riscv_locks.h>
11 #include <sbi/sbi_console.h>
12 #include <sbi/sbi_platform.h>
13 #include <sbi/sbi_scratch.h>
14
15 static const struct sbi_platform *console_plat = NULL;
16 static spinlock_t console_out_lock = SPIN_LOCK_INITIALIZER;
17
sbi_isprintable(char c)18 bool sbi_isprintable(char c)
19 {
20 if (((31 < c) && (c < 127)) || (c == '\f') || (c == '\r') ||
21 (c == '\n') || (c == '\t')) {
22 return TRUE;
23 }
24 return FALSE;
25 }
26
sbi_getc(void)27 int sbi_getc(void)
28 {
29 return sbi_platform_console_getc(console_plat);
30 }
31
sbi_putc(char ch)32 void sbi_putc(char ch)
33 {
34 if (ch == '\n')
35 sbi_platform_console_putc(console_plat, '\r');
36 sbi_platform_console_putc(console_plat, ch);
37 }
38
sbi_puts(const char * str)39 void sbi_puts(const char *str)
40 {
41 spin_lock(&console_out_lock);
42 while (*str) {
43 sbi_putc(*str);
44 str++;
45 }
46 spin_unlock(&console_out_lock);
47 }
48
sbi_gets(char * s,int maxwidth,char endchar)49 void sbi_gets(char *s, int maxwidth, char endchar)
50 {
51 int ch;
52 char *retval = s;
53
54 while ((ch = sbi_getc()) != endchar && ch >= 0 && maxwidth > 1) {
55 *retval = (char)ch;
56 retval++;
57 maxwidth--;
58 }
59 *retval = '\0';
60 }
61
62 #define PAD_RIGHT 1
63 #define PAD_ZERO 2
64 #define PAD_ALTERNATE 4
65 #define PRINT_BUF_LEN 64
66
67 #define va_start(v, l) __builtin_va_start((v), l)
68 #define va_end __builtin_va_end
69 #define va_arg __builtin_va_arg
70 typedef __builtin_va_list va_list;
71
printc(char ** out,u32 * out_len,char ch)72 static void printc(char **out, u32 *out_len, char ch)
73 {
74 if (out) {
75 if (*out) {
76 if (out_len && (0 < *out_len)) {
77 **out = ch;
78 ++(*out);
79 (*out_len)--;
80 } else {
81 **out = ch;
82 ++(*out);
83 }
84 }
85 } else {
86 sbi_putc(ch);
87 }
88 }
89
prints(char ** out,u32 * out_len,const char * string,int width,int flags)90 static int prints(char **out, u32 *out_len, const char *string, int width,
91 int flags)
92 {
93 int pc = 0;
94 char padchar = ' ';
95
96 if (width > 0) {
97 int len = 0;
98 const char *ptr;
99 for (ptr = string; *ptr; ++ptr)
100 ++len;
101 if (len >= width)
102 width = 0;
103 else
104 width -= len;
105 if (flags & PAD_ZERO)
106 padchar = '0';
107 }
108 if (!(flags & PAD_RIGHT)) {
109 for (; width > 0; --width) {
110 printc(out, out_len, padchar);
111 ++pc;
112 }
113 }
114 for (; *string; ++string) {
115 printc(out, out_len, *string);
116 ++pc;
117 }
118 for (; width > 0; --width) {
119 printc(out, out_len, padchar);
120 ++pc;
121 }
122
123 return pc;
124 }
125
printi(char ** out,u32 * out_len,long long i,int b,int sg,int width,int flags,int letbase)126 static int printi(char **out, u32 *out_len, long long i, int b, int sg,
127 int width, int flags, int letbase)
128 {
129 char print_buf[PRINT_BUF_LEN];
130 char *s;
131 int neg = 0, pc = 0;
132 u64 t;
133 unsigned long long u = i;
134
135 if (sg && b == 10 && i < 0) {
136 neg = 1;
137 u = -i;
138 }
139
140 s = print_buf + PRINT_BUF_LEN - 1;
141 *s = '\0';
142
143 if (!u) {
144 *--s = '0';
145 } else {
146 while (u) {
147 t = u % b;
148 u = u / b;
149 if (t >= 10)
150 t += letbase - '0' - 10;
151 *--s = t + '0';
152 }
153 }
154
155 if (flags & PAD_ALTERNATE) {
156 if ((b == 16) && (letbase == 'A')) {
157 *--s = 'X';
158 } else if ((b == 16) && (letbase == 'a')) {
159 *--s = 'x';
160 }
161 *--s = '0';
162 }
163
164 if (neg) {
165 if (width && (flags & PAD_ZERO)) {
166 printc(out, out_len, '-');
167 ++pc;
168 --width;
169 } else {
170 *--s = '-';
171 }
172 }
173
174 return pc + prints(out, out_len, s, width, flags);
175 }
176
print(char ** out,u32 * out_len,const char * format,va_list args)177 static int print(char **out, u32 *out_len, const char *format, va_list args)
178 {
179 int width, flags, acnt = 0;
180 int pc = 0;
181 char scr[2];
182 unsigned long long tmp;
183
184 for (; *format != 0; ++format) {
185 if (*format == '%') {
186 ++format;
187 width = flags = 0;
188 if (*format == '\0')
189 break;
190 if (*format == '%')
191 goto out;
192 /* Get flags */
193 if (*format == '-') {
194 ++format;
195 flags = PAD_RIGHT;
196 }
197 if (*format == '#') {
198 ++format;
199 flags |= PAD_ALTERNATE;
200 }
201 while (*format == '0') {
202 ++format;
203 flags |= PAD_ZERO;
204 }
205 /* Get width */
206 for (; *format >= '0' && *format <= '9'; ++format) {
207 width *= 10;
208 width += *format - '0';
209 }
210 if (*format == 's') {
211 char *s = va_arg(args, char *);
212 acnt += sizeof(char *);
213 pc += prints(out, out_len, s ? s : "(null)",
214 width, flags);
215 continue;
216 }
217 if ((*format == 'd') || (*format == 'i')) {
218 pc += printi(out, out_len, va_arg(args, int),
219 10, 1, width, flags, '0');
220 acnt += sizeof(int);
221 continue;
222 }
223 if (*format == 'x') {
224 pc += printi(out, out_len,
225 va_arg(args, unsigned int), 16, 0,
226 width, flags, 'a');
227 acnt += sizeof(unsigned int);
228 continue;
229 }
230 if (*format == 'X') {
231 pc += printi(out, out_len,
232 va_arg(args, unsigned int), 16, 0,
233 width, flags, 'A');
234 acnt += sizeof(unsigned int);
235 continue;
236 }
237 if (*format == 'u') {
238 pc += printi(out, out_len,
239 va_arg(args, unsigned int), 10, 0,
240 width, flags, 'a');
241 acnt += sizeof(unsigned int);
242 continue;
243 }
244 if (*format == 'p') {
245 pc += printi(out, out_len,
246 va_arg(args, unsigned long), 16, 0,
247 width, flags, 'a');
248 acnt += sizeof(unsigned long);
249 continue;
250 }
251 if (*format == 'P') {
252 pc += printi(out, out_len,
253 va_arg(args, unsigned long), 16, 0,
254 width, flags, 'A');
255 acnt += sizeof(unsigned long);
256 continue;
257 }
258 if (*format == 'l' && *(format + 1) == 'l') {
259 while (acnt &
260 (sizeof(unsigned long long) - 1)) {
261 va_arg(args, int);
262 acnt += sizeof(int);
263 }
264 if (sizeof(unsigned long long) ==
265 sizeof(unsigned long)) {
266 tmp = va_arg(args, unsigned long long);
267 acnt += sizeof(unsigned long long);
268 } else {
269 ((unsigned long *)&tmp)[0] =
270 va_arg(args, unsigned long);
271 ((unsigned long *)&tmp)[1] =
272 va_arg(args, unsigned long);
273 acnt += 2 * sizeof(unsigned long);
274 }
275 if (*(format + 2) == 'u') {
276 format += 2;
277 pc += printi(out, out_len, tmp, 10, 0,
278 width, flags, 'a');
279 } else if (*(format + 2) == 'x') {
280 format += 2;
281 pc += printi(out, out_len, tmp, 16, 0,
282 width, flags, 'a');
283 } else if (*(format + 2) == 'X') {
284 format += 2;
285 pc += printi(out, out_len, tmp, 16, 0,
286 width, flags, 'A');
287 } else {
288 format += 1;
289 pc += printi(out, out_len, tmp, 10, 1,
290 width, flags, '0');
291 }
292 continue;
293 } else if (*format == 'l') {
294 if (*(format + 1) == 'u') {
295 format += 1;
296 pc += printi(
297 out, out_len,
298 va_arg(args, unsigned long), 10,
299 0, width, flags, 'a');
300 } else if (*(format + 1) == 'x') {
301 format += 1;
302 pc += printi(
303 out, out_len,
304 va_arg(args, unsigned long), 16,
305 0, width, flags, 'a');
306 acnt += sizeof(unsigned long);
307 } else if (*(format + 1) == 'X') {
308 format += 1;
309 pc += printi(
310 out, out_len,
311 va_arg(args, unsigned long), 16,
312 0, width, flags, 'A');
313 acnt += sizeof(unsigned long);
314 } else {
315 pc += printi(out, out_len,
316 va_arg(args, long), 10, 1,
317 width, flags, '0');
318 acnt += sizeof(long);
319 }
320 }
321 if (*format == 'c') {
322 /* char are converted to int then pushed on the stack */
323 scr[0] = va_arg(args, int);
324 scr[1] = '\0';
325 pc += prints(out, out_len, scr, width, flags);
326 acnt += sizeof(int);
327 continue;
328 }
329 } else {
330 out:
331 printc(out, out_len, *format);
332 ++pc;
333 }
334 }
335 if (out)
336 **out = '\0';
337
338 return pc;
339 }
340
sbi_sprintf(char * out,const char * format,...)341 int sbi_sprintf(char *out, const char *format, ...)
342 {
343 va_list args;
344 int retval;
345
346 va_start(args, format);
347 retval = print(&out, NULL, format, args);
348 va_end(args);
349
350 return retval;
351 }
352
sbi_snprintf(char * out,u32 out_sz,const char * format,...)353 int sbi_snprintf(char *out, u32 out_sz, const char *format, ...)
354 {
355 va_list args;
356 int retval;
357
358 va_start(args, format);
359 retval = print(&out, &out_sz, format, args);
360 va_end(args);
361
362 return retval;
363 }
364
sbi_printf(const char * format,...)365 int sbi_printf(const char *format, ...)
366 {
367 va_list args;
368 int retval;
369
370 spin_lock(&console_out_lock);
371 va_start(args, format);
372 retval = print(NULL, NULL, format, args);
373 va_end(args);
374 spin_unlock(&console_out_lock);
375
376 return retval;
377 }
378
sbi_dprintf(const char * format,...)379 int sbi_dprintf(const char *format, ...)
380 {
381 va_list args;
382 int retval = 0;
383 struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
384
385 va_start(args, format);
386 if (scratch->options & SBI_SCRATCH_DEBUG_PRINTS)
387 retval = print(NULL, NULL, format, args);
388 va_end(args);
389
390 return retval;
391 }
392
sbi_console_init(struct sbi_scratch * scratch)393 int sbi_console_init(struct sbi_scratch *scratch)
394 {
395 console_plat = sbi_platform_ptr(scratch);
396
397 return sbi_platform_console_init(console_plat);
398 }
399