-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathos_asm.S
189 lines (146 loc) · 2.79 KB
/
os_asm.S
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
#include <avr/io.h>
.section .text
// Task switch interrupt
.global TIMER0_COMPA_vect
TIMER0_COMPA_vect:
push r26
push r27
push r28
push r29
push r30
push r31
// Save SREG
in r26, 0x3F
ldi r31, hi8(sreg_tmp)
ldi r30, lo8(sreg_tmp)
st Z, r26
// Save Instruction + Stack
in r29, 0x3E // SPH
in r28, 0x3D // SPL
adiw YL, 6+2
// Save Stack
ldi r31, hi8(stack_tmp)
ldi r30, lo8(stack_tmp)
st Z+, r28
st Z+, r29
sbiw YL, 1
// Save instruction
ld r26, Y+
ld r27, Y+
ldi r31, hi8(ins_tmp)
ldi r30, lo8(ins_tmp)
st Z+, r27
st Z+, r26
// r26 = i
// r27 = tmp
// Z = register file
// Y = reg_tmp
// Z = 0
ldi r31, 0
ldi r30, 0
ldi r29, hi8(reg_tmp)
ldi r28, lo8(reg_tmp)
ldi r26, 0
_os_interrupt_cpy_next:
ld r27, Z+
st Y+, r27
inc r26
// We can only save the first 26 registers, the rest is used here
cpi r26, 26
breq _os_interrupt_regsaved
rjmp _os_interrupt_cpy_next
_os_interrupt_regsaved:
pop r31
pop r30
pop r29
pop r28
pop r27
pop r26
mov r16, r26
mov r17, r27
mov r18, r28
mov r19, r29
mov r20, r30
mov r21, r31
// Z = register file
// Y = reg_tmp
ldi ZH, 0
ldi ZL, 16
ldi YH, hi8(reg_tmp + 26)
ldi YL, lo8(reg_tmp + 26)
mov r26, 0
// r26 = i
// r27 = tmp
_os_interrupt_cpy_next_2:
ld r27, Z+
st Y+, r27
inc r26
cpi r26, 6
breq _os_interrupt_regsaved_2
rjmp _os_interrupt_cpy_next_2
// Pop last instruction addr
pop r31
_os_interrupt_regsaved_2:
jmp os_interrupt_saved
// Restore task context and jump to task
.global os_asm_switch_to_task
os_asm_switch_to_task:
// Stack
ldi YL, lo8(stack_tmp)
ldi YH, hi8(stack_tmp)
ld ZL, Y+
ld ZH, Y
out 0x3E, ZH // SPH
out 0x3D, ZL // SPL
// Old Instruction
ldi YH, hi8(ins_tmp)
ldi YL, lo8(ins_tmp)
ld ZL, Y+
ld ZH, Y+
push ZL
push ZH
// R30-31
ldi YH, hi8(reg_tmp+30)
ldi YL, lo8(reg_tmp+30)
ld ZH, Y+
push ZH
ld ZH, Y+
push ZH
// SREG
ldi YL, lo8(sreg_tmp)
ldi YH, hi8(sreg_tmp)
ld ZL, Y
push ZL
// R26-R29
ldi YL, lo8(reg_tmp+26)
ldi YH, hi8(reg_tmp+26)
ldi ZL, 0
_reg_copy1_next:
cpi ZL, 4
breq _reg_copy1_finished
ld ZH, Y+
push ZH
inc ZL
jmp _reg_copy1_next
_reg_copy1_finished:
// R0-R25
ldi YL, lo8(reg_tmp)
ldi YH, hi8(reg_tmp)
ldi ZL, 0
ldi ZH, 0
_reg_copy2_next:
cpi ZL, 26
brge _reg_copy2_finished
ld r26, Y+
st Z+, r26
rjmp _reg_copy2_next
_reg_copy2_finished:
pop r29
pop r28
pop r27
pop r26
pop r31
out 0x3F, r31 // SREG
pop r31
pop r30
reti