6 files changed, 172 insertions, 55 deletions

diff --git a/arch/frv/Makefile b/arch/frv/Makefile
index 90c0fb8d9dc3..d163747d17c0 100644
--- a/arch/frv/Makefile
+++ b/arch/frv/Makefile
@@ -81,7 +81,7 @@ endif
 # - reserve CC3 for use with atomic ops
 # - all the extra registers are dealt with only at context switch time
 CFLAGS          += -mno-fdpic -mgpr-32 -msoft-float -mno-media
-CFLAGS          += -ffixed-fcc3 -ffixed-cc3 -ffixed-gr15
+CFLAGS          += -ffixed-fcc3 -ffixed-cc3 -ffixed-gr15 -ffixed-icc2
 AFLAGS          += -mno-fdpic
 ASFLAGS         += -mno-fdpic
 
diff --git a/arch/frv/kernel/break.S b/arch/frv/kernel/break.S
index 33233dc23e29..687c48d62dde 100644
--- a/arch/frv/kernel/break.S
+++ b/arch/frv/kernel/break.S
@@ -200,12 +200,20 @@ __break_step:
         movsg           bpcsr,gr2
         sethi.p         %hi(__entry_kernel_external_interrupt),gr3
         setlo           %lo(__entry_kernel_external_interrupt),gr3
-        subcc           gr2,gr3,gr0,icc0
+        subcc.p         gr2,gr3,gr0,icc0
+        sethi           %hi(__entry_uspace_external_interrupt),gr3
+        setlo.p         %lo(__entry_uspace_external_interrupt),gr3
         beq             icc0,#2,__break_step_kernel_external_interrupt
-        sethi.p         %hi(__entry_uspace_external_interrupt),gr3
-        setlo           %lo(__entry_uspace_external_interrupt),gr3
-        subcc           gr2,gr3,gr0,icc0
+        subcc.p         gr2,gr3,gr0,icc0
+        sethi           %hi(__entry_kernel_external_interrupt_virtually_disabled),gr3
+        setlo.p         %lo(__entry_kernel_external_interrupt_virtually_disabled),gr3
         beq             icc0,#2,__break_step_uspace_external_interrupt
+        subcc.p         gr2,gr3,gr0,icc0
+        sethi           %hi(__entry_kernel_external_interrupt_virtual_reenable),gr3
+        setlo.p         %lo(__entry_kernel_external_interrupt_virtual_reenable),gr3
+        beq             icc0,#2,__break_step_kernel_external_interrupt_virtually_disabled
+        subcc           gr2,gr3,gr0,icc0
+        beq             icc0,#2,__break_step_kernel_external_interrupt_virtual_reenable
 
         LEDS            0x2007,gr2
 
@@ -254,6 +262,9 @@ __break_step_kernel_softprog_interrupt:
 # step through an external interrupt from kernel mode
         .globl          __break_step_kernel_external_interrupt
 __break_step_kernel_external_interrupt:
+        # deal with virtual interrupt disablement
+        beq             icc2,#0,__break_step_kernel_external_interrupt_virtually_disabled
+
         sethi.p         %hi(__entry_kernel_external_interrupt_reentry),gr3
         setlo           %lo(__entry_kernel_external_interrupt_reentry),gr3
 
@@ -294,6 +305,64 @@ __break_return_as_kernel_prologue:
 #endif
         rett            #1
 
+# we single-stepped into an interrupt handler whilst interrupts were merely virtually disabled
+# need to really disable interrupts, set flag, fix up and return
+__break_step_kernel_external_interrupt_virtually_disabled:
+        movsg           psr,gr2
+        andi            gr2,#~PSR_PIL,gr2
+        ori             gr2,#PSR_PIL_14,gr2     /* debugging interrupts only */
+        movgs           gr2,psr
+
+        ldi             @(gr31,#REG_CCR),gr3
+        movgs           gr3,ccr
+        subcc.p         gr0,gr0,gr0,icc2        /* leave Z set, clear C */
+
+        # exceptions must've been enabled and we must've been in supervisor mode
+        setlos          BPSR_BET|BPSR_BS,gr3
+        movgs           gr3,bpsr
+
+        # return to where the interrupt happened
+        movsg           pcsr,gr2
+        movgs           gr2,bpcsr
+
+        lddi.p          @(gr31,#REG_GR(2)),gr2
+
+        xor             gr31,gr31,gr31
+        movgs           gr0,brr
+#ifdef CONFIG_MMU
+        movsg           scr3,gr31
+#endif
+        rett            #1
+
+# we stepped through into the virtual interrupt reenablement trap
+#
+# we also want to single step anyway, but after fixing up so that we get an event on the
+# instruction after the broken-into exception returns
+        .globl          __break_step_kernel_external_interrupt_virtual_reenable
+__break_step_kernel_external_interrupt_virtual_reenable:
+        movsg           psr,gr2
+        andi            gr2,#~PSR_PIL,gr2
+        movgs           gr2,psr
+
+        ldi             @(gr31,#REG_CCR),gr3
+        movgs           gr3,ccr
+        subicc          gr0,#1,gr0,icc2         /* clear Z, set C */
+
+        # save the adjusted ICC2
+        movsg           ccr,gr3
+        sti             gr3,@(gr31,#REG_CCR)
+
+        # exceptions must've been enabled and we must've been in supervisor mode
+        setlos          BPSR_BET|BPSR_BS,gr3
+        movgs           gr3,bpsr
+
+        # return to where the trap happened
+        movsg           pcsr,gr2
+        movgs           gr2,bpcsr
+
+        # and then process the single step
+        bra             __break_continue
+
 # step through an internal exception from uspace mode
         .globl          __break_step_uspace_softprog_interrupt
 __break_step_uspace_softprog_interrupt:
diff --git a/arch/frv/kernel/entry-table.S b/arch/frv/kernel/entry-table.S
index 9b9243e2103c..81568acea9cd 100644
--- a/arch/frv/kernel/entry-table.S
+++ b/arch/frv/kernel/entry-table.S
@@ -116,6 +116,8 @@ __break_kerneltrap_fixup_table:
         .long           __break_step_uspace_external_interrupt
         .section .trap.kernel
         .org            \tbr_tt
+        # deal with virtual interrupt disablement
+        beq             icc2,#0,__entry_kernel_external_interrupt_virtually_disabled
         bra             __entry_kernel_external_interrupt
         .section .trap.fixup.kernel
         .org            \tbr_tt >> 2
@@ -259,25 +261,52 @@ __trap_fixup_kernel_data_tlb_miss:
         .org            TBR_TT_TRAP0
         .rept           127
         bra             __entry_uspace_softprog_interrupt
-        bra             __break_step_uspace_softprog_interrupt
-        .long           0,0
+        .long           0,0,0
         .endr
         .org            TBR_TT_BREAK
         bra             __entry_break
         .long           0,0,0
 
+        .section        .trap.fixup.user
+        .org            TBR_TT_TRAP0 >> 2
+        .rept           127
+        .long           __break_step_uspace_softprog_interrupt
+        .endr
+        .org            TBR_TT_BREAK >> 2
+        .long           0
+
         # miscellaneous kernel mode entry points
         .section        .trap.kernel
         .org            TBR_TT_TRAP0
-        .rept           127
         bra             __entry_kernel_softprog_interrupt
-        bra             __break_step_kernel_softprog_interrupt
-        .long           0,0
+        .org            TBR_TT_TRAP1
+        bra             __entry_kernel_softprog_interrupt
+
+        # trap #2 in kernel - reenable interrupts
+        .org            TBR_TT_TRAP2
+        bra             __entry_kernel_external_interrupt_virtual_reenable
+
+        # miscellaneous kernel traps
+        .org            TBR_TT_TRAP3
+        .rept           124
+        bra             __entry_kernel_softprog_interrupt
+        .long           0,0,0
         .endr
         .org            TBR_TT_BREAK
         bra             __entry_break
         .long           0,0,0
 
+        .section        .trap.fixup.kernel
+        .org            TBR_TT_TRAP0 >> 2
+        .long           __break_step_kernel_softprog_interrupt
+        .long           __break_step_kernel_softprog_interrupt
+        .long           __break_step_kernel_external_interrupt_virtual_reenable
+        .rept           124
+        .long           __break_step_kernel_softprog_interrupt
+        .endr
+        .org            TBR_TT_BREAK >> 2
+        .long           0
+
         # miscellaneous debug mode entry points
         .section        .trap.break
         .org            TBR_TT_BREAK
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
diff --git a/arch/frv/kernel/head.S b/arch/frv/kernel/head.S
index c73b4fe9f6ca..29a5265489b7 100644
--- a/arch/frv/kernel/head.S
+++ b/arch/frv/kernel/head.S
@@ -513,6 +513,9 @@ __head_mmu_enabled:
         movgs           gr0,ccr
         movgs           gr0,cccr
 
+        # initialise the virtual interrupt handling
+        subcc           gr0,gr0,gr0,icc2                /* set Z, clear C */
+
 #ifdef CONFIG_MMU
         movgs           gr3,scr2
         movgs           gr3,scr3
diff --git a/arch/frv/kernel/irq.c b/arch/frv/kernel/irq.c
index 59580c59c62c..27ab4c30aac6 100644
--- a/arch/frv/kernel/irq.c
+++ b/arch/frv/kernel/irq.c
@@ -287,18 +287,11 @@ asmlinkage void do_IRQ(void)
         struct irq_source *source;
         int level, cpu;
 
+        irq_enter();
+
         level = (__frame->tbr >> 4) & 0xf;
         cpu = smp_processor_id();
 
-#if 0
-        {
-                static u32 irqcount;
-                *(volatile u32 *) 0xe1200004 = ~((irqcount++ << 8) | level);
-                *(volatile u16 *) 0xffc00100 = (u16) ~0x9999;
-                mb();
-        }
-#endif
-
         if ((unsigned long) __frame - (unsigned long) (current + 1) < 512)
                 BUG();
 
@@ -308,40 +301,12 @@ asmlinkage void do_IRQ(void)
 
         kstat_this_cpu.irqs[level]++;
 
-        irq_enter();
-
         for (source = frv_irq_levels[level].sources; source; source = source->next)
                 source->doirq(source);
 
-        irq_exit();
-
         __clr_MASK(level);
 
-        /* only process softirqs if we didn't interrupt another interrupt handler */
-        if ((__frame->psr & PSR_PIL) == PSR_PIL_0)
-                if (local_softirq_pending())
-                        do_softirq();
-
-#ifdef CONFIG_PREEMPT
-        local_irq_disable();
-        while (--current->preempt_count == 0) {
-                if (!(__frame->psr & PSR_S) ||
-                    current->need_resched == 0 ||
-                    in_interrupt())
-                        break;
-                current->preempt_count++;
-                local_irq_enable();
-                preempt_schedule();
-                local_irq_disable();
-        }
-#endif
-
-#if 0
-        {
-                *(volatile u16 *) 0xffc00100 = (u16) ~0x6666;
-                mb();
-        }
-#endif
+        irq_exit();
 
 } /* end do_IRQ() */