1 /*
2 * Copyright (c) 2012-2015 Travis Geiselbrecht
3 *
4 * Permission is hereby granted, free of charge, to any person obtaining
5 * a copy of this software and associated documentation files
6 * (the "Software"), to deal in the Software without restriction,
7 * including without limitation the rights to use, copy, modify, merge,
8 * publish, distribute, sublicense, and/or sell copies of the Software,
9 * and to permit persons to whom the Software is furnished to do so,
10 * subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
18 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
19 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
20 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
21 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22 */
23 #include <sys/types.h>
24 #include <string.h>
25 #include <stdlib.h>
26 #include <debug.h>
27 #include <trace.h>
28 #include <assert.h>
29 #include <kernel/thread.h>
30 #include <arch/arm.h>
31 #include <arch/arm/cm.h>
32
33 #define LOCAL_TRACE 0
34
35 struct arm_cm_context_switch_frame {
36 #if (__CORTEX_M >= 0x03)
37 uint32_t r4;
38 uint32_t r5;
39 uint32_t r6;
40 uint32_t r7;
41 uint32_t r8;
42 uint32_t r9;
43 uint32_t r10;
44 uint32_t r11;
45 uint32_t lr;
46 #else
47 /* frame format is slightly different due to ordering of push/pops */
48 uint32_t r8;
49 uint32_t r9;
50 uint32_t r10;
51 uint32_t r11;
52 uint32_t r4;
53 uint32_t r5;
54 uint32_t r6;
55 uint32_t r7;
56 uint32_t lr;
57 #endif
58 };
59
60 /* macros for saving and restoring a context switch frame, depending on what version of
61 * the architecture you are */
62 #if (__CORTEX_M >= 0x03)
63
64 /* cortex-m3 and above (armv7-m) */
65 #define SAVE_REGS "push { r4-r11, lr };"
66 #define RESTORE_REGS "pop { r4-r11, lr };"
67 #define RESTORE_REGS_PC "pop { r4-r11, pc };"
68 #define SAVE_SP(basereg, tempreg, offset) "str sp, [" #basereg "," #offset "];"
69 #define LOAD_SP(basereg, tempreg, offset) "ldr sp, [" #basereg "," #offset "];"
70 #define CLREX "clrex;"
71
72 #else
73
74 /* cortex-m0 and cortex-m0+ (armv6-m) */
75 #define SAVE_REGS \
76 "push { r4-r7, lr };" \
77 "mov r4, r8;" \
78 "mov r5, r9;" \
79 "mov r6, r10;" \
80 "mov r7, r11;" \
81 "push { r4-r7 };"
82 #define RESTORE_REGS \
83 "pop { r4-r7 };" \
84 "mov r8 , r4;" \
85 "mov r9 , r5;" \
86 "mov r10, r6;" \
87 "mov r11, r7;" \
88 "pop { r4-r7 };" \
89 "pop { r0 };" \
90 "mov lr, r0;" /* NOTE: trashes r0 */
91 #define RESTORE_REGS_PC \
92 "pop { r4-r7 };" \
93 "mov r8 , r4;" \
94 "mov r9 , r5;" \
95 "mov r10, r6;" \
96 "mov r11, r7;" \
97 "pop { r4-r7, pc };"
98 #define SAVE_SP(basereg, tempreg, offset) \
99 "mov " #tempreg ", sp;" \
100 "str " #tempreg ", [" #basereg "," #offset "];"
101 #define LOAD_SP(basereg, tempreg, offset) \
102 "ldr " #tempreg ", [" #basereg "," #offset "];" \
103 "mov sp, " #tempreg ";"
104
105 /* there is no clrex on armv6m devices */
106 #define CLREX ""
107
108 #endif
109
110 /* since we're implicitly uniprocessor, store a pointer to the current thread here */
111 thread_t *_current_thread;
112
113 static void initial_thread_func(void) __NO_RETURN;
initial_thread_func(void)114 static void initial_thread_func(void)
115 {
116 int ret;
117
118 LTRACEF("thread %p calling %p with arg %p\n", _current_thread, _current_thread->entry, _current_thread->arg);
119 #if LOCAL_TRACE
120 dump_thread(_current_thread);
121 #endif
122
123 /* release the thread lock that was implicitly held across the reschedule */
124 thread_unlock_ints_disabled();
125 arch_enable_ints();
126
127 ret = _current_thread->entry(_current_thread->arg);
128
129 LTRACEF("thread %p exiting with %d\n", _current_thread, ret);
130
131 thread_exit(ret);
132 }
133
arch_thread_initialize(struct thread * t)134 void arch_thread_initialize(struct thread *t)
135 {
136 LTRACEF("thread %p, stack %p\n", t, t->stack);
137
138 /* find the top of the stack and align it on an 8 byte boundary */
139 uint32_t *sp = (void *)round_down((vaddr_t)t->stack + t->stack_size, 8);
140
141 struct arm_cm_context_switch_frame *frame = (void *)sp;
142 frame--;
143
144 /* arrange for lr to point to our starting routine */
145 frame->lr = (uint32_t)&initial_thread_func;
146
147 t->arch.sp = (addr_t)frame;
148 t->arch.was_preempted = false;
149
150 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
151 /* zero the fpu register state */
152 memset(t->arch.fpregs, 0, sizeof(t->arch.fpregs));
153 t->arch.fpused = false;
154 #endif
155 }
156
157 static volatile struct arm_cm_exception_frame_long *preempt_frame;
158
pendsv(struct arm_cm_exception_frame_long * frame)159 static void pendsv(struct arm_cm_exception_frame_long *frame)
160 {
161 arch_disable_ints();
162
163 LTRACEF("preempting thread %p (%s)\n", _current_thread, _current_thread->name);
164
165 /* save the iframe the pendsv fired on and hit the preemption code */
166 preempt_frame = frame;
167 thread_preempt();
168
169 LTRACEF("fell through\n");
170
171 /* if we got here, there wasn't anything to switch to, so just fall through and exit */
172 preempt_frame = NULL;
173
174 arch_enable_ints();
175 }
176
177 /*
178 * raw pendsv exception handler, triggered by interrupt glue to schedule
179 * a preemption check.
180 */
_pendsv(void)181 __NAKED void _pendsv(void)
182 {
183 __asm__ volatile(
184 SAVE_REGS
185 "mov r0, sp;"
186 "bl %c0;"
187 RESTORE_REGS_PC
188 :: "i" (pendsv)
189 );
190 __UNREACHABLE;
191 }
192 /*
193 * svc handler, used to hard switch the cpu into exception mode to return
194 * to preempted thread.
195 */
_svc(void)196 __NAKED void _svc(void)
197 {
198 __asm__ volatile(
199 /* load the pointer to the original exception frame we want to restore */
200 "mov sp, r4;"
201 RESTORE_REGS_PC
202 );
203 }
204
205 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
_half_save_and_svc(struct thread * oldthread,struct thread * newthread,bool fpu_save,bool restore_fpu)206 __NAKED static void _half_save_and_svc(struct thread *oldthread, struct thread *newthread, bool fpu_save, bool restore_fpu)
207 #else
208 __NAKED static void _half_save_and_svc(struct thread *oldthread, struct thread *newthread)
209 #endif
210 {
211 __asm__ volatile(
212 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
213 /* see if we need to save fpu context */
214 "tst r2, #1;"
215 "beq 0f;"
216
217 /* save part of the fpu context on the stack */
218 "vmrs r2, fpscr;"
219 "push { r2 };"
220 "vpush { s0-s15 };"
221
222 /* save the top regs into the thread struct */
223 "add r2, r0, %[fp_off];"
224 "vstm r2, { s16-s31 };"
225
226 "0:"
227 #endif
228
229 /* save regular context */
230 SAVE_REGS
231 SAVE_SP(r0, r2, %[sp_off])
232
233 /* restore the new thread's stack pointer, but not the integer state (yet) */
234 LOAD_SP(r1, r2, %[sp_off])
235
236 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
237 /* see if we need to restore fpu context */
238 "tst r3, #1;"
239 "beq 0f;"
240
241 /* restore the top part of the fpu context */
242 "add r3, r1, %[fp_off];"
243 "vldm r3, { s16-s31 };"
244
245 /* restore the bottom part of the context, stored up the frame a little bit */
246 "add r3, sp, %[fp_exc_off];"
247 "vldm r3!, { s0-s15 };"
248 "ldr r3, [r3];"
249 "vmsr fpscr, r3;"
250 "b 1f;"
251
252 /* disable fpu context if we're not restoring anything */
253 "0:"
254 "mrs r3, CONTROL;"
255 "bic r3, #(1<<2);" /* unset FPCA */
256 "msr CONTROL, r3;"
257 "isb;"
258
259 "1:"
260 #endif
261
262 CLREX
263 "cpsie i;"
264
265 /* make a svc call to get us into handler mode.
266 * use r4 as an arg, since r0 is saved on the stack for the svc */
267 "mov r4, sp;"
268 "svc #0;"
269 :: [sp_off] "i"(offsetof(thread_t, arch.sp))
270 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
271 ,[fp_off] "i"(offsetof(thread_t, arch.fpregs))
272 ,[fp_exc_off] "i"(sizeof(struct arm_cm_exception_frame_long))
273 #endif
274 );
275 }
276
277 /* simple scenario where the to and from thread yielded */
278 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
_arch_non_preempt_context_switch(struct thread * oldthread,struct thread * newthread,bool save_fpu,bool restore_fpu)279 __NAKED static void _arch_non_preempt_context_switch(struct thread *oldthread, struct thread *newthread, bool save_fpu, bool restore_fpu)
280 #else
281 __NAKED static void _arch_non_preempt_context_switch(struct thread *oldthread, struct thread *newthread)
282 #endif
283 {
284 __asm__ volatile(
285 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
286 /* see if we need to save fpu context */
287 "tst r2, #1;"
288 "beq 0f;"
289
290 /* save part of the fpu context on the stack */
291 "vmrs r2, fpscr;"
292 "push { r2 };"
293 "vpush { s0-s15 };"
294
295 /* save the top regs into the thread struct */
296 "add r2, r0, %[fp_off];"
297 "vstm r2, { s16-s31 };"
298
299 "0:"
300 #endif
301
302 /* save regular context */
303 SAVE_REGS
304 SAVE_SP(r0, r2, %[sp_off])
305
306 /* restore new context */
307 LOAD_SP(r1, r2, %[sp_off])
308 RESTORE_REGS
309
310 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
311 /* see if we need to restore fpu context */
312 "tst r3, #1;"
313 "beq 0f;"
314
315 /* restore fpu context */
316 "add r3, r1, %[fp_off];"
317 "vldm r3, { s16-s31 };"
318
319 "vpop { s0-s15 };"
320 "pop { r3 };"
321 "vmsr fpscr, r3;"
322 "b 1f;"
323
324 /* disable fpu context if we're not restoring anything */
325 "0:"
326 "mrs r3, CONTROL;"
327 "bic r3, #(1<<2);" /* unset FPCA */
328 "msr CONTROL, r3;"
329 "isb;"
330
331 "1:"
332 #endif
333
334 CLREX
335 "bx lr;"
336 :: [sp_off] "i"(offsetof(thread_t, arch.sp))
337 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
338 , [fp_off] "i"(offsetof(thread_t, arch.fpregs))
339 #endif
340 );
341 }
342
_thread_mode_bounce(bool fpused)343 __NAKED static void _thread_mode_bounce(bool fpused)
344 {
345 __asm__ volatile(
346 /* restore main context */
347 RESTORE_REGS
348
349 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
350 /* see if we need to restore fpu context */
351 "tst r0, #1;"
352 "beq 0f;"
353
354 /* restore fpu context */
355 "vpop { s0-s15 };"
356 "pop { r0 };"
357 "vmsr fpscr, r0;"
358 "b 1f;"
359
360 /* disable fpu context if we're not restoring anything */
361 "0:"
362 "mrs r3, CONTROL;"
363 "bic r3, #(1<<2);" /* unset FPCA */
364 "msr CONTROL, r3;"
365 "isb;"
366
367 "1:"
368 #endif
369
370 "bx lr;"
371 );
372 __UNREACHABLE;
373 }
374
375 /*
376 * The raw context switch routine. Called by the scheduler when it decides to switch.
377 * Called either in the context of a thread yielding or blocking (interrupts disabled,
378 * on the system stack), or inside the pendsv handler on a thread that is being preempted
379 * (interrupts disabled, in handler mode). If preempt_frame is set the thread
380 * is being preempted.
381 */
arch_context_switch(struct thread * oldthread,struct thread * newthread)382 void arch_context_switch(struct thread *oldthread, struct thread *newthread)
383 {
384 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
385 LTRACEF("FPCCR.LSPACT %lu, FPCAR 0x%x, CONTROL.FPCA %lu\n",
386 FPU->FPCCR & FPU_FPCCR_LSPACT_Msk, FPU->FPCAR, __get_CONTROL() & CONTROL_FPCA_Msk);
387 #endif
388
389 /* if preempt_frame is set, we are being preempted */
390 if (preempt_frame) {
391 LTRACEF("we're preempted, old frame %p, old lr 0x%x, pc 0x%x, new preempted bool %d\n",
392 preempt_frame, preempt_frame->lr, preempt_frame->pc, newthread->arch.was_preempted);
393
394 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
395 /* see if extended fpu frame was pushed */
396 if ((preempt_frame->lr & (1<<4)) == 0) {
397 LTRACEF("thread %s pushed fpu frame\n", oldthread->name);
398
399 /* save the top part of the context */
400 /* note this should also trigger a lazy fpu save if it hasn't already done so */
401 asm volatile("vstm %0, { s16-s31 }" :: "r" (&oldthread->arch.fpregs[0]));
402 oldthread->arch.fpused = true;
403
404 /* verify that FPCCR.LSPACT was cleared and CONTROL.FPCA was set */
405 DEBUG_ASSERT((FPU->FPCCR & FPU_FPCCR_LSPACT_Msk) == 0);
406 DEBUG_ASSERT(__get_CONTROL() & CONTROL_FPCA_Msk);
407 } else {
408 DEBUG_ASSERT(oldthread->arch.fpused == false);
409 }
410 #endif
411
412 oldthread->arch.was_preempted = true;
413 oldthread->arch.sp = (addr_t)preempt_frame;
414 preempt_frame = NULL;
415
416 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
417 /* if new thread has saved fpu state, restore it */
418 if (newthread->arch.fpused) {
419 LTRACEF("newthread FPCCR.LSPACT %lu, FPCAR 0x%x, CONTROL.FPCA %lu\n",
420 FPU->FPCCR & FPU_FPCCR_LSPACT_Msk, FPU->FPCAR, __get_CONTROL() & CONTROL_FPCA_Msk);
421
422 /* enable the fpu manually */
423 __set_CONTROL(__get_CONTROL() | CONTROL_FPCA_Msk);
424 asm volatile("isb");
425
426 DEBUG_ASSERT((FPU->FPCCR & FPU_FPCCR_LSPACT_Msk) == 0);
427 DEBUG_ASSERT(__get_CONTROL() & CONTROL_FPCA_Msk);
428
429 /* restore the top of the fpu state, the rest will happen below */
430 asm volatile("vldm %0, { s16-s31 }" :: "r" (&newthread->arch.fpregs[0]));
431 }
432 #endif
433
434 if (newthread->arch.was_preempted) {
435 /* return directly to the preempted thread's iframe */
436 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
437 LTRACEF("newthread2 FPCCR.LSPACT %lu, FPCAR 0x%x, CONTROL.FPCA %lu\n",
438 FPU->FPCCR & FPU_FPCCR_LSPACT_Msk, FPU->FPCAR, __get_CONTROL() & CONTROL_FPCA_Msk);
439 #endif
440 __asm__ volatile(
441 "mov sp, %0;"
442 "cpsie i;"
443 CLREX
444 RESTORE_REGS_PC
445 :: "r"(newthread->arch.sp)
446 );
447 __UNREACHABLE;
448 } else {
449 /* we're inside a pendsv, switching to a user mode thread */
450 /* set up a fake frame to exception return to */
451 struct arm_cm_exception_frame_short *frame = (void *)newthread->arch.sp;
452 frame--;
453
454 frame->pc = (uint32_t)&_thread_mode_bounce;
455 frame->psr = (1 << 24); /* thread bit set, IPSR 0 */
456 frame->r0 = frame->r1 = frame->r2 = frame->r3 = frame->r12 = frame->lr = 0;
457 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
458 /* pass the fpused bool to _thread_mode_bounce */
459 frame->r0 = newthread->arch.fpused;
460 #endif
461
462 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
463 LTRACEF("iretting to user space, fpused %u\n", newthread->arch.fpused);
464 #else
465 LTRACEF("iretting to user space\n");
466 #endif
467
468 __asm__ volatile(
469 CLREX
470 "mov sp, %0;"
471 "bx %1;"
472 :: "r"(frame), "r"(0xfffffff9)
473 );
474 __UNREACHABLE;
475 }
476 } else {
477 oldthread->arch.was_preempted = false;
478
479 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
480 /* see if we have fpu state we need to save */
481 if (!oldthread->arch.fpused && __get_CONTROL() & CONTROL_FPCA_Msk) {
482 /* mark this thread as using float */
483 LTRACEF("thread %s uses float\n", oldthread->name);
484 oldthread->arch.fpused = true;
485 }
486 #endif
487
488 if (newthread->arch.was_preempted) {
489 LTRACEF("not being preempted, but switching to preempted thread\n");
490 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
491 _half_save_and_svc(oldthread, newthread, oldthread->arch.fpused, newthread->arch.fpused);
492 #else
493 _half_save_and_svc(oldthread, newthread);
494 #endif
495 } else {
496 /* fast path, both sides did not preempt */
497 LTRACEF("both sides are not preempted newsp 0x%lx\n", newthread->arch.sp);
498 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
499 _arch_non_preempt_context_switch(oldthread, newthread, oldthread->arch.fpused, newthread->arch.fpused);
500 #else
501 _arch_non_preempt_context_switch(oldthread, newthread);
502 #endif
503 }
504 }
505
506 }
507
arch_dump_thread(thread_t * t)508 void arch_dump_thread(thread_t *t)
509 {
510 if (t->state != THREAD_RUNNING) {
511 dprintf(INFO, "\tarch: ");
512 dprintf(INFO, "sp 0x%lx, was preempted %u", t->arch.sp, t->arch.was_preempted);
513 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
514 dprintf(INFO, ", fpused %u", t->arch.fpused);
515 #endif
516 dprintf(INFO, "\n");
517 }
518 }
519
520
521