5 files changed, 14 insertions, 25 deletions
diff --git a/arch/m68k/platform/coldfire/entry.S b/arch/m68k/platform/coldfire/entry.S
index 27c2b001161e..bd27242c2f43 100644
--- a/arch/m68k/platform/coldfire/entry.S
+++ b/arch/m68k/platform/coldfire/entry.S
@@ -182,21 +182,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 74b55cfbc3cb..995093357c59 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 d6a4d9d53e42..650d52e2927e 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 0bbb414856eb..5c0c150b4067 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 a21d3f870b7a..3a7cc524ecd3 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;