1 // SPDX-License-Identifier: GPL-2.0
2 #include <linux/init.h>
3 #include <linux/errno.h>
4 #include <linux/console.h>
5 
6 #include <asm/sibyte/board.h>
7 
8 #include <asm/fw/cfe/cfe_api.h>
9 #include <asm/fw/cfe/cfe_error.h>
10 
11 extern int cfe_cons_handle;
12 
cfe_console_write(struct console * cons,const char * str,unsigned int count)13 static void cfe_console_write(struct console *cons, const char *str,
14 		       unsigned int count)
15 {
16 	int i, last, written;
17 
18 	for (i=0, last=0; i<count; i++) {
19 		if (!str[i])
20 			/* XXXKW can/should this ever happen? */
21 			return;
22 		if (str[i] == '\n') {
23 			do {
24 				written = cfe_write(cfe_cons_handle, &str[last], i-last);
25 				if (written < 0)
26 					;
27 				last += written;
28 			} while (last < i);
29 			while (cfe_write(cfe_cons_handle, "\r", 1) <= 0)
30 				;
31 		}
32 	}
33 	if (last != count) {
34 		do {
35 			written = cfe_write(cfe_cons_handle, &str[last], count-last);
36 			if (written < 0)
37 				;
38 			last += written;
39 		} while (last < count);
40 	}
41 
42 }
43 
cfe_console_setup(struct console * cons,char * str)44 static int cfe_console_setup(struct console *cons, char *str)
45 {
46 	char consdev[32];
47 	/* XXXKW think about interaction with 'console=' cmdline arg */
48 	/* If none of the console options are configured, the build will break. */
49 	if (cfe_getenv("BOOT_CONSOLE", consdev, 32) >= 0) {
50 #ifdef CONFIG_SERIAL_SB1250_DUART
51 		if (!strcmp(consdev, "uart0")) {
52 			setleds("u0cn");
53 		} else if (!strcmp(consdev, "uart1")) {
54 			setleds("u1cn");
55 		} else
56 #endif
57 #ifdef CONFIG_VGA_CONSOLE
58 		       if (!strcmp(consdev, "pcconsole0")) {
59 				setleds("pccn");
60 		} else
61 #endif
62 			return -ENODEV;
63 	}
64 	return 0;
65 }
66 
67 static struct console sb1250_cfe_cons = {
68 	.name		= "cfe",
69 	.write		= cfe_console_write,
70 	.setup		= cfe_console_setup,
71 	.flags		= CON_PRINTBUFFER,
72 	.index		= -1,
73 };
74 
sb1250_cfe_console_init(void)75 static int __init sb1250_cfe_console_init(void)
76 {
77 	register_console(&sb1250_cfe_cons);
78 	return 0;
79 }
80 
81 console_initcall(sb1250_cfe_console_init);
82