1 | /* |
---|
2 | * Copyright (c) 2011 embedded brains GmbH. All rights reserved. |
---|
3 | * |
---|
4 | * embedded brains GmbH |
---|
5 | * Obere Lagerstr. 30 |
---|
6 | * 82178 Puchheim |
---|
7 | * Germany |
---|
8 | * <rtems@embedded-brains.de> |
---|
9 | * |
---|
10 | * The license and distribution terms for this file may be |
---|
11 | * found in the file LICENSE in this distribution or at |
---|
12 | * http://www.rtems.org/license/LICENSE. |
---|
13 | */ |
---|
14 | |
---|
15 | #include <rtems/score/percpu.h> |
---|
16 | |
---|
17 | #define FRAME_OFFSET_AT 0 |
---|
18 | #define FRAME_OFFSET_R2 4 |
---|
19 | #define FRAME_OFFSET_R3 8 |
---|
20 | #define FRAME_OFFSET_R4 12 |
---|
21 | #define FRAME_OFFSET_R5 16 |
---|
22 | #define FRAME_OFFSET_R6 20 |
---|
23 | #define FRAME_OFFSET_R7 24 |
---|
24 | #define FRAME_OFFSET_R8 28 |
---|
25 | #define FRAME_OFFSET_R9 32 |
---|
26 | #define FRAME_OFFSET_R10 36 |
---|
27 | #define FRAME_OFFSET_R11 40 |
---|
28 | #define FRAME_OFFSET_R12 44 |
---|
29 | #define FRAME_OFFSET_R13 48 |
---|
30 | #define FRAME_OFFSET_R14 52 |
---|
31 | #define FRAME_OFFSET_R15 56 |
---|
32 | #define FRAME_OFFSET_RA 60 |
---|
33 | #define FRAME_OFFSET_EA 64 |
---|
34 | #define FRAME_OFFSET_ESTATUS 68 |
---|
35 | #define FRAME_OFFSET_R16 72 |
---|
36 | |
---|
37 | #define FRAME_SIZE (FRAME_OFFSET_R16 + 4) |
---|
38 | |
---|
39 | .set noat |
---|
40 | .section .text |
---|
41 | |
---|
42 | .extern _Per_CPU_Information |
---|
43 | .extern _Thread_Dispatch_disable_level |
---|
44 | |
---|
45 | .globl _Nios2_ISR_Dispatch_with_shadow_preemptive |
---|
46 | |
---|
47 | _Nios2_ISR_Dispatch_with_shadow_preemptive: |
---|
48 | |
---|
49 | /* Obtain stack frame */ |
---|
50 | subi sp, sp, FRAME_SIZE |
---|
51 | |
---|
52 | /* Save volatile registers */ |
---|
53 | stw at, FRAME_OFFSET_AT(sp) |
---|
54 | stw r2, FRAME_OFFSET_R2(sp) |
---|
55 | stw r3, FRAME_OFFSET_R3(sp) |
---|
56 | stw r4, FRAME_OFFSET_R4(sp) |
---|
57 | stw r5, FRAME_OFFSET_R5(sp) |
---|
58 | stw r6, FRAME_OFFSET_R6(sp) |
---|
59 | stw r7, FRAME_OFFSET_R7(sp) |
---|
60 | stw r8, FRAME_OFFSET_R8(sp) |
---|
61 | stw r9, FRAME_OFFSET_R9(sp) |
---|
62 | stw r10, FRAME_OFFSET_R10(sp) |
---|
63 | stw r11, FRAME_OFFSET_R11(sp) |
---|
64 | stw r12, FRAME_OFFSET_R12(sp) |
---|
65 | stw r13, FRAME_OFFSET_R13(sp) |
---|
66 | stw r14, FRAME_OFFSET_R14(sp) |
---|
67 | stw r15, FRAME_OFFSET_R15(sp) |
---|
68 | |
---|
69 | /* Save context */ |
---|
70 | rdctl r2, estatus |
---|
71 | subi ea, ea, 4 |
---|
72 | stw ra, FRAME_OFFSET_RA(sp) |
---|
73 | stw ea, FRAME_OFFSET_EA(sp) |
---|
74 | stw r2, FRAME_OFFSET_ESTATUS(sp) |
---|
75 | |
---|
76 | /* Save one non-volatile register for further usage */ |
---|
77 | stw r16, FRAME_OFFSET_R16(sp) |
---|
78 | |
---|
79 | /* Save stack pointer */ |
---|
80 | mov r16, sp |
---|
81 | |
---|
82 | /* Increment ISR nest level and thread dispatch disable level */ |
---|
83 | ldw r9, %gprel(_Per_CPU_Information + PER_CPU_ISR_NEST_LEVEL)(gp) |
---|
84 | ldw r10, %gprel(_Thread_Dispatch_disable_level)(gp) |
---|
85 | addi r11, r9, 1 |
---|
86 | addi r10, r10, 1 |
---|
87 | stw r11, %gprel(_Per_CPU_Information + PER_CPU_ISR_NEST_LEVEL)(gp) |
---|
88 | stw r10, %gprel(_Thread_Dispatch_disable_level)(gp) |
---|
89 | |
---|
90 | /* Switch to interrupt stack if necessary */ |
---|
91 | bne r9, zero, switch_to_interrupt_stack_done |
---|
92 | ldw sp, %gprel(_Per_CPU_Information + PER_CPU_INTERRUPT_STACK_HIGH)(gp) |
---|
93 | |
---|
94 | switch_to_interrupt_stack_done: |
---|
95 | |
---|
96 | /* Load high level handler address and argument */ |
---|
97 | ldw r12, 4(et) |
---|
98 | ldw r4, 8(et) |
---|
99 | |
---|
100 | /* Enable interrupts */ |
---|
101 | rdctl r13, status |
---|
102 | orhi r13, r13, 0x0080 |
---|
103 | wrctl status, r13 |
---|
104 | |
---|
105 | /* Call high level handler with argument */ |
---|
106 | callr r12 |
---|
107 | |
---|
108 | /* Disable interrupts */ |
---|
109 | rdctl r12, status |
---|
110 | movhi r13, 0xff80 |
---|
111 | subi r13, r13, 1 |
---|
112 | and r12, r12, r13 |
---|
113 | wrctl status, r12 |
---|
114 | |
---|
115 | /* Decrement ISR nest level and thread dispatch disable level */ |
---|
116 | ldw r9, %gprel(_Per_CPU_Information + PER_CPU_ISR_NEST_LEVEL)(gp) |
---|
117 | ldw r10, %gprel(_Thread_Dispatch_disable_level)(gp) |
---|
118 | subi r9, r9, 1 |
---|
119 | subi r10, r10, 1 |
---|
120 | stw r9, %gprel(_Per_CPU_Information + PER_CPU_ISR_NEST_LEVEL)(gp) |
---|
121 | stw r10, %gprel(_Thread_Dispatch_disable_level)(gp) |
---|
122 | |
---|
123 | /* |
---|
124 | * Restore stack pointer. If the ISR nest level is greater than one, |
---|
125 | * then this is a nop, else we switch back to the thread stack. |
---|
126 | */ |
---|
127 | mov sp, r16 |
---|
128 | |
---|
129 | /* Thread dispatch */ |
---|
130 | bne r10, zero, thread_dispatch_done |
---|
131 | call _Thread_Dispatch |
---|
132 | |
---|
133 | thread_dispatch_done: |
---|
134 | |
---|
135 | /* Restore volatile registers */ |
---|
136 | ldw at, FRAME_OFFSET_AT(sp) |
---|
137 | ldw r2, FRAME_OFFSET_R2(sp) |
---|
138 | ldw r3, FRAME_OFFSET_R3(sp) |
---|
139 | ldw r4, FRAME_OFFSET_R4(sp) |
---|
140 | ldw r5, FRAME_OFFSET_R5(sp) |
---|
141 | ldw r6, FRAME_OFFSET_R6(sp) |
---|
142 | ldw r7, FRAME_OFFSET_R7(sp) |
---|
143 | ldw r8, FRAME_OFFSET_R8(sp) |
---|
144 | ldw r9, FRAME_OFFSET_R9(sp) |
---|
145 | ldw r10, FRAME_OFFSET_R10(sp) |
---|
146 | ldw r11, FRAME_OFFSET_R11(sp) |
---|
147 | ldw r12, FRAME_OFFSET_R12(sp) |
---|
148 | ldw r13, FRAME_OFFSET_R13(sp) |
---|
149 | ldw r14, FRAME_OFFSET_R14(sp) |
---|
150 | ldw r15, FRAME_OFFSET_R15(sp) |
---|
151 | |
---|
152 | /* Restore context */ |
---|
153 | ldw ra, FRAME_OFFSET_RA(sp) |
---|
154 | ldw ea, FRAME_OFFSET_EA(sp) |
---|
155 | ldw et, FRAME_OFFSET_ESTATUS(sp) |
---|
156 | |
---|
157 | /* Restore the non-volatile register */ |
---|
158 | ldw r16, FRAME_OFFSET_R16(sp) |
---|
159 | |
---|
160 | /* Release stack frame */ |
---|
161 | addi sp, sp, FRAME_SIZE |
---|
162 | |
---|
163 | /* Restore context */ |
---|
164 | wrctl estatus, et |
---|
165 | |
---|
166 | /* Return */ |
---|
167 | eret |
---|