ARM64 FVP Board Unification and ARMv9-A Support

arch/arm64/core/Kconfig

@@ -79,6 +79,26 @@ config CPU_CORTEX_A78
 	help
 	  This option signifies the use of a Cortex-A78 CPU
 
+config CPU_CORTEX_A510
+	bool
+	select CPU_CORTEX_A
+	select ARMV9_A
+	help
+	  This option signifies the use of a Cortex-A510 CPU, which is ARM's
+	  efficiency core implementing the ARMv9-A architecture. It provides
+	  power-efficient processing optimized for embedded applications with
+	  ARMv9-A features.
+
+config CPU_CORTEX_A320
+	bool
+	select CPU_CORTEX_A
+	select ARMV9_A
+	help
+	  This option signifies the use of a Cortex-A320 CPU, which implements
+	  the ARMv9.2-A architecture. It provides advanced features including
+	  enhanced SVE2, improved security extensions, and specialized performance
+	  optimizations.
+
 config CPU_CORTEX_R82
 	bool
 	select CPU_AARCH64_CORTEX_R
@@ -101,10 +121,15 @@ config MAIN_STACK_SIZE
 config IDLE_STACK_SIZE
 	default 4096
 
+config PRIVILEGED_STACK_SIZE
+	default 4096 if FPU_SHARING
+	default 2048
+
 config ISR_STACK_SIZE
 	default 4096
 
 config TEST_EXTRA_STACK_SIZE
+	default 4096 if FPU_SHARING
 	default 2048
 
 config SYSTEM_WORKQUEUE_STACK_SIZE
@@ -223,6 +248,20 @@ config ARMV8_A
 	  so that it can support some features included in the AArch64 state.
 	  It supports the T32 and A32 instruction sets.
 
+config ARMV9_A
+	bool
+	select ATOMIC_OPERATIONS_BUILTIN
+	select CPU_HAS_MMU
+	select ARCH_HAS_USERSPACE if ARM_MMU
+	select ARCH_HAS_NOCACHE_MEMORY_SUPPORT if ARM_MMU
+	imply ARM64_SVE if FPU_SHARING
+	help
+	  This option signifies the use of an ARMv9-A processor
+	  implementation.
+	  ARMv9-A builds on ARMv8-A and introduces additional security,
+	  performance, and machine learning capabilities while maintaining
+	  backward compatibility with ARMv8-A software.
+
 rsource "xen/Kconfig"
 
 endif # CPU_CORTEX_A
@@ -380,4 +419,27 @@ config ARM64_BOOT_DISABLE_DCACHE
 	  cache and then disable data cache, it will will be re-enabled after
 	  MMU is configured and enabled.
 
+config ARM64_SVE
+	bool "Scalable Vector Extension (SVE) support"
+	depends on ARMV9_A
+	help
+	  Enable support for ARM64 Scalable Vector Extension (SVE).
+	  This allows threads to use SVE instructions and automatically
+	  handles context switching of SVE registers (Z0-Z31, P0-P15, FFR)
+	  if CONFIG_FPU_SHARING is also set. Requires ARMv9-A architecture.
+
+config ARM64_SVE_VL_MAX
+	int "Maximum SVE vector length in bytes"
+	depends on ARM64_SVE
+	default 16
+	range 16 256
+	help
+	  Maximum supported SVE vector length in bytes. This determines
+	  the SVE context size within each thread structure. Valid values
+	  are any power of two from 16 to 256 inclusive (128 to 2048 bits).
+	  This can be smaller than the hardware supported vector length to
+	  save some per-thread memory in which case the hardware will be
+	  limited to the specified length. Having a larger value than what
+	  the hardware supports will only waste memory.
+
 endif # CPU_CORTEX_A || CPU_AARCH64_CORTEX_R

arch/arm64/core/fatal.c

@@ -337,7 +337,7 @@ static bool z_arm64_stack_corruption_check(struct arch_esf *esf, uint64_t esr, u
 			 * a new nested exception triggered by FPU accessing (var_args).
 			 */
 			arch_flush_local_fpu();
-			write_cpacr_el1(read_cpacr_el1() | CPACR_EL1_FPEN_NOTRAP);
+			write_cpacr_el1(read_cpacr_el1() | CPACR_EL1_FPEN);
 #endif
 			arch_curr_cpu()->arch.corrupted_sp = 0UL;
 			EXCEPTION_DUMP("STACK OVERFLOW FROM KERNEL,"

arch/arm64/core/fpu.S

@@ -7,12 +7,20 @@
 
 #include <zephyr/toolchain.h>
 #include <zephyr/linker/sections.h>
+#include <zephyr/offsets.h>
 
 _ASM_FILE_PROLOGUE
 
 GTEXT(z_arm64_fpu_save)
 SECTION_FUNC(TEXT, z_arm64_fpu_save)
 
+	mrs	x1, fpsr
+	mrs	x2, fpcr
+	str	w1, [x0, #__z_arm64_fp_context_fpsr_OFFSET]
+	str	w2, [x0, #__z_arm64_fp_context_fpcr_OFFSET]
+
+	/* Save NEON registers */
+	add	x0, x0, #__z_arm64_fp_context_neon_OFFSET
 	stp	q0,  q1,  [x0, #(16 *  0)]
 	stp	q2,  q3,  [x0, #(16 *  2)]
 	stp	q4,  q5,  [x0, #(16 *  4)]
@@ -30,16 +38,18 @@ SECTION_FUNC(TEXT, z_arm64_fpu_save)
 	stp	q28, q29, [x0, #(16 * 28)]
 	stp	q30, q31, [x0, #(16 * 30)]
 
-	mrs	x1, fpsr
-	mrs	x2, fpcr
-	str	w1, [x0, #(16 * 32 + 0)]
-	str	w2, [x0, #(16 * 32 + 4)]
-
 	ret
 
 GTEXT(z_arm64_fpu_restore)
 SECTION_FUNC(TEXT, z_arm64_fpu_restore)
 
+	ldr	w1, [x0, #__z_arm64_fp_context_fpsr_OFFSET]
+	ldr	w2, [x0, #__z_arm64_fp_context_fpcr_OFFSET]
+	msr	fpsr, x1
+	msr	fpcr, x2
+
+	/* Restore NEON registers */
+	add	x0, x0, #__z_arm64_fp_context_neon_OFFSET
 	ldp	q0,  q1,  [x0, #(16 *  0)]
 	ldp	q2,  q3,  [x0, #(16 *  2)]
 	ldp	q4,  q5,  [x0, #(16 *  4)]
@@ -57,9 +67,164 @@ SECTION_FUNC(TEXT, z_arm64_fpu_restore)
 	ldp	q28, q29, [x0, #(16 * 28)]
 	ldp	q30, q31, [x0, #(16 * 30)]
 
-	ldr	w1, [x0, #(16 * 32 + 0)]
-	ldr	w2, [x0, #(16 * 32 + 4)]
-	msr	fpsr, x1
-	msr	fpcr, x2
+	ret
+
+#ifdef CONFIG_ARM64_SVE
+
+GTEXT(z_arm64_sve_save)
+SECTION_FUNC(TEXT, z_arm64_sve_save)
+
+	/* Save control registers */
+	mrs	x2, fpsr
+	mrs	x3, fpcr
+	str	w2, [x0, #__z_arm64_fp_context_fpsr_OFFSET]
+	str	w3, [x0, #__z_arm64_fp_context_fpcr_OFFSET]
+
+	/* Get Z registers base address */
+	add	x2, x0, #__z_arm64_fp_context_sve_z_regs_OFFSET
+
+	/* Save Z registers */
+	str	z0, [x2, #0, MUL VL]
+	str	z1, [x2, #1, MUL VL]
+	str	z2, [x2, #2, MUL VL]
+	str	z3, [x2, #3, MUL VL]
+	str	z4, [x2, #4, MUL VL]
+	str	z5, [x2, #5, MUL VL]
+	str	z6, [x2, #6, MUL VL]
+	str	z7, [x2, #7, MUL VL]
+	str	z8, [x2, #8, MUL VL]
+	str	z9, [x2, #9, MUL VL]
+	str	z10, [x2, #10, MUL VL]
+	str	z11, [x2, #11, MUL VL]
+	str	z12, [x2, #12, MUL VL]
+	str	z13, [x2, #13, MUL VL]
+	str	z14, [x2, #14, MUL VL]
+	str	z15, [x2, #15, MUL VL]
+	str	z16, [x2, #16, MUL VL]
+	str	z17, [x2, #17, MUL VL]
+	str	z18, [x2, #18, MUL VL]
+	str	z19, [x2, #19, MUL VL]
+	str	z20, [x2, #20, MUL VL]
+	str	z21, [x2, #21, MUL VL]
+	str	z22, [x2, #22, MUL VL]
+	str	z23, [x2, #23, MUL VL]
+	str	z24, [x2, #24, MUL VL]
+	str	z25, [x2, #25, MUL VL]
+	str	z26, [x2, #26, MUL VL]
+	str	z27, [x2, #27, MUL VL]
+	str	z28, [x2, #28, MUL VL]
+	str	z29, [x2, #29, MUL VL]
+	str	z30, [x2, #30, MUL VL]
+	str	z31, [x2, #31, MUL VL]
+
+	/* Get P registers base address */
+	mov	x3, #__z_arm64_fp_context_sve_p_regs_OFFSET
+	add	x3, x0, x3
+
+	/* Save P registers */
+	str	p0, [x3, #0, MUL VL]
+	str	p1, [x3, #1, MUL VL]
+	str	p2, [x3, #2, MUL VL]
+	str	p3, [x3, #3, MUL VL]
+	str	p4, [x3, #4, MUL VL]
+	str	p5, [x3, #5, MUL VL]
+	str	p6, [x3, #6, MUL VL]
+	str	p7, [x3, #7, MUL VL]
+	str	p8, [x3, #8, MUL VL]
+	str	p9, [x3, #9, MUL VL]
+	str	p10, [x3, #10, MUL VL]
+	str	p11, [x3, #11, MUL VL]
+	str	p12, [x3, #12, MUL VL]
+	str	p13, [x3, #13, MUL VL]
+	str	p14, [x3, #14, MUL VL]
+	str	p15, [x3, #15, MUL VL]
+
+	/* Get FFR base address */
+	mov	x4, #__z_arm64_fp_context_sve_ffr_OFFSET
+	add	x4, x0, x4
+
+	/* Save FFR */
+	rdffr	p0.b
+	str	p0, [x4]
+
+	ret
+
+GTEXT(z_arm64_sve_restore)
+SECTION_FUNC(TEXT, z_arm64_sve_restore)
+
+	/* Get Z registers base address */
+	add	x2, x0, #__z_arm64_fp_context_sve_z_regs_OFFSET
+
+	/* Restore Z registers */
+	ldr	z0, [x2, #0, MUL VL]
+	ldr	z1, [x2, #1, MUL VL]
+	ldr	z2, [x2, #2, MUL VL]
+	ldr	z3, [x2, #3, MUL VL]
+	ldr	z4, [x2, #4, MUL VL]
+	ldr	z5, [x2, #5, MUL VL]
+	ldr	z6, [x2, #6, MUL VL]
+	ldr	z7, [x2, #7, MUL VL]
+	ldr	z8, [x2, #8, MUL VL]
+	ldr	z9, [x2, #9, MUL VL]
+	ldr	z10, [x2, #10, MUL VL]
+	ldr	z11, [x2, #11, MUL VL]
+	ldr	z12, [x2, #12, MUL VL]
+	ldr	z13, [x2, #13, MUL VL]
+	ldr	z14, [x2, #14, MUL VL]
+	ldr	z15, [x2, #15, MUL VL]
+	ldr	z16, [x2, #16, MUL VL]
+	ldr	z17, [x2, #17, MUL VL]
+	ldr	z18, [x2, #18, MUL VL]
+	ldr	z19, [x2, #19, MUL VL]
+	ldr	z20, [x2, #20, MUL VL]
+	ldr	z21, [x2, #21, MUL VL]
+	ldr	z22, [x2, #22, MUL VL]
+	ldr	z23, [x2, #23, MUL VL]
+	ldr	z24, [x2, #24, MUL VL]
+	ldr	z25, [x2, #25, MUL VL]
+	ldr	z26, [x2, #26, MUL VL]
+	ldr	z27, [x2, #27, MUL VL]
+	ldr	z28, [x2, #28, MUL VL]
+	ldr	z29, [x2, #29, MUL VL]
+	ldr	z30, [x2, #30, MUL VL]
+	ldr	z31, [x2, #31, MUL VL]
+
+	/* Get FFR base address */
+	mov	x4, #__z_arm64_fp_context_sve_ffr_OFFSET
+	add	x4, x0, x4
+
+	/* Restore FFR */
+	ldr	p0, [x4]
+	wrffr	p0.b
+
+	/* Get P registers base address */
+	mov	x3, #__z_arm64_fp_context_sve_p_regs_OFFSET
+	add	x3, x0, x3
+
+	/* Restore P registers intervals */
+	ldr	p0, [x3, #0, MUL VL]
+	ldr	p1, [x3, #1, MUL VL]
+	ldr	p2, [x3, #2, MUL VL]
+	ldr	p3, [x3, #3, MUL VL]
+	ldr	p4, [x3, #4, MUL VL]
+	ldr	p5, [x3, #5, MUL VL]
+	ldr	p6, [x3, #6, MUL VL]
+	ldr	p7, [x3, #7, MUL VL]
+	ldr	p8, [x3, #8, MUL VL]
+	ldr	p9, [x3, #9, MUL VL]
+	ldr	p10, [x3, #10, MUL VL]
+	ldr	p11, [x3, #11, MUL VL]
+	ldr	p12, [x3, #12, MUL VL]
+	ldr	p13, [x3, #13, MUL VL]
+	ldr	p14, [x3, #14, MUL VL]
+	ldr	p15, [x3, #15, MUL VL]
+
+	/* Restore control registers */
+	ldr	w2, [x0, #__z_arm64_fp_context_fpsr_OFFSET]
+	ldr	w3, [x0, #__z_arm64_fp_context_fpcr_OFFSET]
+	msr	fpsr, x2
+	msr	fpcr, x3
 
 	ret
+
+#endif /* CONFIG_ARM64_SVE */