sbi_console.c 9.6 KB

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