5 files changed, 16 insertions, 29 deletions
diff --git a/arch/m68k/platform/coldfire/entry.S b/arch/m68k/platform/coldfire/entry.S
index 27c2b00..3157461 100644
--- a/arch/m68k/platform/coldfire/entry.S
+++ b/arch/m68k/platform/coldfire/entry.S
@@ -61,7 +61,7 @@ enosys:
 	bra	1f
 
 ENTRY(system_call)
-	SAVE_ALL
+	SAVE_ALL_SYS
 	move	#0x2000,%sr		/* enable intrs again */
 
 	cmpl	#NR_syscalls,%d0
@@ -165,9 +165,7 @@ Lsignal_return:
  * sources). Calls up to high level code to do all the work.
  */
 ENTRY(inthandler)
-	SAVE_ALL
-	moveq	#-1,%d0
-	movel	%d0,%sp@(PT_OFF_ORIG_D0)
+	SAVE_ALL_INT
 
 	movew	%sp@(PT_OFF_FORMATVEC),%d0 /* put exception # in d0 */
 	andl	#0x03fc,%d0		/* mask out vector only */
@@ -182,21 +180,23 @@ ENTRY(inthandler)
 
 /*
  * Beware - when entering resume, prev (the current task) is
- * in a0, next (the new task) is in a1,so don't change these
+ * in a0, next (the new task) is in a1, so don't change these
  * registers until their contents are no longer needed.
- * This is always called in supervisor mode, so don't bother to save
- * and restore sr; user's process sr is actually in the stack.
  */
 ENTRY(resume)
-	movel	%a0, %d1			/* get prev thread in d1 */
-	RDUSP
-	movel	%a2,%a0@(TASK_THREAD+THREAD_USP)
-
+	movew	%sr,%d1				 /* save current status */
+	movew	%d1,%a0@(TASK_THREAD+THREAD_SR)
+	movel	%a0,%d1				 /* get prev thread in d1 */
 	SAVE_SWITCH_STACK
 	movel	%sp,%a0@(TASK_THREAD+THREAD_KSP) /* save kernel stack pointer */
-	movel	%a1@(TASK_THREAD+THREAD_KSP),%sp /* restore new thread stack */
+	RDUSP					 /* movel %usp,%a3 */
+	movel	%a3,%a0@(TASK_THREAD+THREAD_USP) /* save thread user stack */
+
+	movel	%a1@(TASK_THREAD+THREAD_USP),%a3 /* restore thread user stack */
+	WRUSP					 /* movel %a3,%usp */
+	movel	%a1@(TASK_THREAD+THREAD_KSP),%sp /* restore new kernel stack */
+	movew	%a1@(TASK_THREAD+THREAD_SR),%d7	 /* restore new status */
+	movew	%d7,%sr
 	RESTORE_SWITCH_STACK
-
-	movel	%a1@(TASK_THREAD+THREAD_USP),%a0 /* restore thread user stack */
-	WRUSP
 	rts
+
diff --git a/arch/m68k/platform/coldfire/intc-2.c b/arch/m68k/platform/coldfire/intc-2.c
index 74b55cf..9950933 100644
--- a/arch/m68k/platform/coldfire/intc-2.c
+++ b/arch/m68k/platform/coldfire/intc-2.c
@@ -194,8 +194,6 @@ void __init init_IRQ(void)
 {
 	int irq;
 
-	init_vectors();
-
 	/* Mask all interrupt sources */
 	__raw_writel(0x1, MCFICM_INTC0 + MCFINTC_IMRL);
 #ifdef MCFICM_INTC1
diff --git a/arch/m68k/platform/coldfire/intc-simr.c b/arch/m68k/platform/coldfire/intc-simr.c
index d6a4d9d..650d52e 100644
--- a/arch/m68k/platform/coldfire/intc-simr.c
+++ b/arch/m68k/platform/coldfire/intc-simr.c
@@ -171,8 +171,6 @@ void __init init_IRQ(void)
 {
 	int irq, eirq;
 
-	init_vectors();
-
 	/* Mask all interrupt sources */
 	__raw_writeb(0xff, MCFINTC0_SIMR);
 	if (MCFINTC1_SIMR)
diff --git a/arch/m68k/platform/coldfire/intc.c b/arch/m68k/platform/coldfire/intc.c
index 0bbb414..5c0c150 100644
--- a/arch/m68k/platform/coldfire/intc.c
+++ b/arch/m68k/platform/coldfire/intc.c
@@ -139,7 +139,6 @@ void __init init_IRQ(void)
 {
 	int irq;
 
-	init_vectors();
 	mcf_maskimr(0xffffffff);
 
 	for (irq = 0; (irq < NR_IRQS); irq++) {
diff --git a/arch/m68k/platform/coldfire/vectors.c b/arch/m68k/platform/coldfire/vectors.c
index a21d3f8..3a7cc52 100644
--- a/arch/m68k/platform/coldfire/vectors.c
+++ b/arch/m68k/platform/coldfire/vectors.c
@@ -35,21 +35,13 @@ asmlinkage void dbginterrupt_c(struct frame *fp)
 
 extern e_vector	*_ramvec;
 
-void set_evector(int vecnum, void (*handler)(void))
-{
-	if (vecnum >= 0 && vecnum <= 255)
-		_ramvec[vecnum] = handler;
-}
-
-/***************************************************************************/
-
 /* Assembler routines */
 asmlinkage void buserr(void);
 asmlinkage void trap(void);
 asmlinkage void system_call(void);
 asmlinkage void inthandler(void);
 
-void __init init_vectors(void)
+void __init trap_init(void)
 {
 	int i;