/* * 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 _Nios2_ISR_Status_interrupts_disabled .globl _Nios2_ISR_Dispatch_with_shadow_register_set _Nios2_ISR_Dispatch_with_shadow_register_set: /* Load thread dispatch disable level */ ldw r16, %gprel(_Per_CPU_Information + PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL)(gp) /* Read status */ rdctl r18, status /* Load high level handler address and argument */ ldw r8, 4(et) ldw r4, 8(et) /* Increment and store thread dispatch disable level */ addi r17, r16, 1 stw r17, %gprel(_Per_CPU_Information + PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL)(gp) /* * Enable higher level interrupts. This is safe since status.RSIE is * always 0 and thread dispatching is disabled right above. Higher * priority interrupts shall not share shadow register sets with lower * priority interrupts. */ ori r5, r18, 1 wrctl status, r5 /* Call high level handler with argument */ callr r8 /* Load the thread dispatch necessary indicator */ ldb r12, %gprel(_Per_CPU_Information + PER_CPU_DISPATCH_NEEDED)(gp) /* Load the thread dispatch after ISR disable indicator */ ldw r13, %gprel(_Per_CPU_Information + PER_CPU_ISR_DISPATCH_DISABLE)(gp) /* Fix return address */ subi ea, ea, 4 /* * If the current thread dispatch disable level (r17) is one, then * negate the thread dispatch necessary indicator, otherwise the value * is irrelevant. Or it with the previous thread dispatch disable * level value (r16). The r15 which will be used as a status to * determine if a thread dispatch is necessary and allowed. */ xor r12, r17, r12 or r15, r12, r16 /* * Get the previous register set from r18. If it is zero, then this is * the outermost interrupt. Or it to the thread dispatch status (r15). */ andhi r12, r18, 0x3f or r15, r12, r15 /* * Or the thread dispatch after ISR disable indicator (r13) to the * thread dispatch status (r15). */ or r15, r13, r15 /* Is a thread dispatch necessary and allowed? */ bne r15, zero, no_thread_dispatch /* Obtain stack frame in normal register set */ rdprs r15, sp, -FRAME_SIZE /* Disable thread dispatch after ISR */ stw r17, %gprel(_Per_CPU_Information + PER_CPU_ISR_DISPATCH_DISABLE)(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 /* Jump to thread dispatch helper */ eret no_thread_dispatch: /* Restore the thread dispatch disable level */ stw r16, %gprel(_Per_CPU_Information + PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL)(gp) /* Return to interrupted context */ 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) /* * Disable interrupts (1). * * 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. */ movi r5, %lo(_Nios2_ISR_Status_interrupts_disabled) wrctl status, r5 do_thread_dispatch: addi r4, gp, %gprel(_Per_CPU_Information) call _Thread_Do_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, see (1) */ 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, prepare_thread_dispatch /* Enable thread dispatch after ISR */ stw zero, %gprel(_Per_CPU_Information + PER_CPU_ISR_DISPATCH_DISABLE)(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 prepare_thread_dispatch: /* Disable thread dispatching */ movi r4, 1 stw r4, %gprel(_Per_CPU_Information + PER_CPU_ISR_DISPATCH_DISABLE)(gp) stw r4, %gprel(_Per_CPU_Information + PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL)(gp) /* Set interrupt level argument for _Thread_Do_dispatch() */ mov r5, r15 br do_thread_dispatch