kernel/arm: save FPU registers on context switch

Change-Id: Icb62688338d3a3083f8f6d1874cae3218ba042e0
Reviewed-on: https://review.haiku-os.org/c/haiku/+/5614
Reviewed-by: Fredrik Holmqvist <[email protected]>
Tested-by: Commit checker robot <[email protected]>

headers/private/kernel/arch/arm/arch_thread_types.h

@@ -14,10 +14,15 @@ struct iframe_stack {
 	int32	index;
 };
 
+struct arch_fpu_context {
+	uint64_t	fp_regs[32];
+	uint32_t	fpscr;
+};
+
 // architecture specific thread info
 struct arch_thread {
 	void	*sp;	// stack pointer
-	void	*interrupt_stack;
+	struct	arch_fpu_context fpuContext;
 
 	// used to track interrupts on this thread
 	struct iframe_stack	iframes;
@@ -43,6 +48,9 @@ extern "C" {
 #endif
 
 void arch_return_to_userland(struct iframe *);
+void arm_context_switch(struct arch_thread* from, struct arch_thread* to);
+void arm_save_fpu(struct arch_fpu_context* context);
+void arm_restore_fpu(struct arch_fpu_context* context);
 
 #ifdef __cplusplus
 }

src/system/kernel/arch/arm/arch_asm.S

@@ -88,6 +88,26 @@ FUNCTION(arm_context_switch):
 FUNCTION_END(arm_context_switch)
 
 
+/* void arm_save_fpu(struct arch_fpu_context* context); */
+FUNCTION(arm_save_fpu):
+	fstmiad		r0!, {d0-d15}
+	fstmiad		r0!, {d16-d31}
+	vmrs		r1, fpscr
+	str			r1, [r0]
+	bx			lr
+FUNCTION_END(arm_save_fpu)
+
+
+/* void arm_restore_fpu(struct arch_fpu_context* context); */
+FUNCTION(arm_restore_fpu):
+	fldmiad		r0!, {d0-d15}
+	fldmiad		r0!, {d16-d31}
+	ldr			r1, [r0]
+	vmsr		fpscr, r1
+	bx			lr
+FUNCTION_END(arm_restore_fpu)
+
+
 /* addr_t arm_get_fsr(void); */
 FUNCTION(arm_get_fsr):
 	mrc	p15, 0, r0, c5, c0, 0		@ get FSR

src/system/kernel/arch/arm/arch_thread.cpp

@@ -131,9 +131,6 @@ arch_thread_init_tls(Thread *thread)
 	return user_memcpy((void *)thread->user_local_storage, tls, sizeof(tls));
 }
 
-extern "C" void arm_context_switch(void *from, void *to);
-
-
 void
 arm_swap_pgdir(uint32_t pageDirectoryAddress)
 {
@@ -181,6 +178,8 @@ arch_thread_context_switch(Thread *from, Thread *to)
 
 	TRACE("arch_thread_context_switch: %p(%s/%p) -> %p(%s/%p)\n",
 		from, from->name, from->arch_info.sp, to, to->name, to->arch_info.sp);
+	arm_save_fpu(&from->arch_info.fpuContext);
+	arm_restore_fpu(&to->arch_info.fpuContext);
 	arm_context_switch(&from->arch_info, &to->arch_info);
 	TRACE("arch_thread_context_switch %p %p\n", to, from);
 }