arm: Support VFP-D32 and Neon

1 files changed, 159 insertions, 1 deletions

diff --git a/cpukit/score/cpu/arm/arm-context-validate.S b/cpukit/score/cpu/arm/arm-context-validate.S
index 4591d20328..ad2ebb937b 100644
--- a/cpukit/score/cpu/arm/arm-context-validate.S
+++ b/cpukit/score/cpu/arm/arm-context-validate.S
@@ -17,6 +17,7 @@
 #endif
 
 #include <rtems/asm.h>
+#include <rtems/score/cpu.h>
 
 #define FRAME_OFFSET_R4 0
 #define FRAME_OFFSET_R5 4
@@ -28,7 +29,20 @@
 #define FRAME_OFFSET_R11 28
 #define FRAME_OFFSET_LR 32
 
-#define FRAME_SIZE (FRAME_OFFSET_LR + 4)
+#ifdef ARM_MULTILIB_VFP_D32
+  #define FRAME_OFFSET_D8 40
+  #define FRAME_OFFSET_D9 48
+  #define FRAME_OFFSET_D10 56
+  #define FRAME_OFFSET_D11 64
+  #define FRAME_OFFSET_D12 72
+  #define FRAME_OFFSET_D13 80
+  #define FRAME_OFFSET_D14 88
+  #define FRAME_OFFSET_D15 96
+
+  #define FRAME_SIZE (FRAME_OFFSET_D15 + 8)
+#else
+  #define FRAME_SIZE (FRAME_OFFSET_LR + 4)
+#endif
 
 	.section	.text
 
@@ -57,6 +71,17 @@ FUNCTION_THUMB_ENTRY(_CPU_Context_validate)
 	mov	r1, lr
 	str	r1, [sp, #FRAME_OFFSET_LR]
 
+#ifdef ARM_MULTILIB_VFP_D32
+	vstr	d8, [sp, #FRAME_OFFSET_D8]
+	vstr	d9, [sp, #FRAME_OFFSET_D9]
+	vstr	d10, [sp, #FRAME_OFFSET_D10]
+	vstr	d11, [sp, #FRAME_OFFSET_D11]
+	vstr	d12, [sp, #FRAME_OFFSET_D12]
+	vstr	d13, [sp, #FRAME_OFFSET_D13]
+	vstr	d14, [sp, #FRAME_OFFSET_D14]
+	vstr	d15, [sp, #FRAME_OFFSET_D15]
+#endif
+
 	/* Fill */
 
 	/* R1 is used for temporary values */
@@ -70,7 +95,20 @@ FUNCTION_THUMB_ENTRY(_CPU_Context_validate)
 	mov	\reg, r1
 .endm
 
+
+#ifdef ARM_MULTILIB_VFP_D32
+	/* R3 contains the FPSCR */
+	vmrs	r3, FPSCR
+	movs	r4, #0x001f
+	movt	r4, #0xf800
+	bic	r3, r3, r4
+	and	r4, r4, r0
+	orr	r3, r3, r4
+	vmsr	FPSCR, r3
+#else
 	fill_register	r3
+#endif
+
 	fill_register	r4
 	fill_register	r5
 	fill_register	r6
@@ -82,6 +120,46 @@ FUNCTION_THUMB_ENTRY(_CPU_Context_validate)
 	fill_register	r12
 	fill_register	lr
 
+#ifdef ARM_MULTILIB_VFP_D32
+.macro fill_vfp_register reg
+	add	r1, r1, #1
+	vmov	\reg, r1, r1
+.endm
+
+	fill_vfp_register	d0
+	fill_vfp_register	d1
+	fill_vfp_register	d2
+	fill_vfp_register	d3
+	fill_vfp_register	d4
+	fill_vfp_register	d5
+	fill_vfp_register	d6
+	fill_vfp_register	d7
+	fill_vfp_register	d8
+	fill_vfp_register	d9
+	fill_vfp_register	d10
+	fill_vfp_register	d11
+	fill_vfp_register	d12
+	fill_vfp_register	d13
+	fill_vfp_register	d14
+	fill_vfp_register	d15
+	fill_vfp_register	d16
+	fill_vfp_register	d17
+	fill_vfp_register	d18
+	fill_vfp_register	d19
+	fill_vfp_register	d20
+	fill_vfp_register	d21
+	fill_vfp_register	d22
+	fill_vfp_register	d23
+	fill_vfp_register	d24
+	fill_vfp_register	d25
+	fill_vfp_register	d26
+	fill_vfp_register	d27
+	fill_vfp_register	d28
+	fill_vfp_register	d29
+	fill_vfp_register	d30
+	fill_vfp_register	d31
+#endif
+
 	/* Check */
 check:
 
@@ -96,7 +174,10 @@ check:
 
 	mov	r1, r0
 
+#ifndef ARM_MULTILIB_VFP_D32
 	check_register	r3
+#endif
+
 	check_register	r4
 	check_register	r5
 	check_register	r6
@@ -108,6 +189,10 @@ check:
 	check_register	r12
 	check_register	lr
 
+#ifdef ARM_MULTILIB_VFP_D32
+	b	check_vfp
+#endif
+
 	b	check
 
 	/* Restore */
@@ -132,8 +217,81 @@ restore:
 	ldr	r1, [sp, #FRAME_OFFSET_LR]
 	mov	lr, r1
 
+#ifdef ARM_MULTILIB_VFP_D32
+	vldr	d8, [sp, #FRAME_OFFSET_D8]
+	vldr	d9, [sp, #FRAME_OFFSET_D9]
+	vldr	d10, [sp, #FRAME_OFFSET_D10]
+	vldr	d11, [sp, #FRAME_OFFSET_D11]
+	vldr	d12, [sp, #FRAME_OFFSET_D12]
+	vldr	d13, [sp, #FRAME_OFFSET_D13]
+	vldr	d14, [sp, #FRAME_OFFSET_D14]
+	vldr	d15, [sp, #FRAME_OFFSET_D15]
+#endif
+
 	add	sp, sp, #FRAME_SIZE
 
 	bx	lr
 
 FUNCTION_END(_CPU_Context_validate)
+
+#ifdef ARM_MULTILIB_VFP_D32
+check_vfp:
+
+.macro check_vfp_register reg
+	add	r1, r1, #1
+	vmov	r4, r5, \reg
+	cmp	r4, r5
+	bne	1f
+	cmp	r1, r4
+	bne	1f
+	b	2f
+1:
+	b	restore
+2:
+.endm
+
+	vmrs	r4, FPSCR
+	cmp	r4, r3
+	bne	restore
+
+	check_vfp_register	d0
+	check_vfp_register	d1
+	check_vfp_register	d2
+	check_vfp_register	d3
+	check_vfp_register	d4
+	check_vfp_register	d5
+	check_vfp_register	d6
+	check_vfp_register	d7
+	check_vfp_register	d8
+	check_vfp_register	d9
+	check_vfp_register	d10
+	check_vfp_register	d11
+	check_vfp_register	d12
+	check_vfp_register	d13
+	check_vfp_register	d14
+	check_vfp_register	d15
+	check_vfp_register	d16
+	check_vfp_register	d17
+	check_vfp_register	d18
+	check_vfp_register	d19
+	check_vfp_register	d20
+	check_vfp_register	d21
+	check_vfp_register	d22
+	check_vfp_register	d23
+	check_vfp_register	d24
+	check_vfp_register	d25
+	check_vfp_register	d26
+	check_vfp_register	d27
+	check_vfp_register	d28
+	check_vfp_register	d29
+	check_vfp_register	d30
+	check_vfp_register	d31
+
+	/* Restore r4 and r5 */
+	mov	r1, r0
+	fill_register	r4
+	fill_register	r5
+
+	b	check
+
+#endif