plat-vexpress: translate pa base addr before use

Translates physical addresses used for register base addresses before
use to be able to handle non-linear mapping of addresses.

Reviewed-by: Pascal Brand <pascal.brand@linaro.org>
Reviewed-by: Joakim Bech <joakim.bech@linaro.org>
Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>

1 files changed, 34 insertions, 11 deletions

diff --git a/core/arch/arm/plat-vexpress/main.c b/core/arch/arm/plat-vexpress/main.c
index 055d3a68..db740ecd 100644
--- a/core/arch/arm/plat-vexpress/main.c
+++ b/core/arch/arm/plat-vexpress/main.c
@@ -42,6 +42,7 @@
 #include <kernel/tee_time.h>
 #include <tee/entry_fast.h>
 #include <tee/entry_std.h>
+#include <mm/core_memprot.h>
 #include <console.h>
 #include <keep.h>
 #include <initcall.h>
@@ -79,14 +80,22 @@ const struct thread_handlers *generic_boot_get_handlers(void)
 #ifdef GIC_BASE
 void main_init_gic(void)
 {
+	vaddr_t gicc_base;
+	vaddr_t gicd_base;
+
+	gicc_base = (vaddr_t)phys_to_virt(GIC_BASE + GICC_OFFSET,
+					  MEM_AREA_IO_SEC);
+	gicd_base = (vaddr_t)phys_to_virt(GIC_BASE + GICD_OFFSET,
+					  MEM_AREA_IO_SEC);
+	TEE_ASSERT(gicc_base && gicd_base);
+
 #if PLATFORM_FLAVOR_IS(fvp) || PLATFORM_FLAVOR_IS(juno) || \
     PLATFORM_FLAVOR_IS(qemu_armv8a)
 	/* On ARMv8, GIC configuration is initialized in ARM-TF */
-	gic_init_base_addr(&gic_data, GIC_BASE + GICC_OFFSET,
-			   GIC_BASE + GICD_OFFSET);
+	gic_init_base_addr(&gic_data, gicc_base, gicd_base);
 #else
 	/* Initialize GIC */
-	gic_init(&gic_data, GIC_BASE + GICC_OFFSET, GIC_BASE + GICD_OFFSET);
+	gic_init(&gic_data, gicc_base, gicd_base);
 #endif
 	itr_init(&gic_data.chip);
 }
@@ -97,30 +106,44 @@ static void main_fiq(void)
 	gic_it_handle(&gic_data);
 }
 
+static vaddr_t console_base(void)
+{
+	static void *va;
+
+	if (cpu_mmu_enabled()) {
+		if (!va)
+			va = phys_to_virt(CONSOLE_UART_BASE, MEM_AREA_IO_SEC);
+		return (vaddr_t)va;
+	}
+	return CONSOLE_UART_BASE;
+}
+
 void console_init(void)
 {
-	pl011_init(CONSOLE_UART_BASE,
-		   CONSOLE_UART_CLK_IN_HZ,
-		   CONSOLE_BAUDRATE);
+	pl011_init(console_base(), CONSOLE_UART_CLK_IN_HZ, CONSOLE_BAUDRATE);
 }
 
 void console_putc(int ch)
 {
-	pl011_putc(ch, CONSOLE_UART_BASE);
+	vaddr_t base = console_base();
+
+	pl011_putc(ch, base);
 	if (ch == '\n')
-		pl011_putc('\r', CONSOLE_UART_BASE);
+		pl011_putc('\r', base);
 }
 
 void console_flush(void)
 {
-	pl011_flush(CONSOLE_UART_BASE);
+	pl011_flush(console_base());
 }
 
 #ifdef IT_CONSOLE_UART
 static enum itr_return console_itr_cb(struct itr_handler *h __unused)
 {
-	while (pl011_have_rx_data(CONSOLE_UART_BASE)) {
-		int ch __maybe_unused = pl011_getchar(CONSOLE_UART_BASE);
+	paddr_t uart_base = console_base();
+
+	while (pl011_have_rx_data(uart_base)) {
+		int ch __maybe_unused = pl011_getchar(uart_base);
 
 		DMSG("cpu %zu: got 0x%x", get_core_pos(), ch);
 	}