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
|
/*
* 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.
*
* $Id$
*/
#include <rtems/score/percpu.h>
#define FRAME_OFFSET_AT 0
#define FRAME_OFFSET_R2 4
#define FRAME_OFFSET_R3 8
#define FRAME_OFFSET_R4 12
#define FRAME_OFFSET_R5 16
#define FRAME_OFFSET_R6 20
#define FRAME_OFFSET_R7 24
#define FRAME_OFFSET_R8 28
#define FRAME_OFFSET_R9 32
#define FRAME_OFFSET_R10 36
#define FRAME_OFFSET_R11 40
#define FRAME_OFFSET_R12 44
#define FRAME_OFFSET_R13 48
#define FRAME_OFFSET_R14 52
#define FRAME_OFFSET_R15 56
#define FRAME_OFFSET_RA 60
#define FRAME_OFFSET_EA 64
#define FRAME_OFFSET_ESTATUS 68
#define FRAME_OFFSET_R16 72
#define FRAME_SIZE (FRAME_OFFSET_R16 + 4)
.set noat
.section .text
.extern _Per_CPU_Information
.extern _Thread_Dispatch_disable_level
.globl _Nios2_ISR_Dispatch_with_shadow_preemptive
_Nios2_ISR_Dispatch_with_shadow_preemptive:
/* Obtain stack frame */
subi sp, sp, FRAME_SIZE
/* Save volatile registers */
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)
/* Save context */
rdctl r2, estatus
subi ea, ea, 4
stw ra, FRAME_OFFSET_RA(sp)
stw ea, FRAME_OFFSET_EA(sp)
stw r2, FRAME_OFFSET_ESTATUS(sp)
/* Save one non-volatile register for further usage */
stw r16, FRAME_OFFSET_R16(sp)
/* Save stack pointer */
mov r16, sp
/* Increment ISR nest level and thread dispatch disable level */
ldw r9, %gprel(_Per_CPU_Information + PER_CPU_ISR_NEST_LEVEL)(gp)
ldw r10, %gprel(_Thread_Dispatch_disable_level)(gp)
addi r11, r9, 1
addi r10, r10, 1
stw r11, %gprel(_Per_CPU_Information + PER_CPU_ISR_NEST_LEVEL)(gp)
stw r10, %gprel(_Thread_Dispatch_disable_level)(gp)
/* Switch to interrupt stack if necessary */
bne r9, zero, switch_to_interrupt_stack_done
ldw sp, %gprel(_Per_CPU_Information + PER_CPU_INTERRUPT_STACK_HIGH)(gp)
switch_to_interrupt_stack_done:
/* Load high level handler address and argument */
ldw r12, 4(et)
ldw r4, 8(et)
/* Enable interrupts */
rdctl r13, status
orhi r13, r13, 0x0080
wrctl status, r13
/* Call high level handler with argument */
callr r12
/* Disable interrupts */
rdctl r12, status
movhi r13, 0xff80
subi r13, r13, 1
and r12, r12, r13
wrctl status, r12
/* Decrement ISR nest level and thread dispatch disable level */
ldw r9, %gprel(_Per_CPU_Information + PER_CPU_ISR_NEST_LEVEL)(gp)
ldw r10, %gprel(_Thread_Dispatch_disable_level)(gp)
subi r9, r9, 1
subi r10, r10, 1
stw r9, %gprel(_Per_CPU_Information + PER_CPU_ISR_NEST_LEVEL)(gp)
stw r10, %gprel(_Thread_Dispatch_disable_level)(gp)
/*
* Restore stack pointer. If the ISR nest level is greater than one,
* then this is a nop, else we switch back to the thread stack.
*/
mov sp, r16
/* Thread dispatch */
bne r10, zero, thread_dispatch_done
call _Thread_Dispatch
thread_dispatch_done:
/* Restore volatile registers */
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)
ldw r14, FRAME_OFFSET_R14(sp)
ldw r15, FRAME_OFFSET_R15(sp)
/* Restore context */
ldw ra, FRAME_OFFSET_RA(sp)
ldw ea, FRAME_OFFSET_EA(sp)
ldw et, FRAME_OFFSET_ESTATUS(sp)
/* Restore the non-volatile register */
ldw r16, FRAME_OFFSET_R16(sp)
/* Release stack frame */
addi sp, sp, FRAME_SIZE
/* Restore context */
wrctl estatus, et
/* Return */
eret
|
