core: arm: generic boot: dt: switch console to /secure-chosen/stdout-…

…path

If CFG_DT=y, check the Device Tree's /secure-chosen node and look for
the stdout-path property. Adjust the console output accordingly.
The DT bindings for this property have been proposed on the LKML [1].

[1] https://www.spinics.net/lists/arm-kernel/msg566034.html

Signed-off-by: Jerome Forissier <jerome.forissier@linaro.org>
Tested-by: Jerome Forissier <jerome.forissier@linaro.org> (QEMU)
Reviewed-by: Etienne Carriere <etienne.carriere@linaro.org>
Reviewed-by: Jens Wiklander <jens.wiklander@linaro.org>

core/arch/arm/kernel/generic_boot.c

@@ -28,6 +28,7 @@
 #include <arm.h>
 #include <assert.h>
 #include <compiler.h>
+#include <console.h>
 #include <inttypes.h>
 #include <keep.h>
 #include <kernel/generic_boot.h>
@@ -575,6 +576,7 @@ static void init_fdt(unsigned long phys_fdt)
 		panic();
 	}
 }
+
 #else
 static void init_fdt(unsigned long phys_fdt __unused)
 {
@@ -593,18 +595,18 @@ static void init_primary_helper(unsigned long pageable_part,
 	 */
 	thread_set_exceptions(THREAD_EXCP_ALL);
 	init_vfp_sec();
-
 	init_runtime(pageable_part);
 
-	IMSG("Initializing (%s)\n", core_v_str);
-
 	thread_init_primary(generic_boot_get_handlers());
 	thread_init_per_cpu();
 	init_sec_mon(nsec_entry);
 	init_fdt(fdt);
+	configure_console_from_dt(fdt);
+
+	IMSG("OP-TEE version: %s", core_v_str);
+
 	main_init_gic();
 	init_vfp_nsec();
-
 	if (init_teecore() != TEE_SUCCESS)
 		panic();
 	DMSG("Primary CPU switching to normal world boot\n");

core/arch/arm/kernel/thread.c

@@ -62,7 +62,7 @@
 #ifdef CFG_CORE_SANITIZE_KADDRESS
 #define STACK_TMP_SIZE		(3072 + STACK_TMP_OFFS)
 #else
-#define STACK_TMP_SIZE		(1024 + STACK_TMP_OFFS)
+#define STACK_TMP_SIZE		(1536 + STACK_TMP_OFFS)
 #endif
 #define STACK_THREAD_SIZE	8192
 

core/include/console.h

@@ -28,12 +28,21 @@
 #ifndef CONSOLE_H
 #define CONSOLE_H
 
+#include <compiler.h>
+
 void console_init(void);
 void console_putc(int ch);
 void console_flush(void);
 
 struct serial_chip;
 void register_serial_console(struct serial_chip *chip);
 
+#ifdef CFG_DT
+void configure_console_from_dt(unsigned long phys_fdt);
+#else
+static inline void configure_console_from_dt(unsigned long phys_fdt __unused)
+{}
+#endif /* !CFG_DT */
+
 #endif /* CONSOLE_H */
 

core/kernel/console.c

@@ -28,7 +28,14 @@
 #include <console.h>
 #include <compiler.h>
 #include <drivers/serial.h>
+#include <kernel/panic.h>
 #include <stdlib.h>
+#include <string.h>
+
+#ifdef CFG_DT
+#include <kernel/dt.h>
+#include <libfdt.h>
+#endif
 
 static struct serial_chip *serial_console __early_bss;
 
@@ -54,3 +61,90 @@ void register_serial_console(struct serial_chip *chip)
 {
 	serial_console = chip;
 }
+
+#ifdef CFG_DT
+
+/*
+ * Check if the /secure-chosen node in the DT contains an stdout-path value
+ * for which we have a compatible driver. If so, switch the console to
+ * this device.
+ */
+void configure_console_from_dt(unsigned long phys_fdt)
+{
+	const struct dt_driver *dt_drv;
+	const struct serial_driver *sdrv;
+	const struct fdt_property *prop;
+	struct serial_chip *dev;
+	char *stdout_data;
+	const char *uart;
+	const char *parms = NULL;
+	void *fdt;
+	int offs;
+	char *p;
+
+	if (!phys_fdt)
+		return;
+	fdt = phys_to_virt(phys_fdt, MEM_AREA_IO_NSEC);
+	if (!fdt)
+		panic();
+
+	offs = fdt_path_offset(fdt, "/secure-chosen");
+	if (offs < 0)
+		return;
+	prop = fdt_get_property(fdt, offs, "stdout-path", NULL);
+	if (!prop) {
+		/*
+		 * /secure-chosen node present but no stdout-path property
+		 * means we don't want any console output
+		 */
+		IMSG("Switching off console");
+		register_serial_console(NULL);
+		return;
+	}
+
+	stdout_data = strdup(prop->data);
+	if (!stdout_data)
+		return;
+	p = strchr(stdout_data, ':');
+	if (p) {
+		*p = '\0';
+		parms = p + 1;
+	}
+
+	/* stdout-path may refer to an alias */
+	uart = fdt_get_alias(fdt, stdout_data);
+	if (!uart) {
+		/* Not an alias, assume we have a node path */
+		uart = stdout_data;
+	}
+	offs = fdt_path_offset(fdt, uart);
+	if (offs < 0)
+		goto out;
+
+	dt_drv = dt_find_compatible_driver(fdt, offs);
+	if (!dt_drv)
+		goto out;
+
+	sdrv = (const struct serial_driver *)dt_drv->driver;
+	if (!sdrv)
+		goto out;
+	dev = sdrv->dev_alloc();
+	if (!dev)
+		goto out;
+	/*
+	 * If the console is the same as the early console, dev_init() might
+	 * clear pending data. Flush to avoid that.
+	 */
+	console_flush();
+	if (sdrv->dev_init(dev, fdt, offs, parms) < 0) {
+		sdrv->dev_free(dev);
+		goto out;
+	}
+
+	IMSG("Switching console to device: %s", uart);
+	register_serial_console(dev);
+out:
+	free(stdout_data);
+}
+
+#endif /* CFG_DT */