diff options
| -rw-r--r-- | arch/avr32/kernel/entry-avr32b.S | 28 |
1 files changed, 25 insertions, 3 deletions
diff --git a/arch/avr32/kernel/entry-avr32b.S b/arch/avr32/kernel/entry-avr32b.S index 5f5f7e42f51..484e08310f1 100644 --- a/arch/avr32/kernel/entry-avr32b.S +++ b/arch/avr32/kernel/entry-avr32b.S @@ -340,12 +340,34 @@ do_bus_error_read: do_nmi_ll: sub sp, 4 stmts --sp, r0-lr - /* FIXME: Make sure RAR_NMI and RSR_NMI are pushed instead of *_EX */ - rcall save_full_context_ex + mfsr r9, SYSREG_RSR_NMI + mfsr r8, SYSREG_RAR_NMI + bfextu r0, r9, MODE_SHIFT, 3 + brne 2f + +1: pushm r8, r9 /* PC and SR */ mfsr r12, SYSREG_ECR mov r11, sp rcall do_nmi - rjmp bad_return + popm r8-r9 + mtsr SYSREG_RAR_NMI, r8 + tst r0, r0 + mtsr SYSREG_RSR_NMI, r9 + brne 3f + + ldmts sp++, r0-lr + sub sp, -4 /* skip r12_orig */ + rete + +2: sub r10, sp, -(FRAME_SIZE_FULL - REG_LR) + stdsp sp[4], r10 /* replace saved SP */ + rjmp 1b + +3: popm lr + sub sp, -4 /* skip sp */ + popm r0-r12 + sub sp, -4 /* skip r12_orig */ + rete handle_address_fault: sub sp, 4 |