feat(d128): add support for FEAT_D128
This patch disables trapping to EL3 when the FEAT_D128
specific registers are accessed by setting the SCR_EL3.D128En bit.
If FEAT_D128 is implemented, then FEAT_SYSREG128 is implemented.
With FEAT_SYSREG128 certain system registers are treated as 128-bit,
so we should be context saving and restoring 128-bits instead of 64-bit
when FEAT_D128 is enabled.
FEAT_SYSREG128 adds support for MRRS and MSRR instruction which
helps us to read write to 128-bit system register.
Refer to Arm Architecture Manual for further details.
Change the FVP platform to default to handling this as a dynamic option
so the right decision can be made by the code at runtime.
Change-Id: I1a53db5eac29e56c8fbdcd4961ede3abfcb2411a
Signed-off-by: Jayanth Dodderi Chidanand <jayanthdodderi.chidanand@arm.com>
Signed-off-by: Govindraj Raja <govindraj.raja@arm.com>
diff --git a/lib/extensions/sysreg128/sysreg128.S b/lib/extensions/sysreg128/sysreg128.S
new file mode 100644
index 0000000..08cff2f
--- /dev/null
+++ b/lib/extensions/sysreg128/sysreg128.S
@@ -0,0 +1,139 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <lib/extensions/sysreg128.h>
+
+ .global read_par_el1
+ .global write_par_el1
+ .global read_ttbr0_el1
+ .global write_ttbr0_el1
+ .global read_ttbr1_el1
+ .global write_ttbr1_el1
+ .global read_ttbr0_el2
+ .global write_ttbr0_el2
+ .global read_ttbr1_el2
+ .global write_ttbr1_el2
+ .global read_vttbr_el2
+ .global write_vttbr_el2
+ .global read_rcwmask_el1
+ .global write_rcwmask_el1
+ .global read_rcwsmask_el1
+ .global write_rcwsmask_el1
+
+/*
+ * _mrrs - Move System register to two adjacent general-purpose
+ * registers.
+ * Instruction: MRRS <Xt>, <Xt+1>, (<systemreg>|S<op0>_<op1>_<Cn>_<Cm>_<op2>)
+ *
+ * Arguments/Opcode bit field:
+ * regins: System register opcode.
+ *
+ * Clobbers: x0,x1,x2
+ */
+.macro _mrrs regins:req
+#if ENABLE_FEAT_D128 == 2
+ mrs x0, ID_AA64MMFR3_EL1
+ tst x0, #(ID_AA64MMFR3_EL1_D128_MASK << ID_AA64MMFR3_EL1_D128_SHIFT)
+ bne 1f
+ /* If FEAT_D128 is not implemented then use mrs */
+ .inst 0xD5300000 | (\regins)
+ ret
+#endif
+1:
+ .inst 0xD5700000 | (\regins)
+ ret
+.endm
+
+/*
+ * _msrr - Move two adjacent general-purpose registers to System register.
+ * Instruction: MSRR (<systemreg>|S<op0>_<op1>_<Cn>_<Cm>_<op2>), <Xt>, <Xt+1>
+ *
+ * Arguments/Opcode bit field:
+ * regins: System register opcode.
+ *
+ * Clobbers: x0,x1,x2
+ */
+.macro _msrr regins:req
+ /* If FEAT_D128 is not implemented use msr, dont tamper
+ * x0, x1 as they maybe used for mrrs */
+#if ENABLE_FEAT_D128 == 2
+ mrs x2, ID_AA64MMFR3_EL1
+ tst x2, #(ID_AA64MMFR3_EL1_D128_MASK << ID_AA64MMFR3_EL1_D128_SHIFT)
+ bne 1f
+ /* If FEAT_D128 is not implemented then use msr */
+ .inst 0xD5100000 | (\regins)
+ ret
+#endif
+1:
+ .inst 0xD5500000 | (\regins)
+ ret
+.endm
+
+func read_par_el1
+ _mrrs 0x87400 /* S3_0_C7_C4_0 */
+endfunc read_par_el1
+
+func write_par_el1
+ _msrr 0x87400
+endfunc write_par_el1
+
+func read_ttbr0_el1
+ _mrrs 0x82000 /* S3_0_C2_C0_0 */
+endfunc read_ttbr0_el1
+
+func write_ttbr0_el1
+ _msrr 0x82000
+endfunc write_ttbr0_el1
+
+func read_ttbr1_el1
+ _mrrs 0x82020 /* S3_0_C2_C0_1 */
+endfunc read_ttbr1_el1
+
+func write_ttbr1_el1
+ _msrr 0x82020
+endfunc write_ttbr1_el1
+
+func read_ttbr0_el2
+ _mrrs 0xC2000 /* S3_4_C2_C0_0 */
+endfunc read_ttbr0_el2
+
+func write_ttbr0_el2
+ _msrr 0xC2000
+endfunc write_ttbr0_el2
+
+func read_ttbr1_el2
+ _mrrs 0xC2020 /* S3_4_C2_C0_1 */
+endfunc read_ttbr1_el2
+
+func write_ttbr1_el2
+ _msrr 0xC2020
+endfunc write_ttbr1_el2
+
+func read_vttbr_el2
+ _mrrs 0xC2100 /* S3_4_C2_C1_0 */
+endfunc read_vttbr_el2
+
+func write_vttbr_el2
+ _msrr 0xC2100
+endfunc write_vttbr_el2
+
+func read_rcwmask_el1
+ _mrrs 0x8D0C0 /* S3_0_C13_C0_6 */
+endfunc read_rcwmask_el1
+
+func write_rcwmask_el1
+ _msrr 0x8D0C0
+endfunc write_rcwmask_el1
+
+func read_rcwsmask_el1
+ _mrrs 0x8D060 /* S3_0_C13_C0_3 */
+endfunc read_rcwsmask_el1
+
+func write_rcwsmask_el1
+ _msrr 0x8D060
+endfunc write_rcwsmask_el1