@@ -252,31 +252,10 @@ __und_svc:
#else
svc_entry
#endif
- @
- @ call emulation code, which returns using r9 if it has emulated
- @ the instruction, or the more conventional lr if we are to treat
- @ this as a real undefined instruction
- @
- @ r0 - instruction
- @
-#ifndef CONFIG_THUMB2_KERNEL
- ldr r0, [r4, #-4]
-#else
- mov r1, #2
- ldrh r0, [r4, #-2] @ Thumb instruction at LR - 2
- cmp r0, #0xe800 @ 32-bit instruction if xx >= 0
- blo __und_svc_fault
- ldrh r9, [r4] @ bottom 16 bits
- add r4, r4, #2
- str r4, [sp, #S_PC]
- orr r0, r9, r0, lsl #16
-#endif
- badr r9, __und_svc_finish
- mov r2, r4
- bl call_fpe
mov r1, #4 @ PC correction to apply
-__und_svc_fault:
+ THUMB( tst r5, #PSR_T_BIT ) @ exception taken in Thumb mode?
+ THUMB( movne r1, #2 ) @ if so, fix up PC correction
mov r0, sp @ struct pt_regs *regs
bl __und_fault
@@ -79,11 +79,6 @@ ENTRY(vfp_support_entry)
DBGSTR3 "instr %08x pc %08x state %p", r0, r2, r10
.fpu vfpv2
- ldr r3, [sp, #S_PSR] @ Neither lazy restore nor FP exceptions
- and r3, r3, #MODE_MASK @ are supported in kernel mode
- teq r3, #USR_MODE
- bne vfp_kmode_exception @ Returns through lr
-
VFPFMRX r1, FPEXC @ Is the VFP enabled?
DBGSTR1 "fpexc %08x", r1
tst r1, #FPEXC_EN
@@ -23,6 +23,7 @@
#include <asm/cputype.h>
#include <asm/system_info.h>
#include <asm/thread_notify.h>
+#include <asm/traps.h>
#include <asm/vfp.h>
#include "vfpinstr.h"
@@ -642,7 +643,9 @@ static int vfp_starting_cpu(unsigned int unused)
return 0;
}
-void vfp_kmode_exception(void)
+#ifdef CONFIG_KERNEL_MODE_NEON
+
+static int vfp_kmode_exception(struct pt_regs *regs, unsigned int instr)
{
/*
* If we reach this point, a floating point exception has been raised
@@ -660,9 +663,51 @@ void vfp_kmode_exception(void)
pr_crit("BUG: unsupported FP instruction in kernel mode\n");
else
pr_crit("BUG: FP instruction issued in kernel mode with FP unit disabled\n");
+ pr_crit("FPEXC == 0x%08x\n", fmrx(FPEXC));
+ return 1;
}
-#ifdef CONFIG_KERNEL_MODE_NEON
+static struct undef_hook vfp_kmode_exception_hook[] = {{
+ .instr_mask = 0xfe000000,
+ .instr_val = 0xf2000000,
+ .cpsr_mask = MODE_MASK | PSR_T_BIT,
+ .cpsr_val = SVC_MODE,
+ .fn = vfp_kmode_exception,
+}, {
+ .instr_mask = 0xff100000,
+ .instr_val = 0xf4000000,
+ .cpsr_mask = MODE_MASK | PSR_T_BIT,
+ .cpsr_val = SVC_MODE,
+ .fn = vfp_kmode_exception,
+}, {
+ .instr_mask = 0xef000000,
+ .instr_val = 0xef000000,
+ .cpsr_mask = MODE_MASK | PSR_T_BIT,
+ .cpsr_val = SVC_MODE | PSR_T_BIT,
+ .fn = vfp_kmode_exception,
+}, {
+ .instr_mask = 0xff100000,
+ .instr_val = 0xf9000000,
+ .cpsr_mask = MODE_MASK | PSR_T_BIT,
+ .cpsr_val = SVC_MODE | PSR_T_BIT,
+ .fn = vfp_kmode_exception,
+}, {
+ .instr_mask = 0x0c000e00,
+ .instr_val = 0x0c000a00,
+ .cpsr_mask = MODE_MASK,
+ .cpsr_val = SVC_MODE,
+ .fn = vfp_kmode_exception,
+}};
+
+static int __init vfp_kmode_exception_hook_init(void)
+{
+ int i;
+
+ for (i = 0; i < ARRAY_SIZE(vfp_kmode_exception_hook); i++)
+ register_undef_hook(&vfp_kmode_exception_hook[i]);
+ return 0;
+}
+core_initcall(vfp_kmode_exception_hook_init);
/*
* Kernel-side NEON support functions