[PATCH] sh2(A) exception handler update

[Yoshinori Sato <ysato@users.sourceforge.jp>]

This patch is

By sh2
- Remove duplicate code
- Reduce stack usage
- Cleanup and little optimize 

By sh2a
- Add missing handler(256 to 511)
- Use sh2a instructions handler

Signed-off-by: Yoshinori Sato <ysato@users.sourceforge.jp>

diff --git a/arch/sh/kernel/cpu/sh2/entry.S b/arch/sh/kernel/cpu/sh2/entry.S
index 0fc8906..ee894e5 100644
--- a/arch/sh/kernel/cpu/sh2/entry.S
+++ b/arch/sh/kernel/cpu/sh2/entry.S
@@ -3,7 +3,7 @@
  *
  * The SH-2 exception entry
  *
- * Copyright (C) 2005,2006 Yoshinori Sato
+ * Copyright (C) 2005-2008 Yoshinori Sato
  * Copyright (C) 2005  AXE,Inc.
  *
  * This file is subject to the terms and conditions of the GNU General Public
@@ -36,43 +36,41 @@ OFF_TRA	=  (16*4+6*4)
 #include <asm/entry-macros.S>
 
 ENTRY(exception_handler)
-	! already saved r0/r1
+	! stack
+	! r0 <- point sp
+	! r1
+	! pc
+	! sr
+	! r0 = temporary
+	! r1 = vector (pseudo EXPEVT / INTEVT / TRA)
 	mov.l	r2,@-sp
 	mov.l	r3,@-sp
-	mov	r0,r1
 	cli
 	mov.l	$cpu_mode,r2
 	mov.l	@r2,r0
 	mov.l	@(5*4,r15),r3	! previous SR
-	shll2	r3		! set "S" flag
-	rotl	r0		! T <- "S" flag
-	rotl	r0		! "S" flag is LSB
-	rotcr	r3		! T -> r3:b30
-	shlr	r3
-	shlr	r0
-	bt/s	1f
-	 mov.l	r3,@(5*4,r15)	! copy cpu mode to SR
+	or	r0,r3		! set MD
+	tst	r0,r0
+	bf/s	1f		! previous mode check
+	 mov.l	r3,@(5*4,r15)	! update SR
 	! switch to kernel mode
-	mov	#1,r0
-	rotr	r0
-	rotr	r0
+	mov.l	__md_bit,r0
 	mov.l	r0,@r2		! enter kernel mode
 	mov.l	$current_thread_info,r2
 	mov.l	@r2,r2
-	mov	#0x20,r0
+	mov	#(THREAD_SIZE >> 8),r0
 	shll8	r0
 	add	r2,r0
 	mov	r15,r2		! r2 = user stack top
 	mov	r0,r15		! switch kernel stack
-	add	#-4,r15		! dummy
 	mov.l	r1,@-r15	! TRA
 	sts.l	macl, @-r15
 	sts.l	mach, @-r15
 	stc.l	gbr, @-r15
-	mov.l	@(4*4,r2),r0
-	mov.l	@(5*4,r2),r1
-	mov.l	r1,@-r15	! original SR
+	mov.l	@(5*4,r2),r0
+	mov.l	r0,@-r15	! original SR
 	sts.l	pr,@-r15
+	mov.l	@(4*4,r2),r0
 	mov.l	r0,@-r15	! original PC
 	mov	r2,r3
 	add	#(4+2)*4,r3	! rewind r0 - r3 + exception frame
@@ -88,14 +86,15 @@ ENTRY(exception_handler)
 	mov.l	r6,@-r15
 	mov.l	r5,@-r15
 	mov.l	r4,@-r15
+	mov	r1,r9		! save TRA
 	mov	r2,r8		! copy user -> kernel stack
-	mov.l	@r8+,r3
+	mov.l	@(0,r8),r3
 	mov.l	r3,@-r15
-	mov.l	@r8+,r2
+	mov.l	@(4,r8),r2
 	mov.l	r2,@-r15
-	mov.l	@r8+,r1
+	mov.l	@(12,r8),r1
 	mov.l	r1,@-r15
-	mov.l	@r8+,r0
+	mov.l	@(8,r8),r0
 	bra	2f
 	 mov.l	r0,@-r15
 1:
@@ -107,10 +106,11 @@ ENTRY(exception_handler)
 	mov.l	r0,@-r15	
 	mov.l	@r2+,r0		! old R2
 	mov.l	r0,@-r15	
-	mov.l	@r2+,r0		! old R1
-	mov.l	r0,@-r15	
-	mov.l	@r2+,r0		! old R0
+	mov.l	@(4,r2),r0	! old R1
 	mov.l	r0,@-r15	
+	mov.l	@r2,r0		! old R0
+	mov.l	r0,@-r15
+	add	#8,r2
 	mov.l	@r2+,r3		! old PC
 	mov.l	@r2+,r0		! old SR
 	add	#-4,r2		! exception frame stub (sr)
@@ -135,14 +135,12 @@ ENTRY(exception_handler)
 	mov.l	r6,@-r2
 	mov.l	r5,@-r2
 	mov.l	r4,@-r2
+	mov	r1,r9
 	mov.l	@(OFF_R0,r15),r0
 	mov.l	@(OFF_R1,r15),r1
 	mov.l	@(OFF_R2,r15),r2
 	mov.l	@(OFF_R3,r15),r3
 2:
-	mov	#OFF_TRA,r8
-	add	r15,r8
-	mov.l	@r8,r9	
 	mov	#64,r8
 	cmp/hs	r8,r9
 	bt	interrupt_entry	! vec >= 64 is interrupt
@@ -150,26 +148,14 @@ ENTRY(exception_handler)
 	cmp/hs	r8,r9
 	bt	trap_entry	! 64 > vec >= 32  is trap
 
-#if defined(CONFIG_SH_FPU)
-	mov     #13,r8
-	cmp/eq  r8,r9
-	bt      10f             ! fpu
-	nop
-#endif
-
 	mov.l	4f,r8
 	mov	r9,r4
 	shll2	r9
 	add	r9,r8
-	mov.l	@r8,r8
-	mov	#0,r9
-	cmp/eq	r9,r8
+	mov.l	@r8,r8		! exception handler address
+	tst	r8,r8
 	bf	3f
 	mov.l	8f,r8		! unhandled exception
-#if defined(CONFIG_SH_FPU)
-10:
-	mov.l	9f, r8		! unhandled exception
-#endif
 3:
 	mov.l	5f,r10
 	jmp	@r8
@@ -188,10 +174,7 @@ interrupt_entry:
 5:	.long	ret_from_exception
 6:	.long	ret_from_irq
 7:	.long	do_IRQ
-8:	.long	do_exception_error
-#ifdef CONFIG_SH_FPU
-9:	.long	fpu_error_trap_handler
-#endif
+8:	.long	exception_error
 
 trap_entry:
 	mov	#0x30,r8
@@ -200,24 +183,9 @@ trap_entry:
 	add	#-0x10,r9	! convert SH2 to SH3/4 ABI
 1:	
 	shll2	r9			! TRA
-	mov	#OFF_TRA,r8
-	add	r15,r8
-	mov.l	r9,@r8
-	mov	r9,r8
-#ifdef CONFIG_TRACE_IRQFLAGS
-	mov.l	2f, r9
-	jsr	@r9
-	 nop
-#endif
-	sti
-	bra	system_call
-	 nop
+	bra	system_call	! jump common systemcall entry
+	 mov	r9,r8
 	
-	.align	2
-#ifdef CONFIG_TRACE_IRQFLAGS
-2:	.long	trace_hardirqs_on
-#endif
-
 #if defined(CONFIG_SH_STANDARD_BIOS)
 	/* Unwind the stack and jmp to the debug entry */
 ENTRY(sh_bios_handler)
@@ -240,7 +208,7 @@ ENTRY(sh_bios_handler)
 	mov.l	@r2,r2
 	stc	sr,r3
 	mov.l	r2,@r0
-	mov.l	r3,@r0
+	mov.l	r3,@(4,r0)
 	mov.l	r1,@(8,r0)	
 	mov.l	@r15+, r0
 	mov.l	@r15+, r1
@@ -272,22 +240,30 @@ ENTRY(address_error_trap_handler)
 	mov.l	1f,r0
 	jmp	@r0
 	 mov	#0,r5				! writeaccess is unknown
-	.align	2
 
+	.align	2
 1:	.long	do_address_error
 
 restore_all:
-	cli
-#ifdef CONFIG_TRACE_IRQFLAGS
-	mov.l	1f, r0
-	jsr	@r0
-	 nop
-#endif
+	stc	sr,r0
+	or	#0xf0,r0
+	ldc	r0,sr				! all interrupt block (same BL = 1)
+	! restore special register
+	! overlap exception frame
+	mov	r15,r0
+	add	#17*4,r0
+	lds.l	@r0+,pr
+	add	#4,r0
+	ldc.l	@r0+,gbr
+	lds.l	@r0+,mach
+	lds.l	@r0+,macl
 	mov	r15,r0
 	mov.l	$cpu_mode,r2
 	mov	#OFF_SR,r3
 	mov.l	@(r0,r3),r1
-	mov.l	r1,@r2
+	mov.l	__md_bit,r3
+	and	r1,r3				! copy MD bit
+	mov.l	r3,@r2
 	shll2	r1				! clear MD bit
 	shlr2	r1
 	mov.l	@(OFF_SP,r0),r2
@@ -297,12 +273,6 @@ restore_all:
 	mov	#OFF_PC,r3
 	mov.l	@(r0,r3),r1
 	mov.l	r1,@r2				! set pc
-	add	#4*16+4,r0
-	lds.l	@r0+,pr
-	add	#4,r0				! skip sr
-	ldc.l	@r0+,gbr
-	lds.l	@r0+,mach
-	lds.l	@r0+,macl
 	get_current_thread_info r0, r1
 	mov.l	$current_thread_info,r1
 	mov.l	r0,@r1
@@ -326,9 +296,8 @@ restore_all:
 	 nop
 
 	.align 2
-#ifdef CONFIG_TRACE_IRQFLAGS
-1:     .long   trace_hardirqs_off
-#endif
+__md_bit:
+	.long	0x40000000
 $current_thread_info:
 	.long	__current_thread_info
 $cpu_mode:	
diff --git a/arch/sh/kernel/cpu/sh2/ex.S b/arch/sh/kernel/cpu/sh2/ex.S
index 6d285af..85b0bf8 100644
--- a/arch/sh/kernel/cpu/sh2/ex.S
+++ b/arch/sh/kernel/cpu/sh2/ex.S
@@ -18,16 +18,17 @@
 exception_entry:	
 no	=	0
 	.rept	256
-	mov.l	r0,@-sp
-	mov	#no,r0
+	mov.l	r1,@-sp
 	bra	exception_trampoline
-	and	#0xff,r0
+	mov	#no,r1
 no	=	no + 1
 	.endr
 exception_trampoline:
-	mov.l	r1,@-sp
-	mov.l	$exception_handler,r1
-	jmp	@r1
+	mov.l	r0,@-sp
+	mov.l	$exception_handler,r0
+	extu.b	r1,r1
+	jmp	@r0
+	  extu.w	r1,r1
 
 	.align	2
 $exception_entry:
@@ -41,6 +42,6 @@ $exception_handler:
 ENTRY(vbr_base)
 vector	=	0
 	.rept	256
-	.long	exception_entry + vector * 8
+	.long	exception_entry + vector * 6
 vector	=	vector + 1
 	.endr
diff --git a/arch/sh/kernel/cpu/sh2a/Makefile b/arch/sh/kernel/cpu/sh2a/Makefile
index 7e2b90c..1ab1ecf 100644
--- a/arch/sh/kernel/cpu/sh2a/Makefile
+++ b/arch/sh/kernel/cpu/sh2a/Makefile
@@ -4,7 +4,7 @@
 
 obj-y	:= common.o probe.o opcode_helper.o
 
-common-y	+= $(addprefix ../sh2/, ex.o entry.o)
+common-y	+= ex.o entry.o
 
 obj-$(CONFIG_SH_FPU)	+= fpu.o
 
diff --git a/arch/sh/kernel/cpu/sh2a/entry.S b/arch/sh/kernel/cpu/sh2a/entry.S
new file mode 100644
index 0000000..47096dc
--- /dev/null
+++ b/arch/sh/kernel/cpu/sh2a/entry.S
@@ -0,0 +1,249 @@
+/*
+ * arch/sh/kernel/cpu/sh2a/entry.S
+ *
+ * The SH-2A exception entry
+ *
+ * Copyright (C) 2008 Yoshinori Sato
+ * Based on arch/sh/kernel/cpu/sh2/entry.S
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#include <linux/linkage.h>
+#include <asm/asm-offsets.h>
+#include <asm/thread_info.h>
+#include <asm/cpu/mmu_context.h>
+#include <asm/unistd.h>
+#include <asm/errno.h>
+#include <asm/page.h>
+	
+/* Offsets to the stack */
+OFF_R0  =  0		/* Return value. New ABI also arg4 */
+OFF_R1  =  4     	/* New ABI: arg5 */
+OFF_R2  =  8     	/* New ABI: arg6 */
+OFF_R3  =  12     	/* New ABI: syscall_nr */
+OFF_R4  =  16     	/* New ABI: arg0 */
+OFF_R5  =  20     	/* New ABI: arg1 */
+OFF_R6  =  24     	/* New ABI: arg2 */
+OFF_R7  =  28     	/* New ABI: arg3 */
+OFF_SP	=  (15*4)
+OFF_PC  =  (16*4)
+OFF_SR	=  (16*4+2*4)
+OFF_TRA	=  (16*4+6*4)
+
+#include <asm/entry-macros.S>
+
+ENTRY(exception_handler)
+	! stack
+	! r0 <- point sp
+	! r1
+	! pc
+	! sr
+	! r0 = temporary
+	! r1 = vector (pseudo EXPEVT / INTEVT / TRA)
+	mov.l	r2,@-sp
+	cli
+	mov.l	$cpu_mode,r2
+	bld.b	#6,@(0,r2)	!previus SR.MD
+	bst.b	#6,@(4*4,r15)	!set cpu mode to SR.MD
+	bt	1f
+	! switch to kernel mode
+	bset.b	#6,@(0,r2)	!set SR.MD
+	mov.l	$current_thread_info,r2
+	mov.l	@r2,r2
+	mov	#(THREAD_SIZE >> 8),r0
+	shll8	r0
+	add	r2,r0		! r0 = kernel stack tail
+	mov	r15,r2		! r2 = user stack top
+	mov	r0,r15		! switch kernel stack
+	mov.l	r1,@-r15	! TRA
+	sts.l	macl, @-r15
+	sts.l	mach, @-r15
+	stc.l	gbr, @-r15
+	mov.l	@(4*4,r2),r0
+	mov.l	r0,@-r15	! original SR
+	sts.l	pr,@-r15
+	mov.l	@(3*4,r2),r0
+	mov.l	r0,@-r15	! original PC
+	mov	r2,r0
+	add	#(3+2)*4,r0	! rewind r0 - r3 + exception frame
+	lds	r0,pr		! pr = original SP
+	movmu.l	r3,@-r15	! save regs
+	mov	r2,r8		! r8 =  previus stack top
+	mov	r1,r9		! r9 = interrupt vector
+	! restore previous stack
+	mov.l	@r8+,r2
+	mov.l	@r8+,r0
+	mov.l	@r8+,r1
+	bra	2f
+	 movml.l r2,@-r15
+1:
+	! in kernel exception
+	mov	r15,r2
+	add	#-((OFF_TRA + 4) - OFF_PC) + 5*4,r15
+	movmu.l	r3,@-r15
+	mov	r2,r8		! r8 = previous stack top
+	mov	r1,r9		! r9 = interrupt vector
+	! restore exception frame & regs
+	mov.l	@r8+,r2		! old R2
+	mov.l	@r8+,r0		! old R0
+	mov.l	@r8+,r1		! old R1
+	mov.l	@r8+,r10	! old PC
+	mov.l	@r8+,r11	! old SR
+	movml.l	r2,@-r15
+	mov.l	r10,@(OFF_PC,r15)
+	mov.l	r11,@(OFF_SR,r15)
+	mov.l	r8,@(OFF_SP,r15)	! save old sp
+	mov	r15,r8
+	add	#OFF_TRA + 4,r8
+	mov.l	r9,@-r8
+	sts.l	macl,@-r8
+	sts.l	mach,@-r8
+	stc.l	gbr,@-r8
+	add	#-4,r8
+	sts.l	pr,@-r8
+2:
+	! dispatch exception / interrupt
+	mov	#64,r8
+	cmp/hs	r8,r9
+	bt	interrupt_entry	! vec >= 64 is interrupt
+	mov	#32,r8
+	cmp/hs	r8,r9
+	bt	trap_entry	! 64 > vec >= 32  is trap
+
+	mov.l	4f,r8
+	mov	r9,r4
+	shll2	r9
+	add	r9,r8
+	mov.l	@r8,r8		! exception handler address
+	tst	r8,r8
+	bf	3f
+	mov.l	8f,r8		! unhandled exception
+3:
+	mov.l	5f,r10
+	jmp	@r8
+	 lds	r10,pr
+
+interrupt_entry:
+	mov	r9,r4
+	mov	r15,r5
+	mov.l	7f,r8
+	mov.l	6f,r9
+	jmp	@r8
+	 lds	r9,pr
+
+	.align	2
+4:	.long	exception_handling_table
+5:	.long	ret_from_exception
+6:	.long	ret_from_irq
+7:	.long	do_IRQ
+8:	.long	exception_error
+
+trap_entry:
+	mov	#0x30,r8
+	cmp/ge	r8,r9		! vector 0x20-0x2f is systemcall
+	bt	1f
+	add	#-0x10,r9	! convert SH2 to SH3/4 ABI
+1:	
+	shll2	r9			! TRA
+	bra	system_call	! jump common systemcall entry
+	 mov	r9,r8
+	
+#if defined(CONFIG_SH_STANDARD_BIOS)
+	/* Unwind the stack and jmp to the debug entry */
+ENTRY(sh_bios_handler)
+	mov	r15,r0
+	add	#(22-4)*4-4,r0
+	ldc.l	@r0+,gbr
+	lds.l	@r0+,mach
+	lds.l	@r0+,macl
+	mov	r15,r0
+	mov.l	@(OFF_SP,r0),r1
+	mov.l	@(OFF_SR,r2),r3
+	mov.l	r3,@-r1
+	mov.l	@(OFF_SP,r2),r3
+	mov.l	r3,@-r1
+	mov	r15,r0
+	add	#(22-4)*4-8,r0
+	mov.l	1f,r2
+	mov.l	@r2,r2
+	stc	sr,r3
+	mov.l	r2,@r0
+	mov.l	r3,@(4,r0)
+	mov.l	r1,@(8,r0)
+	movml.l	@r15+,r14
+	add	#8,r15
+	lds.l	@r15+, pr
+	rte
+	 mov.l	@r15+,r15
+	.align	2
+1:	.long	gdb_vbr_vector
+#endif /* CONFIG_SH_STANDARD_BIOS */
+
+ENTRY(address_error_trap_handler)
+	mov	r15,r4				! regs
+	mov.l	@(OFF_PC,r15),r6		! pc
+	mov.l	1f,r0
+	jmp	@r0
+	 mov	#0,r5				! writeaccess is unknown
+
+	.align	2
+1:	.long	do_address_error
+
+restore_all:
+	stc	sr,r0
+	or	#0xf0,r0
+	ldc	r0,sr				! all interrupt block (same BL = 1)
+	! restore special register
+	! overlap exception frame
+	mov	r15,r0
+	add	#17*4,r0
+	lds.l	@r0+,pr
+	add	#4,r0
+	ldc.l	@r0+,gbr
+	lds.l	@r0+,mach
+	lds.l	@r0+,macl
+	mov	r15,r0
+	mov.l	$cpu_mode,r2
+	bld.b	#6,@(OFF_SR,r15)
+	bst.b	#6,@(0,r2)			! save CPU mode
+	mov.l	@(OFF_SR,r0),r1
+	shll2	r1
+	shlr2	r1				! clear MD bit
+	mov.l	@(OFF_SP,r0),r2
+	add	#-8,r2
+	mov.l	r2,@(OFF_SP,r0)			! point exception frame top
+	mov.l	r1,@(4,r2)			! set sr
+	mov.l	@(OFF_PC,r0),r1
+	mov.l	r1,@r2				! set pc
+	get_current_thread_info r0, r1
+	mov.l	$current_thread_info,r1
+	mov.l	r0,@r1
+	movml.l	@r15+,r14
+	mov.l	@r15,r15
+	rte
+	 nop
+
+	.align 2
+$current_thread_info:
+	.long	__current_thread_info
+$cpu_mode:	
+	.long	__cpu_mode
+		
+! common exception handler
+#include "../../entry-common.S"
+	
+	.data
+! cpu operation mode 
+! bit30 = MD (compatible SH3/4)
+__cpu_mode:
+	.long	0x40000000
+		
+	.section	.bss
+__current_thread_info:
+	.long	0
+
+ENTRY(exception_handling_table)
+	.space	4*32
diff --git a/arch/sh/kernel/cpu/sh2a/ex.S b/arch/sh/kernel/cpu/sh2a/ex.S
new file mode 100644
index 0000000..3ead9e6
--- /dev/null
+++ b/arch/sh/kernel/cpu/sh2a/ex.S
@@ -0,0 +1,72 @@
+/*
+ * arch/sh/kernel/cpu/sh2a/ex.S
+ *
+ * The SH-2A exception vector table
+ *
+ * Copyright (C) 2008 Yoshinori Sato
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#include <linux/linkage.h>
+
+!
+! convert Exception Vector to Exception Number
+!
+
+! exception no 0 to 255
+exception_entry0:
+no	=	0
+	.rept	256
+	mov.l	r1,@-sp
+	bra	exception_trampoline0
+	mov	#no,r1
+no	=	no + 1
+	.endr
+exception_trampoline0:
+	mov.l	r0,@-sp
+	mov.l	1f,r0
+	extu.b	r1,r1
+	jmp	@r0
+	  extu.w	r1,r1
+	 
+	.align	2
+1:	.long	exception_handler
+
+! exception no 256 to 511
+exception_entry1:
+no	=	0
+	.rept	256
+	mov.l	r1,@-sp
+	bra	exception_trampoline1
+	mov	#no,r1
+no	=	no + 1
+	.endr
+exception_trampoline1:
+	mov.l	r0,@-sp
+	extu.b	r1,r1
+	movi20	#0x100,r0
+	add	r0,r1
+	mov.l	1f,r0
+	jmp	@r0
+	  extu.w	r1,r1
+	
+	.align	2
+1:	.long	exception_handler
+
+	!
+! Exception Vector Base
+!
+	.align	2
+ENTRY(vbr_base)
+vector	=	0
+	.rept	256
+	.long	exception_entry0 + vector * 6
+vector	=	vector + 1
+	.endr
+	.rept	256
+	.long	exception_entry1 + vector * 6
+vector	=	vector + 1
+	.endr
diff --git a/arch/sh/kernel/traps_32.c b/arch/sh/kernel/traps_32.c
index e08b3bf..511a942 100644
--- a/arch/sh/kernel/traps_32.c
+++ b/arch/sh/kernel/traps_32.c
@@ -43,6 +43,7 @@
 # define TRAP_ILLEGAL_SLOT_INST	6
 # define TRAP_ADDRESS_ERROR	9
 # ifdef CONFIG_CPU_SH2A
+#  define TRAP_FPU_ERROR	13
 #  define TRAP_DIVZERO_ERROR	17
 #  define TRAP_DIVOVF_ERROR	18
 # endif
@@ -851,6 +852,9 @@ void __init trap_init(void)
 #ifdef CONFIG_CPU_SH2A
 	set_exception_table_vec(TRAP_DIVZERO_ERROR, do_divide_error);
 	set_exception_table_vec(TRAP_DIVOVF_ERROR, do_divide_error);
+#ifdef CONFIG_SH_FPU
+	set_exception_table_vec(TRAP_FPU_ERROR, fpu_error_trap_handler);
+#endif
 #endif
 
 	/* Setup VBR for boot cpu */

-- 
Yoshinori Sato
<ysato@users.sourceforge.jp>