Merge "feat(fvp_r): configure system registers to boot rich OS" into integration

4 files changed, 52 insertions, 18 deletions

diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 0ad97543b..74bc8cb8a 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -585,6 +585,10 @@
 #define CPTR_EL2_TZ_BIT		(U(1) << 8)
 #define CPTR_EL2_RESET_VAL	CPTR_EL2_RES1
 
+/* VTCR_EL2 definitions */
+#define VTCR_RESET_VAL         U(0x0)
+#define VTCR_EL2_MSA           (U(1) << 31)
+
 /* CPSR/SPSR definitions */
 #define DAIF_FIQ_BIT		(U(1) << 0)
 #define DAIF_IRQ_BIT		(U(1) << 1)
diff --git a/include/arch/aarch64/arch_helpers.h b/include/arch/aarch64/arch_helpers.h
index 1aadf0b8d..cae05dc2e 100644
--- a/include/arch/aarch64/arch_helpers.h
+++ b/include/arch/aarch64/arch_helpers.h
@@ -442,6 +442,8 @@ DEFINE_SYSREG_RW_FUNCS(cntp_cval_el0)
 DEFINE_SYSREG_READ_FUNC(cntpct_el0)
 DEFINE_SYSREG_RW_FUNCS(cnthctl_el2)
 
+DEFINE_SYSREG_RW_FUNCS(vtcr_el2)
+
 #define get_cntp_ctl_enable(x)  (((x) >> CNTP_CTL_ENABLE_SHIFT) & \
 					CNTP_CTL_ENABLE_MASK)
 #define get_cntp_ctl_imask(x)   (((x) >> CNTP_CTL_IMASK_SHIFT) & \
diff --git a/plat/arm/board/fvp_r/fvp_r_bl1_context_mgmt.c b/plat/arm/board/fvp_r/fvp_r_bl1_context_mgmt.c
index 7ae853b76..c6544b45d 100644
--- a/plat/arm/board/fvp_r/fvp_r_bl1_context_mgmt.c
+++ b/plat/arm/board/fvp_r/fvp_r_bl1_context_mgmt.c
@@ -15,7 +15,7 @@
 #include <plat/common/platform.h>
 
 
-void cm_prepare_el2_exit(uint32_t security_state);
+void cm_prepare_el2_exit(void);
 
 /* Following contains the cpu context pointers. */
 static void *bl1_cpu_context_ptr[2];
@@ -81,6 +81,9 @@ void bl1_prepare_next_image(unsigned int image_id)
 	/* Allow platform to make change */
 	bl1_plat_set_ep_info(image_id, next_bl_ep);
 
+	/* Prepare context for the next EL */
+	cm_prepare_el2_exit();
+
 	/* Indicate that image is in execution state. */
 	desc->state = IMAGE_STATE_EXECUTED;
 
diff --git a/plat/arm/board/fvp_r/fvp_r_context_mgmt.c b/plat/arm/board/fvp_r/fvp_r_context_mgmt.c
index 678b8792c..d172d2d90 100644
--- a/plat/arm/board/fvp_r/fvp_r_context_mgmt.c
+++ b/plat/arm/board/fvp_r/fvp_r_context_mgmt.c
@@ -4,25 +4,50 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <lib/el3_runtime/context_mgmt.h>
-#include <lib/el3_runtime/pubsub_events.h>
+#include <arch_helpers.h>
 
-#include <platform_def.h>
+/************************************************************
+ * For R-class everything is in secure world.
+ * Prepare the CPU system registers for first entry into EL1
+ ************************************************************/
+void cm_prepare_el2_exit(void)
+{
+	uint64_t hcr_el2 = 0U;
 
+	/*
+	 * The use of ARMv8.3 pointer authentication (PAuth) is governed
+	 * by fields in HCR_EL2, which trigger a 'trap to EL2' if not
+	 * enabled. This register initialized at boot up, update PAuth
+	 * bits.
+	 *
+	 * HCR_API_BIT: Set to one to disable traps to EL2 if lower ELs
+	 * access PAuth registers
+	 *
+	 * HCR_APK_BIT: Set to one to disable traps to EL2 if lower ELs
+	 * access PAuth instructions
+	 */
+	hcr_el2 = read_hcr_el2();
+	write_hcr_el2(hcr_el2 | HCR_API_BIT | HCR_APK_BIT);
 
-/*******************************************************************************
- * File contains EL2 equivalents of EL3 functions from
- * .../lib/el3_runtime/aarch64/context_mgmt.c
- ******************************************************************************/
+	/*
+	 * Initialise CNTHCTL_EL2. All fields are architecturally UNKNOWN
+	 * on reset and are set to zero except for field(s) listed below.
+	 *
+	 * CNTHCTL_EL2.EL1PCEN: Set to one to disable traps to EL2
+	 * if lower ELs accesses to the physical timer registers.
+	 *
+	 * CNTHCTL_EL2.EL1PCTEN: Set to one to disable traps to EL2
+	 * if lower ELs access to the physical counter registers.
+	 */
+	write_cnthctl_el2(CNTHCTL_RESET_VAL | EL1PCEN_BIT | EL1PCTEN_BIT);
 
-/*******************************************************************************
- * Prepare the CPU system registers for first entry into secure or normal world
- *
- * The majority of the work needed is only for switching to non-secure, which
- * is not available on v8-R64 cores, so this function is very simple.
- ******************************************************************************/
-void cm_prepare_el2_exit(uint32_t security_state)
-{
-	cm_el1_sysregs_context_restore(security_state);
-	cm_set_next_eret_context(security_state);
+	/*
+	 * On Armv8-R, the EL1&0 memory system architecture is configurable
+	 * as a VMSA or PMSA. All the fields architecturally UNKNOWN on reset
+	* and are set to zero except for field listed below.
+	*
+	* VCTR_EL2.MSA: Set to one to ensure the VMSA is enabled so that
+	* rich OS can boot.
+	*/
+	write_vtcr_el2(VTCR_RESET_VAL | VTCR_EL2_MSA);
 }