arm64: fpsimd: Enable FP(floating-point) settings for msm8996

Enable FP and SIMD settings for the MSM8996 during the execution
of the AArch32 processes and disable these settings when you switch
to the AArch64 processes.

This commit also adds CONFIG_ENABLE_FP_SIMD_SETTINGS to support
FP and SIMD settings.

CRs-Fixed: 952837
Change-Id: If9537ca7390d8f08a6b48fb8865d1b349a93bcee
Signed-off-by: Se Wang (Patrick) Oh <sewango@codeaurora.org>
[ztu@codeaurora.org: Resolved conflicts]
Signed-off-by: Zhiqiang Tu <ztu@codeaurora.org>

arch/arm64/Kconfig

@@ -536,6 +536,16 @@ config ARM64_64K_PAGES
 
 endchoice
 
+config ENABLE_FP_SIMD_SETTINGS
+	bool "Enable FP(Floating Point) Settings for Qualcomm MSM8996"
+	depends on ARCH_MSM8996
+	help
+	  Enable FP(Floating Point) and SIMD settings for the MSM8996 during
+	  the execution of the aarch32 processes and disable these settings
+	  when you switch to the aarch64 processes.
+
+	  If you are not sure what to do, select 'N' here.
+
 choice
 	prompt "Virtual address space size"
 	default ARM64_VA_BITS_39 if ARM64_4K_PAGES

arch/arm64/include/asm/elf.h

@@ -23,6 +23,7 @@
  */
 #include <asm/ptrace.h>
 #include <asm/user.h>
+#include <asm/fpsimd.h>
 
 /*
  * AArch64 static relocation types.
@@ -182,7 +183,13 @@ typedef compat_elf_greg_t		compat_elf_gregset_t[COMPAT_ELF_NGREG];
 					 ((x)->e_flags & EF_ARM_EABI_MASK))
 
 #define compat_start_thread		compat_start_thread
-#define COMPAT_SET_PERSONALITY(ex)	set_thread_flag(TIF_32BIT);
+#define COMPAT_SET_PERSONALITY(ex)					\
+do {									\
+	if (current->mm)						\
+		fpsimd_enable_trap();					\
+	set_thread_flag(TIF_32BIT);					\
+} while (0)
+
 #define COMPAT_ARCH_DLINFO
 extern int aarch32_setup_vectors_page(struct linux_binprm *bprm,
 				      int uses_interp);

arch/arm64/include/asm/fpsimd.h

@@ -81,6 +81,18 @@ extern void fpsimd_save_partial_state(struct fpsimd_partial_state *state,
 				      u32 num_regs);
 extern void fpsimd_load_partial_state(struct fpsimd_partial_state *state);
 
+#ifdef CONFIG_ENABLE_FP_SIMD_SETTINGS
+extern void fpsimd_disable_trap(void);
+extern void fpsimd_enable_trap(void);
+extern void fpsimd_settings_disable(void);
+extern void fpsimd_settings_enable(void);
+#else
+static inline void fpsimd_disable_trap(void) {}
+static inline void fpsimd_enable_trap(void) {}
+static inline void fpsimd_settings_disable(void) {}
+static inline void fpsimd_settings_enable(void) {}
+#endif
+
 #endif
 
 #endif

arch/arm64/kernel/entry-fpsimd.S

@@ -64,4 +64,36 @@ ENTRY(fpsimd_load_partial_state)
 	ret
 ENDPROC(fpsimd_load_partial_state)
 
+#ifdef CONFIG_ENABLE_FP_SIMD_SETTINGS
+ENTRY(fpsimd_enable_trap)
+	mrs x0, cpacr_el1
+	bic x0, x0, #(3 << 20)
+	orr x0, x0, #(1 << 20)
+	msr cpacr_el1, x0
+	ret
+ENDPROC(fpsimd_enable_trap)
+ENTRY(fpsimd_disable_trap)
+	mrs x0, cpacr_el1
+	orr x0, x0, #(3 << 20)
+	msr cpacr_el1, x0
+	ret
+ENDPROC(fpsimd_disable_trap)
+ENTRY(fpsimd_settings_enable)
+	mrs x0, s3_1_c15_c15_0
+	orr x0, x0, #(1 << 31)
+	isb
+	msr s3_1_c15_c15_0, x0
+	isb
+	ret
+ENDPROC(fpsimd_settings_enable)
+ENTRY(fpsimd_settings_disable)
+	mrs x0, s3_1_c15_c15_0
+	bic x0, x0, #(1 << 31)
+	isb
+	msr s3_1_c15_c15_0, x0
+	isb
+	ret
+ENDPROC(fpsimd_settings_disable)
+#endif
+
 #endif

arch/arm64/kernel/entry.S

@@ -494,7 +494,7 @@ el0_sync_compat:
 	cmp	x24, #ESR_ELx_EC_IABT_LOW	// instruction abort in EL0
 	b.eq	el0_ia
 	cmp	x24, #ESR_ELx_EC_FP_ASIMD	// FP/ASIMD access
-	b.eq	el0_fpsimd_acc
+	b.eq	el0_fpsimd_acc_compat
 	cmp	x24, #ESR_ELx_EC_FP_EXC32	// FP/ASIMD exception
 	b.eq	el0_fpsimd_exc
 	cmp	x24, #ESR_ELx_EC_PC_ALIGN	// pc alignment exception
@@ -565,6 +565,17 @@ el0_fpsimd_acc:
 	mov	x1, sp
 	bl	do_fpsimd_acc
 	b	ret_to_user
+el0_fpsimd_acc_compat:
+	/*
+	 * Floating Point or Advanced SIMD access
+	 */
+	enable_dbg
+	ct_user_exit
+	mov	x0, x25
+	mov	x1, sp
+	bl	do_fpsimd_acc_compat
+	b	ret_to_user
+
 el0_fpsimd_exc:
 	/*
 	 * Floating Point or Advanced SIMD exception

arch/arm64/kernel/fpsimd.c

@@ -88,14 +88,23 @@
  *   whatever is in the FPSIMD registers is not saved to memory, but discarded.
  */
 static DEFINE_PER_CPU(struct fpsimd_state *, fpsimd_last_state);
+static DEFINE_PER_CPU(int, fpsimd_stg_enable);
 
 /*
  * Trapped FP/ASIMD access.
  */
 void do_fpsimd_acc(unsigned int esr, struct pt_regs *regs)
 {
-	/* TODO: implement lazy context saving/restoring */
-	WARN_ON(1);
+	fpsimd_disable_trap();
+	fpsimd_settings_disable();
+	this_cpu_write(fpsimd_stg_enable, 0);
+}
+
+void do_fpsimd_acc_compat(unsigned int esr, struct pt_regs *regs)
+{
+	fpsimd_disable_trap();
+	fpsimd_settings_enable();
+	this_cpu_write(fpsimd_stg_enable, 1);
 }
 
 /*
@@ -135,6 +144,11 @@ void fpsimd_thread_switch(struct task_struct *next)
 	if (current->mm && !test_thread_flag(TIF_FOREIGN_FPSTATE))
 		fpsimd_save_state(&current->thread.fpsimd_state);
 
+	if (__this_cpu_read(fpsimd_stg_enable)) {
+		fpsimd_settings_disable();
+		this_cpu_write(fpsimd_stg_enable, 0);
+	}
+
 	if (next->mm) {
 		/*
 		 * If we are switching to a task whose most recent userland
@@ -152,6 +166,11 @@ void fpsimd_thread_switch(struct task_struct *next)
 		else
 			set_ti_thread_flag(task_thread_info(next),
 					   TIF_FOREIGN_FPSTATE);
+
+		if (test_ti_thread_flag(task_thread_info(next), TIF_32BIT))
+			fpsimd_enable_trap();
+		else
+			fpsimd_disable_trap();
 	}
 }