Merge branch 'master' of git://git.denx.de/u-boot-samsung

[platform/kernel/u-boot.git] / arch / arm / lib / interrupts.c

diff --git a/arch/arm/lib/interrupts.c b/arch/arm/lib/interrupts.c
index ed83043..80869ad 100644 (file)
--- a/arch/arm/lib/interrupts.c
+++ b/arch/arm/lib/interrupts.c
@@ -26,75 +26,6 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
-#ifdef CONFIG_USE_IRQ
-int interrupt_init (void)
-{
-       unsigned long cpsr;
-
-       /*
-        * setup up stacks if necessary
-        */
-       IRQ_STACK_START = gd->irq_sp - 4;
-       IRQ_STACK_START_IN = gd->irq_sp + 8;
-       FIQ_STACK_START = IRQ_STACK_START - CONFIG_STACKSIZE_IRQ;
-
-
-       __asm__ __volatile__("mrs %0, cpsr\n"
-                            : "=r" (cpsr)
-                            :
-                            : "memory");
-
-       __asm__ __volatile__("msr cpsr_c, %0\n"
-                            "mov sp, %1\n"
-                            :
-                            : "r" (IRQ_MODE | I_BIT | F_BIT | (cpsr & ~FIQ_MODE)),
-                              "r" (IRQ_STACK_START)
-                            : "memory");
-
-       __asm__ __volatile__("msr cpsr_c, %0\n"
-                            "mov sp, %1\n"
-                            :
-                            : "r" (FIQ_MODE | I_BIT | F_BIT | (cpsr & ~IRQ_MODE)),
-                              "r" (FIQ_STACK_START)
-                            : "memory");
-
-       __asm__ __volatile__("msr cpsr_c, %0"
-                            :
-                            : "r" (cpsr)
-                            : "memory");
-
-       return arch_interrupt_init();
-}
-
-/* enable IRQ interrupts */
-void enable_interrupts (void)
-{
-       unsigned long temp;
-       __asm__ __volatile__("mrs %0, cpsr\n"
-                            "bic %0, %0, #0x80\n"
-                            "msr cpsr_c, %0"
-                            : "=r" (temp)
-                            :
-                            : "memory");
-}
-
-
-/*
- * disable IRQ/FIQ interrupts
- * returns true if interrupts had been enabled before we disabled them
- */
-int disable_interrupts (void)
-{
-       unsigned long old,temp;
-       __asm__ __volatile__("mrs %0, cpsr\n"
-                            "orr %1, %0, #0xc0\n"
-                            "msr cpsr_c, %1"
-                            : "=r" (old), "=r" (temp)
-                            :
-                            : "memory");
-       return (old & 0x80) == 0;
-}
-#else
 int interrupt_init (void)
 {
        /*
@@ -113,8 +44,6 @@ int disable_interrupts (void)
 {
        return 0;
 }
-#endif
-
 
 void bad_mode (void)
 {
@@ -164,10 +93,18 @@ void show_regs (struct pt_regs *regs)
                thumb_mode (regs) ? " (T)" : "");
 }
 
+/* fixup PC to point to the instruction leading to the exception */
+static inline void fixup_pc(struct pt_regs *regs, int offset)
+{
+       uint32_t pc = instruction_pointer(regs) + offset;
+       regs->ARM_pc = pc | (regs->ARM_pc & PCMASK);
+}
+
 void do_undefined_instruction (struct pt_regs *pt_regs)
 {
        efi_restore_gd();
        printf ("undefined instruction\n");
+       fixup_pc(pt_regs, -4);
        show_regs (pt_regs);
        bad_mode ();
 }
@@ -176,6 +113,7 @@ void do_software_interrupt (struct pt_regs *pt_regs)
 {
        efi_restore_gd();
        printf ("software interrupt\n");
+       fixup_pc(pt_regs, -4);
        show_regs (pt_regs);
        bad_mode ();
 }
@@ -184,6 +122,7 @@ void do_prefetch_abort (struct pt_regs *pt_regs)
 {
        efi_restore_gd();
        printf ("prefetch abort\n");
+       fixup_pc(pt_regs, -8);
        show_regs (pt_regs);
        bad_mode ();
 }
@@ -192,6 +131,7 @@ void do_data_abort (struct pt_regs *pt_regs)
 {
        efi_restore_gd();
        printf ("data abort\n");
+       fixup_pc(pt_regs, -8);
        show_regs (pt_regs);
        bad_mode ();
 }
@@ -200,6 +140,7 @@ void do_not_used (struct pt_regs *pt_regs)
 {
        efi_restore_gd();
        printf ("not used\n");
+       fixup_pc(pt_regs, -8);
        show_regs (pt_regs);
        bad_mode ();
 }
@@ -208,16 +149,16 @@ void do_fiq (struct pt_regs *pt_regs)
 {
        efi_restore_gd();
        printf ("fast interrupt request\n");
+       fixup_pc(pt_regs, -8);
        show_regs (pt_regs);
        bad_mode ();
 }
 
-#ifndef CONFIG_USE_IRQ
 void do_irq (struct pt_regs *pt_regs)
 {
        efi_restore_gd();
        printf ("interrupt request\n");
+       fixup_pc(pt_regs, -8);
        show_regs (pt_regs);
        bad_mode ();
 }
-#endif