add stage2 override for PCIe MMIO regionssynquacer-c6882acbdb71

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

5 files changed, 131 insertions, 0 deletions

diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S
index 1c8eed9d..ad767198 100644
--- a/bl31/aarch64/bl31_entrypoint.S
+++ b/bl31/aarch64/bl31_entrypoint.S
@@ -132,3 +132,114 @@ 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.
+	 *
+	 * For simplicity, let's use 1 GB block mappings only, and use
+	 * two concatenated level 1 tables to describe a 40-bit address
+	 * space.
+	 * -------------------------------------------------------------
+	 */
+
+#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	(3 << 12)	/* inner 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
+	adr		x0, __s2_table
+	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
+
+#define	TT_S2_AF			(0x1 << 10)
+#define	TT_S2_SH_NON_SHAREABLE		(0x0 << 8)
+#define	TT_S2_AP_RW			(0x3 << 6)
+#define	TT_S2_MEMATTR_DEVICE_nGRE	(0x2 << 2)
+#define	TT_S2_MEMATTR_MEMORY_WB		(0xf << 2)
+#define	TT_S2_BLOCK_VALID		(0x1 << 0)
+
+	.macro		s2_device_entry, base
+	.quad		(\base << 30) | TT_S2_AF | TT_S2_AP_RW | \
+			TT_S2_SH_NON_SHAREABLE | TT_S2_MEMATTR_DEVICE_nGRE | \
+			TT_S2_BLOCK_VALID
+	.endm
+
+	.macro		s2_memory_entry, base
+	.quad		(\base << 30) | TT_S2_AF | TT_S2_AP_RW | \
+			TT_S2_SH_NON_SHAREABLE | TT_S2_MEMATTR_MEMORY_WB | \
+			TT_S2_BLOCK_VALID
+	.endm
+
+	.section	".rodata", "a", %progbits
+	.align		13
+__s2_table:
+	/* 0x0000_0000 - 0x3fff_ffff -> NOR flash, EEPROM */
+	s2_memory_entry		0
+
+	/* 0x4000_0000 - 0x7fff_ffff -> PCIe config/MMIO32 */
+	s2_device_entry		1
+
+	/* 0x8000_0000 - 0xffff_ffff -> DRAM */
+	s2_memory_entry		2
+	s2_memory_entry		3
+
+	/* 0x1_0000_0000 - 0x8_7fff_ffff -> unused */
+	.rept			30
+	.quad			0
+	.endr
+
+	/* 0x8_8000_0000 - 0xf_ffff_ffff -> DRAM */
+	.irp			b, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, \
+				13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, \
+				24, 25, 26, 27, 28, 29
+	s2_memory_entry		(34 + \b)
+	.endr
+
+	/* 0x10_0000_0000 - 0x3d_ffff_ffff -> unused */
+	.rept			184
+	.quad			0
+	.endr
+
+	/* 0x3e_0000_0000 - 0x3f_ffff_ffff -> PCIe MMIO64 */
+	.irp			b, 0, 1, 2, 3, 4, 5, 6, 7
+	s2_device_entry		(248 + \b)
+	.endr
+
+	/* 0x40_0000_0000 - 0x87_ffff_ffff -> unused */
+	.rept			288
+	.quad			0
+	.endr
+
+	/* 0x88_0000_0000 - 0x8f_ffff_ffff -> DRAM */
+	.irp			b, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, \
+				13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, \
+				24, 25, 26, 27, 28, 29, 30, 31
+	s2_memory_entry		(544 + \b)
+	.endr
+
+	/* 0x90_0000_0000 - 0xff_ffff_ffff -> unused */
+	.rept			448
+	.quad			0
+	.endr
+	.size			__s2_table, . - __s2_table
+	.type			__s2_table, %object
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();
+
 }