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;