Setup console from Device Tree

core/arch/arm/kernel/generic_boot.c

@@ -59,6 +59,9 @@
 #endif
 
 #if defined(CFG_DT)
+#include <console.h>
+#include <drivers/serial.h>
+#include <kernel/dt.h>
 #include <libfdt.h>
 #endif
 
@@ -462,20 +465,6 @@ static int add_optee_dt_node(void *fdt)
 	return 0;
 }
 
-static int get_dt_cell_size(void *fdt, int offs, const char *cell_name,
-			    uint32_t *cell_size)
-{
-	int len;
-	const uint32_t *cell = fdt_getprop(fdt, offs, cell_name, &len);
-
-	if (len != sizeof(*cell))
-		return -1;
-	*cell_size = fdt32_to_cpu(*cell);
-	if (*cell_size != 1 && *cell_size != 2)
-		return -1;
-	return 0;
-}
-
 static void set_dt_val(void *data, uint32_t cell_size, uint64_t val)
 {
 	if (cell_size == 1) {
@@ -493,20 +482,20 @@ static int add_optee_res_mem_dt_node(void *fdt)
 {
 	int offs;
 	int ret;
-	uint32_t addr_size = 2;
-	uint32_t len_size = 2;
+	int addr_size = 2;
+	int len_size = 2;
 	vaddr_t shm_va_start;
 	vaddr_t shm_va_end;
 	paddr_t shm_pa;
 	char subnode_name[80];
 
 	offs = fdt_path_offset(fdt, "/reserved-memory");
 	if (offs >= 0) {
-		ret = get_dt_cell_size(fdt, offs, "#address-cells", &addr_size);
-		if (ret < 0)
+		addr_size = fdt_address_cells(fdt, offs);
+		if (addr_size < 0)
 			return -1;
-		ret = get_dt_cell_size(fdt, offs, "#size-cells", &len_size);
-		if (ret < 0)
+		len_size = fdt_size_cells(fdt, offs);
+		if (len_size < 0)
 			return -1;
 	} else {
 		offs = fdt_path_offset(fdt, "/");
@@ -594,10 +583,107 @@ static void init_fdt(unsigned long phys_fdt)
 		panic();
 	}
 }
+
+/*
+ * 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.
+ */
+static void configure_console_from_dt(unsigned long phys_fdt)
+{
+	const struct dt_driver *drv, *found = NULL;
+	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;
+	int st;
+
+	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");
+		serial_console = NULL;
+		return;
+	}
+
+	stdout_data = strdup(prop->data);
+	if (!stdout_data)
+		return;
+	for (p = stdout_data; *p; p++) {
+		if (*p == ':') {
+			*p = '\0';
+			parms = p + 1;
+			break;
+		}
+	}
+
+	/* 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;
+
+	for_each_dt_driver(drv) {
+		const struct dt_device_match *dm;
+
+		for (dm = drv->match_table; dm; dm++) {
+			st = fdt_node_check_compatible(fdt, offs,
+						       dm->compatible);
+			if (!st) {
+				found = drv;
+				break;
+			}
+		}
+	}
+	if (!found)
+		goto out;
+
+	sdrv = (const struct serial_driver *)found->driver;
+	if (!sdrv)
+		goto out;
+	dev = sdrv->dev_alloc();
+	if (!dev)
+		goto out;
+	if (sdrv->dev_init(dev, fdt, offs, parms) < 0) {
+		sdrv->dev_free(dev);
+		goto out;
+	}
+
+	IMSG("Switching console to device: %s", uart);
+	serial_console = dev;
+out:
+	free(stdout_data);
+}
+
 #else
 static void init_fdt(unsigned long phys_fdt __unused)
 {
 }
+
+static void configure_console_from_dt(unsigned long phys_fdt __unused)
+{
+}
 #endif /*!CFG_DT*/
 
 static void init_primary_helper(unsigned long pageable_part,
@@ -612,18 +698,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/generic_entry_a32.S

@@ -335,7 +335,7 @@ shadow_stack_access_ok:
 	/* complete ARM secure MP common configuration */
 	bl	plat_cpu_reset_late
 
-	/* Enable Console */
+	/* Enable the console using physical addresses */
 	bl	console_init
 
 #ifdef CFG_PL310
@@ -345,7 +345,7 @@ shadow_stack_access_ok:
 
 	/*
 	 * Invalidate dcache for all memory used during initialization to
-	 * avoid nasty surprices when the cache is turned on. We must not
+	 * avoid nasty surprises when the cache is turned on. We must not
 	 * invalidate memory not used by OP-TEE since we may invalidate
 	 * entries used by for instance ARM Trusted Firmware.
 	 */
@@ -361,12 +361,24 @@ shadow_stack_access_ok:
 	bl	arm_cl2_enable
 #endif
 
+	/* Configure the MMU */
 	bl	core_init_mmu_map
 	bl	core_init_mmu_regs
+
+	/* Avoid losing any output due to the console_init call below */
+	bl	console_flush
+
+	/* Enable the MMU */
 	bl	cpu_mmu_enable
 	bl	cpu_mmu_enable_icache
 	bl	cpu_mmu_enable_dcache
 
+	/*
+	 * MMU is turned on: re-initialize the console so that the driver
+	 * uses the proper virtual addresses from now on
+	 */
+	bl	console_init
+
 	mov	r0, r4		/* pageable part address */
 	mov	r1, r5		/* ns-entry address */
 	mov	r2, r6		/* DT address */

core/arch/arm/kernel/generic_entry_a64.S

@@ -118,15 +118,27 @@ copy_init:
 	sub	x1, x1, x0
 	bl	inv_dcache_range
 
-	/* Enable Console */
+	/* Enable the console using physical addresses */
 	bl	console_init
 
+	/* Configure the MMU */
 	bl	core_init_mmu_map
 	bl	core_init_mmu_regs
+
+	/* Avoid losing any output due to the console_init call below */
+	bl	console_flush
+
+	/* Enable the MMU */
 	bl	cpu_mmu_enable
 	bl	cpu_mmu_enable_icache
 	bl	cpu_mmu_enable_dcache
 
+	/*
+	 * MMU is turned on: re-initialize the console so that the driver
+	 * uses the proper virtual addresses from now on
+	 */
+	bl	console_init
+
 	mov	x0, x19		/* pagable part address */
 	mov	x1, #-1
 	mov	x2, x20		/* DT address */

core/arch/arm/kernel/kern.ld.S

@@ -92,6 +92,11 @@ SECTIONS
 		*(.rodata .rodata.__unpaged)
 #include <rodata_unpaged.ld.S>
 #else
+#ifdef CFG_DT
+		__rodata_dtdrv_start = .;
+		KEEP(*(.rodata.dtdrv))
+		__rodata_dtdrv_end = .;
+#endif
 		*(.rodata .rodata.*)
 
 		/*
@@ -269,6 +274,11 @@ SECTIONS
 
 	.rodata_pageable : ALIGN(8) {
 		__rodata_pageable_start = .;
+#ifdef CFG_DT
+		__rodata_dtdrv_start = .;
+		KEEP(*(.rodata.dtdrv))
+		__rodata_dtdrv_end = .;
+#endif
 		*(.rodata*)
 		/*
 		 * 8 to avoid unwanted padding between __start_ta_head_section

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/arch/arm/plat-d02/main.c

@@ -51,6 +51,8 @@ static const struct thread_handlers handlers = {
 	.system_reset = pm_do_nothing,
 };
 
+static struct hi16xx_uart_data console_data __early_bss;
+
 register_phys_mem(MEM_AREA_IO_NSEC, CONSOLE_UART_BASE, HI16XX_UART_REG_SIZE);
 
 const struct thread_handlers *generic_boot_get_handlers(void)
@@ -63,34 +65,14 @@ static void main_fiq(void)
 	panic();
 }
 
-static vaddr_t console_base(void)
+void console_init(void)
 {
-	static void *va;
-
 	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(CONSOLE_UART_BASE, MEM_AREA_IO_NSEC);
-		return (vaddr_t)va;
+		console_data.vbase = (vaddr_t)phys_to_virt(console_data.pbase,
+							   MEM_AREA_IO_SEC);
+		return;
 	}
-	return CONSOLE_UART_BASE;
-}
-
-void console_init(void)
-{
-	hi16xx_uart_init(console_base(), CONSOLE_UART_CLK_IN_HZ,
-			 CONSOLE_BAUDRATE);
-}
-
-void console_putc(int ch)
-{
-	vaddr_t base = console_base();
-
-	if (ch == '\n')
-		hi16xx_uart_putc('\r', base);
-	hi16xx_uart_putc(ch, base);
-}
-
-void console_flush(void)
-{
-	hi16xx_uart_flush(console_base());
+	hi16xx_uart_init(&console_data, CONSOLE_UART_BASE,
+			 CONSOLE_UART_CLK_IN_HZ, CONSOLE_BAUDRATE);
+	serial_console = &console_data.chip;
 }

core/arch/arm/plat-hikey/main.c

@@ -58,6 +58,8 @@ static const struct thread_handlers handlers = {
 	.system_reset = pm_do_nothing,
 };
 
+static struct pl011_data console_data __early_bss;
+
 register_phys_mem(MEM_AREA_IO_NSEC, CONSOLE_UART_BASE, PL011_REG_SIZE);
 register_phys_mem(MEM_AREA_IO_NSEC, PMUSSI_BASE, PMUSSI_REG_SIZE);
 #ifdef CFG_SPI
@@ -78,35 +80,16 @@ static void main_fiq(void)
 	panic();
 }
 
-static vaddr_t console_base(void)
+void console_init(void)
 {
-	static void *va;
-
 	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(CONSOLE_UART_BASE, MEM_AREA_IO_NSEC);
-		return (vaddr_t)va;
+		console_data.vbase = (vaddr_t)phys_to_virt(console_data.pbase,
+							   MEM_AREA_IO_NSEC);
+		return;
 	}
-	return CONSOLE_UART_BASE;
-}
-
-void console_init(void)
-{
-	pl011_init(console_base(), CONSOLE_UART_CLK_IN_HZ, CONSOLE_BAUDRATE);
-}
-
-void console_putc(int ch)
-{
-	vaddr_t base = console_base();
-
-	if (ch == '\n')
-		pl011_putc('\r', base);
-	pl011_putc(ch, base);
-}
-
-void console_flush(void)
-{
-	pl011_flush(console_base());
+	pl011_init(&console_data, CONSOLE_UART_BASE, CONSOLE_UART_CLK_IN_HZ,
+		   CONSOLE_BAUDRATE);
+	serial_console = &console_data.chip;
 }
 
 vaddr_t nsec_periph_base(paddr_t pa)