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
|
/*
* Copyright (c) 2011 embedded brains GmbH. All rights reserved.
*
* embedded brains GmbH
* Obere Lagerstr. 30
* 82178 Puchheim
* Germany
* <rtems@embedded-brains.de>
*
* The license and distribution terms for this file may be
* found in the file LICENSE in this distribution or at
* http://www.rtems.com/license/LICENSE.
*/
#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
.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)
ldw r13, FRAME_OFFSET_R13(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 r12, %gprel(_Per_CPU_Information + PER_CPU_DISPATCH_NEEDED)(gp)
/* Is thread dispatch necessary? */
bne r12, 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 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
|
