1 files changed, 52 insertions, 0 deletions

diff --git a/arch/sparc64/kernel/irq.c b/arch/sparc64/kernel/irq.c
index ba43d85e8dde..9b6689d9d570 100644
--- a/arch/sparc64/kernel/irq.c
+++ b/arch/sparc64/kernel/irq.c
@@ -682,10 +682,32 @@ void ack_bad_irq(unsigned int virt_irq)
                ino, virt_irq);
 }
 
+void *hardirq_stack[NR_CPUS];
+void *softirq_stack[NR_CPUS];
+
+static __attribute__((always_inline)) void *set_hardirq_stack(void)
+{
+        void *orig_sp, *sp = hardirq_stack[smp_processor_id()];
+
+        __asm__ __volatile__("mov %%sp, %0" : "=r" (orig_sp));
+        if (orig_sp < sp ||
+            orig_sp > (sp + THREAD_SIZE)) {
+                sp += THREAD_SIZE - 192 - STACK_BIAS;
+                __asm__ __volatile__("mov %0, %%sp" : : "r" (sp));
+        }
+
+        return orig_sp;
+}
+static __attribute__((always_inline)) void restore_hardirq_stack(void *orig_sp)
+{
+        __asm__ __volatile__("mov %0, %%sp" : : "r" (orig_sp));
+}
+
 void handler_irq(int irq, struct pt_regs *regs)
 {
         unsigned long pstate, bucket_pa;
         struct pt_regs *old_regs;
+        void *orig_sp;
 
         clear_softint(1 << irq);
 
@@ -703,6 +725,8 @@ void handler_irq(int irq, struct pt_regs *regs)
                                "i" (PSTATE_IE)
                              : "memory");
 
+        orig_sp = set_hardirq_stack();
+
         while (bucket_pa) {
                 struct irq_desc *desc;
                 unsigned long next_pa;
@@ -719,10 +743,38 @@ void handler_irq(int irq, struct pt_regs *regs)
                 bucket_pa = next_pa;
         }
 
+        restore_hardirq_stack(orig_sp);
+
         irq_exit();
         set_irq_regs(old_regs);
 }
 
+void do_softirq(void)
+{
+        unsigned long flags;
+
+        if (in_interrupt())
+                return;
+
+        local_irq_save(flags);
+
+        if (local_softirq_pending()) {
+                void *orig_sp, *sp = softirq_stack[smp_processor_id()];
+
+                sp += THREAD_SIZE - 192 - STACK_BIAS;
+
+                __asm__ __volatile__("mov %%sp, %0\n\t"
+                                     "mov %1, %%sp"
+                                     : "=&r" (orig_sp)
+                                     : "r" (sp));
+                __do_softirq();
+                __asm__ __volatile__("mov %0, %%sp"
+                                     : : "r" (orig_sp));
+        }
+
+        local_irq_restore(flags);
+}
+
 #ifdef CONFIG_HOTPLUG_CPU
 void fixup_irqs(void)
 {