fix a stupid bug which made mountpoint detection based on folders non-functional...
[Rockbox.git] / firmware / thread.c
blobe6ab0e4a719476b94068c03ec4629ac7d1ce8aad
1 /***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
10 * Copyright (C) 2002 by Ulf Ralberg
12 * All files in this archive are subject to the GNU General Public License.
13 * See the file COPYING in the source tree root for full license agreement.
15 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
16 * KIND, either express or implied.
18 ****************************************************************************/
19 #include "config.h"
20 #include <stdbool.h>
21 #include "thread.h"
22 #include "panic.h"
23 #include "sprintf.h"
24 #include "system.h"
25 #include "kernel.h"
26 #include "cpu.h"
27 #include "string.h"
28 #ifdef RB_PROFILE
29 #include <profile.h>
30 #endif
31 /****************************************************************************
32 * ATTENTION!! *
33 * See notes below on implementing processor-specific portions! *
34 ***************************************************************************/
36 /* Define THREAD_EXTRA_CHECKS as 1 to enable additional state checks */
37 #ifdef DEBUG
38 #define THREAD_EXTRA_CHECKS 1 /* Always 1 for DEBUG */
39 #else
40 #define THREAD_EXTRA_CHECKS 0
41 #endif
43 /**
44 * General locking order to guarantee progress. Order must be observed but
45 * all stages are not nescessarily obligatory. Going from 1) to 3) is
46 * perfectly legal.
48 * 1) IRQ
49 * This is first because of the likelyhood of having an interrupt occur that
50 * also accesses one of the objects farther down the list. Any non-blocking
51 * synchronization done may already have a lock on something during normal
52 * execution and if an interrupt handler running on the same processor as
53 * the one that has the resource locked were to attempt to access the
54 * resource, the interrupt handler would wait forever waiting for an unlock
55 * that will never happen. There is no danger if the interrupt occurs on
56 * a different processor because the one that has the lock will eventually
57 * unlock and the other processor's handler may proceed at that time. Not
58 * nescessary when the resource in question is definitely not available to
59 * interrupt handlers.
61 * 2) Kernel Object
62 * 1) May be needed beforehand if the kernel object allows dual-use such as
63 * event queues. The kernel object must have a scheme to protect itself from
64 * access by another processor and is responsible for serializing the calls
65 * to block_thread(_w_tmo) and wakeup_thread both to themselves and to each
66 * other. Objects' queues are also protected here.
68 * 3) Thread Slot
69 * This locks access to the thread's slot such that its state cannot be
70 * altered by another processor when a state change is in progress such as
71 * when it is in the process of going on a blocked list. An attempt to wake
72 * a thread while it is still blocking will likely desync its state with
73 * the other resources used for that state.
75 * 4) Core Lists
76 * These lists are specific to a particular processor core and are accessible
77 * by all processor cores and interrupt handlers. The running (rtr) list is
78 * the prime example where a thread may be added by any means.
81 /*---------------------------------------------------------------------------
82 * Processor specific: core_sleep/core_wake/misc. notes
84 * ARM notes:
85 * FIQ is not dealt with by the scheduler code and is simply restored if it
86 * must by masked for some reason - because threading modifies a register
87 * that FIQ may also modify and there's no way to accomplish it atomically.
88 * s3c2440 is such a case.
90 * Audio interrupts are generally treated at a higher priority than others
91 * usage of scheduler code with interrupts higher than HIGHEST_IRQ_LEVEL
92 * are not in general safe. Special cases may be constructed on a per-
93 * source basis and blocking operations are not available.
95 * core_sleep procedure to implement for any CPU to ensure an asychronous
96 * wakup never results in requiring a wait until the next tick (up to
97 * 10000uS!). May require assembly and careful instruction ordering.
99 * 1) On multicore, stay awake if directed to do so by another. If so, goto
100 * step 4.
101 * 2) If processor requires, atomically reenable interrupts and perform step
102 * 3.
103 * 3) Sleep the CPU core. If wakeup itself enables interrupts (stop #0x2000
104 * on Coldfire) goto step 5.
105 * 4) Enable interrupts.
106 * 5) Exit procedure.
108 * core_wake and multprocessor notes for sleep/wake coordination:
109 * If possible, to wake up another processor, the forcing of an interrupt on
110 * the woken core by the waker core is the easiest way to ensure a non-
111 * delayed wake and immediate execution of any woken threads. If that isn't
112 * available then some careful non-blocking synchonization is needed (as on
113 * PP targets at the moment).
114 *---------------------------------------------------------------------------
117 /* Cast to the the machine pointer size, whose size could be < 4 or > 32
118 * (someday :). */
119 #define DEADBEEF ((uintptr_t)0xdeadbeefdeadbeefull)
120 struct core_entry cores[NUM_CORES] IBSS_ATTR;
121 struct thread_entry threads[MAXTHREADS] IBSS_ATTR;
123 static const char main_thread_name[] = "main";
124 extern uintptr_t stackbegin[];
125 extern uintptr_t stackend[];
127 static inline void core_sleep(IF_COP_VOID(unsigned int core))
128 __attribute__((always_inline));
130 void check_tmo_threads(void)
131 __attribute__((noinline));
133 static inline void block_thread_on_l(struct thread_entry *thread, unsigned state)
134 __attribute__((always_inline));
136 static void add_to_list_tmo(struct thread_entry *thread)
137 __attribute__((noinline));
139 static void core_schedule_wakeup(struct thread_entry *thread)
140 __attribute__((noinline));
142 #if NUM_CORES > 1
143 static inline void run_blocking_ops(
144 unsigned int core, struct thread_entry *thread)
145 __attribute__((always_inline));
146 #endif
148 static void thread_stkov(struct thread_entry *thread)
149 __attribute__((noinline));
151 static inline void store_context(void* addr)
152 __attribute__((always_inline));
154 static inline void load_context(const void* addr)
155 __attribute__((always_inline));
157 void switch_thread(void)
158 __attribute__((noinline));
160 /****************************************************************************
161 * Processor-specific section
164 #ifdef MAX_PHYS_SECTOR_SIZE
165 /* Support a special workaround object for large-sector disks */
166 #define IF_NO_SKIP_YIELD(...) __VA_ARGS__
167 #else
168 #define IF_NO_SKIP_YIELD(...)
169 #endif
171 #if defined(CPU_ARM)
172 /*---------------------------------------------------------------------------
173 * Start the thread running and terminate it if it returns
174 *---------------------------------------------------------------------------
176 static void __attribute__((naked,used)) start_thread(void)
178 /* r0 = context */
179 asm volatile (
180 "ldr sp, [r0, #32] \n" /* Load initial sp */
181 "ldr r4, [r0, #40] \n" /* start in r4 since it's non-volatile */
182 "mov r1, #0 \n" /* Mark thread as running */
183 "str r1, [r0, #40] \n"
184 #if NUM_CORES > 1
185 "ldr r0, =invalidate_icache \n" /* Invalidate this core's cache. */
186 "mov lr, pc \n" /* This could be the first entry into */
187 "bx r0 \n" /* plugin or codec code for this core. */
188 #endif
189 "mov lr, pc \n" /* Call thread function */
190 "bx r4 \n"
191 ); /* No clobber list - new thread doesn't care */
192 thread_exit();
193 //asm volatile (".ltorg"); /* Dump constant pool */
196 /* For startup, place context pointer in r4 slot, start_thread pointer in r5
197 * slot, and thread function pointer in context.start. See load_context for
198 * what happens when thread is initially going to run. */
199 #define THREAD_STARTUP_INIT(core, thread, function) \
200 ({ (thread)->context.r[0] = (uint32_t)&(thread)->context, \
201 (thread)->context.r[1] = (uint32_t)start_thread, \
202 (thread)->context.start = (uint32_t)function; })
204 /*---------------------------------------------------------------------------
205 * Store non-volatile context.
206 *---------------------------------------------------------------------------
208 static inline void store_context(void* addr)
210 asm volatile(
211 "stmia %0, { r4-r11, sp, lr } \n"
212 : : "r" (addr)
216 /*---------------------------------------------------------------------------
217 * Load non-volatile context.
218 *---------------------------------------------------------------------------
220 static inline void load_context(const void* addr)
222 asm volatile(
223 "ldr r0, [%0, #40] \n" /* Load start pointer */
224 "cmp r0, #0 \n" /* Check for NULL */
225 "ldmneia %0, { r0, pc } \n" /* If not already running, jump to start */
226 "ldmia %0, { r4-r11, sp, lr } \n" /* Load regs r4 to r14 from context */
227 : : "r" (addr) : "r0" /* only! */
231 #if defined (CPU_PP)
233 #if NUM_CORES > 1
234 extern uintptr_t cpu_idlestackbegin[];
235 extern uintptr_t cpu_idlestackend[];
236 extern uintptr_t cop_idlestackbegin[];
237 extern uintptr_t cop_idlestackend[];
238 static uintptr_t * const idle_stacks[NUM_CORES] NOCACHEDATA_ATTR =
240 [CPU] = cpu_idlestackbegin,
241 [COP] = cop_idlestackbegin
244 #if CONFIG_CPU == PP5002
245 /* Bytes to emulate the PP502x mailbox bits */
246 struct core_semaphores
248 volatile uint8_t intend_wake; /* 00h */
249 volatile uint8_t stay_awake; /* 01h */
250 volatile uint8_t intend_sleep; /* 02h */
251 volatile uint8_t unused; /* 03h */
254 static struct core_semaphores core_semaphores[NUM_CORES] NOCACHEBSS_ATTR;
255 #endif /* CONFIG_CPU == PP5002 */
257 #endif /* NUM_CORES */
259 #if CONFIG_CORELOCK == SW_CORELOCK
260 /* Software core locks using Peterson's mutual exclusion algorithm */
262 /*---------------------------------------------------------------------------
263 * Initialize the corelock structure.
264 *---------------------------------------------------------------------------
266 void corelock_init(struct corelock *cl)
268 memset(cl, 0, sizeof (*cl));
271 #if 1 /* Assembly locks to minimize overhead */
272 /*---------------------------------------------------------------------------
273 * Wait for the corelock to become free and acquire it when it does.
274 *---------------------------------------------------------------------------
276 void corelock_lock(struct corelock *cl) __attribute__((naked));
277 void corelock_lock(struct corelock *cl)
279 asm volatile (
280 "mov r1, %0 \n" /* r1 = PROCESSOR_ID */
281 "ldrb r1, [r1] \n"
282 "strb r1, [r0, r1, lsr #7] \n" /* cl->myl[core] = core */
283 "and r2, r1, #1 \n" /* r2 = othercore */
284 "strb r2, [r0, #2] \n" /* cl->turn = othercore */
285 "1: \n"
286 "ldrb r3, [r0, r2] \n" /* cl->myl[othercore] == 0 ? */
287 "cmp r3, #0 \n"
288 "ldrneb r3, [r0, #2] \n" /* || cl->turn == core ? */
289 "cmpne r3, r1, lsr #7 \n"
290 "bxeq lr \n" /* yes? lock acquired */
291 "b 1b \n" /* keep trying */
292 : : "i"(&PROCESSOR_ID)
294 (void)cl;
297 /*---------------------------------------------------------------------------
298 * Try to aquire the corelock. If free, caller gets it, otherwise return 0.
299 *---------------------------------------------------------------------------
301 int corelock_try_lock(struct corelock *cl) __attribute__((naked));
302 int corelock_try_lock(struct corelock *cl)
304 asm volatile (
305 "mov r1, %0 \n" /* r1 = PROCESSOR_ID */
306 "ldrb r1, [r1] \n"
307 "strb r1, [r0, r1, lsr #7] \n" /* cl->myl[core] = core */
308 "and r2, r1, #1 \n" /* r2 = othercore */
309 "strb r2, [r0, #2] \n" /* cl->turn = othercore */
310 "1: \n"
311 "ldrb r3, [r0, r2] \n" /* cl->myl[othercore] == 0 ? */
312 "cmp r3, #0 \n"
313 "ldrneb r3, [r0, #2] \n" /* || cl->turn == core? */
314 "cmpne r3, r1, lsr #7 \n"
315 "moveq r0, #1 \n" /* yes? lock acquired */
316 "bxeq lr \n"
317 "mov r2, #0 \n" /* cl->myl[core] = 0 */
318 "strb r2, [r0, r1, lsr #7] \n"
319 "mov r0, r2 \n"
320 "bx lr \n" /* acquisition failed */
321 : : "i"(&PROCESSOR_ID)
324 return 0;
325 (void)cl;
328 /*---------------------------------------------------------------------------
329 * Release ownership of the corelock
330 *---------------------------------------------------------------------------
332 void corelock_unlock(struct corelock *cl) __attribute__((naked));
333 void corelock_unlock(struct corelock *cl)
335 asm volatile (
336 "mov r1, %0 \n" /* r1 = PROCESSOR_ID */
337 "ldrb r1, [r1] \n"
338 "mov r2, #0 \n" /* cl->myl[core] = 0 */
339 "strb r2, [r0, r1, lsr #7] \n"
340 "bx lr \n"
341 : : "i"(&PROCESSOR_ID)
343 (void)cl;
345 #else /* C versions for reference */
346 /*---------------------------------------------------------------------------
347 * Wait for the corelock to become free and aquire it when it does.
348 *---------------------------------------------------------------------------
350 void corelock_lock(struct corelock *cl)
352 const unsigned int core = CURRENT_CORE;
353 const unsigned int othercore = 1 - core;
355 cl->myl[core] = core;
356 cl->turn = othercore;
358 for (;;)
360 if (cl->myl[othercore] == 0 || cl->turn == core)
361 break;
365 /*---------------------------------------------------------------------------
366 * Try to aquire the corelock. If free, caller gets it, otherwise return 0.
367 *---------------------------------------------------------------------------
369 int corelock_try_lock(struct corelock *cl)
371 const unsigned int core = CURRENT_CORE;
372 const unsigned int othercore = 1 - core;
374 cl->myl[core] = core;
375 cl->turn = othercore;
377 if (cl->myl[othercore] == 0 || cl->turn == core)
379 return 1;
382 cl->myl[core] = 0;
383 return 0;
386 /*---------------------------------------------------------------------------
387 * Release ownership of the corelock
388 *---------------------------------------------------------------------------
390 void corelock_unlock(struct corelock *cl)
392 cl->myl[CURRENT_CORE] = 0;
394 #endif /* ASM / C selection */
396 #endif /* CONFIG_CORELOCK == SW_CORELOCK */
398 /*---------------------------------------------------------------------------
399 * Put core in a power-saving state if waking list wasn't repopulated and if
400 * no other core requested a wakeup for it to perform a task.
401 *---------------------------------------------------------------------------
403 #ifdef CPU_PP502x
404 #if NUM_CORES == 1
405 static inline void core_sleep(void)
407 PROC_CTL(CURRENT_CORE) = PROC_SLEEP;
408 nop; nop; nop;
409 enable_irq();
411 #else
412 static inline void core_sleep(unsigned int core)
414 #if 1
415 asm volatile (
416 "mov r0, #4 \n" /* r0 = 0x4 << core */
417 "mov r0, r0, lsl %[c] \n"
418 "str r0, [%[mbx], #4] \n" /* signal intent to sleep */
419 "ldr r1, [%[mbx], #0] \n" /* && !(MBX_MSG_STAT & (0x10<<core)) ? */
420 "tst r1, r0, lsl #2 \n"
421 "moveq r1, #0x80000000 \n" /* Then sleep */
422 "streq r1, [%[ctl], %[c], lsl #2] \n"
423 "moveq r1, #0 \n" /* Clear control reg */
424 "streq r1, [%[ctl], %[c], lsl #2] \n"
425 "orr r1, r0, r0, lsl #2 \n" /* Signal intent to wake - clear wake flag */
426 "str r1, [%[mbx], #8] \n"
427 "1: \n" /* Wait for wake procedure to finish */
428 "ldr r1, [%[mbx], #0] \n"
429 "tst r1, r0, lsr #2 \n"
430 "bne 1b \n"
432 : [ctl]"r"(&PROC_CTL(CPU)), [mbx]"r"(MBX_BASE), [c]"r"(core)
433 : "r0", "r1");
434 #else /* C version for reference */
435 /* Signal intent to sleep */
436 MBX_MSG_SET = 0x4 << core;
438 /* Something waking or other processor intends to wake us? */
439 if ((MBX_MSG_STAT & (0x10 << core)) == 0)
441 PROC_CTL(core) = PROC_SLEEP; nop; /* Snooze */
442 PROC_CTL(core) = 0; /* Clear control reg */
445 /* Signal wake - clear wake flag */
446 MBX_MSG_CLR = 0x14 << core;
448 /* Wait for other processor to finish wake procedure */
449 while (MBX_MSG_STAT & (0x1 << core));
450 #endif /* ASM/C selection */
451 enable_irq();
453 #endif /* NUM_CORES */
454 #elif CONFIG_CPU == PP5002
455 #if NUM_CORES == 1
456 static inline void core_sleep(void)
458 asm volatile (
459 /* Sleep: PP5002 crashes if the instruction that puts it to sleep is
460 * located at 0xNNNNNNN0. 4/8/C works. This sequence makes sure
461 * that the correct alternative is executed. Don't change the order
462 * of the next 4 instructions! */
463 "tst pc, #0x0c \n"
464 "mov r0, #0xca \n"
465 "strne r0, [%[ctl]] \n"
466 "streq r0, [%[ctl]] \n"
467 "nop \n" /* nop's needed because of pipeline */
468 "nop \n"
469 "nop \n"
471 : [ctl]"r"(&PROC_CTL(CURRENT_CORE))
472 : "r0"
474 enable_irq();
476 #else
477 /* PP5002 has no mailboxes - emulate using bytes */
478 static inline void core_sleep(unsigned int core)
480 #if 1
481 asm volatile (
482 "mov r0, #1 \n" /* Signal intent to sleep */
483 "strb r0, [%[sem], #2] \n"
484 "ldrb r0, [%[sem], #1] \n" /* && stay_awake == 0? */
485 "cmp r0, #0 \n"
486 "bne 2f \n"
487 /* Sleep: PP5002 crashes if the instruction that puts it to sleep is
488 * located at 0xNNNNNNN0. 4/8/C works. This sequence makes sure
489 * that the correct alternative is executed. Don't change the order
490 * of the next 4 instructions! */
491 "tst pc, #0x0c \n"
492 "mov r0, #0xca \n"
493 "strne r0, [%[ctl], %[c], lsl #2] \n"
494 "streq r0, [%[ctl], %[c], lsl #2] \n"
495 "nop \n" /* nop's needed because of pipeline */
496 "nop \n"
497 "nop \n"
498 "2: \n"
499 "mov r0, #0 \n" /* Clear stay_awake and sleep intent */
500 "strb r0, [%[sem], #1] \n"
501 "strb r0, [%[sem], #2] \n"
502 "1: \n" /* Wait for wake procedure to finish */
503 "ldrb r0, [%[sem], #0] \n"
504 "cmp r0, #0 \n"
505 "bne 1b \n"
507 : [sem]"r"(&core_semaphores[core]), [c]"r"(core),
508 [ctl]"r"(&PROC_CTL(CPU))
509 : "r0"
511 #else /* C version for reference */
512 /* Signal intent to sleep */
513 core_semaphores[core].intend_sleep = 1;
515 /* Something waking or other processor intends to wake us? */
516 if (core_semaphores[core].stay_awake == 0)
518 PROC_CTL(core) = PROC_SLEEP; /* Snooze */
519 nop; nop; nop;
522 /* Signal wake - clear wake flag */
523 core_semaphores[core].stay_awake = 0;
524 core_semaphores[core].intend_sleep = 0;
526 /* Wait for other processor to finish wake procedure */
527 while (core_semaphores[core].intend_wake != 0);
529 /* Enable IRQ */
530 #endif /* ASM/C selection */
531 enable_irq();
533 #endif /* NUM_CORES */
534 #endif /* PP CPU type */
536 /*---------------------------------------------------------------------------
537 * Wake another processor core that is sleeping or prevent it from doing so
538 * if it was already destined. FIQ, IRQ should be disabled before calling.
539 *---------------------------------------------------------------------------
541 #if NUM_CORES == 1
542 /* Shared single-core build debugging version */
543 void core_wake(void)
545 /* No wakey - core already wakey */
547 #elif defined (CPU_PP502x)
548 void core_wake(unsigned int othercore)
550 #if 1
551 /* avoid r0 since that contains othercore */
552 asm volatile (
553 "mrs r3, cpsr \n" /* Disable IRQ */
554 "orr r1, r3, #0x80 \n"
555 "msr cpsr_c, r1 \n"
556 "mov r2, #0x11 \n" /* r2 = (0x11 << othercore) */
557 "mov r2, r2, lsl %[oc] \n" /* Signal intent to wake othercore */
558 "str r2, [%[mbx], #4] \n"
559 "1: \n" /* If it intends to sleep, let it first */
560 "ldr r1, [%[mbx], #0] \n" /* (MSG_MSG_STAT & (0x4 << othercore)) != 0 ? */
561 "eor r1, r1, #0xc \n"
562 "tst r1, r2, lsr #2 \n"
563 "ldr r1, [%[ctl], %[oc], lsl #2] \n" /* && (PROC_CTL(othercore) & PROC_SLEEP) == 0 ? */
564 "tsteq r1, #0x80000000 \n"
565 "beq 1b \n" /* Wait for sleep or wake */
566 "tst r1, #0x80000000 \n" /* If sleeping, wake it */
567 "movne r1, #0x0 \n"
568 "strne r1, [%[ctl], %[oc], lsl #2] \n"
569 "mov r1, r2, lsr #4 \n"
570 "str r1, [%[mbx], #8] \n" /* Done with wake procedure */
571 "msr cpsr_c, r3 \n" /* Restore IRQ */
573 : [ctl]"r"(&PROC_CTL(CPU)), [mbx]"r"(MBX_BASE),
574 [oc]"r"(othercore)
575 : "r1", "r2", "r3");
576 #else /* C version for reference */
577 /* Disable interrupts - avoid reentrancy from the tick */
578 int oldlevel = disable_irq_save();
580 /* Signal intent to wake other processor - set stay awake */
581 MBX_MSG_SET = 0x11 << othercore;
583 /* If it intends to sleep, wait until it does or aborts */
584 while ((MBX_MSG_STAT & (0x4 << othercore)) != 0 &&
585 (PROC_CTL(othercore) & PROC_SLEEP) == 0);
587 /* If sleeping, wake it up */
588 if (PROC_CTL(othercore) & PROC_SLEEP)
589 PROC_CTL(othercore) = 0;
591 /* Done with wake procedure */
592 MBX_MSG_CLR = 0x1 << othercore;
593 restore_irq(oldlevel);
594 #endif /* ASM/C selection */
596 #elif CONFIG_CPU == PP5002
597 /* PP5002 has no mailboxes - emulate using bytes */
598 void core_wake(unsigned int othercore)
600 #if 1
601 /* avoid r0 since that contains othercore */
602 asm volatile (
603 "mrs r3, cpsr \n" /* Disable IRQ */
604 "orr r1, r3, #0x80 \n"
605 "msr cpsr_c, r1 \n"
606 "mov r1, #1 \n" /* Signal intent to wake other core */
607 "orr r1, r1, r1, lsl #8 \n" /* and set stay_awake */
608 "strh r1, [%[sem], #0] \n"
609 "mov r2, #0x8000 \n"
610 "1: \n" /* If it intends to sleep, let it first */
611 "ldrb r1, [%[sem], #2] \n" /* intend_sleep != 0 ? */
612 "cmp r1, #1 \n"
613 "ldr r1, [%[st]] \n" /* && not sleeping ? */
614 "tsteq r1, r2, lsr %[oc] \n"
615 "beq 1b \n" /* Wait for sleep or wake */
616 "tst r1, r2, lsr %[oc] \n"
617 "ldrne r2, =0xcf004054 \n" /* If sleeping, wake it */
618 "movne r1, #0xce \n"
619 "strne r1, [r2, %[oc], lsl #2] \n"
620 "mov r1, #0 \n" /* Done with wake procedure */
621 "strb r1, [%[sem], #0] \n"
622 "msr cpsr_c, r3 \n" /* Restore IRQ */
624 : [sem]"r"(&core_semaphores[othercore]),
625 [st]"r"(&PROC_STAT),
626 [oc]"r"(othercore)
627 : "r1", "r2", "r3"
629 #else /* C version for reference */
630 /* Disable interrupts - avoid reentrancy from the tick */
631 int oldlevel = disable_irq_save();
633 /* Signal intent to wake other processor - set stay awake */
634 core_semaphores[othercore].intend_wake = 1;
635 core_semaphores[othercore].stay_awake = 1;
637 /* If it intends to sleep, wait until it does or aborts */
638 while (core_semaphores[othercore].intend_sleep != 0 &&
639 (PROC_STAT & PROC_SLEEPING(othercore)) == 0);
641 /* If sleeping, wake it up */
642 if (PROC_STAT & PROC_SLEEPING(othercore))
643 PROC_CTL(othercore) = PROC_WAKE;
645 /* Done with wake procedure */
646 core_semaphores[othercore].intend_wake = 0;
647 restore_irq(oldlevel);
648 #endif /* ASM/C selection */
650 #endif /* CPU type */
652 #if NUM_CORES > 1
653 /*---------------------------------------------------------------------------
654 * Switches to a stack that always resides in the Rockbox core.
656 * Needed when a thread suicides on a core other than the main CPU since the
657 * stack used when idling is the stack of the last thread to run. This stack
658 * may not reside in the core firmware in which case the core will continue
659 * to use a stack from an unloaded module until another thread runs on it.
660 *---------------------------------------------------------------------------
662 static inline void switch_to_idle_stack(const unsigned int core)
664 asm volatile (
665 "str sp, [%0] \n" /* save original stack pointer on idle stack */
666 "mov sp, %0 \n" /* switch stacks */
667 : : "r"(&idle_stacks[core][IDLE_STACK_WORDS-1]));
668 (void)core;
671 /*---------------------------------------------------------------------------
672 * Perform core switch steps that need to take place inside switch_thread.
674 * These steps must take place while before changing the processor and after
675 * having entered switch_thread since switch_thread may not do a normal return
676 * because the stack being used for anything the compiler saved will not belong
677 * to the thread's destination core and it may have been recycled for other
678 * purposes by the time a normal context load has taken place. switch_thread
679 * will also clobber anything stashed in the thread's context or stored in the
680 * nonvolatile registers if it is saved there before the call since the
681 * compiler's order of operations cannot be known for certain.
683 static void core_switch_blk_op(unsigned int core, struct thread_entry *thread)
685 /* Flush our data to ram */
686 flush_icache();
687 /* Stash thread in r4 slot */
688 thread->context.r[0] = (uint32_t)thread;
689 /* Stash restart address in r5 slot */
690 thread->context.r[1] = thread->context.start;
691 /* Save sp in context.sp while still running on old core */
692 thread->context.sp = idle_stacks[core][IDLE_STACK_WORDS-1];
695 /*---------------------------------------------------------------------------
696 * Machine-specific helper function for switching the processor a thread is
697 * running on. Basically, the thread suicides on the departing core and is
698 * reborn on the destination. Were it not for gcc's ill-behavior regarding
699 * naked functions written in C where it actually clobbers non-volatile
700 * registers before the intended prologue code, this would all be much
701 * simpler. Generic setup is done in switch_core itself.
704 /*---------------------------------------------------------------------------
705 * This actually performs the core switch.
707 static void __attribute__((naked))
708 switch_thread_core(unsigned int core, struct thread_entry *thread)
710 /* Pure asm for this because compiler behavior isn't sufficiently predictable.
711 * Stack access also isn't permitted until restoring the original stack and
712 * context. */
713 asm volatile (
714 "stmfd sp!, { r4-r12, lr } \n" /* Stack all non-volatile context on current core */
715 "ldr r2, =idle_stacks \n" /* r2 = &idle_stacks[core][IDLE_STACK_WORDS] */
716 "ldr r2, [r2, r0, lsl #2] \n"
717 "add r2, r2, %0*4 \n"
718 "stmfd r2!, { sp } \n" /* save original stack pointer on idle stack */
719 "mov sp, r2 \n" /* switch stacks */
720 "adr r2, 1f \n" /* r2 = new core restart address */
721 "str r2, [r1, #40] \n" /* thread->context.start = r2 */
722 "ldr pc, =switch_thread \n" /* r0 = thread after call - see load_context */
723 "1: \n"
724 "ldr sp, [r0, #32] \n" /* Reload original sp from context structure */
725 "mov r1, #0 \n" /* Clear start address */
726 "str r1, [r0, #40] \n"
727 "ldr r0, =invalidate_icache \n" /* Invalidate new core's cache */
728 "mov lr, pc \n"
729 "bx r0 \n"
730 "ldmfd sp!, { r4-r12, pc } \n" /* Restore non-volatile context to new core and return */
731 ".ltorg \n" /* Dump constant pool */
732 : : "i"(IDLE_STACK_WORDS)
734 (void)core; (void)thread;
736 #endif /* NUM_CORES */
738 #elif CONFIG_CPU == S3C2440
740 /*---------------------------------------------------------------------------
741 * Put core in a power-saving state if waking list wasn't repopulated.
742 *---------------------------------------------------------------------------
744 static inline void core_sleep(void)
746 /* FIQ also changes the CLKCON register so FIQ must be disabled
747 when changing it here */
748 asm volatile (
749 "mrs r0, cpsr \n"
750 "orr r2, r0, #0x40 \n" /* Disable FIQ */
751 "bic r0, r0, #0x80 \n" /* Prepare IRQ enable */
752 "msr cpsr_c, r2 \n"
753 "mov r1, #0x4c000000 \n" /* CLKCON = 0x4c00000c */
754 "ldr r2, [r1, #0xc] \n" /* Set IDLE bit */
755 "orr r2, r2, #4 \n"
756 "str r2, [r1, #0xc] \n"
757 "msr cpsr_c, r0 \n" /* Enable IRQ, restore FIQ */
758 "mov r2, #0 \n" /* wait for IDLE */
759 "1: \n"
760 "add r2, r2, #1 \n"
761 "cmp r2, #10 \n"
762 "bne 1b \n"
763 "orr r2, r0, #0xc0 \n" /* Disable IRQ, FIQ */
764 "msr cpsr_c, r2 \n"
765 "ldr r2, [r1, #0xc] \n" /* Reset IDLE bit */
766 "bic r2, r2, #4 \n"
767 "str r2, [r1, #0xc] \n"
768 "msr cpsr_c, r0 \n" /* Enable IRQ, restore FIQ */
769 : : : "r0", "r1", "r2");
771 #elif defined(CPU_TCC77X)
772 static inline void core_sleep(void)
774 #warning TODO: Implement core_sleep
775 enable_irq();
777 #elif defined(CPU_TCC780X)
778 static inline void core_sleep(void)
780 /* Single core only for now. Use the generic ARMv5 wait for IRQ */
781 asm volatile (
782 "mov r0, #0 \n"
783 "mcr p15, 0, r0, c7, c0, 4 \n" /* Wait for interrupt */
784 : : : "r0"
786 enable_irq();
788 #elif CONFIG_CPU == IMX31L
789 static inline void core_sleep(void)
791 asm volatile (
792 "mov r0, #0 \n"
793 "mcr p15, 0, r0, c7, c0, 4 \n" /* Wait for interrupt */
794 : : : "r0"
796 enable_irq();
798 #else
799 static inline void core_sleep(void)
801 #warning core_sleep not implemented, battery life will be decreased
802 enable_irq();
804 #endif /* CONFIG_CPU == */
806 #elif defined(CPU_COLDFIRE)
807 /*---------------------------------------------------------------------------
808 * Start the thread running and terminate it if it returns
809 *---------------------------------------------------------------------------
811 void start_thread(void); /* Provide C access to ASM label */
812 static void __attribute__((used)) __start_thread(void)
814 /* a0=macsr, a1=context */
815 asm volatile (
816 "start_thread: \n" /* Start here - no naked attribute */
817 "move.l %a0, %macsr \n" /* Set initial mac status reg */
818 "lea.l 48(%a1), %a1 \n"
819 "move.l (%a1)+, %sp \n" /* Set initial stack */
820 "move.l (%a1), %a2 \n" /* Fetch thread function pointer */
821 "clr.l (%a1) \n" /* Mark thread running */
822 "jsr (%a2) \n" /* Call thread function */
824 thread_exit();
827 /* Set EMAC unit to fractional mode with saturation for each new thread,
828 * since that's what'll be the most useful for most things which the dsp
829 * will do. Codecs should still initialize their preferred modes
830 * explicitly. Context pointer is placed in d2 slot and start_thread
831 * pointer in d3 slot. thread function pointer is placed in context.start.
832 * See load_context for what happens when thread is initially going to
833 * run.
835 #define THREAD_STARTUP_INIT(core, thread, function) \
836 ({ (thread)->context.macsr = EMAC_FRACTIONAL | EMAC_SATURATE, \
837 (thread)->context.d[0] = (uint32_t)&(thread)->context, \
838 (thread)->context.d[1] = (uint32_t)start_thread, \
839 (thread)->context.start = (uint32_t)(function); })
841 /*---------------------------------------------------------------------------
842 * Store non-volatile context.
843 *---------------------------------------------------------------------------
845 static inline void store_context(void* addr)
847 asm volatile (
848 "move.l %%macsr,%%d0 \n"
849 "movem.l %%d0/%%d2-%%d7/%%a2-%%a7,(%0) \n"
850 : : "a" (addr) : "d0" /* only! */
854 /*---------------------------------------------------------------------------
855 * Load non-volatile context.
856 *---------------------------------------------------------------------------
858 static inline void load_context(const void* addr)
860 asm volatile (
861 "move.l 52(%0), %%d0 \n" /* Get start address */
862 "beq.b 1f \n" /* NULL -> already running */
863 "movem.l (%0), %%a0-%%a2 \n" /* a0=macsr, a1=context, a2=start_thread */
864 "jmp (%%a2) \n" /* Start the thread */
865 "1: \n"
866 "movem.l (%0), %%d0/%%d2-%%d7/%%a2-%%a7 \n" /* Load context */
867 "move.l %%d0, %%macsr \n"
868 : : "a" (addr) : "d0" /* only! */
872 /*---------------------------------------------------------------------------
873 * Put core in a power-saving state if waking list wasn't repopulated.
874 *---------------------------------------------------------------------------
876 static inline void core_sleep(void)
878 /* Supervisor mode, interrupts enabled upon wakeup */
879 asm volatile ("stop #0x2000");
882 #elif CONFIG_CPU == SH7034
883 /*---------------------------------------------------------------------------
884 * Start the thread running and terminate it if it returns
885 *---------------------------------------------------------------------------
887 void start_thread(void); /* Provide C access to ASM label */
888 static void __attribute__((used)) __start_thread(void)
890 /* r8 = context */
891 asm volatile (
892 "_start_thread: \n" /* Start here - no naked attribute */
893 "mov.l @(4, r8), r0 \n" /* Fetch thread function pointer */
894 "mov.l @(28, r8), r15 \n" /* Set initial sp */
895 "mov #0, r1 \n" /* Start the thread */
896 "jsr @r0 \n"
897 "mov.l r1, @(36, r8) \n" /* Clear start address */
899 thread_exit();
902 /* Place context pointer in r8 slot, function pointer in r9 slot, and
903 * start_thread pointer in context_start */
904 #define THREAD_STARTUP_INIT(core, thread, function) \
905 ({ (thread)->context.r[0] = (uint32_t)&(thread)->context, \
906 (thread)->context.r[1] = (uint32_t)(function), \
907 (thread)->context.start = (uint32_t)start_thread; })
909 /*---------------------------------------------------------------------------
910 * Store non-volatile context.
911 *---------------------------------------------------------------------------
913 static inline void store_context(void* addr)
915 asm volatile (
916 "add #36, %0 \n" /* Start at last reg. By the time routine */
917 "sts.l pr, @-%0 \n" /* is done, %0 will have the original value */
918 "mov.l r15,@-%0 \n"
919 "mov.l r14,@-%0 \n"
920 "mov.l r13,@-%0 \n"
921 "mov.l r12,@-%0 \n"
922 "mov.l r11,@-%0 \n"
923 "mov.l r10,@-%0 \n"
924 "mov.l r9, @-%0 \n"
925 "mov.l r8, @-%0 \n"
926 : : "r" (addr)
930 /*---------------------------------------------------------------------------
931 * Load non-volatile context.
932 *---------------------------------------------------------------------------
934 static inline void load_context(const void* addr)
936 asm volatile (
937 "mov.l @(36, %0), r0 \n" /* Get start address */
938 "tst r0, r0 \n"
939 "bt .running \n" /* NULL -> already running */
940 "jmp @r0 \n" /* r8 = context */
941 ".running: \n"
942 "mov.l @%0+, r8 \n" /* Executes in delay slot and outside it */
943 "mov.l @%0+, r9 \n"
944 "mov.l @%0+, r10 \n"
945 "mov.l @%0+, r11 \n"
946 "mov.l @%0+, r12 \n"
947 "mov.l @%0+, r13 \n"
948 "mov.l @%0+, r14 \n"
949 "mov.l @%0+, r15 \n"
950 "lds.l @%0+, pr \n"
951 : : "r" (addr) : "r0" /* only! */
955 /*---------------------------------------------------------------------------
956 * Put core in a power-saving state.
957 *---------------------------------------------------------------------------
959 static inline void core_sleep(void)
961 asm volatile (
962 "and.b #0x7f, @(r0, gbr) \n" /* Clear SBY (bit 7) in SBYCR */
963 "mov #0, r1 \n" /* Enable interrupts */
964 "ldc r1, sr \n" /* Following instruction cannot be interrupted */
965 "sleep \n" /* Execute standby */
966 : : "z"(&SBYCR-GBR) : "r1");
969 #endif /* CONFIG_CPU == */
972 * End Processor-specific section
973 ***************************************************************************/
975 #if THREAD_EXTRA_CHECKS
976 static void thread_panicf(const char *msg, struct thread_entry *thread)
978 IF_COP( const unsigned int core = thread->core; )
979 static char name[32];
980 thread_get_name(name, 32, thread);
981 panicf ("%s %s" IF_COP(" (%d)"), msg, name IF_COP(, core));
983 static void thread_stkov(struct thread_entry *thread)
985 thread_panicf("Stkov", thread);
987 #define THREAD_PANICF(msg, thread) \
988 thread_panicf(msg, thread)
989 #define THREAD_ASSERT(exp, msg, thread) \
990 ({ if (!({ exp; })) thread_panicf((msg), (thread)); })
991 #else
992 static void thread_stkov(struct thread_entry *thread)
994 IF_COP( const unsigned int core = thread->core; )
995 static char name[32];
996 thread_get_name(name, 32, thread);
997 panicf("Stkov %s" IF_COP(" (%d)"), name IF_COP(, core));
999 #define THREAD_PANICF(msg, thread)
1000 #define THREAD_ASSERT(exp, msg, thread)
1001 #endif /* THREAD_EXTRA_CHECKS */
1003 /* Thread locking */
1004 #if NUM_CORES > 1
1005 #define LOCK_THREAD(thread) \
1006 ({ corelock_lock(&(thread)->slot_cl); })
1007 #define TRY_LOCK_THREAD(thread) \
1008 ({ corelock_try_lock(&thread->slot_cl); })
1009 #define UNLOCK_THREAD(thread) \
1010 ({ corelock_unlock(&(thread)->slot_cl); })
1011 #define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \
1012 ({ unsigned int _core = (thread)->core; \
1013 cores[_core].blk_ops.flags |= TBOP_UNLOCK_CORELOCK; \
1014 cores[_core].blk_ops.cl_p = &(thread)->slot_cl; })
1015 #else
1016 #define LOCK_THREAD(thread) \
1017 ({ })
1018 #define TRY_LOCK_THREAD(thread) \
1019 ({ })
1020 #define UNLOCK_THREAD(thread) \
1021 ({ })
1022 #define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \
1023 ({ })
1024 #endif
1026 /* RTR list */
1027 #define RTR_LOCK(core) \
1028 ({ corelock_lock(&cores[core].rtr_cl); })
1029 #define RTR_UNLOCK(core) \
1030 ({ corelock_unlock(&cores[core].rtr_cl); })
1032 #ifdef HAVE_PRIORITY_SCHEDULING
1033 #define rtr_add_entry(core, priority) \
1034 prio_add_entry(&cores[core].rtr, (priority))
1036 #define rtr_subtract_entry(core, priority) \
1037 prio_subtract_entry(&cores[core].rtr, (priority))
1039 #define rtr_move_entry(core, from, to) \
1040 prio_move_entry(&cores[core].rtr, (from), (to))
1041 #else
1042 #define rtr_add_entry(core, priority)
1043 #define rtr_add_entry_inl(core, priority)
1044 #define rtr_subtract_entry(core, priority)
1045 #define rtr_subtract_entry_inl(core, priotity)
1046 #define rtr_move_entry(core, from, to)
1047 #define rtr_move_entry_inl(core, from, to)
1048 #endif
1050 /*---------------------------------------------------------------------------
1051 * Thread list structure - circular:
1052 * +------------------------------+
1053 * | |
1054 * +--+---+<-+---+<-+---+<-+---+<-+
1055 * Head->| T | | T | | T | | T |
1056 * +->+---+->+---+->+---+->+---+--+
1057 * | |
1058 * +------------------------------+
1059 *---------------------------------------------------------------------------
1062 /*---------------------------------------------------------------------------
1063 * Adds a thread to a list of threads using "insert last". Uses the "l"
1064 * links.
1065 *---------------------------------------------------------------------------
1067 static void add_to_list_l(struct thread_entry **list,
1068 struct thread_entry *thread)
1070 struct thread_entry *l = *list;
1072 if (l == NULL)
1074 /* Insert into unoccupied list */
1075 thread->l.prev = thread;
1076 thread->l.next = thread;
1077 *list = thread;
1078 return;
1081 /* Insert last */
1082 thread->l.prev = l->l.prev;
1083 thread->l.next = l;
1084 l->l.prev->l.next = thread;
1085 l->l.prev = thread;
1088 /*---------------------------------------------------------------------------
1089 * Removes a thread from a list of threads. Uses the "l" links.
1090 *---------------------------------------------------------------------------
1092 static void remove_from_list_l(struct thread_entry **list,
1093 struct thread_entry *thread)
1095 struct thread_entry *prev, *next;
1097 next = thread->l.next;
1099 if (thread == next)
1101 /* The only item */
1102 *list = NULL;
1103 return;
1106 if (thread == *list)
1108 /* List becomes next item */
1109 *list = next;
1112 prev = thread->l.prev;
1114 /* Fix links to jump over the removed entry. */
1115 next->l.prev = prev;
1116 prev->l.next = next;
1119 /*---------------------------------------------------------------------------
1120 * Timeout list structure - circular reverse (to make "remove item" O(1)),
1121 * NULL-terminated forward (to ease the far more common forward traversal):
1122 * +------------------------------+
1123 * | |
1124 * +--+---+<-+---+<-+---+<-+---+<-+
1125 * Head->| T | | T | | T | | T |
1126 * +---+->+---+->+---+->+---+-X
1127 *---------------------------------------------------------------------------
1130 /*---------------------------------------------------------------------------
1131 * Add a thread from the core's timout list by linking the pointers in its
1132 * tmo structure.
1133 *---------------------------------------------------------------------------
1135 static void add_to_list_tmo(struct thread_entry *thread)
1137 struct thread_entry *tmo = cores[IF_COP_CORE(thread->core)].timeout;
1138 THREAD_ASSERT(thread->tmo.prev == NULL,
1139 "add_to_list_tmo->already listed", thread);
1141 thread->tmo.next = NULL;
1143 if (tmo == NULL)
1145 /* Insert into unoccupied list */
1146 thread->tmo.prev = thread;
1147 cores[IF_COP_CORE(thread->core)].timeout = thread;
1148 return;
1151 /* Insert Last */
1152 thread->tmo.prev = tmo->tmo.prev;
1153 tmo->tmo.prev->tmo.next = thread;
1154 tmo->tmo.prev = thread;
1157 /*---------------------------------------------------------------------------
1158 * Remove a thread from the core's timout list by unlinking the pointers in
1159 * its tmo structure. Sets thread->tmo.prev to NULL to indicate the timeout
1160 * is cancelled.
1161 *---------------------------------------------------------------------------
1163 static void remove_from_list_tmo(struct thread_entry *thread)
1165 struct thread_entry **list = &cores[IF_COP_CORE(thread->core)].timeout;
1166 struct thread_entry *prev = thread->tmo.prev;
1167 struct thread_entry *next = thread->tmo.next;
1169 THREAD_ASSERT(prev != NULL, "remove_from_list_tmo->not listed", thread);
1171 if (next != NULL)
1172 next->tmo.prev = prev;
1174 if (thread == *list)
1176 /* List becomes next item and empty if next == NULL */
1177 *list = next;
1178 /* Mark as unlisted */
1179 thread->tmo.prev = NULL;
1181 else
1183 if (next == NULL)
1184 (*list)->tmo.prev = prev;
1185 prev->tmo.next = next;
1186 /* Mark as unlisted */
1187 thread->tmo.prev = NULL;
1192 #ifdef HAVE_PRIORITY_SCHEDULING
1193 /*---------------------------------------------------------------------------
1194 * Priority distribution structure (one category for each possible priority):
1196 * +----+----+----+ ... +-----+
1197 * hist: | F0 | F1 | F2 | | F31 |
1198 * +----+----+----+ ... +-----+
1199 * mask: | b0 | b1 | b2 | | b31 |
1200 * +----+----+----+ ... +-----+
1202 * F = count of threads at priority category n (frequency)
1203 * b = bitmask of non-zero priority categories (occupancy)
1205 * / if H[n] != 0 : 1
1206 * b[n] = |
1207 * \ else : 0
1209 *---------------------------------------------------------------------------
1210 * Basic priority inheritance priotocol (PIP):
1212 * Mn = mutex n, Tn = thread n
1214 * A lower priority thread inherits the priority of the highest priority
1215 * thread blocked waiting for it to complete an action (such as release a
1216 * mutex or respond to a message via queue_send):
1218 * 1) T2->M1->T1
1220 * T1 owns M1, T2 is waiting for M1 to realease M1. If T2 has a higher
1221 * priority than T1 then T1 inherits the priority of T2.
1223 * 2) T3
1224 * \/
1225 * T2->M1->T1
1227 * Situation is like 1) but T2 and T3 are both queued waiting for M1 and so
1228 * T1 inherits the higher of T2 and T3.
1230 * 3) T3->M2->T2->M1->T1
1232 * T1 owns M1, T2 owns M2. If T3 has a higher priority than both T1 and T2,
1233 * then T1 inherits the priority of T3 through T2.
1235 * Blocking chains can grow arbitrarily complex (though it's best that they
1236 * not form at all very often :) and build-up from these units.
1237 *---------------------------------------------------------------------------
1240 /*---------------------------------------------------------------------------
1241 * Increment frequency at category "priority"
1242 *---------------------------------------------------------------------------
1244 static inline unsigned int prio_add_entry(
1245 struct priority_distribution *pd, int priority)
1247 unsigned int count;
1248 /* Enough size/instruction count difference for ARM makes it worth it to
1249 * use different code (192 bytes for ARM). Only thing better is ASM. */
1250 #ifdef CPU_ARM
1251 count = pd->hist[priority];
1252 if (++count == 1)
1253 pd->mask |= 1 << priority;
1254 pd->hist[priority] = count;
1255 #else /* This one's better for Coldfire */
1256 if ((count = ++pd->hist[priority]) == 1)
1257 pd->mask |= 1 << priority;
1258 #endif
1260 return count;
1263 /*---------------------------------------------------------------------------
1264 * Decrement frequency at category "priority"
1265 *---------------------------------------------------------------------------
1267 static inline unsigned int prio_subtract_entry(
1268 struct priority_distribution *pd, int priority)
1270 unsigned int count;
1272 #ifdef CPU_ARM
1273 count = pd->hist[priority];
1274 if (--count == 0)
1275 pd->mask &= ~(1 << priority);
1276 pd->hist[priority] = count;
1277 #else
1278 if ((count = --pd->hist[priority]) == 0)
1279 pd->mask &= ~(1 << priority);
1280 #endif
1282 return count;
1285 /*---------------------------------------------------------------------------
1286 * Remove from one category and add to another
1287 *---------------------------------------------------------------------------
1289 static inline void prio_move_entry(
1290 struct priority_distribution *pd, int from, int to)
1292 uint32_t mask = pd->mask;
1294 #ifdef CPU_ARM
1295 unsigned int count;
1297 count = pd->hist[from];
1298 if (--count == 0)
1299 mask &= ~(1 << from);
1300 pd->hist[from] = count;
1302 count = pd->hist[to];
1303 if (++count == 1)
1304 mask |= 1 << to;
1305 pd->hist[to] = count;
1306 #else
1307 if (--pd->hist[from] == 0)
1308 mask &= ~(1 << from);
1310 if (++pd->hist[to] == 1)
1311 mask |= 1 << to;
1312 #endif
1314 pd->mask = mask;
1317 /*---------------------------------------------------------------------------
1318 * Change the priority and rtr entry for a running thread
1319 *---------------------------------------------------------------------------
1321 static inline void set_running_thread_priority(
1322 struct thread_entry *thread, int priority)
1324 const unsigned int core = IF_COP_CORE(thread->core);
1325 RTR_LOCK(core);
1326 rtr_move_entry(core, thread->priority, priority);
1327 thread->priority = priority;
1328 RTR_UNLOCK(core);
1331 /*---------------------------------------------------------------------------
1332 * Finds the highest priority thread in a list of threads. If the list is
1333 * empty, the PRIORITY_IDLE is returned.
1335 * It is possible to use the struct priority_distribution within an object
1336 * instead of scanning the remaining threads in the list but as a compromise,
1337 * the resulting per-object memory overhead is saved at a slight speed
1338 * penalty under high contention.
1339 *---------------------------------------------------------------------------
1341 static int find_highest_priority_in_list_l(
1342 struct thread_entry * const thread)
1344 if (thread != NULL)
1346 /* Go though list until the ending up at the initial thread */
1347 int highest_priority = thread->priority;
1348 struct thread_entry *curr = thread;
1352 int priority = curr->priority;
1354 if (priority < highest_priority)
1355 highest_priority = priority;
1357 curr = curr->l.next;
1359 while (curr != thread);
1361 return highest_priority;
1364 return PRIORITY_IDLE;
1367 /*---------------------------------------------------------------------------
1368 * Register priority with blocking system and bubble it down the chain if
1369 * any until we reach the end or something is already equal or higher.
1371 * NOTE: A simultaneous circular wait could spin deadlock on multiprocessor
1372 * targets but that same action also guarantees a circular block anyway and
1373 * those are prevented, right? :-)
1374 *---------------------------------------------------------------------------
1376 static struct thread_entry *
1377 blocker_inherit_priority(struct thread_entry *current)
1379 const int priority = current->priority;
1380 struct blocker *bl = current->blocker;
1381 struct thread_entry * const tstart = current;
1382 struct thread_entry *bl_t = bl->thread;
1384 /* Blocker cannot change since the object protection is held */
1385 LOCK_THREAD(bl_t);
1387 for (;;)
1389 struct thread_entry *next;
1390 int bl_pr = bl->priority;
1392 if (priority >= bl_pr)
1393 break; /* Object priority already high enough */
1395 bl->priority = priority;
1397 /* Add this one */
1398 prio_add_entry(&bl_t->pdist, priority);
1400 if (bl_pr < PRIORITY_IDLE)
1402 /* Not first waiter - subtract old one */
1403 prio_subtract_entry(&bl_t->pdist, bl_pr);
1406 if (priority >= bl_t->priority)
1407 break; /* Thread priority high enough */
1409 if (bl_t->state == STATE_RUNNING)
1411 /* Blocking thread is a running thread therefore there are no
1412 * further blockers. Change the "run queue" on which it
1413 * resides. */
1414 set_running_thread_priority(bl_t, priority);
1415 break;
1418 bl_t->priority = priority;
1420 /* If blocking thread has a blocker, apply transitive inheritance */
1421 bl = bl_t->blocker;
1423 if (bl == NULL)
1424 break; /* End of chain or object doesn't support inheritance */
1426 next = bl->thread;
1428 if (next == tstart)
1429 break; /* Full-circle - deadlock! */
1431 UNLOCK_THREAD(current);
1433 #if NUM_CORES > 1
1434 for (;;)
1436 LOCK_THREAD(next);
1438 /* Blocker could change - retest condition */
1439 if (bl->thread == next)
1440 break;
1442 UNLOCK_THREAD(next);
1443 next = bl->thread;
1445 #endif
1446 current = bl_t;
1447 bl_t = next;
1450 UNLOCK_THREAD(bl_t);
1452 return current;
1455 /*---------------------------------------------------------------------------
1456 * Readjust priorities when waking a thread blocked waiting for another
1457 * in essence "releasing" the thread's effect on the object owner. Can be
1458 * performed from any context.
1459 *---------------------------------------------------------------------------
1461 struct thread_entry *
1462 wakeup_priority_protocol_release(struct thread_entry *thread)
1464 const int priority = thread->priority;
1465 struct blocker *bl = thread->blocker;
1466 struct thread_entry * const tstart = thread;
1467 struct thread_entry *bl_t = bl->thread;
1469 /* Blocker cannot change since object will be locked */
1470 LOCK_THREAD(bl_t);
1472 thread->blocker = NULL; /* Thread not blocked */
1474 for (;;)
1476 struct thread_entry *next;
1477 int bl_pr = bl->priority;
1479 if (priority > bl_pr)
1480 break; /* Object priority higher */
1482 next = *thread->bqp;
1484 if (next == NULL)
1486 /* No more threads in queue */
1487 prio_subtract_entry(&bl_t->pdist, bl_pr);
1488 bl->priority = PRIORITY_IDLE;
1490 else
1492 /* Check list for highest remaining priority */
1493 int queue_pr = find_highest_priority_in_list_l(next);
1495 if (queue_pr == bl_pr)
1496 break; /* Object priority not changing */
1498 /* Change queue priority */
1499 prio_move_entry(&bl_t->pdist, bl_pr, queue_pr);
1500 bl->priority = queue_pr;
1503 if (bl_pr > bl_t->priority)
1504 break; /* thread priority is higher */
1506 bl_pr = find_first_set_bit(bl_t->pdist.mask);
1508 if (bl_pr == bl_t->priority)
1509 break; /* Thread priority not changing */
1511 if (bl_t->state == STATE_RUNNING)
1513 /* No further blockers */
1514 set_running_thread_priority(bl_t, bl_pr);
1515 break;
1518 bl_t->priority = bl_pr;
1520 /* If blocking thread has a blocker, apply transitive inheritance */
1521 bl = bl_t->blocker;
1523 if (bl == NULL)
1524 break; /* End of chain or object doesn't support inheritance */
1526 next = bl->thread;
1528 if (next == tstart)
1529 break; /* Full-circle - deadlock! */
1531 UNLOCK_THREAD(thread);
1533 #if NUM_CORES > 1
1534 for (;;)
1536 LOCK_THREAD(next);
1538 /* Blocker could change - retest condition */
1539 if (bl->thread == next)
1540 break;
1542 UNLOCK_THREAD(next);
1543 next = bl->thread;
1545 #endif
1546 thread = bl_t;
1547 bl_t = next;
1550 UNLOCK_THREAD(bl_t);
1552 #if NUM_CORES > 1
1553 if (thread != tstart)
1555 /* Relock original if it changed */
1556 LOCK_THREAD(tstart);
1558 #endif
1560 return cores[CURRENT_CORE].running;
1563 /*---------------------------------------------------------------------------
1564 * Transfer ownership to a thread waiting for an objects and transfer
1565 * inherited priority boost from other waiters. This algorithm knows that
1566 * blocking chains may only unblock from the very end.
1568 * Only the owning thread itself may call this and so the assumption that
1569 * it is the running thread is made.
1570 *---------------------------------------------------------------------------
1572 struct thread_entry *
1573 wakeup_priority_protocol_transfer(struct thread_entry *thread)
1575 /* Waking thread inherits priority boost from object owner */
1576 struct blocker *bl = thread->blocker;
1577 struct thread_entry *bl_t = bl->thread;
1578 struct thread_entry *next;
1579 int bl_pr;
1581 THREAD_ASSERT(thread_get_current() == bl_t,
1582 "UPPT->wrong thread", thread_get_current());
1584 LOCK_THREAD(bl_t);
1586 bl_pr = bl->priority;
1588 /* Remove the object's boost from the owning thread */
1589 if (prio_subtract_entry(&bl_t->pdist, bl_pr) == 0 &&
1590 bl_pr <= bl_t->priority)
1592 /* No more threads at this priority are waiting and the old level is
1593 * at least the thread level */
1594 int priority = find_first_set_bit(bl_t->pdist.mask);
1596 if (priority != bl_t->priority)
1598 /* Adjust this thread's priority */
1599 set_running_thread_priority(bl_t, priority);
1603 next = *thread->bqp;
1605 if (next == NULL)
1607 /* Expected shortcut - no more waiters */
1608 bl_pr = PRIORITY_IDLE;
1610 else
1612 if (thread->priority <= bl_pr)
1614 /* Need to scan threads remaining in queue */
1615 bl_pr = find_highest_priority_in_list_l(next);
1618 if (prio_add_entry(&thread->pdist, bl_pr) == 1 &&
1619 bl_pr < thread->priority)
1621 /* Thread priority must be raised */
1622 thread->priority = bl_pr;
1626 bl->thread = thread; /* This thread pwns */
1627 bl->priority = bl_pr; /* Save highest blocked priority */
1628 thread->blocker = NULL; /* Thread not blocked */
1630 UNLOCK_THREAD(bl_t);
1632 return bl_t;
1635 /*---------------------------------------------------------------------------
1636 * No threads must be blocked waiting for this thread except for it to exit.
1637 * The alternative is more elaborate cleanup and object registration code.
1638 * Check this for risk of silent data corruption when objects with
1639 * inheritable blocking are abandoned by the owner - not precise but may
1640 * catch something.
1641 *---------------------------------------------------------------------------
1643 void check_for_obj_waiters(const char *function, struct thread_entry *thread)
1645 /* Only one bit in the mask should be set with a frequency on 1 which
1646 * represents the thread's own base priority */
1647 uint32_t mask = thread->pdist.mask;
1648 if ((mask & (mask - 1)) != 0 ||
1649 thread->pdist.hist[find_first_set_bit(mask)] > 1)
1651 unsigned char name[32];
1652 thread_get_name(name, 32, thread);
1653 panicf("%s->%s with obj. waiters", function, name);
1656 #endif /* HAVE_PRIORITY_SCHEDULING */
1658 /*---------------------------------------------------------------------------
1659 * Move a thread back to a running state on its core.
1660 *---------------------------------------------------------------------------
1662 static void core_schedule_wakeup(struct thread_entry *thread)
1664 const unsigned int core = IF_COP_CORE(thread->core);
1666 RTR_LOCK(core);
1668 thread->state = STATE_RUNNING;
1670 add_to_list_l(&cores[core].running, thread);
1671 rtr_add_entry(core, thread->priority);
1673 RTR_UNLOCK(core);
1675 #if NUM_CORES > 1
1676 if (core != CURRENT_CORE)
1677 core_wake(core);
1678 #endif
1681 /*---------------------------------------------------------------------------
1682 * Check the core's timeout list when at least one thread is due to wake.
1683 * Filtering for the condition is done before making the call. Resets the
1684 * tick when the next check will occur.
1685 *---------------------------------------------------------------------------
1687 void check_tmo_threads(void)
1689 const unsigned int core = CURRENT_CORE;
1690 const long tick = current_tick; /* snapshot the current tick */
1691 long next_tmo_check = tick + 60*HZ; /* minimum duration: once/minute */
1692 struct thread_entry *next = cores[core].timeout;
1694 /* If there are no processes waiting for a timeout, just keep the check
1695 tick from falling into the past. */
1697 /* Break the loop once we have walked through the list of all
1698 * sleeping processes or have removed them all. */
1699 while (next != NULL)
1701 /* Check sleeping threads. Allow interrupts between checks. */
1702 enable_irq();
1704 struct thread_entry *curr = next;
1706 next = curr->tmo.next;
1708 /* Lock thread slot against explicit wakeup */
1709 disable_irq();
1710 LOCK_THREAD(curr);
1712 unsigned state = curr->state;
1714 if (state < TIMEOUT_STATE_FIRST)
1716 /* Cleanup threads no longer on a timeout but still on the
1717 * list. */
1718 remove_from_list_tmo(curr);
1720 else if (TIME_BEFORE(tick, curr->tmo_tick))
1722 /* Timeout still pending - this will be the usual case */
1723 if (TIME_BEFORE(curr->tmo_tick, next_tmo_check))
1725 /* Earliest timeout found so far - move the next check up
1726 to its time */
1727 next_tmo_check = curr->tmo_tick;
1730 else
1732 /* Sleep timeout has been reached so bring the thread back to
1733 * life again. */
1734 if (state == STATE_BLOCKED_W_TMO)
1736 #if NUM_CORES > 1
1737 /* Lock the waiting thread's kernel object */
1738 struct corelock *ocl = curr->obj_cl;
1740 if (corelock_try_lock(ocl) == 0)
1742 /* Need to retry in the correct order though the need is
1743 * unlikely */
1744 UNLOCK_THREAD(curr);
1745 corelock_lock(ocl);
1746 LOCK_THREAD(curr);
1748 if (curr->state != STATE_BLOCKED_W_TMO)
1750 /* Thread was woken or removed explicitely while slot
1751 * was unlocked */
1752 corelock_unlock(ocl);
1753 remove_from_list_tmo(curr);
1754 UNLOCK_THREAD(curr);
1755 continue;
1758 #endif /* NUM_CORES */
1760 remove_from_list_l(curr->bqp, curr);
1762 #ifdef HAVE_WAKEUP_EXT_CB
1763 if (curr->wakeup_ext_cb != NULL)
1764 curr->wakeup_ext_cb(curr);
1765 #endif
1767 #ifdef HAVE_PRIORITY_SCHEDULING
1768 if (curr->blocker != NULL)
1769 wakeup_priority_protocol_release(curr);
1770 #endif
1771 corelock_unlock(ocl);
1773 /* else state == STATE_SLEEPING */
1775 remove_from_list_tmo(curr);
1777 RTR_LOCK(core);
1779 curr->state = STATE_RUNNING;
1781 add_to_list_l(&cores[core].running, curr);
1782 rtr_add_entry(core, curr->priority);
1784 RTR_UNLOCK(core);
1787 UNLOCK_THREAD(curr);
1790 cores[core].next_tmo_check = next_tmo_check;
1793 /*---------------------------------------------------------------------------
1794 * Performs operations that must be done before blocking a thread but after
1795 * the state is saved.
1796 *---------------------------------------------------------------------------
1798 #if NUM_CORES > 1
1799 static inline void run_blocking_ops(
1800 unsigned int core, struct thread_entry *thread)
1802 struct thread_blk_ops *ops = &cores[core].blk_ops;
1803 const unsigned flags = ops->flags;
1805 if (flags == TBOP_CLEAR)
1806 return;
1808 switch (flags)
1810 case TBOP_SWITCH_CORE:
1811 core_switch_blk_op(core, thread);
1812 /* Fall-through */
1813 case TBOP_UNLOCK_CORELOCK:
1814 corelock_unlock(ops->cl_p);
1815 break;
1818 ops->flags = TBOP_CLEAR;
1820 #endif /* NUM_CORES > 1 */
1822 #ifdef RB_PROFILE
1823 void profile_thread(void)
1825 profstart(cores[CURRENT_CORE].running - threads);
1827 #endif
1829 /*---------------------------------------------------------------------------
1830 * Prepares a thread to block on an object's list and/or for a specified
1831 * duration - expects object and slot to be appropriately locked if needed
1832 * and interrupts to be masked.
1833 *---------------------------------------------------------------------------
1835 static inline void block_thread_on_l(struct thread_entry *thread,
1836 unsigned state)
1838 /* If inlined, unreachable branches will be pruned with no size penalty
1839 because state is passed as a constant parameter. */
1840 const unsigned int core = IF_COP_CORE(thread->core);
1842 /* Remove the thread from the list of running threads. */
1843 RTR_LOCK(core);
1844 remove_from_list_l(&cores[core].running, thread);
1845 rtr_subtract_entry(core, thread->priority);
1846 RTR_UNLOCK(core);
1848 /* Add a timeout to the block if not infinite */
1849 switch (state)
1851 case STATE_BLOCKED:
1852 case STATE_BLOCKED_W_TMO:
1853 /* Put the thread into a new list of inactive threads. */
1854 add_to_list_l(thread->bqp, thread);
1856 if (state == STATE_BLOCKED)
1857 break;
1859 /* Fall-through */
1860 case STATE_SLEEPING:
1861 /* If this thread times out sooner than any other thread, update
1862 next_tmo_check to its timeout */
1863 if (TIME_BEFORE(thread->tmo_tick, cores[core].next_tmo_check))
1865 cores[core].next_tmo_check = thread->tmo_tick;
1868 if (thread->tmo.prev == NULL)
1870 add_to_list_tmo(thread);
1872 /* else thread was never removed from list - just keep it there */
1873 break;
1876 /* Remember the the next thread about to block. */
1877 cores[core].block_task = thread;
1879 /* Report new state. */
1880 thread->state = state;
1883 /*---------------------------------------------------------------------------
1884 * Switch thread in round robin fashion for any given priority. Any thread
1885 * that removed itself from the running list first must specify itself in
1886 * the paramter.
1888 * INTERNAL: Intended for use by kernel and not for programs.
1889 *---------------------------------------------------------------------------
1891 void switch_thread(void)
1893 const unsigned int core = CURRENT_CORE;
1894 struct thread_entry *block = cores[core].block_task;
1895 struct thread_entry *thread = cores[core].running;
1897 /* Get context to save - next thread to run is unknown until all wakeups
1898 * are evaluated */
1899 if (block != NULL)
1901 cores[core].block_task = NULL;
1903 #if NUM_CORES > 1
1904 if (thread == block)
1906 /* This was the last thread running and another core woke us before
1907 * reaching here. Force next thread selection to give tmo threads or
1908 * other threads woken before this block a first chance. */
1909 block = NULL;
1911 else
1912 #endif
1914 /* Blocking task is the old one */
1915 thread = block;
1919 #ifdef RB_PROFILE
1920 profile_thread_stopped(thread - threads);
1921 #endif
1923 /* Begin task switching by saving our current context so that we can
1924 * restore the state of the current thread later to the point prior
1925 * to this call. */
1926 store_context(&thread->context);
1928 /* Check if the current thread stack is overflown */
1929 if (thread->stack[0] != DEADBEEF)
1930 thread_stkov(thread);
1932 #if NUM_CORES > 1
1933 /* Run any blocking operations requested before switching/sleeping */
1934 run_blocking_ops(core, thread);
1935 #endif
1937 #ifdef HAVE_PRIORITY_SCHEDULING
1938 IF_NO_SKIP_YIELD( if (thread->skip_count != -1) )
1939 /* Reset the value of thread's skip count */
1940 thread->skip_count = 0;
1941 #endif
1943 for (;;)
1945 /* If there are threads on a timeout and the earliest wakeup is due,
1946 * check the list and wake any threads that need to start running
1947 * again. */
1948 if (!TIME_BEFORE(current_tick, cores[core].next_tmo_check))
1950 check_tmo_threads();
1953 disable_irq();
1954 RTR_LOCK(core);
1956 thread = cores[core].running;
1958 if (thread == NULL)
1960 /* Enter sleep mode to reduce power usage - woken up on interrupt
1961 * or wakeup request from another core - expected to enable
1962 * interrupts. */
1963 RTR_UNLOCK(core);
1964 core_sleep(IF_COP(core));
1966 else
1968 #ifdef HAVE_PRIORITY_SCHEDULING
1969 /* Select the new task based on priorities and the last time a
1970 * process got CPU time relative to the highest priority runnable
1971 * task. */
1972 struct priority_distribution *pd = &cores[core].rtr;
1973 int max = find_first_set_bit(pd->mask);
1975 if (block == NULL)
1977 /* Not switching on a block, tentatively select next thread */
1978 thread = thread->l.next;
1981 for (;;)
1983 int priority = thread->priority;
1984 int diff;
1986 /* This ridiculously simple method of aging seems to work
1987 * suspiciously well. It does tend to reward CPU hogs (under
1988 * yielding) but that's generally not desirable at all. On the
1989 * plus side, it, relatively to other threads, penalizes excess
1990 * yielding which is good if some high priority thread is
1991 * performing no useful work such as polling for a device to be
1992 * ready. Of course, aging is only employed when higher and lower
1993 * priority threads are runnable. The highest priority runnable
1994 * thread(s) are never skipped. */
1995 if (priority <= max ||
1996 IF_NO_SKIP_YIELD( thread->skip_count == -1 || )
1997 (diff = priority - max, ++thread->skip_count > diff*diff))
1999 cores[core].running = thread;
2000 break;
2003 thread = thread->l.next;
2005 #else
2006 /* Without priority use a simple FCFS algorithm */
2007 if (block == NULL)
2009 /* Not switching on a block, select next thread */
2010 thread = thread->l.next;
2011 cores[core].running = thread;
2013 #endif /* HAVE_PRIORITY_SCHEDULING */
2015 RTR_UNLOCK(core);
2016 enable_irq();
2017 break;
2021 /* And finally give control to the next thread. */
2022 load_context(&thread->context);
2024 #ifdef RB_PROFILE
2025 profile_thread_started(thread - threads);
2026 #endif
2029 /*---------------------------------------------------------------------------
2030 * Sleeps a thread for at least a specified number of ticks with zero being
2031 * a wait until the next tick.
2033 * INTERNAL: Intended for use by kernel and not for programs.
2034 *---------------------------------------------------------------------------
2036 void sleep_thread(int ticks)
2038 struct thread_entry *current = cores[CURRENT_CORE].running;
2040 LOCK_THREAD(current);
2042 /* Set our timeout, remove from run list and join timeout list. */
2043 current->tmo_tick = current_tick + ticks + 1;
2044 block_thread_on_l(current, STATE_SLEEPING);
2046 UNLOCK_THREAD(current);
2049 /*---------------------------------------------------------------------------
2050 * Indefinitely block a thread on a blocking queue for explicit wakeup.
2052 * INTERNAL: Intended for use by kernel objects and not for programs.
2053 *---------------------------------------------------------------------------
2055 void block_thread(struct thread_entry *current)
2057 /* Set the state to blocked and take us off of the run queue until we
2058 * are explicitly woken */
2059 LOCK_THREAD(current);
2061 /* Set the list for explicit wakeup */
2062 block_thread_on_l(current, STATE_BLOCKED);
2064 #ifdef HAVE_PRIORITY_SCHEDULING
2065 if (current->blocker != NULL)
2067 /* Object supports PIP */
2068 current = blocker_inherit_priority(current);
2070 #endif
2072 UNLOCK_THREAD(current);
2075 /*---------------------------------------------------------------------------
2076 * Block a thread on a blocking queue for a specified time interval or until
2077 * explicitly woken - whichever happens first.
2079 * INTERNAL: Intended for use by kernel objects and not for programs.
2080 *---------------------------------------------------------------------------
2082 void block_thread_w_tmo(struct thread_entry *current, int timeout)
2084 /* Get the entry for the current running thread. */
2085 LOCK_THREAD(current);
2087 /* Set the state to blocked with the specified timeout */
2088 current->tmo_tick = current_tick + timeout;
2090 /* Set the list for explicit wakeup */
2091 block_thread_on_l(current, STATE_BLOCKED_W_TMO);
2093 #ifdef HAVE_PRIORITY_SCHEDULING
2094 if (current->blocker != NULL)
2096 /* Object supports PIP */
2097 current = blocker_inherit_priority(current);
2099 #endif
2101 UNLOCK_THREAD(current);
2104 /*---------------------------------------------------------------------------
2105 * Explicitly wakeup a thread on a blocking queue. Only effects threads of
2106 * STATE_BLOCKED and STATE_BLOCKED_W_TMO.
2108 * This code should be considered a critical section by the caller meaning
2109 * that the object's corelock should be held.
2111 * INTERNAL: Intended for use by kernel objects and not for programs.
2112 *---------------------------------------------------------------------------
2114 unsigned int wakeup_thread(struct thread_entry **list)
2116 struct thread_entry *thread = *list;
2117 unsigned int result = THREAD_NONE;
2119 /* Check if there is a blocked thread at all. */
2120 if (thread == NULL)
2121 return result;
2123 LOCK_THREAD(thread);
2125 /* Determine thread's current state. */
2126 switch (thread->state)
2128 case STATE_BLOCKED:
2129 case STATE_BLOCKED_W_TMO:
2130 remove_from_list_l(list, thread);
2132 result = THREAD_OK;
2134 #ifdef HAVE_PRIORITY_SCHEDULING
2135 struct thread_entry *current;
2136 struct blocker *bl = thread->blocker;
2138 if (bl == NULL)
2140 /* No inheritance - just boost the thread by aging */
2141 IF_NO_SKIP_YIELD( if (thread->skip_count != -1) )
2142 thread->skip_count = thread->priority;
2143 current = cores[CURRENT_CORE].running;
2145 else
2147 /* Call the specified unblocking PIP */
2148 current = bl->wakeup_protocol(thread);
2151 if (current != NULL && thread->priority < current->priority
2152 IF_COP( && thread->core == current->core ))
2154 /* Woken thread is higher priority and exists on the same CPU core;
2155 * recommend a task switch. Knowing if this is an interrupt call
2156 * would be helpful here. */
2157 result |= THREAD_SWITCH;
2159 #endif /* HAVE_PRIORITY_SCHEDULING */
2161 core_schedule_wakeup(thread);
2162 break;
2164 /* Nothing to do. State is not blocked. */
2165 #if THREAD_EXTRA_CHECKS
2166 default:
2167 THREAD_PANICF("wakeup_thread->block invalid", thread);
2168 case STATE_RUNNING:
2169 case STATE_KILLED:
2170 break;
2171 #endif
2174 UNLOCK_THREAD(thread);
2175 return result;
2178 /*---------------------------------------------------------------------------
2179 * Wakeup an entire queue of threads - returns bitwise-or of return bitmask
2180 * from each operation or THREAD_NONE of nothing was awakened. Object owning
2181 * the queue must be locked first.
2183 * INTERNAL: Intended for use by kernel objects and not for programs.
2184 *---------------------------------------------------------------------------
2186 unsigned int thread_queue_wake(struct thread_entry **list)
2188 unsigned result = THREAD_NONE;
2190 for (;;)
2192 unsigned int rc = wakeup_thread(list);
2194 if (rc == THREAD_NONE)
2195 break; /* No more threads */
2197 result |= rc;
2200 return result;
2203 /*---------------------------------------------------------------------------
2204 * Find an empty thread slot or MAXTHREADS if none found. The slot returned
2205 * will be locked on multicore.
2206 *---------------------------------------------------------------------------
2208 static struct thread_entry * find_empty_thread_slot(void)
2210 /* Any slot could be on an interrupt-accessible list */
2211 IF_COP( int oldlevel = disable_irq_save(); )
2212 struct thread_entry *thread = NULL;
2213 int n;
2215 for (n = 0; n < MAXTHREADS; n++)
2217 /* Obtain current slot state - lock it on multicore */
2218 struct thread_entry *t = &threads[n];
2219 LOCK_THREAD(t);
2221 if (t->state == STATE_KILLED IF_COP( && t->name != THREAD_DESTRUCT ))
2223 /* Slot is empty - leave it locked and caller will unlock */
2224 thread = t;
2225 break;
2228 /* Finished examining slot - no longer busy - unlock on multicore */
2229 UNLOCK_THREAD(t);
2232 IF_COP( restore_irq(oldlevel); ) /* Reenable interrups - this slot is
2233 not accesible to them yet */
2234 return thread;
2238 /*---------------------------------------------------------------------------
2239 * Place the current core in idle mode - woken up on interrupt or wake
2240 * request from another core.
2241 *---------------------------------------------------------------------------
2243 void core_idle(void)
2245 IF_COP( const unsigned int core = CURRENT_CORE; )
2246 disable_irq();
2247 core_sleep(IF_COP(core));
2250 /*---------------------------------------------------------------------------
2251 * Create a thread. If using a dual core architecture, specify which core to
2252 * start the thread on.
2254 * Return ID if context area could be allocated, else NULL.
2255 *---------------------------------------------------------------------------
2257 struct thread_entry*
2258 create_thread(void (*function)(void), void* stack, size_t stack_size,
2259 unsigned flags, const char *name
2260 IF_PRIO(, int priority)
2261 IF_COP(, unsigned int core))
2263 unsigned int i;
2264 unsigned int stack_words;
2265 uintptr_t stackptr, stackend;
2266 struct thread_entry *thread;
2267 unsigned state;
2268 int oldlevel;
2270 thread = find_empty_thread_slot();
2271 if (thread == NULL)
2273 return NULL;
2276 oldlevel = disable_irq_save();
2278 /* Munge the stack to make it easy to spot stack overflows */
2279 stackptr = ALIGN_UP((uintptr_t)stack, sizeof (uintptr_t));
2280 stackend = ALIGN_DOWN((uintptr_t)stack + stack_size, sizeof (uintptr_t));
2281 stack_size = stackend - stackptr;
2282 stack_words = stack_size / sizeof (uintptr_t);
2284 for (i = 0; i < stack_words; i++)
2286 ((uintptr_t *)stackptr)[i] = DEADBEEF;
2289 /* Store interesting information */
2290 thread->name = name;
2291 thread->stack = (uintptr_t *)stackptr;
2292 thread->stack_size = stack_size;
2293 thread->queue = NULL;
2294 #ifdef HAVE_WAKEUP_EXT_CB
2295 thread->wakeup_ext_cb = NULL;
2296 #endif
2297 #ifdef HAVE_SCHEDULER_BOOSTCTRL
2298 thread->cpu_boost = 0;
2299 #endif
2300 #ifdef HAVE_PRIORITY_SCHEDULING
2301 memset(&thread->pdist, 0, sizeof(thread->pdist));
2302 thread->blocker = NULL;
2303 thread->base_priority = priority;
2304 thread->priority = priority;
2305 thread->skip_count = priority;
2306 prio_add_entry(&thread->pdist, priority);
2307 #endif
2309 #if NUM_CORES > 1
2310 thread->core = core;
2312 /* Writeback stack munging or anything else before starting */
2313 if (core != CURRENT_CORE)
2315 flush_icache();
2317 #endif
2319 /* Thread is not on any timeout list but be a bit paranoid */
2320 thread->tmo.prev = NULL;
2322 state = (flags & CREATE_THREAD_FROZEN) ?
2323 STATE_FROZEN : STATE_RUNNING;
2325 thread->context.sp = (typeof (thread->context.sp))stackend;
2327 /* Load the thread's context structure with needed startup information */
2328 THREAD_STARTUP_INIT(core, thread, function);
2330 thread->state = state;
2332 if (state == STATE_RUNNING)
2333 core_schedule_wakeup(thread);
2335 UNLOCK_THREAD(thread);
2337 restore_irq(oldlevel);
2339 return thread;
2342 #ifdef HAVE_SCHEDULER_BOOSTCTRL
2343 /*---------------------------------------------------------------------------
2344 * Change the boost state of a thread boosting or unboosting the CPU
2345 * as required.
2346 *---------------------------------------------------------------------------
2348 static inline void boost_thread(struct thread_entry *thread, bool boost)
2350 if ((thread->cpu_boost != 0) != boost)
2352 thread->cpu_boost = boost;
2353 cpu_boost(boost);
2357 void trigger_cpu_boost(void)
2359 struct thread_entry *current = cores[CURRENT_CORE].running;
2360 boost_thread(current, true);
2363 void cancel_cpu_boost(void)
2365 struct thread_entry *current = cores[CURRENT_CORE].running;
2366 boost_thread(current, false);
2368 #endif /* HAVE_SCHEDULER_BOOSTCTRL */
2370 /*---------------------------------------------------------------------------
2371 * Block the current thread until another thread terminates. A thread may
2372 * wait on itself to terminate which prevents it from running again and it
2373 * will need to be killed externally.
2374 * Parameter is the ID as returned from create_thread().
2375 *---------------------------------------------------------------------------
2377 void thread_wait(struct thread_entry *thread)
2379 struct thread_entry *current = cores[CURRENT_CORE].running;
2381 if (thread == NULL)
2382 thread = current;
2384 /* Lock thread-as-waitable-object lock */
2385 corelock_lock(&thread->waiter_cl);
2387 /* Be sure it hasn't been killed yet */
2388 if (thread->state != STATE_KILLED)
2390 IF_COP( current->obj_cl = &thread->waiter_cl; )
2391 current->bqp = &thread->queue;
2393 disable_irq();
2394 block_thread(current);
2396 corelock_unlock(&thread->waiter_cl);
2398 switch_thread();
2399 return;
2402 corelock_unlock(&thread->waiter_cl);
2405 /*---------------------------------------------------------------------------
2406 * Exit the current thread. The Right Way to Do Things (TM).
2407 *---------------------------------------------------------------------------
2409 void thread_exit(void)
2411 const unsigned int core = CURRENT_CORE;
2412 struct thread_entry *current = cores[core].running;
2414 /* Cancel CPU boost if any */
2415 cancel_cpu_boost();
2417 disable_irq();
2419 corelock_lock(&current->waiter_cl);
2420 LOCK_THREAD(current);
2422 #if defined (ALLOW_REMOVE_THREAD) && NUM_CORES > 1
2423 if (current->name == THREAD_DESTRUCT)
2425 /* Thread being killed - become a waiter */
2426 UNLOCK_THREAD(current);
2427 corelock_unlock(&current->waiter_cl);
2428 thread_wait(current);
2429 THREAD_PANICF("thread_exit->WK:*R", current);
2431 #endif
2433 #ifdef HAVE_PRIORITY_SCHEDULING
2434 check_for_obj_waiters("thread_exit", current);
2435 #endif
2437 if (current->tmo.prev != NULL)
2439 /* Cancel pending timeout list removal */
2440 remove_from_list_tmo(current);
2443 /* Switch tasks and never return */
2444 block_thread_on_l(current, STATE_KILLED);
2446 #if NUM_CORES > 1
2447 /* Switch to the idle stack if not on the main core (where "main"
2448 * runs) - we can hope gcc doesn't need the old stack beyond this
2449 * point. */
2450 if (core != CPU)
2452 switch_to_idle_stack(core);
2455 flush_icache();
2456 #endif
2457 current->name = NULL;
2459 /* Signal this thread */
2460 thread_queue_wake(&current->queue);
2461 corelock_unlock(&current->waiter_cl);
2462 /* Slot must be unusable until thread is really gone */
2463 UNLOCK_THREAD_AT_TASK_SWITCH(current);
2464 switch_thread();
2465 /* This should never and must never be reached - if it is, the
2466 * state is corrupted */
2467 THREAD_PANICF("thread_exit->K:*R", current);
2470 #ifdef ALLOW_REMOVE_THREAD
2471 /*---------------------------------------------------------------------------
2472 * Remove a thread from the scheduler. Not The Right Way to Do Things in
2473 * normal programs.
2475 * Parameter is the ID as returned from create_thread().
2477 * Use with care on threads that are not under careful control as this may
2478 * leave various objects in an undefined state.
2479 *---------------------------------------------------------------------------
2481 void remove_thread(struct thread_entry *thread)
2483 #if NUM_CORES > 1
2484 /* core is not constant here because of core switching */
2485 unsigned int core = CURRENT_CORE;
2486 unsigned int old_core = NUM_CORES;
2487 struct corelock *ocl = NULL;
2488 #else
2489 const unsigned int core = CURRENT_CORE;
2490 #endif
2491 struct thread_entry *current = cores[core].running;
2493 unsigned state;
2494 int oldlevel;
2496 if (thread == NULL)
2497 thread = current;
2499 if (thread == current)
2500 thread_exit(); /* Current thread - do normal exit */
2502 oldlevel = disable_irq_save();
2504 corelock_lock(&thread->waiter_cl);
2505 LOCK_THREAD(thread);
2507 state = thread->state;
2509 if (state == STATE_KILLED)
2511 goto thread_killed;
2514 #if NUM_CORES > 1
2515 if (thread->name == THREAD_DESTRUCT)
2517 /* Thread being killed - become a waiter */
2518 UNLOCK_THREAD(thread);
2519 corelock_unlock(&thread->waiter_cl);
2520 restore_irq(oldlevel);
2521 thread_wait(thread);
2522 return;
2525 thread->name = THREAD_DESTRUCT; /* Slot can't be used for now */
2527 #ifdef HAVE_PRIORITY_SCHEDULING
2528 check_for_obj_waiters("remove_thread", thread);
2529 #endif
2531 if (thread->core != core)
2533 /* Switch cores and safely extract the thread there */
2534 /* Slot HAS to be unlocked or a deadlock could occur which means other
2535 * threads have to be guided into becoming thread waiters if they
2536 * attempt to remove it. */
2537 unsigned int new_core = thread->core;
2539 corelock_unlock(&thread->waiter_cl);
2541 UNLOCK_THREAD(thread);
2542 restore_irq(oldlevel);
2544 old_core = switch_core(new_core);
2546 oldlevel = disable_irq_save();
2548 corelock_lock(&thread->waiter_cl);
2549 LOCK_THREAD(thread);
2551 state = thread->state;
2552 core = new_core;
2553 /* Perform the extraction and switch ourselves back to the original
2554 processor */
2556 #endif /* NUM_CORES > 1 */
2558 if (thread->tmo.prev != NULL)
2560 /* Clean thread off the timeout list if a timeout check hasn't
2561 * run yet */
2562 remove_from_list_tmo(thread);
2565 #ifdef HAVE_SCHEDULER_BOOSTCTRL
2566 /* Cancel CPU boost if any */
2567 boost_thread(thread, false);
2568 #endif
2570 IF_COP( retry_state: )
2572 switch (state)
2574 case STATE_RUNNING:
2575 RTR_LOCK(core);
2576 /* Remove thread from ready to run tasks */
2577 remove_from_list_l(&cores[core].running, thread);
2578 rtr_subtract_entry(core, thread->priority);
2579 RTR_UNLOCK(core);
2580 break;
2581 case STATE_BLOCKED:
2582 case STATE_BLOCKED_W_TMO:
2583 /* Remove thread from the queue it's blocked on - including its
2584 * own if waiting there */
2585 #if NUM_CORES > 1
2586 if (&thread->waiter_cl != thread->obj_cl)
2588 ocl = thread->obj_cl;
2590 if (corelock_try_lock(ocl) == 0)
2592 UNLOCK_THREAD(thread);
2593 corelock_lock(ocl);
2594 LOCK_THREAD(thread);
2596 if (thread->state != state)
2598 /* Something woke the thread */
2599 state = thread->state;
2600 corelock_unlock(ocl);
2601 goto retry_state;
2605 #endif
2606 remove_from_list_l(thread->bqp, thread);
2608 #ifdef HAVE_WAKEUP_EXT_CB
2609 if (thread->wakeup_ext_cb != NULL)
2610 thread->wakeup_ext_cb(thread);
2611 #endif
2613 #ifdef HAVE_PRIORITY_SCHEDULING
2614 if (thread->blocker != NULL)
2616 /* Remove thread's priority influence from its chain */
2617 wakeup_priority_protocol_release(thread);
2619 #endif
2621 #if NUM_CORES > 1
2622 if (ocl != NULL)
2623 corelock_unlock(ocl);
2624 #endif
2625 break;
2626 /* Otherwise thread is frozen and hasn't run yet */
2629 thread->state = STATE_KILLED;
2631 /* If thread was waiting on itself, it will have been removed above.
2632 * The wrong order would result in waking the thread first and deadlocking
2633 * since the slot is already locked. */
2634 thread_queue_wake(&thread->queue);
2636 thread->name = NULL;
2638 thread_killed: /* Thread was already killed */
2639 /* Removal complete - safe to unlock and reenable interrupts */
2640 corelock_unlock(&thread->waiter_cl);
2641 UNLOCK_THREAD(thread);
2642 restore_irq(oldlevel);
2644 #if NUM_CORES > 1
2645 if (old_core < NUM_CORES)
2647 /* Did a removal on another processor's thread - switch back to
2648 native core */
2649 switch_core(old_core);
2651 #endif
2653 #endif /* ALLOW_REMOVE_THREAD */
2655 #ifdef HAVE_PRIORITY_SCHEDULING
2656 /*---------------------------------------------------------------------------
2657 * Sets the thread's relative base priority for the core it runs on. Any
2658 * needed inheritance changes also may happen.
2659 *---------------------------------------------------------------------------
2661 int thread_set_priority(struct thread_entry *thread, int priority)
2663 int old_base_priority = -1;
2665 /* A little safety measure */
2666 if (priority < HIGHEST_PRIORITY || priority > LOWEST_PRIORITY)
2667 return -1;
2669 if (thread == NULL)
2670 thread = cores[CURRENT_CORE].running;
2672 /* Thread could be on any list and therefore on an interrupt accessible
2673 one - disable interrupts */
2674 int oldlevel = disable_irq_save();
2676 LOCK_THREAD(thread);
2678 /* Make sure it's not killed */
2679 if (thread->state != STATE_KILLED)
2681 int old_priority = thread->priority;
2683 old_base_priority = thread->base_priority;
2684 thread->base_priority = priority;
2686 prio_move_entry(&thread->pdist, old_base_priority, priority);
2687 priority = find_first_set_bit(thread->pdist.mask);
2689 if (old_priority == priority)
2691 /* No priority change - do nothing */
2693 else if (thread->state == STATE_RUNNING)
2695 /* This thread is running - change location on the run
2696 * queue. No transitive inheritance needed. */
2697 set_running_thread_priority(thread, priority);
2699 else
2701 thread->priority = priority;
2703 if (thread->blocker != NULL)
2705 /* Bubble new priority down the chain */
2706 struct blocker *bl = thread->blocker; /* Blocker struct */
2707 struct thread_entry *bl_t = bl->thread; /* Blocking thread */
2708 struct thread_entry * const tstart = thread; /* Initial thread */
2709 const int highest = MIN(priority, old_priority); /* Higher of new or old */
2711 for (;;)
2713 struct thread_entry *next; /* Next thread to check */
2714 int bl_pr; /* Highest blocked thread */
2715 int queue_pr; /* New highest blocked thread */
2716 #if NUM_CORES > 1
2717 /* Owner can change but thread cannot be dislodged - thread
2718 * may not be the first in the queue which allows other
2719 * threads ahead in the list to be given ownership during the
2720 * operation. If thread is next then the waker will have to
2721 * wait for us and the owner of the object will remain fixed.
2722 * If we successfully grab the owner -- which at some point
2723 * is guaranteed -- then the queue remains fixed until we
2724 * pass by. */
2725 for (;;)
2727 LOCK_THREAD(bl_t);
2729 /* Double-check the owner - retry if it changed */
2730 if (bl->thread == bl_t)
2731 break;
2733 UNLOCK_THREAD(bl_t);
2734 bl_t = bl->thread;
2736 #endif
2737 bl_pr = bl->priority;
2739 if (highest > bl_pr)
2740 break; /* Object priority won't change */
2742 /* This will include the thread being set */
2743 queue_pr = find_highest_priority_in_list_l(*thread->bqp);
2745 if (queue_pr == bl_pr)
2746 break; /* Object priority not changing */
2748 /* Update thread boost for this object */
2749 bl->priority = queue_pr;
2750 prio_move_entry(&bl_t->pdist, bl_pr, queue_pr);
2751 bl_pr = find_first_set_bit(bl_t->pdist.mask);
2753 if (bl_t->priority == bl_pr)
2754 break; /* Blocking thread priority not changing */
2756 if (bl_t->state == STATE_RUNNING)
2758 /* Thread not blocked - we're done */
2759 set_running_thread_priority(bl_t, bl_pr);
2760 break;
2763 bl_t->priority = bl_pr;
2764 bl = bl_t->blocker; /* Blocking thread has a blocker? */
2766 if (bl == NULL)
2767 break; /* End of chain */
2769 next = bl->thread;
2771 if (next == tstart)
2772 break; /* Full-circle */
2774 UNLOCK_THREAD(thread);
2776 thread = bl_t;
2777 bl_t = next;
2778 } /* for (;;) */
2780 UNLOCK_THREAD(bl_t);
2785 UNLOCK_THREAD(thread);
2787 restore_irq(oldlevel);
2789 return old_base_priority;
2792 /*---------------------------------------------------------------------------
2793 * Returns the current base priority for a thread.
2794 *---------------------------------------------------------------------------
2796 int thread_get_priority(struct thread_entry *thread)
2798 /* Simple, quick probe. */
2799 if (thread == NULL)
2800 thread = cores[CURRENT_CORE].running;
2802 return thread->base_priority;
2804 #endif /* HAVE_PRIORITY_SCHEDULING */
2806 /*---------------------------------------------------------------------------
2807 * Starts a frozen thread - similar semantics to wakeup_thread except that
2808 * the thread is on no scheduler or wakeup queue at all. It exists simply by
2809 * virtue of the slot having a state of STATE_FROZEN.
2810 *---------------------------------------------------------------------------
2812 void thread_thaw(struct thread_entry *thread)
2814 int oldlevel = disable_irq_save();
2815 LOCK_THREAD(thread);
2817 if (thread->state == STATE_FROZEN)
2818 core_schedule_wakeup(thread);
2820 UNLOCK_THREAD(thread);
2821 restore_irq(oldlevel);
2824 /*---------------------------------------------------------------------------
2825 * Return the ID of the currently executing thread.
2826 *---------------------------------------------------------------------------
2828 struct thread_entry * thread_get_current(void)
2830 return cores[CURRENT_CORE].running;
2833 #if NUM_CORES > 1
2834 /*---------------------------------------------------------------------------
2835 * Switch the processor that the currently executing thread runs on.
2836 *---------------------------------------------------------------------------
2838 unsigned int switch_core(unsigned int new_core)
2840 const unsigned int core = CURRENT_CORE;
2841 struct thread_entry *current = cores[core].running;
2843 if (core == new_core)
2845 /* No change - just return same core */
2846 return core;
2849 int oldlevel = disable_irq_save();
2850 LOCK_THREAD(current);
2852 if (current->name == THREAD_DESTRUCT)
2854 /* Thread being killed - deactivate and let process complete */
2855 UNLOCK_THREAD(current);
2856 restore_irq(oldlevel);
2857 thread_wait(current);
2858 /* Should never be reached */
2859 THREAD_PANICF("switch_core->D:*R", current);
2862 /* Get us off the running list for the current core */
2863 RTR_LOCK(core);
2864 remove_from_list_l(&cores[core].running, current);
2865 rtr_subtract_entry(core, current->priority);
2866 RTR_UNLOCK(core);
2868 /* Stash return value (old core) in a safe place */
2869 current->retval = core;
2871 /* If a timeout hadn't yet been cleaned-up it must be removed now or
2872 * the other core will likely attempt a removal from the wrong list! */
2873 if (current->tmo.prev != NULL)
2875 remove_from_list_tmo(current);
2878 /* Change the core number for this thread slot */
2879 current->core = new_core;
2881 /* Do not use core_schedule_wakeup here since this will result in
2882 * the thread starting to run on the other core before being finished on
2883 * this one. Delay the list unlock to keep the other core stuck
2884 * until this thread is ready. */
2885 RTR_LOCK(new_core);
2887 rtr_add_entry(new_core, current->priority);
2888 add_to_list_l(&cores[new_core].running, current);
2890 /* Make a callback into device-specific code, unlock the wakeup list so
2891 * that execution may resume on the new core, unlock our slot and finally
2892 * restore the interrupt level */
2893 cores[core].blk_ops.flags = TBOP_SWITCH_CORE;
2894 cores[core].blk_ops.cl_p = &cores[new_core].rtr_cl;
2895 cores[core].block_task = current;
2897 UNLOCK_THREAD(current);
2899 /* Alert other core to activity */
2900 core_wake(new_core);
2902 /* Do the stack switching, cache_maintenence and switch_thread call -
2903 requires native code */
2904 switch_thread_core(core, current);
2906 /* Finally return the old core to caller */
2907 return current->retval;
2909 #endif /* NUM_CORES > 1 */
2911 /*---------------------------------------------------------------------------
2912 * Initialize threading API. This assumes interrupts are not yet enabled. On
2913 * multicore setups, no core is allowed to proceed until create_thread calls
2914 * are safe to perform.
2915 *---------------------------------------------------------------------------
2917 void init_threads(void)
2919 const unsigned int core = CURRENT_CORE;
2920 struct thread_entry *thread;
2922 /* CPU will initialize first and then sleep */
2923 thread = find_empty_thread_slot();
2925 if (thread == NULL)
2927 /* WTF? There really must be a slot available at this stage.
2928 * This can fail if, for example, .bss isn't zero'ed out by the loader
2929 * or threads is in the wrong section. */
2930 THREAD_PANICF("init_threads->no slot", NULL);
2933 /* Initialize initially non-zero members of core */
2934 cores[core].next_tmo_check = current_tick; /* Something not in the past */
2936 /* Initialize initially non-zero members of slot */
2937 UNLOCK_THREAD(thread); /* No sync worries yet */
2938 thread->name = main_thread_name;
2939 thread->state = STATE_RUNNING;
2940 IF_COP( thread->core = core; )
2941 #ifdef HAVE_PRIORITY_SCHEDULING
2942 corelock_init(&cores[core].rtr_cl);
2943 thread->base_priority = PRIORITY_USER_INTERFACE;
2944 prio_add_entry(&thread->pdist, PRIORITY_USER_INTERFACE);
2945 thread->priority = PRIORITY_USER_INTERFACE;
2946 rtr_add_entry(core, PRIORITY_USER_INTERFACE);
2947 #endif
2948 corelock_init(&thread->waiter_cl);
2949 corelock_init(&thread->slot_cl);
2951 add_to_list_l(&cores[core].running, thread);
2953 if (core == CPU)
2955 thread->stack = stackbegin;
2956 thread->stack_size = (uintptr_t)stackend - (uintptr_t)stackbegin;
2957 #if NUM_CORES > 1 /* This code path will not be run on single core targets */
2958 /* TODO: HAL interface for this */
2959 /* Wake up coprocessor and let it initialize kernel and threads */
2960 #ifdef CPU_PP502x
2961 MBX_MSG_CLR = 0x3f;
2962 #endif
2963 COP_CTL = PROC_WAKE;
2964 /* Sleep until finished */
2965 CPU_CTL = PROC_SLEEP;
2966 nop; nop; nop; nop;
2968 else
2970 /* Initial stack is the COP idle stack */
2971 thread->stack = cop_idlestackbegin;
2972 thread->stack_size = IDLE_STACK_SIZE;
2973 /* Get COP safely primed inside switch_thread where it will remain
2974 * until a thread actually exists on it */
2975 CPU_CTL = PROC_WAKE;
2976 thread_exit();
2977 #endif /* NUM_CORES */
2981 /* Shared stack scan helper for thread_stack_usage and idle_stack_usage */
2982 #if NUM_CORES == 1
2983 static inline int stack_usage(uintptr_t *stackptr, size_t stack_size)
2984 #else
2985 static int stack_usage(uintptr_t *stackptr, size_t stack_size)
2986 #endif
2988 unsigned int stack_words = stack_size / sizeof (uintptr_t);
2989 unsigned int i;
2990 int usage = 0;
2992 for (i = 0; i < stack_words; i++)
2994 if (stackptr[i] != DEADBEEF)
2996 usage = ((stack_words - i) * 100) / stack_words;
2997 break;
3001 return usage;
3004 /*---------------------------------------------------------------------------
3005 * Returns the maximum percentage of stack a thread ever used while running.
3006 * NOTE: Some large buffer allocations that don't use enough the buffer to
3007 * overwrite stackptr[0] will not be seen.
3008 *---------------------------------------------------------------------------
3010 int thread_stack_usage(const struct thread_entry *thread)
3012 return stack_usage(thread->stack, thread->stack_size);
3015 #if NUM_CORES > 1
3016 /*---------------------------------------------------------------------------
3017 * Returns the maximum percentage of the core's idle stack ever used during
3018 * runtime.
3019 *---------------------------------------------------------------------------
3021 int idle_stack_usage(unsigned int core)
3023 return stack_usage(idle_stacks[core], IDLE_STACK_SIZE);
3025 #endif
3027 /*---------------------------------------------------------------------------
3028 * Fills in the buffer with the specified thread's name. If the name is NULL,
3029 * empty, or the thread is in destruct state a formatted ID is written
3030 * instead.
3031 *---------------------------------------------------------------------------
3033 void thread_get_name(char *buffer, int size,
3034 struct thread_entry *thread)
3036 if (size <= 0)
3037 return;
3039 *buffer = '\0';
3041 if (thread)
3043 /* Display thread name if one or ID if none */
3044 const char *name = thread->name;
3045 const char *fmt = "%s";
3046 if (name == NULL IF_COP(|| name == THREAD_DESTRUCT) || *name == '\0')
3048 name = (const char *)thread;
3049 fmt = "%08lX";
3051 snprintf(buffer, size, fmt, name);