diff options
Diffstat (limited to 'cpukit/score/cpu/microblaze/cpu_asm.S')
| -rw-r--r-- | cpukit/score/cpu/microblaze/cpu_asm.S | 65 |
1 files changed, 8 insertions, 57 deletions
diff --git a/cpukit/score/cpu/microblaze/cpu_asm.S b/cpukit/score/cpu/microblaze/cpu_asm.S index d095e62f69..0a2c5d8fff 100644 --- a/cpukit/score/cpu/microblaze/cpu_asm.S +++ b/cpukit/score/cpu/microblaze/cpu_asm.S @@ -53,9 +53,8 @@ _ISR_Handler: swi r12, r1, MICROBLAZE_INTERRUPT_FRAME_R12 swi r15, r1, MICROBLAZE_INTERRUPT_FRAME_R15 swi r18, r1, MICROBLAZE_INTERRUPT_FRAME_R18 - - xori r3, r5, 0xFFFF - beqi r3, do_exception + mfs r3, rmsr + swi r3, r1, MICROBLAZE_INTERRUPT_FRAME_MSR /* Disable dispatching */ lwi r3, r0, _Per_CPU_Information + 16 @@ -79,7 +78,7 @@ _ISR_Handler: switch_to_interrupt_stack: add r4, r0, r1 lwi r1, r0, _Per_CPU_Information + 4 - addik r1, r1, -52 + addik r1, r1, -(CPU_INTERRUPT_FRAME_SIZE) swi r4, r1, 0 on_interrupt_stack: @@ -116,13 +115,15 @@ after_stack_switch: beqi r3, quick_exit /* Return to interrupted thread and make it do a dispatch */ - addik r14, r0, thread_dispatch - rtid r14, 0 + bralid r15, _Thread_Dispatch nop + /* Fall through to quick exit */ quick_exit: /* Simple return from nested interrupt */ /* Restore registers */ + lwi r3, r1, MICROBLAZE_INTERRUPT_FRAME_MSR + mts rmsr, r3 lwi r3, r1, MICROBLAZE_INTERRUPT_FRAME_R3 lwi r4, r1, MICROBLAZE_INTERRUPT_FRAME_R4 lwi r5, r1, MICROBLAZE_INTERRUPT_FRAME_R5 @@ -138,57 +139,7 @@ quick_exit: lwi r18, r1, MICROBLAZE_INTERRUPT_FRAME_R18 /* Remove stack frame */ - addik r1, r1, 52 + addik r1, r1, CPU_INTERRUPT_FRAME_SIZE rtid r14, 0 nop - -thread_dispatch: - /* Reserve stack */ - addik r1, r1, -52 - /* Save scratch registers */ - swi r3, r1, MICROBLAZE_INTERRUPT_FRAME_R3 - swi r4, r1, MICROBLAZE_INTERRUPT_FRAME_R4 - swi r5, r1, MICROBLAZE_INTERRUPT_FRAME_R5 - swi r6, r1, MICROBLAZE_INTERRUPT_FRAME_R6 - swi r7, r1, MICROBLAZE_INTERRUPT_FRAME_R7 - swi r8, r1, MICROBLAZE_INTERRUPT_FRAME_R8 - swi r9, r1, MICROBLAZE_INTERRUPT_FRAME_R9 - swi r10, r1, MICROBLAZE_INTERRUPT_FRAME_R10 - swi r11, r1, MICROBLAZE_INTERRUPT_FRAME_R11 - swi r12, r1, MICROBLAZE_INTERRUPT_FRAME_R12 - swi r14, r1, MICROBLAZE_INTERRUPT_FRAME_R14 - swi r15, r1, MICROBLAZE_INTERRUPT_FRAME_R15 - swi r18, r1, MICROBLAZE_INTERRUPT_FRAME_R18 - - bralid r15, _Thread_Dispatch - nop - - /* Restore scratch registers */ - lwi r3, r1, MICROBLAZE_INTERRUPT_FRAME_R3 - lwi r4, r1, MICROBLAZE_INTERRUPT_FRAME_R4 - lwi r5, r1, MICROBLAZE_INTERRUPT_FRAME_R5 - lwi r6, r1, MICROBLAZE_INTERRUPT_FRAME_R6 - lwi r7, r1, MICROBLAZE_INTERRUPT_FRAME_R7 - lwi r8, r1, MICROBLAZE_INTERRUPT_FRAME_R8 - lwi r9, r1, MICROBLAZE_INTERRUPT_FRAME_R9 - lwi r10, r1, MICROBLAZE_INTERRUPT_FRAME_R10 - lwi r11, r1, MICROBLAZE_INTERRUPT_FRAME_R11 - lwi r12, r1, MICROBLAZE_INTERRUPT_FRAME_R12 - lwi r14, r1, MICROBLAZE_INTERRUPT_FRAME_R14 - lwi r15, r1, MICROBLAZE_INTERRUPT_FRAME_R15 - lwi r18, r1, MICROBLAZE_INTERRUPT_FRAME_R18 - /* Free stack space */ - addik r1, r1, 52 - - bri quick_exit - -do_exception: - /* exception no longer in progress */ - mfs r3, rmsr - andni r3, r3, 0x200 - mts rmsr, r3 - addi r5, r0, 9 - add r6, r0, r1 - - brai _Terminate |