1 files changed, 58 insertions, 7 deletions

diff --git a/arch/frv/kernel/entry.S b/arch/frv/kernel/entry.S
index c69d499d28cf..1d21c8d34d8a 100644
--- a/arch/frv/kernel/entry.S
+++ b/arch/frv/kernel/entry.S
@@ -141,7 +141,10 @@ __entry_uspace_external_interrupt_reentry:
 
         movsg           gner0,gr4
         movsg           gner1,gr5
-        stdi            gr4,@(gr28,#REG_GNER0)
+        stdi.p          gr4,@(gr28,#REG_GNER0)
+
+        # interrupts start off fully disabled in the interrupt handler
+        subcc           gr0,gr0,gr0,icc2                /* set Z and clear C */
 
         # set up kernel global registers
         sethi.p         %hi(__kernel_current_task),gr5
@@ -193,9 +196,8 @@ __entry_uspace_external_interrupt_reentry:
         .type           __entry_kernel_external_interrupt,@function
 __entry_kernel_external_interrupt:
         LEDS            0x6210
-
-        sub             sp,gr15,gr31
-        LEDS32
+//      sub             sp,gr15,gr31
+//      LEDS32
 
         # set up the stack pointer
         or.p            sp,gr0,gr30
@@ -231,7 +233,10 @@ __entry_kernel_external_interrupt_reentry:
         stdi            gr24,@(gr28,#REG_GR(24))
         stdi            gr26,@(gr28,#REG_GR(26))
         sti             gr29,@(gr28,#REG_GR(29))
-        stdi            gr30,@(gr28,#REG_GR(30))
+        stdi.p          gr30,@(gr28,#REG_GR(30))
+
+        # note virtual interrupts will be fully enabled upon return
+        subicc          gr0,#1,gr0,icc2                 /* clear Z, set C */
 
         movsg           tbr ,gr20
         movsg           psr ,gr22
@@ -267,7 +272,10 @@ __entry_kernel_external_interrupt_reentry:
 
         movsg           gner0,gr4
         movsg           gner1,gr5
-        stdi            gr4,@(gr28,#REG_GNER0)
+        stdi.p          gr4,@(gr28,#REG_GNER0)
+
+        # interrupts start off fully disabled in the interrupt handler
+        subcc           gr0,gr0,gr0,icc2                        /* set Z and clear C */
 
         # set the return address
         sethi.p         %hi(__entry_return_from_kernel_interrupt),gr4
@@ -291,6 +299,45 @@ __entry_kernel_external_interrupt_reentry:
 
         .size           __entry_kernel_external_interrupt,.-__entry_kernel_external_interrupt
 
+###############################################################################
+#
+# deal with interrupts that were actually virtually disabled
+# - we need to really disable them, flag the fact and return immediately
+# - if you change this, you must alter break.S also
+#
+###############################################################################
+        .balign         L1_CACHE_BYTES
+        .globl          __entry_kernel_external_interrupt_virtually_disabled
+        .type           __entry_kernel_external_interrupt_virtually_disabled,@function
+__entry_kernel_external_interrupt_virtually_disabled:
+        movsg           psr,gr30
+        andi            gr30,#~PSR_PIL,gr30
+        ori             gr30,#PSR_PIL_14,gr30           ; debugging interrupts only
+        movgs           gr30,psr
+        subcc           gr0,gr0,gr0,icc2                ; leave Z set, clear C
+        rett            #0
+
+        .size           __entry_kernel_external_interrupt_virtually_disabled,.-__entry_kernel_external_interrupt_virtually_disabled
+
+###############################################################################
+#
+# deal with re-enablement of interrupts that were pending when virtually re-enabled
+# - set ICC2.C, re-enable the real interrupts and return
+# - we can clear ICC2.Z because we shouldn't be here if it's not 0 [due to TIHI]
+# - if you change this, you must alter break.S also
+#
+###############################################################################
+        .balign         L1_CACHE_BYTES
+        .globl          __entry_kernel_external_interrupt_virtual_reenable
+        .type           __entry_kernel_external_interrupt_virtual_reenable,@function
+__entry_kernel_external_interrupt_virtual_reenable:
+        movsg           psr,gr30
+        andi            gr30,#~PSR_PIL,gr30             ; re-enable interrupts
+        movgs           gr30,psr
+        subicc          gr0,#1,gr0,icc2                 ; clear Z, set C
+        rett            #0
+
+        .size           __entry_kernel_external_interrupt_virtual_reenable,.-__entry_kernel_external_interrupt_virtual_reenable
 
 ###############################################################################
 #
@@ -335,6 +382,7 @@ __entry_uspace_softprog_interrupt_reentry:
 
         sethi.p         %hi(__entry_return_from_user_exception),gr23
         setlo           %lo(__entry_return_from_user_exception),gr23
+
         bra             __entry_common
 
         .size           __entry_uspace_softprog_interrupt,.-__entry_uspace_softprog_interrupt
@@ -495,7 +543,10 @@ __entry_common:
 
         movsg           gner0,gr4
         movsg           gner1,gr5
-        stdi            gr4,@(gr28,#REG_GNER0)
+        stdi.p          gr4,@(gr28,#REG_GNER0)
+
+        # set up virtual interrupt disablement
+        subicc          gr0,#1,gr0,icc2                 /* clear Z flag, set C flag */
 
         # set up kernel global registers
         sethi.p         %hi(__kernel_current_task),gr5