/* * Copyright (c) 2011-2015 embedded brains GmbH. All rights reserved. * * embedded brains GmbH * Dornierstr. 4 * 82178 Puchheim * Germany * * * The license and distribution terms for this file may be * found in the file LICENSE in this distribution or at * http://www.rtems.org/license/LICENSE. */ #include #include #define FRAME_OFFSET_RA 0 #define FRAME_OFFSET_AT 4 #define FRAME_OFFSET_R2 8 #define FRAME_OFFSET_R3 12 #define FRAME_OFFSET_R4 16 #define FRAME_OFFSET_R5 20 #define FRAME_OFFSET_R6 24 #define FRAME_OFFSET_R7 28 #define FRAME_OFFSET_R8 32 #define FRAME_OFFSET_R9 36 #define FRAME_OFFSET_R10 40 #define FRAME_OFFSET_R11 44 #define FRAME_OFFSET_R12 48 #define FRAME_OFFSET_R13 52 #define FRAME_OFFSET_R14 56 #define FRAME_OFFSET_R15 60 #define FRAME_OFFSET_STATUS 64 #define FRAME_OFFSET_EA 68 #define FRAME_SIZE (FRAME_OFFSET_EA + 4) .set noat .set nobreak .section .text .extern _Per_CPU_Information .extern _Thread_Dispatch_disable_level .extern _Nios2_Thread_dispatch_disabled .extern _Nios2_ISR_Status_interrupts_disabled .globl _Nios2_ISR_Dispatch_with_shadow_non_preemptive _Nios2_ISR_Dispatch_with_shadow_non_preemptive: /* Load thread dispatch disable level */ ldw r16, %gprel(_Thread_Dispatch_disable_level)(gp) /* Load high level handler address and argument */ ldw r8, 4(et) ldw r4, 8(et) /* Increment and store thread dispatch disable level */ addi r9, r16, 1 stw r9, %gprel(_Thread_Dispatch_disable_level)(gp) /* Call high level handler with argument */ callr r8 /* Load thread dispatch necessary */ ldb r12, %gprel(_Per_CPU_Information + PER_CPU_DISPATCH_NEEDED)(gp) /* Load Nios II specific thread dispatch disabled */ ldw r13, %gprel(_Nios2_Thread_dispatch_disabled)(gp) /* Read status */ rdctl r14, status /* Fix return address */ subi ea, ea, 4 /* * Restore the thread dispatch disable level. We must do this before * we return to the normal register set, because otherwise we have * problems if someone deletes or restarts the interrupted thread while * we are in the thread dispatch helper. */ stw r16, %gprel(_Thread_Dispatch_disable_level)(gp) /* Is thread dispatch allowed? */ bne r16, zero, no_thread_dispatch /* Is thread dispatch necessary? */ beq r12, zero, no_thread_dispatch /* Is outermost interrupt? */ andhi r14, r14, 0x3f bne r14, zero, no_thread_dispatch /* Is Nios II specific thread dispatch allowed? */ bne r13, zero, no_thread_dispatch /* Obtain stack frame in normal register set */ rdprs r15, sp, -FRAME_SIZE /* Disable Nios II specific thread dispatch */ stw r12, %gprel(_Nios2_Thread_dispatch_disabled)(gp) /* Save context */ stw sstatus, FRAME_OFFSET_STATUS(r15) stw ea, FRAME_OFFSET_EA(r15) /* Set thread dispatch helper address */ movhi ea, %hiadj(thread_dispatch_helper) addi ea, ea, %lo(thread_dispatch_helper) /* Update stack pointer in normal register set */ wrprs sp, r15 no_thread_dispatch: /* * Return to thread dispatch helper, interrupted thread or interrupted * lower level interrupt service routine. */ eret thread_dispatch_helper: /* This code executes in the context of the interrupted thread */ /* Save volatile registers */ stw ra, FRAME_OFFSET_RA(sp) stw at, FRAME_OFFSET_AT(sp) stw r2, FRAME_OFFSET_R2(sp) stw r3, FRAME_OFFSET_R3(sp) stw r4, FRAME_OFFSET_R4(sp) stw r5, FRAME_OFFSET_R5(sp) stw r6, FRAME_OFFSET_R6(sp) stw r7, FRAME_OFFSET_R7(sp) stw r8, FRAME_OFFSET_R8(sp) stw r9, FRAME_OFFSET_R9(sp) stw r10, FRAME_OFFSET_R10(sp) stw r11, FRAME_OFFSET_R11(sp) stw r12, FRAME_OFFSET_R12(sp) stw r13, FRAME_OFFSET_R13(sp) stw r14, FRAME_OFFSET_R14(sp) stw r15, FRAME_OFFSET_R15(sp) do_thread_dispatch: call _Thread_Dispatch /* Restore some volatile registers */ ldw ra, FRAME_OFFSET_RA(sp) ldw at, FRAME_OFFSET_AT(sp) ldw r2, FRAME_OFFSET_R2(sp) ldw r3, FRAME_OFFSET_R3(sp) ldw r4, FRAME_OFFSET_R4(sp) ldw r5, FRAME_OFFSET_R5(sp) ldw r6, FRAME_OFFSET_R6(sp) ldw r7, FRAME_OFFSET_R7(sp) ldw r8, FRAME_OFFSET_R8(sp) ldw r9, FRAME_OFFSET_R9(sp) ldw r10, FRAME_OFFSET_R10(sp) ldw r11, FRAME_OFFSET_R11(sp) ldw r12, FRAME_OFFSET_R12(sp) /* * Disable interrupts. * * We have the following invariants: * 1. status.RSIE == 0: thread context initialization * 2. status.CRS == 0: thread context initialization * 3. status.PRS: arbitrary * 4. status.IL < interrupt disable IL: else we would not be here * 5. status.IH == 0: thread context initialization * 6. status.U == 0: thread context initialization * 7. status.PIE == 1: thread context initialization * Thus we can use a constant to disable interrupts. */ rdctl r14, status movi r15, %lo(_Nios2_ISR_Status_interrupts_disabled) wrctl status, r15 /* Load thread dispatch necessary */ ldb r13, %gprel(_Per_CPU_Information + PER_CPU_DISPATCH_NEEDED)(gp) /* Is thread dispatch necessary? */ bne r13, zero, enable_interrupts_before_thread_dispatch /* Enable Nios II specific thread dispatch */ stw zero, %gprel(_Nios2_Thread_dispatch_disabled)(gp) /* Restore remaining volatile register */ ldw r13, FRAME_OFFSET_R13(sp) ldw r14, FRAME_OFFSET_R14(sp) ldw r15, FRAME_OFFSET_R15(sp) /* Restore context */ ldw et, FRAME_OFFSET_STATUS(sp) ldw ea, FRAME_OFFSET_EA(sp) /* Release stack frame */ addi sp, sp, FRAME_SIZE /* Restore context */ wrctl estatus, et /* Return to interrupted thread */ eret enable_interrupts_before_thread_dispatch: /* Restore status */ wrctl status, r14 br do_thread_dispatch