Enable v8.6 WFE trap delays

This patch enables the v8.6 extension to add a delay before WFE traps
are taken. A weak hook plat_arm_set_twedel_scr_el3 has been added in
plat/common/aarch64/plat_common.c that disables this feature by default
but platform-specific code can override it when needed.

The only hook provided sets the TWED fields in SCR_EL3, there are similar
fields in HCR_EL2, SCTLR_EL2, and SCTLR_EL1 to control WFE trap delays in
lower ELs but these should be configured by code running at EL2 and/or EL1
depending on the platform configuration and is outside the scope of TF-A.

Signed-off-by: John Powell <john.powell@arm.com>
Change-Id: I0a9bb814205efeab693a3d0a0623e62144abba2d

docs/getting_started/porting-guide.rst

@@ -1890,6 +1890,21 @@ frequency for the CPU's generic timer. This value will be programmed into the
 of the system counter, which is retrieved from the first entry in the frequency
 modes table.
 
+Function : plat_arm_set_twedel_scr_el3() [optional]
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+    Argument : void
+    Return   : uint32_t
+
+This function is used in v8.6+ systems to set the WFE trap delay value in
+SCR_EL3. If this function returns TWED_DISABLED or is left unimplemented, this
+feature is not enabled.  The only hook provided is to set the TWED fields in
+SCR_EL3, there are similar fields in HCR_EL2, SCTLR_EL2, and SCTLR_EL1 to adjust
+the WFE trap delays in lower ELs and these fields should be set by the
+appropriate EL2 or EL1 code depending on the platform configuration.
+
 #define : PLAT_PERCPU_BAKERY_LOCK_SIZE [optional]
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 

include/arch/aarch64/arch.h

@@ -226,6 +226,12 @@
 #define ID_AA64MMFR0_EL1_TGRAN16_SUPPORTED	ULL(0x1)
 #define ID_AA64MMFR0_EL1_TGRAN16_NOT_SUPPORTED	ULL(0x0)
 
+/* ID_AA64MMFR1_EL1 definitions */
+#define ID_AA64MMFR1_EL1_TWED_SHIFT		U(32)
+#define ID_AA64MMFR1_EL1_TWED_MASK		ULL(0xf)
+#define ID_AA64MMFR1_EL1_TWED_SUPPORTED		ULL(0x1)
+#define ID_AA64MMFR1_EL1_TWED_NOT_SUPPORTED	ULL(0x0)
+
 /* ID_AA64MMFR2_EL1 definitions */
 #define ID_AA64MMFR2_EL1		S3_0_C0_C7_2
 
@@ -312,6 +318,9 @@
 
 /* SCR definitions */
 #define SCR_RES1_BITS		((U(1) << 4) | (U(1) << 5))
+#define SCR_TWEDEL_SHIFT	U(30)
+#define SCR_TWEDEL_MASK		ULL(0xf)
+#define SCR_TWEDEn_BIT		(UL(1) << 29)
 #define SCR_ATA_BIT		(U(1) << 26)
 #define SCR_FIEN_BIT		(U(1) << 21)
 #define SCR_EEL2_BIT		(U(1) << 18)

include/arch/aarch64/arch_features.h

@@ -58,4 +58,10 @@ static inline bool is_armv8_4_sel2_present(void)
 		ID_AA64PFR0_SEL2_MASK) == 1ULL;
 }
 
+static inline bool is_armv8_6_twed_present(void)
+{
+	return (((read_id_aa64mmfr1_el1() >> ID_AA64MMFR1_EL1_TWED_SHIFT) &
+		ID_AA64MMFR1_EL1_TWED_MASK) == ID_AA64MMFR1_EL1_TWED_SUPPORTED);
+}
+
 #endif /* ARCH_FEATURES_H */

include/arch/aarch64/arch_helpers.h

@@ -358,6 +358,7 @@ void __dead2 smc(uint64_t x0, uint64_t x1, uint64_t x2, uint64_t x3,
 DEFINE_SYSREG_READ_FUNC(midr_el1)
 DEFINE_SYSREG_READ_FUNC(mpidr_el1)
 DEFINE_SYSREG_READ_FUNC(id_aa64mmfr0_el1)
+DEFINE_SYSREG_READ_FUNC(id_aa64mmfr1_el1)
 
 DEFINE_SYSREG_RW_FUNCS(scr_el3)
 DEFINE_SYSREG_RW_FUNCS(hcr_el2)

include/lib/extensions/twed.h

@@ -0,0 +1,16 @@
+/*
+ * Copyright (c) 2020, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef TWED_H
+#define TWED_H
+
+#include <stdint.h>
+
+#define TWED_DISABLED U(0xFFFFFFFF)
+
+uint32_t plat_arm_set_twedel_scr_el3(void);
+
+#endif /* TWEDE_H */

lib/el3_runtime/aarch64/context_mgmt.c

@@ -22,6 +22,7 @@
 #include <lib/extensions/mpam.h>
 #include <lib/extensions/spe.h>
 #include <lib/extensions/sve.h>
+#include <lib/extensions/twed.h>
 #include <lib/utils.h>
 
 
@@ -229,6 +230,24 @@ void cm_setup_context(cpu_context_t *ctx, const entry_point_info_t *ep)
 	sctlr_elx |= SCTLR_IESB_BIT;
 #endif
 
+	/* Enable WFE trap delay in SCR_EL3 if supported and configured */
+	if (is_armv8_6_twed_present()) {
+		uint32_t delay = plat_arm_set_twedel_scr_el3();
+
+		if (delay != TWED_DISABLED) {
+			/* Make sure delay value fits */
+			assert((delay & ~SCR_TWEDEL_MASK) == 0U);
+
+			/* Set delay in SCR_EL3 */
+			scr_el3 &= ~(SCR_TWEDEL_MASK << SCR_TWEDEL_SHIFT);
+			scr_el3 |= ((delay & SCR_TWEDEL_MASK)
+					<< SCR_TWEDEL_SHIFT);
+
+			/* Enable WFE delay */
+			scr_el3 |= SCR_TWEDEn_BIT;
+		}
+	}
+
 	/*
 	 * Store the initialised SCTLR_EL1 value in the cpu_context - SCTLR_EL2
 	 * and other EL2 registers are set up by cm_prepare_ns_entry() as they

plat/common/aarch64/plat_common.c

@@ -11,6 +11,7 @@
 #if RAS_EXTENSION
 #include <lib/extensions/ras.h>
 #endif
+#include <lib/extensions/twed.h>
 #include <lib/xlat_tables/xlat_mmu_helpers.h>
 #include <plat/common/platform.h>
 
@@ -20,6 +21,7 @@
  * platforms but may also be overridden by a platform if required.
  */
 #pragma weak bl31_plat_runtime_setup
+#pragma weak plat_arm_set_twedel_scr_el3
 
 #if SDEI_SUPPORT
 #pragma weak plat_sdei_handle_masked_trigger
@@ -100,3 +102,16 @@ void plat_ea_handler(unsigned int ea_reason, uint64_t syndrome, void *cookie,
 #endif
 	panic();
 }
+
+/*******************************************************************************
+ * In v8.6+ platforms with delayed trapping of WFE this hook sets the delay. It
+ * is a weak function definition so can be overridden depending on the
+ * requirements of a platform.  The only hook provided is for the TWED fields
+ * in SCR_EL3, the TWED fields in HCR_EL2, SCTLR_EL2, and SCTLR_EL1 should be
+ * configured as needed in lower exception levels.
+ ******************************************************************************/
+
+uint32_t plat_arm_set_twedel_scr_el3(void)
+{
+	return TWED_DISABLED;
+}