sbi_console.c 8.3 KB

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