1 files changed, 16 insertions, 0 deletions

diff --git a/arch/m68knommu/platform/coldfire/entry.S b/arch/m68knommu/platform/coldfire/entry.S
index 111b66dc737b..1af7c1d650f6 100644
--- a/arch/m68knommu/platform/coldfire/entry.S
+++ b/arch/m68knommu/platform/coldfire/entry.S
@@ -106,6 +106,22 @@ ret_from_exception:
         btst    #5,%sp@(PT_SR)          /* check if returning to kernel */
         jeq     Luser_return            /* if so, skip resched, signals */
 
+#ifdef CONFIG_PREEMPT
+        movel   %sp,%d1                 /* get thread_info pointer */
+        andl    #-THREAD_SIZE,%d1       /* at base of kernel stack */
+        movel   %d1,%a0
+        movel   %a0@(TI_FLAGS),%d1      /* get thread_info->flags */
+        andl    #_TIF_NEED_RESCHED,%d1
+        jeq     Lkernel_return
+
+        movel   %a0@(TI_PREEMPTCOUNT),%d1
+        cmpl    #0,%d1
+        jne     Lkernel_return
+
+        pea     Lkernel_return
+        jmp     preempt_schedule_irq    /* preempt the kernel */
+#endif
+
 Lkernel_return:
         moveml  %sp@,%d1-%d5/%a0-%a2
         lea     %sp@(32),%sp            /* space for 8 regs */