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