core: arm32: fix saving vfp state

Prior to this patch TF-A in AArch32 didn't save the normal world VFP
data registers and thus always restored then as zeroes and causing
problems in normal world.

With this patch if running with TF-A in AArch32 save the VFP state using
the same logic as in AArch64. Since TF-A saves and restores CPACR_EL1 we
cannot tell if normal world currently is using VFP or not so we have to
assume that it is and always save the VFP data registers if they are
about to be changed.

Reviewed-by: Jerome Forissier <jerome.forissier@linaro.org>
Tested-by: Jens Wiklander <jens.wiklander@linaro.org> (Hikey AArch32)
Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>

4 files changed, 22 insertions, 16 deletions

diff --git a/core/arch/arm/include/kernel/vfp.h b/core/arch/arm/include/kernel/vfp.h
index dc65c372..ef9952fe 100644
--- a/core/arch/arm/include/kernel/vfp.h
+++ b/core/arch/arm/include/kernel/vfp.h
@@ -50,7 +50,6 @@ struct vfp_state {
 	uint32_t fpsr;
 	uint32_t fpcr;
 	uint32_t cpacr_el1;
-	bool force_save; /* Save to reg even if VFP was not enabled */
 };
 #endif
 
@@ -87,11 +86,13 @@ void vfp_lazy_save_state_init(struct vfp_state *state);
 /*
  * vfp_lazy_save_state_final() - Saves rest of VFP state
  * @state:	VFP state to save to
+ * @force_save:	Forces saving of state regardless of previous state if true.
  *
- * If VFP was enabled when vfp_lazy_save_state_init() was called: save rest
- * of state and disable VFP. Otherwise, do nothing.
+ * If VFP was enabled when vfp_lazy_save_state_init() was called or
+ * @force_save is true: save rest of state and disable VFP. Otherwise, do
+ * nothing.
  */
-void vfp_lazy_save_state_final(struct vfp_state *state);
+void vfp_lazy_save_state_final(struct vfp_state *state, bool force_save);
 
 /*
  * vfp_lazy_restore_state() - Lazy restore VFP state
diff --git a/core/arch/arm/kernel/thread.c b/core/arch/arm/kernel/thread.c
index 963916e7..634581eb 100644
--- a/core/arch/arm/kernel/thread.c
+++ b/core/arch/arm/kernel/thread.c
@@ -294,13 +294,13 @@ static void thread_lazy_save_ns_vfp(void)
 	struct thread_ctx *thr = threads + thread_get_id();
 
 	thr->vfp_state.ns_saved = false;
-#if defined(ARM64) && defined(CFG_WITH_ARM_TRUSTED_FW)
+#if defined(CFG_WITH_ARM_TRUSTED_FW)
 	/*
 	 * ARM TF saves and restores CPACR_EL1, so we must assume NS world
 	 * uses VFP and always preserve the register file when secure world
 	 * is about to use it
 	 */
-	thr->vfp_state.ns.force_save = true;
+	thr->vfp_state.ns_force_save = true;
 #endif
 	vfp_lazy_save_state_init(&thr->vfp_state.ns);
 #endif /*CFG_WITH_VFP*/
@@ -315,7 +315,7 @@ static void thread_lazy_restore_ns_vfp(void)
 	assert(!thr->vfp_state.sec_lazy_saved && !thr->vfp_state.sec_saved);
 
 	if (tuv && tuv->lazy_saved && !tuv->saved) {
-		vfp_lazy_save_state_final(&tuv->vfp);
+		vfp_lazy_save_state_final(&tuv->vfp, false /*!force_save*/);
 		tuv->saved = true;
 	}
 
@@ -1076,7 +1076,8 @@ uint32_t thread_kernel_enable_vfp(void)
 	assert(!vfp_is_enabled());
 
 	if (!thr->vfp_state.ns_saved) {
-		vfp_lazy_save_state_final(&thr->vfp_state.ns);
+		vfp_lazy_save_state_final(&thr->vfp_state.ns,
+					  thr->vfp_state.ns_force_save);
 		thr->vfp_state.ns_saved = true;
 	} else if (thr->vfp_state.sec_lazy_saved &&
 		   !thr->vfp_state.sec_saved) {
@@ -1084,14 +1085,15 @@ uint32_t thread_kernel_enable_vfp(void)
 		 * This happens when we're handling an abort while the
 		 * thread was using the VFP state.
 		 */
-		vfp_lazy_save_state_final(&thr->vfp_state.sec);
+		vfp_lazy_save_state_final(&thr->vfp_state.sec,
+					  false /*!force_save*/);
 		thr->vfp_state.sec_saved = true;
 	} else if (tuv && tuv->lazy_saved && !tuv->saved) {
 		/*
 		 * This can happen either during syscall or abort
 		 * processing (while processing a syscall).
 		 */
-		vfp_lazy_save_state_final(&tuv->vfp);
+		vfp_lazy_save_state_final(&tuv->vfp, false /*!force_save*/);
 		tuv->saved = true;
 	}
 
@@ -1147,11 +1149,13 @@ void thread_user_enable_vfp(struct thread_user_vfp_state *uvfp)
 	assert(!vfp_is_enabled());
 
 	if (!thr->vfp_state.ns_saved) {
-		vfp_lazy_save_state_final(&thr->vfp_state.ns);
+		vfp_lazy_save_state_final(&thr->vfp_state.ns,
+					  thr->vfp_state.ns_force_save);
 		thr->vfp_state.ns_saved = true;
 	} else if (tuv && uvfp != tuv) {
 		if (tuv->lazy_saved && !tuv->saved) {
-			vfp_lazy_save_state_final(&tuv->vfp);
+			vfp_lazy_save_state_final(&tuv->vfp,
+						  false /*!force_save*/);
 			tuv->saved = true;
 		}
 	}
diff --git a/core/arch/arm/kernel/thread_private.h b/core/arch/arm/kernel/thread_private.h
index aeab2451..fe0a1055 100644
--- a/core/arch/arm/kernel/thread_private.h
+++ b/core/arch/arm/kernel/thread_private.h
@@ -66,6 +66,7 @@ struct thread_user_mode_rec {
 #ifdef CFG_WITH_VFP
 struct thread_vfp_state {
 	bool ns_saved;
+	bool ns_force_save; /* Save to reg even if VFP was not enabled */
 	bool sec_saved;
 	bool sec_lazy_saved;
 	struct vfp_state ns;
diff --git a/core/arch/arm/kernel/vfp.c b/core/arch/arm/kernel/vfp.c
index 0d552cb1..8e945bfa 100644
--- a/core/arch/arm/kernel/vfp.c
+++ b/core/arch/arm/kernel/vfp.c
@@ -32,9 +32,9 @@ void vfp_lazy_save_state_init(struct vfp_state *state)
 	vfp_write_fpexc(fpexc & ~FPEXC_EN);
 }
 
-void vfp_lazy_save_state_final(struct vfp_state *state)
+void vfp_lazy_save_state_final(struct vfp_state *state, bool force_save)
 {
-	if (state->fpexc & FPEXC_EN) {
+	if ((state->fpexc & FPEXC_EN) || force_save) {
 		uint32_t fpexc = vfp_read_fpexc();
 
 		assert(!(fpexc & FPEXC_EN));
@@ -94,10 +94,10 @@ void vfp_lazy_save_state_init(struct vfp_state *state)
 	vfp_disable();
 }
 
-void vfp_lazy_save_state_final(struct vfp_state *state)
+void vfp_lazy_save_state_final(struct vfp_state *state, bool force_save)
 {
 	if ((CPACR_EL1_FPEN(state->cpacr_el1) & CPACR_EL1_FPEN_EL0EL1) ||
-	    state->force_save) {
+	    force_save) {
 		assert(!vfp_is_enabled());
 		vfp_enable();
 		state->fpcr = read_fpcr();