[2/2] csky: add CONFIG_CPU_HAS_MATHEMU and use in traps
diff mbox series

Message ID 20210421024916.13603-1-wangjunqiang@iscas.ac.cn
State New
Headers show
Series
  • [1/2] csky: add CSKY 810/860 FPU instruction simulation
Related show

Commit Message

Wang Junqiang April 21, 2021, 2:49 a.m. UTC
This patch contains the use of fpu instruction emulation by trap_c.
It defined _fcr, _fesr for user space in ptrace.h which is used in fpu.c
and instruction emulation. During initialization, all fpe are enabled,
and the configuration that read/write fcr and fesr in glibc library is captured
when privilege exception is triggerd by mfcr and mtcr < ,15>.

steps:
 1.enable all fpe interrupts
 2.mfcr/mtcr trigger privilege exception
 3.synchronize fcr and fesr of user app through fpu_libc_helper function
 4.fpe interrupt at runtime
 5.fpu instruction simulation is based on user_fcr and user_fesr
 6.clearing interrupts and synchronizing fcr fesr
 7.state restore and continue running

Signed-off-by: Wang Junqiang <wangjunqiang@iscas.ac.cn>
Signed-off-by: Li Weiwei <liweiwei@iscas.ac.cn>
---
 arch/csky/Kconfig                   |  8 ++++++++
 arch/csky/Makefile                  |  1 +
 arch/csky/abiv2/fpu.c               | 31 ++++++++++++++++++++++++-----
 arch/csky/abiv2/inc/abi/fpu.h       |  9 ++++++++-
 arch/csky/include/uapi/asm/ptrace.h |  6 ++++++
 arch/csky/kernel/traps.c            | 30 +++++++++++++++++++++++++---
 6 files changed, 76 insertions(+), 9 deletions(-)

Comments

Randy Dunlap April 21, 2021, 3:01 a.m. UTC | #1
Hi--

On 4/20/21 7:49 PM, Wang Junqiang wrote:
> diff --git a/arch/csky/Kconfig b/arch/csky/Kconfig
> index 34e91224adc3..a414225570c1 100644
> --- a/arch/csky/Kconfig
> +++ b/arch/csky/Kconfig
> @@ -97,6 +97,9 @@ config CPU_HAS_CACHEV2
>  config CPU_HAS_FPUV2
>  	bool
>  
> +config CPU_HAS_MATHEMU
> +	bool
> +

Why do you have this new kconfig symbol two times in the same file?

>  config CPU_HAS_HILO
>  	bool
>  
> @@ -281,6 +284,11 @@ config CPU_HAS_VDSP
>  	bool "CPU has VDSP coprocessor"
>  	depends on CPU_HAS_FPU && CPU_HAS_FPUV2
>  
> +config CPU_HAS_MATHEMU
> +	bool "CPU has FPU Mathemu Instructions"
> +	depends on CPU_CK810 || CPU_CK860
> +	default n
> +
>  config CPU_HAS_FPU
>  	bool "CPU has FPU coprocessor"
>  	depends on CPU_CK807 || CPU_CK810 || CPU_CK860


thanks.
Wang Junqiang April 21, 2021, 3:08 a.m. UTC | #2
thanks, I will fix it

"Randy Dunlap" &lt;rdunlap@infradead.org&gt;写道:
&gt; Hi--
&gt; 
&gt; On 4/20/21 7:49 PM, Wang Junqiang wrote:
&gt; &gt; diff --git a/arch/csky/Kconfig b/arch/csky/Kconfig
&gt; &gt; index 34e91224adc3..a414225570c1 100644
&gt; &gt; --- a/arch/csky/Kconfig
&gt; &gt; +++ b/arch/csky/Kconfig
&gt; &gt; @@ -97,6 +97,9 @@ config CPU_HAS_CACHEV2
&gt; &gt;  config CPU_HAS_FPUV2
&gt; &gt;  	bool
&gt; &gt;  
&gt; &gt; +config CPU_HAS_MATHEMU
&gt; &gt; +	bool
&gt; &gt; +
&gt; 
&gt; Why do you have this new kconfig symbol two times in the same file?
&gt; 
&gt; &gt;  config CPU_HAS_HILO
&gt; &gt;  	bool
&gt; &gt;  
&gt; &gt; @@ -281,6 +284,11 @@ config CPU_HAS_VDSP
&gt; &gt;  	bool "CPU has VDSP coprocessor"
&gt; &gt;  	depends on CPU_HAS_FPU &amp;&amp; CPU_HAS_FPUV2
&gt; &gt;  
&gt; &gt; +config CPU_HAS_MATHEMU
&gt; &gt; +	bool "CPU has FPU Mathemu Instructions"
&gt; &gt; +	depends on CPU_CK810 || CPU_CK860
&gt; &gt; +	default n
&gt; &gt; +
&gt; &gt;  config CPU_HAS_FPU
&gt; &gt;  	bool "CPU has FPU coprocessor"
&gt; &gt;  	depends on CPU_CK807 || CPU_CK810 || CPU_CK860
&gt; 
&gt; 
&gt; thanks.
&gt; -- 
&gt; ~Randy
kernel test robot April 21, 2021, 10:46 p.m. UTC | #3
Hi Wang,

Thank you for the patch! Yet something to improve:

[auto build test ERROR on linus/master]
[also build test ERROR on v5.12-rc8 next-20210421]
[If your patch is applied to the wrong git tree, kindly drop us a note.
And when submitting patch, we suggest to use '--base' as documented in
https://git-scm.com/docs/git-format-patch]

url:    https://github.com/0day-ci/linux/commits/Wang-Junqiang/csky-add-CSKY-810-860-FPU-instruction-simulation/20210422-023433
base:   https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git 1fe5501ba1abf2b7e78295df73675423bd6899a0
config: csky-randconfig-r034-20210421 (attached as .config)
compiler: csky-linux-gcc (GCC) 9.3.0
reproduce (this is a W=1 build):
        wget https://raw.githubusercontent.com/intel/lkp-tests/master/sbin/make.cross -O ~/bin/make.cross
        chmod +x ~/bin/make.cross
        # https://github.com/0day-ci/linux/commit/104fd6608bfa849efefd7e77e08e9a1bdf031026
        git remote add linux-review https://github.com/0day-ci/linux
        git fetch --no-tags linux-review Wang-Junqiang/csky-add-CSKY-810-860-FPU-instruction-simulation/20210422-023433
        git checkout 104fd6608bfa849efefd7e77e08e9a1bdf031026
        # save the attached .config to linux build tree
        COMPILER_INSTALL_PATH=$HOME/0day COMPILER=gcc-9.3.0 make.cross W=1 ARCH=csky 

If you fix the issue, kindly add following tag as appropriate
Reported-by: kernel test robot <lkp@intel.com>

All errors (new ones prefixed by >>):

>> error: arch/csky/include/uapi/asm/ptrace.h: leak CONFIG_CPU_HAS_MATHEMU to user-space
   make[2]: *** [scripts/Makefile.headersinst:63: usr/include/asm/ptrace.h] Error 1
   make[2]: Target '__headers' not remade because of errors.
   make[1]: *** [Makefile:1338: headers] Error 2
   make[1]: Target 'headers_install' not remade because of errors.
   make: *** [Makefile:215: __sub-make] Error 2
   make: Target 'headers_install' not remade because of errors.

Kconfig warnings: (for reference only)
   WARNING: unmet direct dependencies detected for LOCKDEP
   Depends on DEBUG_KERNEL && LOCK_DEBUGGING_SUPPORT && (FRAME_POINTER || MIPS || PPC || S390 || MICROBLAZE || ARM || ARC || X86)
   Selected by
   - PROVE_LOCKING && DEBUG_KERNEL && LOCK_DEBUGGING_SUPPORT
   - LOCK_STAT && DEBUG_KERNEL && LOCK_DEBUGGING_SUPPORT
   - DEBUG_LOCK_ALLOC && DEBUG_KERNEL && LOCK_DEBUGGING_SUPPORT

---
0-DAY CI Kernel Test Service, Intel Corporation
https://lists.01.org/hyperkitty/list/kbuild-all@lists.01.org
Guo Ren April 27, 2021, 3:02 p.m. UTC | #4
Hi Wang & Li,

On Wed, Apr 21, 2021 at 10:56 AM Wang Junqiang <wangjunqiang@iscas.ac.cn> wrote:
>
> This patch contains the use of fpu instruction emulation by trap_c.
> It defined _fcr, _fesr for user space in ptrace.h which is used in fpu.c
> and instruction emulation. During initialization, all fpe are enabled,
> and the configuration that read/write fcr and fesr in glibc library is captured
> when privilege exception is triggerd by mfcr and mtcr < ,15>.
>
> steps:
>  1.enable all fpe interrupts
>  2.mfcr/mtcr trigger privilege exception
>  3.synchronize fcr and fesr of user app through fpu_libc_helper function
>  4.fpe interrupt at runtime
>  5.fpu instruction simulation is based on user_fcr and user_fesr
>  6.clearing interrupts and synchronizing fcr fesr
>  7.state restore and continue running
>
> Signed-off-by: Wang Junqiang <wangjunqiang@iscas.ac.cn>
> Signed-off-by: Li Weiwei <liweiwei@iscas.ac.cn>
> ---
>  arch/csky/Kconfig                   |  8 ++++++++
>  arch/csky/Makefile                  |  1 +
>  arch/csky/abiv2/fpu.c               | 31 ++++++++++++++++++++++++-----
>  arch/csky/abiv2/inc/abi/fpu.h       |  9 ++++++++-
>  arch/csky/include/uapi/asm/ptrace.h |  6 ++++++
>  arch/csky/kernel/traps.c            | 30 +++++++++++++++++++++++++---
>  6 files changed, 76 insertions(+), 9 deletions(-)
>
> diff --git a/arch/csky/Kconfig b/arch/csky/Kconfig
> index 34e91224adc3..a414225570c1 100644
> --- a/arch/csky/Kconfig
> +++ b/arch/csky/Kconfig
> @@ -97,6 +97,9 @@ config CPU_HAS_CACHEV2
>  config CPU_HAS_FPUV2
>         bool
>
> +config CPU_HAS_MATHEMU
> +       bool
> +
Duplicated

>  config CPU_HAS_HILO
>         bool
>
> @@ -281,6 +284,11 @@ config CPU_HAS_VDSP
>         bool "CPU has VDSP coprocessor"
>         depends on CPU_HAS_FPU && CPU_HAS_FPUV2
>
> +config CPU_HAS_MATHEMU
> +       bool "CPU has FPU Mathemu Instructions"
> +       depends on CPU_CK810 || CPU_CK860
> +       default n
No need "default n", only "default y" need.

> +
>  config CPU_HAS_FPU
>         bool "CPU has FPU coprocessor"
>         depends on CPU_CK807 || CPU_CK810 || CPU_CK860
> diff --git a/arch/csky/Makefile b/arch/csky/Makefile
> index 37f593a4bf53..495f3a2fad6a 100644
> --- a/arch/csky/Makefile
> +++ b/arch/csky/Makefile
> @@ -64,6 +64,7 @@ head-y := arch/csky/kernel/head.o
>  core-y += arch/csky/kernel/
>  core-y += arch/csky/mm/
>  core-y += arch/csky/$(CSKYABI)/
> +core-$(CONFIG_CPU_HAS_MATHEMU) += arch/csky/math-emu/
>
>  libs-y += arch/csky/lib/ \
>         $(shell $(CC) $(KBUILD_CFLAGS) $(KCFLAGS) -print-libgcc-file-name)
> diff --git a/arch/csky/abiv2/fpu.c b/arch/csky/abiv2/fpu.c
> index 5acc5c2e544e..3cd17cf9319e 100644
> --- a/arch/csky/abiv2/fpu.c
> +++ b/arch/csky/abiv2/fpu.c
> @@ -38,8 +38,11 @@ int fpu_libc_helper(struct pt_regs *regs)
>                 return 0;
>
>         tinstr = instr_hi | ((unsigned long)instr_low << 16);
> -
> +#ifdef CONFIG_CPU_HAS_MATHEMU
> +       if (((tinstr >> 21) & 0x1F) != 15)
> +#else
>         if (((tinstr >> 21) & 0x1F) != 2)
> +#endif
>                 return 0;
>
>         if ((tinstr & MTCR_MASK) == MTCR_DIST) {
> @@ -53,10 +56,19 @@ int fpu_libc_helper(struct pt_regs *regs)
>
>                 regx =  *(&regs->a0 + index);
>
> -               if (tmp == 1)
> +               if (tmp == 1) {
> +#ifdef CONFIG_CPU_HAS_MATHEMU
> +                       mtcr("cr<1, 2>", regx | 0x3f);
> +                       current->thread.user_fp.user_fcr = regx;
> +#else
>                         mtcr("cr<1, 2>", regx);
> -               else if (tmp == 2)
> +#endif
> +               } else if (tmp == 2) {
>                         mtcr("cr<2, 2>", regx);
> +#ifdef CONFIG_CPU_HAS_MATHEMU
> +                       current->thread.user_fp.user_fesr = regx;
> +#endif
> +               }
>                 else
>                         return 0;
>
> @@ -73,10 +85,19 @@ int fpu_libc_helper(struct pt_regs *regs)
>                 if (tmp > 2)
>                         return 0;
>
> -               if (tmp == 1)
> +               if (tmp == 1) {
> +#ifndef CONFIG_CPU_HAS_MATHEMU
>                         regx = mfcr("cr<1, 2>");
> -               else if (tmp == 2)
> +#else
> +                       regx = current->thread.user_fp.user_fcr;
> +#endif
> +               } else if (tmp == 2) {
> +#ifndef CONFIG_CPU_HAS_MATHEMU
>                         regx = mfcr("cr<2, 2>");
> +#else
> +                       regx = current->thread.user_fp.user_fesr;
> +#endif
> +               }
>                 else
>                         return 0;
>
> diff --git a/arch/csky/abiv2/inc/abi/fpu.h b/arch/csky/abiv2/inc/abi/fpu.h
> index aabb79355013..c3509ace542c 100644
> --- a/arch/csky/abiv2/inc/abi/fpu.h
> +++ b/arch/csky/abiv2/inc/abi/fpu.h
> @@ -9,7 +9,14 @@
>  int fpu_libc_helper(struct pt_regs *regs);
>  void fpu_fpe(struct pt_regs *regs);
>
> -static inline void init_fpu(void) { mtcr("cr<1, 2>", 0); }
> +static inline void init_fpu(void)
> +{
> +#ifdef CONFIG_CPU_HAS_MATHEMU
> +       mtcr("cr<1, 2>", 0x3f);
> +#else
> +       mtcr("cr<1, 2>", 0);
> +#endif
> +}
Could we put all CONFIG_CPU_HAS_MATHEMU differences in fpu.h?

#ifdef CONFIG_CPU_HAS_MATHEMU
#define MFCR_FESR       current->thread.user_fp.user_fesr
#define MFCR_FCR        current->thread.user_fp.user_fcr
#define MTCR_FESR(regx) {current->thread.user_fp.user_fesr = regx; ...}
#define MTCR_FCR(regx)  {current->thread.user_fp.user_fcr = regx; ...}
#define CR_NUM          15
#else
#define MFCR_FESR(regx) mtcr("cr<1, 2>", regx)
#define MFCR_FESR(regx) mtcr("cr<1, 2>", regx)
...
#define CR_NUM    2
#endif

>
>  void save_to_user_fp(struct user_fp *user_fp);
>  void restore_from_user_fp(struct user_fp *user_fp);
> diff --git a/arch/csky/include/uapi/asm/ptrace.h b/arch/csky/include/uapi/asm/ptrace.h
> index 3be9c14334a6..1ffb6190686a 100644
> --- a/arch/csky/include/uapi/asm/ptrace.h
> +++ b/arch/csky/include/uapi/asm/ptrace.h
> @@ -45,6 +45,12 @@ struct user_fp {
>         unsigned long   fesr;
>         unsigned long   fid;
>         unsigned long   reserved;
> +#ifdef CONFIG_CPU_HAS_MATHEMU
> +       unsigned long   user_fcr;
> +       unsigned long   user_fesr;
> +       unsigned long   reserved1;
> +       unsigned long   reserved2;
> +#endif
>  };
No, we couldn't modify user_fp, it'll break the old version toolchain.
(signal context)

A new struct of saving them in thread struct is enough.

>
>  #endif /* __ASSEMBLY__ */
> diff --git a/arch/csky/kernel/traps.c b/arch/csky/kernel/traps.c
> index e5fbf8653a21..053a4f7dff9e 100644
> --- a/arch/csky/kernel/traps.c
> +++ b/arch/csky/kernel/traps.c
> @@ -27,6 +27,10 @@
>
>  #ifdef CONFIG_CPU_HAS_FPU
>  #include <abi/fpu.h>
> +#ifdef CONFIG_CPU_HAS_MATHEMU
> +extern inline unsigned int get_fpu_insn(struct pt_regs *regs);
> +extern inline int do_fpu_insn(unsigned int inst, struct pt_regs *regs);
> +#endif
>  #endif
>
>  int show_unhandled_signals = 1;
> @@ -186,8 +190,19 @@ asmlinkage void do_trap_bkpt(struct pt_regs *regs)
>
>  asmlinkage void do_trap_illinsn(struct pt_regs *regs)
>  {
> -       current->thread.trap_no = trap_no(regs);
> +#ifdef CONFIG_CPU_HAS_FPU
> +#ifdef CONFIG_CPU_HAS_MATHEMU
CONFIG_CPU_HAS_FPU && CONFIG_CPU_HAS_MATHEMU ?

> +       unsigned int inst;
> +
> +       inst = get_fpu_insn(regs);
> +       if (inst && !do_fpu_insn(inst, regs)) {
> +               regs->pc += 4;
> +               return;
> +       }
> +#endif
> +#endif
Don't double.

>
> +       current->thread.trap_no = trap_no(regs);
>  #ifdef CONFIG_KPROBES
>         if (kprobe_breakpoint_handler(regs))
>                 return;
> @@ -209,7 +224,16 @@ asmlinkage void do_trap_illinsn(struct pt_regs *regs)
>
>  asmlinkage void do_trap_fpe(struct pt_regs *regs)
>  {
> -#ifdef CONFIG_CPU_HAS_FP
Thx for fixup.

> +#ifdef CONFIG_CPU_HAS_FPU
> +#ifdef CONFIG_CPU_HAS_MATHEMU
> +       unsigned int inst;
> +
> +       inst = get_fpu_insn(regs);
> +       if (inst && !do_fpu_insn(inst, regs)) {
> +               regs->pc += 4;
> +               return;
> +       }
> +#endif
Move above into fpu_fpe.

>         return fpu_fpe(regs);
>  #else
>         do_trap_error(regs, SIGILL, ILL_ILLOPC, regs->pc,
> @@ -219,7 +243,7 @@ asmlinkage void do_trap_fpe(struct pt_regs *regs)
>
>  asmlinkage void do_trap_priv(struct pt_regs *regs)
>  {
> -#ifdef CONFIG_CPU_HAS_FP
Thx for fixup.

> +#ifdef CONFIG_CPU_HAS_FPU
>         if (user_mode(regs) && fpu_libc_helper(regs))
>                 return;
>  #endif
> --
> 2.17.1
>

Patch
diff mbox series

diff --git a/arch/csky/Kconfig b/arch/csky/Kconfig
index 34e91224adc3..a414225570c1 100644
--- a/arch/csky/Kconfig
+++ b/arch/csky/Kconfig
@@ -97,6 +97,9 @@  config CPU_HAS_CACHEV2
 config CPU_HAS_FPUV2
 	bool
 
+config CPU_HAS_MATHEMU
+	bool
+
 config CPU_HAS_HILO
 	bool
 
@@ -281,6 +284,11 @@  config CPU_HAS_VDSP
 	bool "CPU has VDSP coprocessor"
 	depends on CPU_HAS_FPU && CPU_HAS_FPUV2
 
+config CPU_HAS_MATHEMU
+	bool "CPU has FPU Mathemu Instructions"
+	depends on CPU_CK810 || CPU_CK860
+	default n
+
 config CPU_HAS_FPU
 	bool "CPU has FPU coprocessor"
 	depends on CPU_CK807 || CPU_CK810 || CPU_CK860
diff --git a/arch/csky/Makefile b/arch/csky/Makefile
index 37f593a4bf53..495f3a2fad6a 100644
--- a/arch/csky/Makefile
+++ b/arch/csky/Makefile
@@ -64,6 +64,7 @@  head-y := arch/csky/kernel/head.o
 core-y += arch/csky/kernel/
 core-y += arch/csky/mm/
 core-y += arch/csky/$(CSKYABI)/
+core-$(CONFIG_CPU_HAS_MATHEMU) += arch/csky/math-emu/
 
 libs-y += arch/csky/lib/ \
 	$(shell $(CC) $(KBUILD_CFLAGS) $(KCFLAGS) -print-libgcc-file-name)
diff --git a/arch/csky/abiv2/fpu.c b/arch/csky/abiv2/fpu.c
index 5acc5c2e544e..3cd17cf9319e 100644
--- a/arch/csky/abiv2/fpu.c
+++ b/arch/csky/abiv2/fpu.c
@@ -38,8 +38,11 @@  int fpu_libc_helper(struct pt_regs *regs)
 		return 0;
 
 	tinstr = instr_hi | ((unsigned long)instr_low << 16);
-
+#ifdef CONFIG_CPU_HAS_MATHEMU
+	if (((tinstr >> 21) & 0x1F) != 15)
+#else
 	if (((tinstr >> 21) & 0x1F) != 2)
+#endif
 		return 0;
 
 	if ((tinstr & MTCR_MASK) == MTCR_DIST) {
@@ -53,10 +56,19 @@  int fpu_libc_helper(struct pt_regs *regs)
 
 		regx =  *(&regs->a0 + index);
 
-		if (tmp == 1)
+		if (tmp == 1) {
+#ifdef CONFIG_CPU_HAS_MATHEMU
+			mtcr("cr<1, 2>", regx | 0x3f);
+			current->thread.user_fp.user_fcr = regx;
+#else
 			mtcr("cr<1, 2>", regx);
-		else if (tmp == 2)
+#endif
+		} else if (tmp == 2) {
 			mtcr("cr<2, 2>", regx);
+#ifdef CONFIG_CPU_HAS_MATHEMU
+			current->thread.user_fp.user_fesr = regx;
+#endif
+		}
 		else
 			return 0;
 
@@ -73,10 +85,19 @@  int fpu_libc_helper(struct pt_regs *regs)
 		if (tmp > 2)
 			return 0;
 
-		if (tmp == 1)
+		if (tmp == 1) {
+#ifndef CONFIG_CPU_HAS_MATHEMU
 			regx = mfcr("cr<1, 2>");
-		else if (tmp == 2)
+#else
+			regx = current->thread.user_fp.user_fcr;
+#endif
+		} else if (tmp == 2) {
+#ifndef CONFIG_CPU_HAS_MATHEMU
 			regx = mfcr("cr<2, 2>");
+#else
+			regx = current->thread.user_fp.user_fesr;
+#endif
+		}
 		else
 			return 0;
 
diff --git a/arch/csky/abiv2/inc/abi/fpu.h b/arch/csky/abiv2/inc/abi/fpu.h
index aabb79355013..c3509ace542c 100644
--- a/arch/csky/abiv2/inc/abi/fpu.h
+++ b/arch/csky/abiv2/inc/abi/fpu.h
@@ -9,7 +9,14 @@ 
 int fpu_libc_helper(struct pt_regs *regs);
 void fpu_fpe(struct pt_regs *regs);
 
-static inline void init_fpu(void) { mtcr("cr<1, 2>", 0); }
+static inline void init_fpu(void)
+{
+#ifdef CONFIG_CPU_HAS_MATHEMU
+	mtcr("cr<1, 2>", 0x3f);
+#else
+	mtcr("cr<1, 2>", 0);
+#endif
+}
 
 void save_to_user_fp(struct user_fp *user_fp);
 void restore_from_user_fp(struct user_fp *user_fp);
diff --git a/arch/csky/include/uapi/asm/ptrace.h b/arch/csky/include/uapi/asm/ptrace.h
index 3be9c14334a6..1ffb6190686a 100644
--- a/arch/csky/include/uapi/asm/ptrace.h
+++ b/arch/csky/include/uapi/asm/ptrace.h
@@ -45,6 +45,12 @@  struct user_fp {
 	unsigned long	fesr;
 	unsigned long	fid;
 	unsigned long	reserved;
+#ifdef CONFIG_CPU_HAS_MATHEMU
+	unsigned long	user_fcr;
+	unsigned long	user_fesr;
+	unsigned long	reserved1;
+	unsigned long	reserved2;
+#endif
 };
 
 #endif /* __ASSEMBLY__ */
diff --git a/arch/csky/kernel/traps.c b/arch/csky/kernel/traps.c
index e5fbf8653a21..053a4f7dff9e 100644
--- a/arch/csky/kernel/traps.c
+++ b/arch/csky/kernel/traps.c
@@ -27,6 +27,10 @@ 
 
 #ifdef CONFIG_CPU_HAS_FPU
 #include <abi/fpu.h>
+#ifdef CONFIG_CPU_HAS_MATHEMU
+extern inline unsigned int get_fpu_insn(struct pt_regs *regs);
+extern inline int do_fpu_insn(unsigned int inst, struct pt_regs *regs);
+#endif
 #endif
 
 int show_unhandled_signals = 1;
@@ -186,8 +190,19 @@  asmlinkage void do_trap_bkpt(struct pt_regs *regs)
 
 asmlinkage void do_trap_illinsn(struct pt_regs *regs)
 {
-	current->thread.trap_no = trap_no(regs);
+#ifdef CONFIG_CPU_HAS_FPU
+#ifdef CONFIG_CPU_HAS_MATHEMU
+	unsigned int inst;
+
+	inst = get_fpu_insn(regs);
+	if (inst && !do_fpu_insn(inst, regs)) {
+		regs->pc += 4;
+		return;
+	}
+#endif
+#endif
 
+	current->thread.trap_no = trap_no(regs);
 #ifdef CONFIG_KPROBES
 	if (kprobe_breakpoint_handler(regs))
 		return;
@@ -209,7 +224,16 @@  asmlinkage void do_trap_illinsn(struct pt_regs *regs)
 
 asmlinkage void do_trap_fpe(struct pt_regs *regs)
 {
-#ifdef CONFIG_CPU_HAS_FP
+#ifdef CONFIG_CPU_HAS_FPU
+#ifdef CONFIG_CPU_HAS_MATHEMU
+	unsigned int inst;
+
+	inst = get_fpu_insn(regs);
+	if (inst && !do_fpu_insn(inst, regs)) {
+		regs->pc += 4;
+		return;
+	}
+#endif
 	return fpu_fpe(regs);
 #else
 	do_trap_error(regs, SIGILL, ILL_ILLOPC, regs->pc,
@@ -219,7 +243,7 @@  asmlinkage void do_trap_fpe(struct pt_regs *regs)
 
 asmlinkage void do_trap_priv(struct pt_regs *regs)
 {
-#ifdef CONFIG_CPU_HAS_FP
+#ifdef CONFIG_CPU_HAS_FPU
 	if (user_mode(regs) && fpu_libc_helper(regs))
 		return;
 #endif