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