synquacer: add stage2 override for PCIe config and MMIO regions

If dip switch DSW3-2 is set, install the static stage2 translation
tables that are located in the NOR flash and drop to EL1 when entering
the non-secure world. This works around SynQuacer SoC errata related
to PCIe.

Signed-off-by: Ard Biesheuvel <ard.biesheuvel@linaro.org>

5 files changed, 56 insertions, 0 deletions

diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S
index 1c8eed9d..3e1ebbad 100644
--- a/bl31/aarch64/bl31_entrypoint.S
+++ b/bl31/aarch64/bl31_entrypoint.S
@@ -132,3 +132,39 @@ func bl31_entrypoint
 
 	b	el3_exit
 endfunc bl31_entrypoint
+
+	/* -------------------------------------------------------------
+	 * On the Socionext SynQuacer platform, we need to prevent the
+	 * OS from using write combine mappings on PCIe MMIO regions.
+	 * Let's do so by setting a stage 2 override for those regions,
+	 * and run the OS in EL1 with stage 2 translations enabled.
+	 * While we're at it, remap the config space of bus #0 on both
+	 * RCs so we can use a generic ECAM driver.
+	 * -------------------------------------------------------------
+	 */
+
+#define	VTCR_T0SZ	(64 - 40)
+#define	VTCR_SL0	(1 << 6)	/* start at level 1 */
+#define	VTCR_IRGN0	(3 << 8)	/* inner wbwa */
+#define	VTCR_ORGN0	(3 << 10)	/* outer wbwa */
+#define	VTCR_SH0	(0 << 12)	/* non shareable */
+#define	VTCR_TG0	(0 << 14)	/* 4 KB granule */
+#define	VTCR_PS		(2 << 16)	/* 40 bit PAs */
+#define	VTCR_RES1	(1 << 31)	/* RES1 */
+
+	.globl		bl31_setup_s2_translation
+func bl31_setup_s2_translation
+	mov_imm		x0, (BL33_BASE - 0x8000)
+	msr		vttbr_el2, x0
+	mov_imm		x0, VTCR_T0SZ | VTCR_SL0 | VTCR_IRGN0 | \
+			    VTCR_ORGN0 | VTCR_SH0 | VTCR_TG0 | \
+			    VTCR_PS | VTCR_RES1
+	msr		vtcr_el2, x0
+	isb
+
+	mrs		x0, hcr_el2
+	orr		x0, x0, #1
+	msr		hcr_el2, x0
+	isb
+	ret
+endfunc bl31_setup_s2_translation
diff --git a/bl31/bl31_main.c b/bl31/bl31_main.c
index 835d41e8..f9d4abaa 100644
--- a/bl31/bl31_main.c
+++ b/bl31/bl31_main.c
@@ -142,6 +142,8 @@ uint32_t bl31_get_next_image_type(void)
  ******************************************************************************/
 void bl31_prepare_next_image_entry(void)
 {
+	extern void bl31_setup_s2_translation(void);
+
 	entry_point_info_t *next_image_info;
 	uint32_t image_type;
 
@@ -158,6 +160,9 @@ void bl31_prepare_next_image_entry(void)
 	print_entry_point_info(next_image_info);
 	cm_init_my_context(next_image_info);
 	cm_prepare_el3_exit(image_type);
+
+	if (image_type != SECURE)
+		bl31_setup_s2_translation();
 }
 
 /*******************************************************************************
diff --git a/plat/arm/common/aarch64/arm_common.c b/plat/arm/common/aarch64/arm_common.c
index 80bd417b..03d185b5 100644
--- a/plat/arm/common/aarch64/arm_common.c
+++ b/plat/arm/common/aarch64/arm_common.c
@@ -135,6 +135,7 @@ uint32_t arm_get_spsr_for_bl33_entry(void)
 	unsigned long el_status;
 	unsigned int mode;
 	uint32_t spsr;
+	uint32_t *gpio = (uint32_t *)0x51000000;
 
 	/* Figure out what mode we enter the non-secure world in */
 	el_status = read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL2_SHIFT;
@@ -142,6 +143,10 @@ uint32_t arm_get_spsr_for_bl33_entry(void)
 
 	mode = (el_status) ? MODE_EL2 : MODE_EL1;
 
+	/* enable the stage 2 override page tables if PD[1] is set */
+	if (gpio[0] & 0x2)
+		mode = MODE_EL1;
+
 	/*
 	 * TODO: Consider the possibility of specifying the SPSR in
 	 * the FIP ToC and allowing the platform to have a say as
diff --git a/services/std_svc/psci/psci_on.c b/services/std_svc/psci/psci_on.c
index 200e6222..6e4cdb64 100644
--- a/services/std_svc/psci/psci_on.c
+++ b/services/std_svc/psci/psci_on.c
@@ -156,6 +156,8 @@ exit:
 void psci_cpu_on_finish(unsigned int cpu_idx,
 			psci_power_state_t *state_info)
 {
+	extern void bl31_setup_s2_translation(void);
+
 	/*
 	 * Plat. management: Perform the platform specific actions
 	 * for this cpu e.g. enabling the gic or zeroing the mailbox
@@ -206,4 +208,7 @@ void psci_cpu_on_finish(unsigned int cpu_idx,
 	 * call to set this cpu on its way.
 	 */
 	cm_prepare_el3_exit(NON_SECURE);
+
+	bl31_setup_s2_translation();
+
 }
diff --git a/services/std_svc/psci/psci_suspend.c b/services/std_svc/psci/psci_suspend.c
index bd0c5dbc..f9946ff1 100644
--- a/services/std_svc/psci/psci_suspend.c
+++ b/services/std_svc/psci/psci_suspend.c
@@ -214,6 +214,8 @@ exit:
 void psci_cpu_suspend_finish(unsigned int cpu_idx,
 			     psci_power_state_t *state_info)
 {
+	extern void bl31_setup_s2_translation(void);
+
 	unsigned long long counter_freq;
 	unsigned int max_off_lvl;
 
@@ -261,4 +263,7 @@ void psci_cpu_suspend_finish(unsigned int cpu_idx,
 	 * call to set this cpu on its way.
 	 */
 	cm_prepare_el3_exit(NON_SECURE);
+
+	bl31_setup_s2_translation();
+
 }