1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
|
/* SPDX-License-Identifier: BSD-2-Clause */
/*
* Copyright (c) 2011-2015 embedded brains GmbH. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtems/score/percpu.h>
#include <rtems/score/nios2-utility.h>
#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
|
