fuzev2: prevent button light flickering when accessing µSD
[kugel-rb.git] / firmware / thread.c
blobd3031d55e319c102c706b929f43d0a8fda2d18cb
1 /***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
10 * Copyright (C) 2002 by Ulf Ralberg
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
20 ****************************************************************************/
21 #include "config.h"
22 #include <stdbool.h>
23 #include <stdio.h>
24 #include "thread.h"
25 #include "panic.h"
26 #include "system.h"
27 #include "kernel.h"
28 #include "cpu.h"
29 #include "string.h"
30 #ifdef RB_PROFILE
31 #include <profile.h>
32 #endif
33 /****************************************************************************
34 * ATTENTION!! *
35 * See notes below on implementing processor-specific portions! *
36 ***************************************************************************/
38 /* Define THREAD_EXTRA_CHECKS as 1 to enable additional state checks */
39 #ifdef DEBUG
40 #define THREAD_EXTRA_CHECKS 1 /* Always 1 for DEBUG */
41 #else
42 #define THREAD_EXTRA_CHECKS 0
43 #endif
45 /**
46 * General locking order to guarantee progress. Order must be observed but
47 * all stages are not nescessarily obligatory. Going from 1) to 3) is
48 * perfectly legal.
50 * 1) IRQ
51 * This is first because of the likelyhood of having an interrupt occur that
52 * also accesses one of the objects farther down the list. Any non-blocking
53 * synchronization done may already have a lock on something during normal
54 * execution and if an interrupt handler running on the same processor as
55 * the one that has the resource locked were to attempt to access the
56 * resource, the interrupt handler would wait forever waiting for an unlock
57 * that will never happen. There is no danger if the interrupt occurs on
58 * a different processor because the one that has the lock will eventually
59 * unlock and the other processor's handler may proceed at that time. Not
60 * nescessary when the resource in question is definitely not available to
61 * interrupt handlers.
63 * 2) Kernel Object
64 * 1) May be needed beforehand if the kernel object allows dual-use such as
65 * event queues. The kernel object must have a scheme to protect itself from
66 * access by another processor and is responsible for serializing the calls
67 * to block_thread(_w_tmo) and wakeup_thread both to themselves and to each
68 * other. Objects' queues are also protected here.
70 * 3) Thread Slot
71 * This locks access to the thread's slot such that its state cannot be
72 * altered by another processor when a state change is in progress such as
73 * when it is in the process of going on a blocked list. An attempt to wake
74 * a thread while it is still blocking will likely desync its state with
75 * the other resources used for that state.
77 * 4) Core Lists
78 * These lists are specific to a particular processor core and are accessible
79 * by all processor cores and interrupt handlers. The running (rtr) list is
80 * the prime example where a thread may be added by any means.
83 /*---------------------------------------------------------------------------
84 * Processor specific: core_sleep/core_wake/misc. notes
86 * ARM notes:
87 * FIQ is not dealt with by the scheduler code and is simply restored if it
88 * must by masked for some reason - because threading modifies a register
89 * that FIQ may also modify and there's no way to accomplish it atomically.
90 * s3c2440 is such a case.
92 * Audio interrupts are generally treated at a higher priority than others
93 * usage of scheduler code with interrupts higher than HIGHEST_IRQ_LEVEL
94 * are not in general safe. Special cases may be constructed on a per-
95 * source basis and blocking operations are not available.
97 * core_sleep procedure to implement for any CPU to ensure an asychronous
98 * wakup never results in requiring a wait until the next tick (up to
99 * 10000uS!). May require assembly and careful instruction ordering.
101 * 1) On multicore, stay awake if directed to do so by another. If so, goto
102 * step 4.
103 * 2) If processor requires, atomically reenable interrupts and perform step
104 * 3.
105 * 3) Sleep the CPU core. If wakeup itself enables interrupts (stop #0x2000
106 * on Coldfire) goto step 5.
107 * 4) Enable interrupts.
108 * 5) Exit procedure.
110 * core_wake and multprocessor notes for sleep/wake coordination:
111 * If possible, to wake up another processor, the forcing of an interrupt on
112 * the woken core by the waker core is the easiest way to ensure a non-
113 * delayed wake and immediate execution of any woken threads. If that isn't
114 * available then some careful non-blocking synchonization is needed (as on
115 * PP targets at the moment).
116 *---------------------------------------------------------------------------
119 /* Cast to the the machine pointer size, whose size could be < 4 or > 32
120 * (someday :). */
121 #define DEADBEEF ((uintptr_t)0xdeadbeefdeadbeefull)
122 static struct core_entry cores[NUM_CORES] IBSS_ATTR;
123 struct thread_entry threads[MAXTHREADS] IBSS_ATTR;
125 static const char main_thread_name[] = "main";
126 extern uintptr_t stackbegin[];
127 extern uintptr_t stackend[];
129 static inline void core_sleep(IF_COP_VOID(unsigned int core))
130 __attribute__((always_inline));
132 void check_tmo_threads(void)
133 __attribute__((noinline));
135 static inline void block_thread_on_l(struct thread_entry *thread, unsigned state)
136 __attribute__((always_inline));
138 static void add_to_list_tmo(struct thread_entry *thread)
139 __attribute__((noinline));
141 static void core_schedule_wakeup(struct thread_entry *thread)
142 __attribute__((noinline));
144 #if NUM_CORES > 1
145 static inline void run_blocking_ops(
146 unsigned int core, struct thread_entry *thread)
147 __attribute__((always_inline));
148 #endif
150 static void thread_stkov(struct thread_entry *thread)
151 __attribute__((noinline));
153 static inline void store_context(void* addr)
154 __attribute__((always_inline));
156 static inline void load_context(const void* addr)
157 __attribute__((always_inline));
159 void switch_thread(void)
160 __attribute__((noinline));
162 /****************************************************************************
163 * Processor-specific section
166 #if defined(MAX_PHYS_SECTOR_SIZE) && MEM == 64
167 /* Support a special workaround object for large-sector disks */
168 #define IF_NO_SKIP_YIELD(...) __VA_ARGS__
169 #else
170 #define IF_NO_SKIP_YIELD(...)
171 #endif
173 #if defined(CPU_ARM)
174 /*---------------------------------------------------------------------------
175 * Start the thread running and terminate it if it returns
176 *---------------------------------------------------------------------------
178 static void __attribute__((naked,used)) start_thread(void)
180 /* r0 = context */
181 asm volatile (
182 "ldr sp, [r0, #32] \n" /* Load initial sp */
183 "ldr r4, [r0, #40] \n" /* start in r4 since it's non-volatile */
184 "mov r1, #0 \n" /* Mark thread as running */
185 "str r1, [r0, #40] \n"
186 #if NUM_CORES > 1
187 "ldr r0, =cpucache_invalidate \n" /* Invalidate this core's cache. */
188 "mov lr, pc \n" /* This could be the first entry into */
189 "bx r0 \n" /* plugin or codec code for this core. */
190 #endif
191 "mov lr, pc \n" /* Call thread function */
192 "bx r4 \n"
193 ); /* No clobber list - new thread doesn't care */
194 thread_exit();
195 //asm volatile (".ltorg"); /* Dump constant pool */
198 /* For startup, place context pointer in r4 slot, start_thread pointer in r5
199 * slot, and thread function pointer in context.start. See load_context for
200 * what happens when thread is initially going to run. */
201 #define THREAD_STARTUP_INIT(core, thread, function) \
202 ({ (thread)->context.r[0] = (uint32_t)&(thread)->context, \
203 (thread)->context.r[1] = (uint32_t)start_thread, \
204 (thread)->context.start = (uint32_t)function; })
206 /*---------------------------------------------------------------------------
207 * Store non-volatile context.
208 *---------------------------------------------------------------------------
210 static inline void store_context(void* addr)
212 asm volatile(
213 "stmia %0, { r4-r11, sp, lr } \n"
214 : : "r" (addr)
218 /*---------------------------------------------------------------------------
219 * Load non-volatile context.
220 *---------------------------------------------------------------------------
222 static inline void load_context(const void* addr)
224 asm volatile(
225 "ldr r0, [%0, #40] \n" /* Load start pointer */
226 "cmp r0, #0 \n" /* Check for NULL */
227 "ldmneia %0, { r0, pc } \n" /* If not already running, jump to start */
228 "ldmia %0, { r4-r11, sp, lr } \n" /* Load regs r4 to r14 from context */
229 : : "r" (addr) : "r0" /* only! */
233 #if defined (CPU_PP)
235 #if NUM_CORES > 1
236 extern uintptr_t cpu_idlestackbegin[];
237 extern uintptr_t cpu_idlestackend[];
238 extern uintptr_t cop_idlestackbegin[];
239 extern uintptr_t cop_idlestackend[];
240 static uintptr_t * const idle_stacks[NUM_CORES] =
242 [CPU] = cpu_idlestackbegin,
243 [COP] = cop_idlestackbegin
246 #if CONFIG_CPU == PP5002
247 /* Bytes to emulate the PP502x mailbox bits */
248 struct core_semaphores
250 volatile uint8_t intend_wake; /* 00h */
251 volatile uint8_t stay_awake; /* 01h */
252 volatile uint8_t intend_sleep; /* 02h */
253 volatile uint8_t unused; /* 03h */
256 static struct core_semaphores core_semaphores[NUM_CORES] IBSS_ATTR;
257 #endif /* CONFIG_CPU == PP5002 */
259 #endif /* NUM_CORES */
261 #if CONFIG_CORELOCK == SW_CORELOCK
262 /* Software core locks using Peterson's mutual exclusion algorithm */
264 /*---------------------------------------------------------------------------
265 * Initialize the corelock structure.
266 *---------------------------------------------------------------------------
268 void corelock_init(struct corelock *cl)
270 memset(cl, 0, sizeof (*cl));
273 #if 1 /* Assembly locks to minimize overhead */
274 /*---------------------------------------------------------------------------
275 * Wait for the corelock to become free and acquire it when it does.
276 *---------------------------------------------------------------------------
278 void corelock_lock(struct corelock *cl) __attribute__((naked));
279 void corelock_lock(struct corelock *cl)
281 /* Relies on the fact that core IDs are complementary bitmasks (0x55,0xaa) */
282 asm volatile (
283 "mov r1, %0 \n" /* r1 = PROCESSOR_ID */
284 "ldrb r1, [r1] \n"
285 "strb r1, [r0, r1, lsr #7] \n" /* cl->myl[core] = core */
286 "eor r2, r1, #0xff \n" /* r2 = othercore */
287 "strb r2, [r0, #2] \n" /* cl->turn = othercore */
288 "1: \n"
289 "ldrb r3, [r0, r2, lsr #7] \n" /* cl->myl[othercore] == 0 ? */
290 "cmp r3, #0 \n" /* yes? lock acquired */
291 "bxeq lr \n"
292 "ldrb r3, [r0, #2] \n" /* || cl->turn == core ? */
293 "cmp r3, r1 \n"
294 "bxeq lr \n" /* yes? lock acquired */
295 "b 1b \n" /* keep trying */
296 : : "i"(&PROCESSOR_ID)
298 (void)cl;
301 /*---------------------------------------------------------------------------
302 * Try to aquire the corelock. If free, caller gets it, otherwise return 0.
303 *---------------------------------------------------------------------------
305 int corelock_try_lock(struct corelock *cl) __attribute__((naked));
306 int corelock_try_lock(struct corelock *cl)
308 /* Relies on the fact that core IDs are complementary bitmasks (0x55,0xaa) */
309 asm volatile (
310 "mov r1, %0 \n" /* r1 = PROCESSOR_ID */
311 "ldrb r1, [r1] \n"
312 "mov r3, r0 \n"
313 "strb r1, [r0, r1, lsr #7] \n" /* cl->myl[core] = core */
314 "eor r2, r1, #0xff \n" /* r2 = othercore */
315 "strb r2, [r0, #2] \n" /* cl->turn = othercore */
316 "ldrb r0, [r3, r2, lsr #7] \n" /* cl->myl[othercore] == 0 ? */
317 "eors r0, r0, r2 \n" /* yes? lock acquired */
318 "bxne lr \n"
319 "ldrb r0, [r3, #2] \n" /* || cl->turn == core? */
320 "ands r0, r0, r1 \n"
321 "streqb r0, [r3, r1, lsr #7] \n" /* if not, cl->myl[core] = 0 */
322 "bx lr \n" /* return result */
323 : : "i"(&PROCESSOR_ID)
326 return 0;
327 (void)cl;
330 /*---------------------------------------------------------------------------
331 * Release ownership of the corelock
332 *---------------------------------------------------------------------------
334 void corelock_unlock(struct corelock *cl) __attribute__((naked));
335 void corelock_unlock(struct corelock *cl)
337 asm volatile (
338 "mov r1, %0 \n" /* r1 = PROCESSOR_ID */
339 "ldrb r1, [r1] \n"
340 "mov r2, #0 \n" /* cl->myl[core] = 0 */
341 "strb r2, [r0, r1, lsr #7] \n"
342 "bx lr \n"
343 : : "i"(&PROCESSOR_ID)
345 (void)cl;
347 #else /* C versions for reference */
348 /*---------------------------------------------------------------------------
349 * Wait for the corelock to become free and aquire it when it does.
350 *---------------------------------------------------------------------------
352 void corelock_lock(struct corelock *cl)
354 const unsigned int core = CURRENT_CORE;
355 const unsigned int othercore = 1 - core;
357 cl->myl[core] = core;
358 cl->turn = othercore;
360 for (;;)
362 if (cl->myl[othercore] == 0 || cl->turn == core)
363 break;
367 /*---------------------------------------------------------------------------
368 * Try to aquire the corelock. If free, caller gets it, otherwise return 0.
369 *---------------------------------------------------------------------------
371 int corelock_try_lock(struct corelock *cl)
373 const unsigned int core = CURRENT_CORE;
374 const unsigned int othercore = 1 - core;
376 cl->myl[core] = core;
377 cl->turn = othercore;
379 if (cl->myl[othercore] == 0 || cl->turn == core)
381 return 1;
384 cl->myl[core] = 0;
385 return 0;
388 /*---------------------------------------------------------------------------
389 * Release ownership of the corelock
390 *---------------------------------------------------------------------------
392 void corelock_unlock(struct corelock *cl)
394 cl->myl[CURRENT_CORE] = 0;
396 #endif /* ASM / C selection */
398 #endif /* CONFIG_CORELOCK == SW_CORELOCK */
400 /*---------------------------------------------------------------------------
401 * Put core in a power-saving state if waking list wasn't repopulated and if
402 * no other core requested a wakeup for it to perform a task.
403 *---------------------------------------------------------------------------
405 #ifdef CPU_PP502x
406 #if NUM_CORES == 1
407 static inline void core_sleep(void)
409 sleep_core(CURRENT_CORE);
410 enable_irq();
412 #else
413 static inline void core_sleep(unsigned int core)
415 #if 1
416 asm volatile (
417 "mov r0, #4 \n" /* r0 = 0x4 << core */
418 "mov r0, r0, lsl %[c] \n"
419 "str r0, [%[mbx], #4] \n" /* signal intent to sleep */
420 "ldr r1, [%[mbx], #0] \n" /* && !(MBX_MSG_STAT & (0x10<<core)) ? */
421 "tst r1, r0, lsl #2 \n"
422 "moveq r1, #0x80000000 \n" /* Then sleep */
423 "streq r1, [%[ctl], %[c], lsl #2] \n"
424 "moveq r1, #0 \n" /* Clear control reg */
425 "streq r1, [%[ctl], %[c], lsl #2] \n"
426 "orr r1, r0, r0, lsl #2 \n" /* Signal intent to wake - clear wake flag */
427 "str r1, [%[mbx], #8] \n"
428 "1: \n" /* Wait for wake procedure to finish */
429 "ldr r1, [%[mbx], #0] \n"
430 "tst r1, r0, lsr #2 \n"
431 "bne 1b \n"
433 : [ctl]"r"(&CPU_CTL), [mbx]"r"(MBX_BASE), [c]"r"(core)
434 : "r0", "r1");
435 #else /* C version for reference */
436 /* Signal intent to sleep */
437 MBX_MSG_SET = 0x4 << core;
439 /* Something waking or other processor intends to wake us? */
440 if ((MBX_MSG_STAT & (0x10 << core)) == 0)
442 sleep_core(core);
443 wake_core(core);
446 /* Signal wake - clear wake flag */
447 MBX_MSG_CLR = 0x14 << core;
449 /* Wait for other processor to finish wake procedure */
450 while (MBX_MSG_STAT & (0x1 << core));
451 #endif /* ASM/C selection */
452 enable_irq();
454 #endif /* NUM_CORES */
455 #elif CONFIG_CPU == PP5002
456 #if NUM_CORES == 1
457 static inline void core_sleep(void)
459 sleep_core(CURRENT_CORE);
460 enable_irq();
462 #else
463 /* PP5002 has no mailboxes - emulate using bytes */
464 static inline void core_sleep(unsigned int core)
466 #if 1
467 asm volatile (
468 "mov r0, #1 \n" /* Signal intent to sleep */
469 "strb r0, [%[sem], #2] \n"
470 "ldrb r0, [%[sem], #1] \n" /* && stay_awake == 0? */
471 "cmp r0, #0 \n"
472 "bne 2f \n"
473 /* Sleep: PP5002 crashes if the instruction that puts it to sleep is
474 * located at 0xNNNNNNN0. 4/8/C works. This sequence makes sure
475 * that the correct alternative is executed. Don't change the order
476 * of the next 4 instructions! */
477 "tst pc, #0x0c \n"
478 "mov r0, #0xca \n"
479 "strne r0, [%[ctl], %[c], lsl #2] \n"
480 "streq r0, [%[ctl], %[c], lsl #2] \n"
481 "nop \n" /* nop's needed because of pipeline */
482 "nop \n"
483 "nop \n"
484 "2: \n"
485 "mov r0, #0 \n" /* Clear stay_awake and sleep intent */
486 "strb r0, [%[sem], #1] \n"
487 "strb r0, [%[sem], #2] \n"
488 "1: \n" /* Wait for wake procedure to finish */
489 "ldrb r0, [%[sem], #0] \n"
490 "cmp r0, #0 \n"
491 "bne 1b \n"
493 : [sem]"r"(&core_semaphores[core]), [c]"r"(core),
494 [ctl]"r"(&CPU_CTL)
495 : "r0"
497 #else /* C version for reference */
498 /* Signal intent to sleep */
499 core_semaphores[core].intend_sleep = 1;
501 /* Something waking or other processor intends to wake us? */
502 if (core_semaphores[core].stay_awake == 0)
504 sleep_core(core);
507 /* Signal wake - clear wake flag */
508 core_semaphores[core].stay_awake = 0;
509 core_semaphores[core].intend_sleep = 0;
511 /* Wait for other processor to finish wake procedure */
512 while (core_semaphores[core].intend_wake != 0);
514 /* Enable IRQ */
515 #endif /* ASM/C selection */
516 enable_irq();
518 #endif /* NUM_CORES */
519 #endif /* PP CPU type */
521 /*---------------------------------------------------------------------------
522 * Wake another processor core that is sleeping or prevent it from doing so
523 * if it was already destined. FIQ, IRQ should be disabled before calling.
524 *---------------------------------------------------------------------------
526 #if NUM_CORES == 1
527 /* Shared single-core build debugging version */
528 void core_wake(void)
530 /* No wakey - core already wakey */
532 #elif defined (CPU_PP502x)
533 void core_wake(unsigned int othercore)
535 #if 1
536 /* avoid r0 since that contains othercore */
537 asm volatile (
538 "mrs r3, cpsr \n" /* Disable IRQ */
539 "orr r1, r3, #0x80 \n"
540 "msr cpsr_c, r1 \n"
541 "mov r2, #0x11 \n" /* r2 = (0x11 << othercore) */
542 "mov r2, r2, lsl %[oc] \n" /* Signal intent to wake othercore */
543 "str r2, [%[mbx], #4] \n"
544 "1: \n" /* If it intends to sleep, let it first */
545 "ldr r1, [%[mbx], #0] \n" /* (MSG_MSG_STAT & (0x4 << othercore)) != 0 ? */
546 "eor r1, r1, #0xc \n"
547 "tst r1, r2, lsr #2 \n"
548 "ldr r1, [%[ctl], %[oc], lsl #2] \n" /* && (PROC_CTL(othercore) & PROC_SLEEP) == 0 ? */
549 "tsteq r1, #0x80000000 \n"
550 "beq 1b \n" /* Wait for sleep or wake */
551 "tst r1, #0x80000000 \n" /* If sleeping, wake it */
552 "movne r1, #0x0 \n"
553 "strne r1, [%[ctl], %[oc], lsl #2] \n"
554 "mov r1, r2, lsr #4 \n"
555 "str r1, [%[mbx], #8] \n" /* Done with wake procedure */
556 "msr cpsr_c, r3 \n" /* Restore IRQ */
558 : [ctl]"r"(&PROC_CTL(CPU)), [mbx]"r"(MBX_BASE),
559 [oc]"r"(othercore)
560 : "r1", "r2", "r3");
561 #else /* C version for reference */
562 /* Disable interrupts - avoid reentrancy from the tick */
563 int oldlevel = disable_irq_save();
565 /* Signal intent to wake other processor - set stay awake */
566 MBX_MSG_SET = 0x11 << othercore;
568 /* If it intends to sleep, wait until it does or aborts */
569 while ((MBX_MSG_STAT & (0x4 << othercore)) != 0 &&
570 (PROC_CTL(othercore) & PROC_SLEEP) == 0);
572 /* If sleeping, wake it up */
573 if (PROC_CTL(othercore) & PROC_SLEEP)
574 PROC_CTL(othercore) = 0;
576 /* Done with wake procedure */
577 MBX_MSG_CLR = 0x1 << othercore;
578 restore_irq(oldlevel);
579 #endif /* ASM/C selection */
581 #elif CONFIG_CPU == PP5002
582 /* PP5002 has no mailboxes - emulate using bytes */
583 void core_wake(unsigned int othercore)
585 #if 1
586 /* avoid r0 since that contains othercore */
587 asm volatile (
588 "mrs r3, cpsr \n" /* Disable IRQ */
589 "orr r1, r3, #0x80 \n"
590 "msr cpsr_c, r1 \n"
591 "mov r1, #1 \n" /* Signal intent to wake other core */
592 "orr r1, r1, r1, lsl #8 \n" /* and set stay_awake */
593 "strh r1, [%[sem], #0] \n"
594 "mov r2, #0x8000 \n"
595 "1: \n" /* If it intends to sleep, let it first */
596 "ldrb r1, [%[sem], #2] \n" /* intend_sleep != 0 ? */
597 "cmp r1, #1 \n"
598 "ldr r1, [%[st]] \n" /* && not sleeping ? */
599 "tsteq r1, r2, lsr %[oc] \n"
600 "beq 1b \n" /* Wait for sleep or wake */
601 "tst r1, r2, lsr %[oc] \n"
602 "ldrne r2, =0xcf004054 \n" /* If sleeping, wake it */
603 "movne r1, #0xce \n"
604 "strne r1, [r2, %[oc], lsl #2] \n"
605 "mov r1, #0 \n" /* Done with wake procedure */
606 "strb r1, [%[sem], #0] \n"
607 "msr cpsr_c, r3 \n" /* Restore IRQ */
609 : [sem]"r"(&core_semaphores[othercore]),
610 [st]"r"(&PROC_STAT),
611 [oc]"r"(othercore)
612 : "r1", "r2", "r3"
614 #else /* C version for reference */
615 /* Disable interrupts - avoid reentrancy from the tick */
616 int oldlevel = disable_irq_save();
618 /* Signal intent to wake other processor - set stay awake */
619 core_semaphores[othercore].intend_wake = 1;
620 core_semaphores[othercore].stay_awake = 1;
622 /* If it intends to sleep, wait until it does or aborts */
623 while (core_semaphores[othercore].intend_sleep != 0 &&
624 (PROC_STAT & PROC_SLEEPING(othercore)) == 0);
626 /* If sleeping, wake it up */
627 if (PROC_STAT & PROC_SLEEPING(othercore))
628 wake_core(othercore);
630 /* Done with wake procedure */
631 core_semaphores[othercore].intend_wake = 0;
632 restore_irq(oldlevel);
633 #endif /* ASM/C selection */
635 #endif /* CPU type */
637 #if NUM_CORES > 1
638 /*---------------------------------------------------------------------------
639 * Switches to a stack that always resides in the Rockbox core.
641 * Needed when a thread suicides on a core other than the main CPU since the
642 * stack used when idling is the stack of the last thread to run. This stack
643 * may not reside in the core firmware in which case the core will continue
644 * to use a stack from an unloaded module until another thread runs on it.
645 *---------------------------------------------------------------------------
647 static inline void switch_to_idle_stack(const unsigned int core)
649 asm volatile (
650 "str sp, [%0] \n" /* save original stack pointer on idle stack */
651 "mov sp, %0 \n" /* switch stacks */
652 : : "r"(&idle_stacks[core][IDLE_STACK_WORDS-1]));
653 (void)core;
656 /*---------------------------------------------------------------------------
657 * Perform core switch steps that need to take place inside switch_thread.
659 * These steps must take place while before changing the processor and after
660 * having entered switch_thread since switch_thread may not do a normal return
661 * because the stack being used for anything the compiler saved will not belong
662 * to the thread's destination core and it may have been recycled for other
663 * purposes by the time a normal context load has taken place. switch_thread
664 * will also clobber anything stashed in the thread's context or stored in the
665 * nonvolatile registers if it is saved there before the call since the
666 * compiler's order of operations cannot be known for certain.
668 static void core_switch_blk_op(unsigned int core, struct thread_entry *thread)
670 /* Flush our data to ram */
671 cpucache_flush();
672 /* Stash thread in r4 slot */
673 thread->context.r[0] = (uint32_t)thread;
674 /* Stash restart address in r5 slot */
675 thread->context.r[1] = thread->context.start;
676 /* Save sp in context.sp while still running on old core */
677 thread->context.sp = idle_stacks[core][IDLE_STACK_WORDS-1];
680 /*---------------------------------------------------------------------------
681 * Machine-specific helper function for switching the processor a thread is
682 * running on. Basically, the thread suicides on the departing core and is
683 * reborn on the destination. Were it not for gcc's ill-behavior regarding
684 * naked functions written in C where it actually clobbers non-volatile
685 * registers before the intended prologue code, this would all be much
686 * simpler. Generic setup is done in switch_core itself.
689 /*---------------------------------------------------------------------------
690 * This actually performs the core switch.
692 static void __attribute__((naked))
693 switch_thread_core(unsigned int core, struct thread_entry *thread)
695 /* Pure asm for this because compiler behavior isn't sufficiently predictable.
696 * Stack access also isn't permitted until restoring the original stack and
697 * context. */
698 asm volatile (
699 "stmfd sp!, { r4-r11, lr } \n" /* Stack all non-volatile context on current core */
700 "ldr r2, =idle_stacks \n" /* r2 = &idle_stacks[core][IDLE_STACK_WORDS] */
701 "ldr r2, [r2, r0, lsl #2] \n"
702 "add r2, r2, %0*4 \n"
703 "stmfd r2!, { sp } \n" /* save original stack pointer on idle stack */
704 "mov sp, r2 \n" /* switch stacks */
705 "adr r2, 1f \n" /* r2 = new core restart address */
706 "str r2, [r1, #40] \n" /* thread->context.start = r2 */
707 "ldr pc, =switch_thread \n" /* r0 = thread after call - see load_context */
708 "1: \n"
709 "ldr sp, [r0, #32] \n" /* Reload original sp from context structure */
710 "mov r1, #0 \n" /* Clear start address */
711 "str r1, [r0, #40] \n"
712 "ldr r0, =cpucache_invalidate \n" /* Invalidate new core's cache */
713 "mov lr, pc \n"
714 "bx r0 \n"
715 "ldmfd sp!, { r4-r11, pc } \n" /* Restore non-volatile context to new core and return */
716 ".ltorg \n" /* Dump constant pool */
717 : : "i"(IDLE_STACK_WORDS)
719 (void)core; (void)thread;
722 /*---------------------------------------------------------------------------
723 * Do any device-specific inits for the threads and synchronize the kernel
724 * initializations.
725 *---------------------------------------------------------------------------
727 static void core_thread_init(unsigned int core) INIT_ATTR;
728 static void core_thread_init(unsigned int core)
730 if (core == CPU)
732 /* Wake up coprocessor and let it initialize kernel and threads */
733 #ifdef CPU_PP502x
734 MBX_MSG_CLR = 0x3f;
735 #endif
736 wake_core(COP);
737 /* Sleep until COP has finished */
738 sleep_core(CPU);
740 else
742 /* Wake the CPU and return */
743 wake_core(CPU);
746 #endif /* NUM_CORES */
748 #elif defined(CPU_TCC780X) || defined(CPU_TCC77X) /* Single core only for now */ \
749 || CONFIG_CPU == IMX31L || CONFIG_CPU == DM320 || CONFIG_CPU == AS3525 \
750 || CONFIG_CPU == S3C2440 || CONFIG_CPU == S5L8701 || CONFIG_CPU == AS3525v2
751 /* Use the generic ARMv4/v5/v6 wait for IRQ */
752 static inline void core_sleep(void)
754 asm volatile (
755 "mcr p15, 0, %0, c7, c0, 4" /* Wait for interrupt */
756 : : "r"(0)
758 enable_irq();
760 #else
761 static inline void core_sleep(void)
763 #warning core_sleep not implemented, battery life will be decreased
764 enable_irq();
766 #endif /* CONFIG_CPU == */
768 #elif defined(CPU_COLDFIRE)
769 /*---------------------------------------------------------------------------
770 * Start the thread running and terminate it if it returns
771 *---------------------------------------------------------------------------
773 void start_thread(void); /* Provide C access to ASM label */
774 static void __attribute__((used)) __start_thread(void)
776 /* a0=macsr, a1=context */
777 asm volatile (
778 "start_thread: \n" /* Start here - no naked attribute */
779 "move.l %a0, %macsr \n" /* Set initial mac status reg */
780 "lea.l 48(%a1), %a1 \n"
781 "move.l (%a1)+, %sp \n" /* Set initial stack */
782 "move.l (%a1), %a2 \n" /* Fetch thread function pointer */
783 "clr.l (%a1) \n" /* Mark thread running */
784 "jsr (%a2) \n" /* Call thread function */
786 thread_exit();
789 /* Set EMAC unit to fractional mode with saturation for each new thread,
790 * since that's what'll be the most useful for most things which the dsp
791 * will do. Codecs should still initialize their preferred modes
792 * explicitly. Context pointer is placed in d2 slot and start_thread
793 * pointer in d3 slot. thread function pointer is placed in context.start.
794 * See load_context for what happens when thread is initially going to
795 * run.
797 #define THREAD_STARTUP_INIT(core, thread, function) \
798 ({ (thread)->context.macsr = EMAC_FRACTIONAL | EMAC_SATURATE, \
799 (thread)->context.d[0] = (uint32_t)&(thread)->context, \
800 (thread)->context.d[1] = (uint32_t)start_thread, \
801 (thread)->context.start = (uint32_t)(function); })
803 /*---------------------------------------------------------------------------
804 * Store non-volatile context.
805 *---------------------------------------------------------------------------
807 static inline void store_context(void* addr)
809 asm volatile (
810 "move.l %%macsr,%%d0 \n"
811 "movem.l %%d0/%%d2-%%d7/%%a2-%%a7,(%0) \n"
812 : : "a" (addr) : "d0" /* only! */
816 /*---------------------------------------------------------------------------
817 * Load non-volatile context.
818 *---------------------------------------------------------------------------
820 static inline void load_context(const void* addr)
822 asm volatile (
823 "move.l 52(%0), %%d0 \n" /* Get start address */
824 "beq.b 1f \n" /* NULL -> already running */
825 "movem.l (%0), %%a0-%%a2 \n" /* a0=macsr, a1=context, a2=start_thread */
826 "jmp (%%a2) \n" /* Start the thread */
827 "1: \n"
828 "movem.l (%0), %%d0/%%d2-%%d7/%%a2-%%a7 \n" /* Load context */
829 "move.l %%d0, %%macsr \n"
830 : : "a" (addr) : "d0" /* only! */
834 /*---------------------------------------------------------------------------
835 * Put core in a power-saving state if waking list wasn't repopulated.
836 *---------------------------------------------------------------------------
838 static inline void core_sleep(void)
840 /* Supervisor mode, interrupts enabled upon wakeup */
841 asm volatile ("stop #0x2000");
844 #elif CONFIG_CPU == SH7034
845 /*---------------------------------------------------------------------------
846 * Start the thread running and terminate it if it returns
847 *---------------------------------------------------------------------------
849 void start_thread(void); /* Provide C access to ASM label */
850 static void __attribute__((used)) __start_thread(void)
852 /* r8 = context */
853 asm volatile (
854 "_start_thread: \n" /* Start here - no naked attribute */
855 "mov.l @(4, r8), r0 \n" /* Fetch thread function pointer */
856 "mov.l @(28, r8), r15 \n" /* Set initial sp */
857 "mov #0, r1 \n" /* Start the thread */
858 "jsr @r0 \n"
859 "mov.l r1, @(36, r8) \n" /* Clear start address */
861 thread_exit();
864 /* Place context pointer in r8 slot, function pointer in r9 slot, and
865 * start_thread pointer in context_start */
866 #define THREAD_STARTUP_INIT(core, thread, function) \
867 ({ (thread)->context.r[0] = (uint32_t)&(thread)->context, \
868 (thread)->context.r[1] = (uint32_t)(function), \
869 (thread)->context.start = (uint32_t)start_thread; })
871 /*---------------------------------------------------------------------------
872 * Store non-volatile context.
873 *---------------------------------------------------------------------------
875 static inline void store_context(void* addr)
877 asm volatile (
878 "add #36, %0 \n" /* Start at last reg. By the time routine */
879 "sts.l pr, @-%0 \n" /* is done, %0 will have the original value */
880 "mov.l r15,@-%0 \n"
881 "mov.l r14,@-%0 \n"
882 "mov.l r13,@-%0 \n"
883 "mov.l r12,@-%0 \n"
884 "mov.l r11,@-%0 \n"
885 "mov.l r10,@-%0 \n"
886 "mov.l r9, @-%0 \n"
887 "mov.l r8, @-%0 \n"
888 : : "r" (addr)
892 /*---------------------------------------------------------------------------
893 * Load non-volatile context.
894 *---------------------------------------------------------------------------
896 static inline void load_context(const void* addr)
898 asm volatile (
899 "mov.l @(36, %0), r0 \n" /* Get start address */
900 "tst r0, r0 \n"
901 "bt .running \n" /* NULL -> already running */
902 "jmp @r0 \n" /* r8 = context */
903 ".running: \n"
904 "mov.l @%0+, r8 \n" /* Executes in delay slot and outside it */
905 "mov.l @%0+, r9 \n"
906 "mov.l @%0+, r10 \n"
907 "mov.l @%0+, r11 \n"
908 "mov.l @%0+, r12 \n"
909 "mov.l @%0+, r13 \n"
910 "mov.l @%0+, r14 \n"
911 "mov.l @%0+, r15 \n"
912 "lds.l @%0+, pr \n"
913 : : "r" (addr) : "r0" /* only! */
917 /*---------------------------------------------------------------------------
918 * Put core in a power-saving state.
919 *---------------------------------------------------------------------------
921 static inline void core_sleep(void)
923 asm volatile (
924 "and.b #0x7f, @(r0, gbr) \n" /* Clear SBY (bit 7) in SBYCR */
925 "mov #0, r1 \n" /* Enable interrupts */
926 "ldc r1, sr \n" /* Following instruction cannot be interrupted */
927 "sleep \n" /* Execute standby */
928 : : "z"(&SBYCR-GBR) : "r1");
931 #elif defined(CPU_MIPS) && CPU_MIPS == 32
933 /*---------------------------------------------------------------------------
934 * Start the thread running and terminate it if it returns
935 *---------------------------------------------------------------------------
938 void start_thread(void); /* Provide C access to ASM label */
939 static void __attribute__((used)) _start_thread(void)
941 /* t1 = context */
942 asm volatile (
943 "start_thread: \n"
944 ".set noreorder \n"
945 ".set noat \n"
946 "lw $8, 4($9) \n" /* Fetch thread function pointer ($8 = t0, $9 = t1) */
947 "lw $29, 36($9) \n" /* Set initial sp(=$29) */
948 "jalr $8 \n" /* Start the thread */
949 "sw $0, 44($9) \n" /* Clear start address */
950 ".set at \n"
951 ".set reorder \n"
953 thread_exit();
956 /* Place context pointer in s0 slot, function pointer in s1 slot, and
957 * start_thread pointer in context_start */
958 #define THREAD_STARTUP_INIT(core, thread, function) \
959 ({ (thread)->context.r[0] = (uint32_t)&(thread)->context, \
960 (thread)->context.r[1] = (uint32_t)(function), \
961 (thread)->context.start = (uint32_t)start_thread; })
963 /*---------------------------------------------------------------------------
964 * Store non-volatile context.
965 *---------------------------------------------------------------------------
967 static inline void store_context(void* addr)
969 asm volatile (
970 ".set noreorder \n"
971 ".set noat \n"
972 "sw $16, 0(%0) \n" /* s0 */
973 "sw $17, 4(%0) \n" /* s1 */
974 "sw $18, 8(%0) \n" /* s2 */
975 "sw $19, 12(%0) \n" /* s3 */
976 "sw $20, 16(%0) \n" /* s4 */
977 "sw $21, 20(%0) \n" /* s5 */
978 "sw $22, 24(%0) \n" /* s6 */
979 "sw $23, 28(%0) \n" /* s7 */
980 "sw $30, 32(%0) \n" /* fp */
981 "sw $29, 36(%0) \n" /* sp */
982 "sw $31, 40(%0) \n" /* ra */
983 ".set at \n"
984 ".set reorder \n"
985 : : "r" (addr)
989 /*---------------------------------------------------------------------------
990 * Load non-volatile context.
991 *---------------------------------------------------------------------------
993 static inline void load_context(const void* addr)
995 asm volatile (
996 ".set noat \n"
997 ".set noreorder \n"
998 "lw $8, 44(%0) \n" /* Get start address ($8 = t0) */
999 "beqz $8, running \n" /* NULL -> already running */
1000 "nop \n"
1001 "jr $8 \n"
1002 "move $9, %0 \n" /* t1 = context */
1003 "running: \n"
1004 "lw $16, 0(%0) \n" /* s0 */
1005 "lw $17, 4(%0) \n" /* s1 */
1006 "lw $18, 8(%0) \n" /* s2 */
1007 "lw $19, 12(%0) \n" /* s3 */
1008 "lw $20, 16(%0) \n" /* s4 */
1009 "lw $21, 20(%0) \n" /* s5 */
1010 "lw $22, 24(%0) \n" /* s6 */
1011 "lw $23, 28(%0) \n" /* s7 */
1012 "lw $30, 32(%0) \n" /* fp */
1013 "lw $29, 36(%0) \n" /* sp */
1014 "lw $31, 40(%0) \n" /* ra */
1015 ".set at \n"
1016 ".set reorder \n"
1017 : : "r" (addr) : "t0", "t1"
1021 /*---------------------------------------------------------------------------
1022 * Put core in a power-saving state.
1023 *---------------------------------------------------------------------------
1025 static inline void core_sleep(void)
1027 #if CONFIG_CPU == JZ4732
1028 __cpm_idle_mode();
1029 #endif
1030 asm volatile(".set mips32r2 \n"
1031 "mfc0 $8, $12 \n" /* mfc t0, $12 */
1032 "move $9, $8 \n" /* move t1, t0 */
1033 "la $10, 0x8000000 \n" /* la t2, 0x8000000 */
1034 "or $8, $8, $10 \n" /* Enable reduced power mode */
1035 "mtc0 $8, $12 \n" /* mtc t0, $12 */
1036 "wait \n"
1037 "mtc0 $9, $12 \n" /* mtc t1, $12 */
1038 ".set mips0 \n"
1039 ::: "t0", "t1", "t2"
1041 enable_irq();
1045 #endif /* CONFIG_CPU == */
1048 * End Processor-specific section
1049 ***************************************************************************/
1051 #if THREAD_EXTRA_CHECKS
1052 static void thread_panicf(const char *msg, struct thread_entry *thread)
1054 IF_COP( const unsigned int core = thread->core; )
1055 static char name[32];
1056 thread_get_name(name, 32, thread);
1057 panicf ("%s %s" IF_COP(" (%d)"), msg, name IF_COP(, core));
1059 static void thread_stkov(struct thread_entry *thread)
1061 thread_panicf("Stkov", thread);
1063 #define THREAD_PANICF(msg, thread) \
1064 thread_panicf(msg, thread)
1065 #define THREAD_ASSERT(exp, msg, thread) \
1066 ({ if (!({ exp; })) thread_panicf((msg), (thread)); })
1067 #else
1068 static void thread_stkov(struct thread_entry *thread)
1070 IF_COP( const unsigned int core = thread->core; )
1071 static char name[32];
1072 thread_get_name(name, 32, thread);
1073 panicf("Stkov %s" IF_COP(" (%d)"), name IF_COP(, core));
1075 #define THREAD_PANICF(msg, thread)
1076 #define THREAD_ASSERT(exp, msg, thread)
1077 #endif /* THREAD_EXTRA_CHECKS */
1079 /* Thread locking */
1080 #if NUM_CORES > 1
1081 #define LOCK_THREAD(thread) \
1082 ({ corelock_lock(&(thread)->slot_cl); })
1083 #define TRY_LOCK_THREAD(thread) \
1084 ({ corelock_try_lock(&thread->slot_cl); })
1085 #define UNLOCK_THREAD(thread) \
1086 ({ corelock_unlock(&(thread)->slot_cl); })
1087 #define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \
1088 ({ unsigned int _core = (thread)->core; \
1089 cores[_core].blk_ops.flags |= TBOP_UNLOCK_CORELOCK; \
1090 cores[_core].blk_ops.cl_p = &(thread)->slot_cl; })
1091 #else
1092 #define LOCK_THREAD(thread) \
1093 ({ })
1094 #define TRY_LOCK_THREAD(thread) \
1095 ({ })
1096 #define UNLOCK_THREAD(thread) \
1097 ({ })
1098 #define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \
1099 ({ })
1100 #endif
1102 /* RTR list */
1103 #define RTR_LOCK(core) \
1104 ({ corelock_lock(&cores[core].rtr_cl); })
1105 #define RTR_UNLOCK(core) \
1106 ({ corelock_unlock(&cores[core].rtr_cl); })
1108 #ifdef HAVE_PRIORITY_SCHEDULING
1109 #define rtr_add_entry(core, priority) \
1110 prio_add_entry(&cores[core].rtr, (priority))
1112 #define rtr_subtract_entry(core, priority) \
1113 prio_subtract_entry(&cores[core].rtr, (priority))
1115 #define rtr_move_entry(core, from, to) \
1116 prio_move_entry(&cores[core].rtr, (from), (to))
1117 #else
1118 #define rtr_add_entry(core, priority)
1119 #define rtr_add_entry_inl(core, priority)
1120 #define rtr_subtract_entry(core, priority)
1121 #define rtr_subtract_entry_inl(core, priotity)
1122 #define rtr_move_entry(core, from, to)
1123 #define rtr_move_entry_inl(core, from, to)
1124 #endif
1126 /*---------------------------------------------------------------------------
1127 * Thread list structure - circular:
1128 * +------------------------------+
1129 * | |
1130 * +--+---+<-+---+<-+---+<-+---+<-+
1131 * Head->| T | | T | | T | | T |
1132 * +->+---+->+---+->+---+->+---+--+
1133 * | |
1134 * +------------------------------+
1135 *---------------------------------------------------------------------------
1138 /*---------------------------------------------------------------------------
1139 * Adds a thread to a list of threads using "insert last". Uses the "l"
1140 * links.
1141 *---------------------------------------------------------------------------
1143 static void add_to_list_l(struct thread_entry **list,
1144 struct thread_entry *thread)
1146 struct thread_entry *l = *list;
1148 if (l == NULL)
1150 /* Insert into unoccupied list */
1151 thread->l.prev = thread;
1152 thread->l.next = thread;
1153 *list = thread;
1154 return;
1157 /* Insert last */
1158 thread->l.prev = l->l.prev;
1159 thread->l.next = l;
1160 l->l.prev->l.next = thread;
1161 l->l.prev = thread;
1164 /*---------------------------------------------------------------------------
1165 * Removes a thread from a list of threads. Uses the "l" links.
1166 *---------------------------------------------------------------------------
1168 static void remove_from_list_l(struct thread_entry **list,
1169 struct thread_entry *thread)
1171 struct thread_entry *prev, *next;
1173 next = thread->l.next;
1175 if (thread == next)
1177 /* The only item */
1178 *list = NULL;
1179 return;
1182 if (thread == *list)
1184 /* List becomes next item */
1185 *list = next;
1188 prev = thread->l.prev;
1190 /* Fix links to jump over the removed entry. */
1191 next->l.prev = prev;
1192 prev->l.next = next;
1195 /*---------------------------------------------------------------------------
1196 * Timeout list structure - circular reverse (to make "remove item" O(1)),
1197 * NULL-terminated forward (to ease the far more common forward traversal):
1198 * +------------------------------+
1199 * | |
1200 * +--+---+<-+---+<-+---+<-+---+<-+
1201 * Head->| T | | T | | T | | T |
1202 * +---+->+---+->+---+->+---+-X
1203 *---------------------------------------------------------------------------
1206 /*---------------------------------------------------------------------------
1207 * Add a thread from the core's timout list by linking the pointers in its
1208 * tmo structure.
1209 *---------------------------------------------------------------------------
1211 static void add_to_list_tmo(struct thread_entry *thread)
1213 struct thread_entry *tmo = cores[IF_COP_CORE(thread->core)].timeout;
1214 THREAD_ASSERT(thread->tmo.prev == NULL,
1215 "add_to_list_tmo->already listed", thread);
1217 thread->tmo.next = NULL;
1219 if (tmo == NULL)
1221 /* Insert into unoccupied list */
1222 thread->tmo.prev = thread;
1223 cores[IF_COP_CORE(thread->core)].timeout = thread;
1224 return;
1227 /* Insert Last */
1228 thread->tmo.prev = tmo->tmo.prev;
1229 tmo->tmo.prev->tmo.next = thread;
1230 tmo->tmo.prev = thread;
1233 /*---------------------------------------------------------------------------
1234 * Remove a thread from the core's timout list by unlinking the pointers in
1235 * its tmo structure. Sets thread->tmo.prev to NULL to indicate the timeout
1236 * is cancelled.
1237 *---------------------------------------------------------------------------
1239 static void remove_from_list_tmo(struct thread_entry *thread)
1241 struct thread_entry **list = &cores[IF_COP_CORE(thread->core)].timeout;
1242 struct thread_entry *prev = thread->tmo.prev;
1243 struct thread_entry *next = thread->tmo.next;
1245 THREAD_ASSERT(prev != NULL, "remove_from_list_tmo->not listed", thread);
1247 if (next != NULL)
1248 next->tmo.prev = prev;
1250 if (thread == *list)
1252 /* List becomes next item and empty if next == NULL */
1253 *list = next;
1254 /* Mark as unlisted */
1255 thread->tmo.prev = NULL;
1257 else
1259 if (next == NULL)
1260 (*list)->tmo.prev = prev;
1261 prev->tmo.next = next;
1262 /* Mark as unlisted */
1263 thread->tmo.prev = NULL;
1268 #ifdef HAVE_PRIORITY_SCHEDULING
1269 /*---------------------------------------------------------------------------
1270 * Priority distribution structure (one category for each possible priority):
1272 * +----+----+----+ ... +-----+
1273 * hist: | F0 | F1 | F2 | | F31 |
1274 * +----+----+----+ ... +-----+
1275 * mask: | b0 | b1 | b2 | | b31 |
1276 * +----+----+----+ ... +-----+
1278 * F = count of threads at priority category n (frequency)
1279 * b = bitmask of non-zero priority categories (occupancy)
1281 * / if H[n] != 0 : 1
1282 * b[n] = |
1283 * \ else : 0
1285 *---------------------------------------------------------------------------
1286 * Basic priority inheritance priotocol (PIP):
1288 * Mn = mutex n, Tn = thread n
1290 * A lower priority thread inherits the priority of the highest priority
1291 * thread blocked waiting for it to complete an action (such as release a
1292 * mutex or respond to a message via queue_send):
1294 * 1) T2->M1->T1
1296 * T1 owns M1, T2 is waiting for M1 to realease M1. If T2 has a higher
1297 * priority than T1 then T1 inherits the priority of T2.
1299 * 2) T3
1300 * \/
1301 * T2->M1->T1
1303 * Situation is like 1) but T2 and T3 are both queued waiting for M1 and so
1304 * T1 inherits the higher of T2 and T3.
1306 * 3) T3->M2->T2->M1->T1
1308 * T1 owns M1, T2 owns M2. If T3 has a higher priority than both T1 and T2,
1309 * then T1 inherits the priority of T3 through T2.
1311 * Blocking chains can grow arbitrarily complex (though it's best that they
1312 * not form at all very often :) and build-up from these units.
1313 *---------------------------------------------------------------------------
1316 /*---------------------------------------------------------------------------
1317 * Increment frequency at category "priority"
1318 *---------------------------------------------------------------------------
1320 static inline unsigned int prio_add_entry(
1321 struct priority_distribution *pd, int priority)
1323 unsigned int count;
1324 /* Enough size/instruction count difference for ARM makes it worth it to
1325 * use different code (192 bytes for ARM). Only thing better is ASM. */
1326 #ifdef CPU_ARM
1327 count = pd->hist[priority];
1328 if (++count == 1)
1329 pd->mask |= 1 << priority;
1330 pd->hist[priority] = count;
1331 #else /* This one's better for Coldfire */
1332 if ((count = ++pd->hist[priority]) == 1)
1333 pd->mask |= 1 << priority;
1334 #endif
1336 return count;
1339 /*---------------------------------------------------------------------------
1340 * Decrement frequency at category "priority"
1341 *---------------------------------------------------------------------------
1343 static inline unsigned int prio_subtract_entry(
1344 struct priority_distribution *pd, int priority)
1346 unsigned int count;
1348 #ifdef CPU_ARM
1349 count = pd->hist[priority];
1350 if (--count == 0)
1351 pd->mask &= ~(1 << priority);
1352 pd->hist[priority] = count;
1353 #else
1354 if ((count = --pd->hist[priority]) == 0)
1355 pd->mask &= ~(1 << priority);
1356 #endif
1358 return count;
1361 /*---------------------------------------------------------------------------
1362 * Remove from one category and add to another
1363 *---------------------------------------------------------------------------
1365 static inline void prio_move_entry(
1366 struct priority_distribution *pd, int from, int to)
1368 uint32_t mask = pd->mask;
1370 #ifdef CPU_ARM
1371 unsigned int count;
1373 count = pd->hist[from];
1374 if (--count == 0)
1375 mask &= ~(1 << from);
1376 pd->hist[from] = count;
1378 count = pd->hist[to];
1379 if (++count == 1)
1380 mask |= 1 << to;
1381 pd->hist[to] = count;
1382 #else
1383 if (--pd->hist[from] == 0)
1384 mask &= ~(1 << from);
1386 if (++pd->hist[to] == 1)
1387 mask |= 1 << to;
1388 #endif
1390 pd->mask = mask;
1393 /*---------------------------------------------------------------------------
1394 * Change the priority and rtr entry for a running thread
1395 *---------------------------------------------------------------------------
1397 static inline void set_running_thread_priority(
1398 struct thread_entry *thread, int priority)
1400 const unsigned int core = IF_COP_CORE(thread->core);
1401 RTR_LOCK(core);
1402 rtr_move_entry(core, thread->priority, priority);
1403 thread->priority = priority;
1404 RTR_UNLOCK(core);
1407 /*---------------------------------------------------------------------------
1408 * Finds the highest priority thread in a list of threads. If the list is
1409 * empty, the PRIORITY_IDLE is returned.
1411 * It is possible to use the struct priority_distribution within an object
1412 * instead of scanning the remaining threads in the list but as a compromise,
1413 * the resulting per-object memory overhead is saved at a slight speed
1414 * penalty under high contention.
1415 *---------------------------------------------------------------------------
1417 static int find_highest_priority_in_list_l(
1418 struct thread_entry * const thread)
1420 if (LIKELY(thread != NULL))
1422 /* Go though list until the ending up at the initial thread */
1423 int highest_priority = thread->priority;
1424 struct thread_entry *curr = thread;
1428 int priority = curr->priority;
1430 if (priority < highest_priority)
1431 highest_priority = priority;
1433 curr = curr->l.next;
1435 while (curr != thread);
1437 return highest_priority;
1440 return PRIORITY_IDLE;
1443 /*---------------------------------------------------------------------------
1444 * Register priority with blocking system and bubble it down the chain if
1445 * any until we reach the end or something is already equal or higher.
1447 * NOTE: A simultaneous circular wait could spin deadlock on multiprocessor
1448 * targets but that same action also guarantees a circular block anyway and
1449 * those are prevented, right? :-)
1450 *---------------------------------------------------------------------------
1452 static struct thread_entry *
1453 blocker_inherit_priority(struct thread_entry *current)
1455 const int priority = current->priority;
1456 struct blocker *bl = current->blocker;
1457 struct thread_entry * const tstart = current;
1458 struct thread_entry *bl_t = bl->thread;
1460 /* Blocker cannot change since the object protection is held */
1461 LOCK_THREAD(bl_t);
1463 for (;;)
1465 struct thread_entry *next;
1466 int bl_pr = bl->priority;
1468 if (priority >= bl_pr)
1469 break; /* Object priority already high enough */
1471 bl->priority = priority;
1473 /* Add this one */
1474 prio_add_entry(&bl_t->pdist, priority);
1476 if (bl_pr < PRIORITY_IDLE)
1478 /* Not first waiter - subtract old one */
1479 prio_subtract_entry(&bl_t->pdist, bl_pr);
1482 if (priority >= bl_t->priority)
1483 break; /* Thread priority high enough */
1485 if (bl_t->state == STATE_RUNNING)
1487 /* Blocking thread is a running thread therefore there are no
1488 * further blockers. Change the "run queue" on which it
1489 * resides. */
1490 set_running_thread_priority(bl_t, priority);
1491 break;
1494 bl_t->priority = priority;
1496 /* If blocking thread has a blocker, apply transitive inheritance */
1497 bl = bl_t->blocker;
1499 if (bl == NULL)
1500 break; /* End of chain or object doesn't support inheritance */
1502 next = bl->thread;
1504 if (UNLIKELY(next == tstart))
1505 break; /* Full-circle - deadlock! */
1507 UNLOCK_THREAD(current);
1509 #if NUM_CORES > 1
1510 for (;;)
1512 LOCK_THREAD(next);
1514 /* Blocker could change - retest condition */
1515 if (LIKELY(bl->thread == next))
1516 break;
1518 UNLOCK_THREAD(next);
1519 next = bl->thread;
1521 #endif
1522 current = bl_t;
1523 bl_t = next;
1526 UNLOCK_THREAD(bl_t);
1528 return current;
1531 /*---------------------------------------------------------------------------
1532 * Readjust priorities when waking a thread blocked waiting for another
1533 * in essence "releasing" the thread's effect on the object owner. Can be
1534 * performed from any context.
1535 *---------------------------------------------------------------------------
1537 struct thread_entry *
1538 wakeup_priority_protocol_release(struct thread_entry *thread)
1540 const int priority = thread->priority;
1541 struct blocker *bl = thread->blocker;
1542 struct thread_entry * const tstart = thread;
1543 struct thread_entry *bl_t = bl->thread;
1545 /* Blocker cannot change since object will be locked */
1546 LOCK_THREAD(bl_t);
1548 thread->blocker = NULL; /* Thread not blocked */
1550 for (;;)
1552 struct thread_entry *next;
1553 int bl_pr = bl->priority;
1555 if (priority > bl_pr)
1556 break; /* Object priority higher */
1558 next = *thread->bqp;
1560 if (next == NULL)
1562 /* No more threads in queue */
1563 prio_subtract_entry(&bl_t->pdist, bl_pr);
1564 bl->priority = PRIORITY_IDLE;
1566 else
1568 /* Check list for highest remaining priority */
1569 int queue_pr = find_highest_priority_in_list_l(next);
1571 if (queue_pr == bl_pr)
1572 break; /* Object priority not changing */
1574 /* Change queue priority */
1575 prio_move_entry(&bl_t->pdist, bl_pr, queue_pr);
1576 bl->priority = queue_pr;
1579 if (bl_pr > bl_t->priority)
1580 break; /* thread priority is higher */
1582 bl_pr = find_first_set_bit(bl_t->pdist.mask);
1584 if (bl_pr == bl_t->priority)
1585 break; /* Thread priority not changing */
1587 if (bl_t->state == STATE_RUNNING)
1589 /* No further blockers */
1590 set_running_thread_priority(bl_t, bl_pr);
1591 break;
1594 bl_t->priority = bl_pr;
1596 /* If blocking thread has a blocker, apply transitive inheritance */
1597 bl = bl_t->blocker;
1599 if (bl == NULL)
1600 break; /* End of chain or object doesn't support inheritance */
1602 next = bl->thread;
1604 if (UNLIKELY(next == tstart))
1605 break; /* Full-circle - deadlock! */
1607 UNLOCK_THREAD(thread);
1609 #if NUM_CORES > 1
1610 for (;;)
1612 LOCK_THREAD(next);
1614 /* Blocker could change - retest condition */
1615 if (LIKELY(bl->thread == next))
1616 break;
1618 UNLOCK_THREAD(next);
1619 next = bl->thread;
1621 #endif
1622 thread = bl_t;
1623 bl_t = next;
1626 UNLOCK_THREAD(bl_t);
1628 #if NUM_CORES > 1
1629 if (UNLIKELY(thread != tstart))
1631 /* Relock original if it changed */
1632 LOCK_THREAD(tstart);
1634 #endif
1636 return cores[CURRENT_CORE].running;
1639 /*---------------------------------------------------------------------------
1640 * Transfer ownership to a thread waiting for an objects and transfer
1641 * inherited priority boost from other waiters. This algorithm knows that
1642 * blocking chains may only unblock from the very end.
1644 * Only the owning thread itself may call this and so the assumption that
1645 * it is the running thread is made.
1646 *---------------------------------------------------------------------------
1648 struct thread_entry *
1649 wakeup_priority_protocol_transfer(struct thread_entry *thread)
1651 /* Waking thread inherits priority boost from object owner */
1652 struct blocker *bl = thread->blocker;
1653 struct thread_entry *bl_t = bl->thread;
1654 struct thread_entry *next;
1655 int bl_pr;
1657 THREAD_ASSERT(cores[CURRENT_CORE].running == bl_t,
1658 "UPPT->wrong thread", cores[CURRENT_CORE].running);
1660 LOCK_THREAD(bl_t);
1662 bl_pr = bl->priority;
1664 /* Remove the object's boost from the owning thread */
1665 if (prio_subtract_entry(&bl_t->pdist, bl_pr) == 0 &&
1666 bl_pr <= bl_t->priority)
1668 /* No more threads at this priority are waiting and the old level is
1669 * at least the thread level */
1670 int priority = find_first_set_bit(bl_t->pdist.mask);
1672 if (priority != bl_t->priority)
1674 /* Adjust this thread's priority */
1675 set_running_thread_priority(bl_t, priority);
1679 next = *thread->bqp;
1681 if (LIKELY(next == NULL))
1683 /* Expected shortcut - no more waiters */
1684 bl_pr = PRIORITY_IDLE;
1686 else
1688 if (thread->priority <= bl_pr)
1690 /* Need to scan threads remaining in queue */
1691 bl_pr = find_highest_priority_in_list_l(next);
1694 if (prio_add_entry(&thread->pdist, bl_pr) == 1 &&
1695 bl_pr < thread->priority)
1697 /* Thread priority must be raised */
1698 thread->priority = bl_pr;
1702 bl->thread = thread; /* This thread pwns */
1703 bl->priority = bl_pr; /* Save highest blocked priority */
1704 thread->blocker = NULL; /* Thread not blocked */
1706 UNLOCK_THREAD(bl_t);
1708 return bl_t;
1711 /*---------------------------------------------------------------------------
1712 * No threads must be blocked waiting for this thread except for it to exit.
1713 * The alternative is more elaborate cleanup and object registration code.
1714 * Check this for risk of silent data corruption when objects with
1715 * inheritable blocking are abandoned by the owner - not precise but may
1716 * catch something.
1717 *---------------------------------------------------------------------------
1719 static void check_for_obj_waiters(const char *function, struct thread_entry *thread)
1721 /* Only one bit in the mask should be set with a frequency on 1 which
1722 * represents the thread's own base priority */
1723 uint32_t mask = thread->pdist.mask;
1724 if ((mask & (mask - 1)) != 0 ||
1725 thread->pdist.hist[find_first_set_bit(mask)] > 1)
1727 unsigned char name[32];
1728 thread_get_name(name, 32, thread);
1729 panicf("%s->%s with obj. waiters", function, name);
1732 #endif /* HAVE_PRIORITY_SCHEDULING */
1734 /*---------------------------------------------------------------------------
1735 * Move a thread back to a running state on its core.
1736 *---------------------------------------------------------------------------
1738 static void core_schedule_wakeup(struct thread_entry *thread)
1740 const unsigned int core = IF_COP_CORE(thread->core);
1742 RTR_LOCK(core);
1744 thread->state = STATE_RUNNING;
1746 add_to_list_l(&cores[core].running, thread);
1747 rtr_add_entry(core, thread->priority);
1749 RTR_UNLOCK(core);
1751 #if NUM_CORES > 1
1752 if (core != CURRENT_CORE)
1753 core_wake(core);
1754 #endif
1757 /*---------------------------------------------------------------------------
1758 * Check the core's timeout list when at least one thread is due to wake.
1759 * Filtering for the condition is done before making the call. Resets the
1760 * tick when the next check will occur.
1761 *---------------------------------------------------------------------------
1763 void check_tmo_threads(void)
1765 const unsigned int core = CURRENT_CORE;
1766 const long tick = current_tick; /* snapshot the current tick */
1767 long next_tmo_check = tick + 60*HZ; /* minimum duration: once/minute */
1768 struct thread_entry *next = cores[core].timeout;
1770 /* If there are no processes waiting for a timeout, just keep the check
1771 tick from falling into the past. */
1773 /* Break the loop once we have walked through the list of all
1774 * sleeping processes or have removed them all. */
1775 while (next != NULL)
1777 /* Check sleeping threads. Allow interrupts between checks. */
1778 enable_irq();
1780 struct thread_entry *curr = next;
1782 next = curr->tmo.next;
1784 /* Lock thread slot against explicit wakeup */
1785 disable_irq();
1786 LOCK_THREAD(curr);
1788 unsigned state = curr->state;
1790 if (state < TIMEOUT_STATE_FIRST)
1792 /* Cleanup threads no longer on a timeout but still on the
1793 * list. */
1794 remove_from_list_tmo(curr);
1796 else if (LIKELY(TIME_BEFORE(tick, curr->tmo_tick)))
1798 /* Timeout still pending - this will be the usual case */
1799 if (TIME_BEFORE(curr->tmo_tick, next_tmo_check))
1801 /* Earliest timeout found so far - move the next check up
1802 to its time */
1803 next_tmo_check = curr->tmo_tick;
1806 else
1808 /* Sleep timeout has been reached so bring the thread back to
1809 * life again. */
1810 if (state == STATE_BLOCKED_W_TMO)
1812 #if NUM_CORES > 1
1813 /* Lock the waiting thread's kernel object */
1814 struct corelock *ocl = curr->obj_cl;
1816 if (UNLIKELY(corelock_try_lock(ocl) == 0))
1818 /* Need to retry in the correct order though the need is
1819 * unlikely */
1820 UNLOCK_THREAD(curr);
1821 corelock_lock(ocl);
1822 LOCK_THREAD(curr);
1824 if (UNLIKELY(curr->state != STATE_BLOCKED_W_TMO))
1826 /* Thread was woken or removed explicitely while slot
1827 * was unlocked */
1828 corelock_unlock(ocl);
1829 remove_from_list_tmo(curr);
1830 UNLOCK_THREAD(curr);
1831 continue;
1834 #endif /* NUM_CORES */
1836 remove_from_list_l(curr->bqp, curr);
1838 #ifdef HAVE_WAKEUP_EXT_CB
1839 if (curr->wakeup_ext_cb != NULL)
1840 curr->wakeup_ext_cb(curr);
1841 #endif
1843 #ifdef HAVE_PRIORITY_SCHEDULING
1844 if (curr->blocker != NULL)
1845 wakeup_priority_protocol_release(curr);
1846 #endif
1847 corelock_unlock(ocl);
1849 /* else state == STATE_SLEEPING */
1851 remove_from_list_tmo(curr);
1853 RTR_LOCK(core);
1855 curr->state = STATE_RUNNING;
1857 add_to_list_l(&cores[core].running, curr);
1858 rtr_add_entry(core, curr->priority);
1860 RTR_UNLOCK(core);
1863 UNLOCK_THREAD(curr);
1866 cores[core].next_tmo_check = next_tmo_check;
1869 /*---------------------------------------------------------------------------
1870 * Performs operations that must be done before blocking a thread but after
1871 * the state is saved.
1872 *---------------------------------------------------------------------------
1874 #if NUM_CORES > 1
1875 static inline void run_blocking_ops(
1876 unsigned int core, struct thread_entry *thread)
1878 struct thread_blk_ops *ops = &cores[core].blk_ops;
1879 const unsigned flags = ops->flags;
1881 if (LIKELY(flags == TBOP_CLEAR))
1882 return;
1884 switch (flags)
1886 case TBOP_SWITCH_CORE:
1887 core_switch_blk_op(core, thread);
1888 /* Fall-through */
1889 case TBOP_UNLOCK_CORELOCK:
1890 corelock_unlock(ops->cl_p);
1891 break;
1894 ops->flags = TBOP_CLEAR;
1896 #endif /* NUM_CORES > 1 */
1898 #ifdef RB_PROFILE
1899 void profile_thread(void)
1901 profstart(cores[CURRENT_CORE].running - threads);
1903 #endif
1905 /*---------------------------------------------------------------------------
1906 * Prepares a thread to block on an object's list and/or for a specified
1907 * duration - expects object and slot to be appropriately locked if needed
1908 * and interrupts to be masked.
1909 *---------------------------------------------------------------------------
1911 static inline void block_thread_on_l(struct thread_entry *thread,
1912 unsigned state)
1914 /* If inlined, unreachable branches will be pruned with no size penalty
1915 because state is passed as a constant parameter. */
1916 const unsigned int core = IF_COP_CORE(thread->core);
1918 /* Remove the thread from the list of running threads. */
1919 RTR_LOCK(core);
1920 remove_from_list_l(&cores[core].running, thread);
1921 rtr_subtract_entry(core, thread->priority);
1922 RTR_UNLOCK(core);
1924 /* Add a timeout to the block if not infinite */
1925 switch (state)
1927 case STATE_BLOCKED:
1928 case STATE_BLOCKED_W_TMO:
1929 /* Put the thread into a new list of inactive threads. */
1930 add_to_list_l(thread->bqp, thread);
1932 if (state == STATE_BLOCKED)
1933 break;
1935 /* Fall-through */
1936 case STATE_SLEEPING:
1937 /* If this thread times out sooner than any other thread, update
1938 next_tmo_check to its timeout */
1939 if (TIME_BEFORE(thread->tmo_tick, cores[core].next_tmo_check))
1941 cores[core].next_tmo_check = thread->tmo_tick;
1944 if (thread->tmo.prev == NULL)
1946 add_to_list_tmo(thread);
1948 /* else thread was never removed from list - just keep it there */
1949 break;
1952 /* Remember the the next thread about to block. */
1953 cores[core].block_task = thread;
1955 /* Report new state. */
1956 thread->state = state;
1959 /*---------------------------------------------------------------------------
1960 * Switch thread in round robin fashion for any given priority. Any thread
1961 * that removed itself from the running list first must specify itself in
1962 * the paramter.
1964 * INTERNAL: Intended for use by kernel and not for programs.
1965 *---------------------------------------------------------------------------
1967 void switch_thread(void)
1970 const unsigned int core = CURRENT_CORE;
1971 struct thread_entry *block = cores[core].block_task;
1972 struct thread_entry *thread = cores[core].running;
1974 /* Get context to save - next thread to run is unknown until all wakeups
1975 * are evaluated */
1976 if (block != NULL)
1978 cores[core].block_task = NULL;
1980 #if NUM_CORES > 1
1981 if (UNLIKELY(thread == block))
1983 /* This was the last thread running and another core woke us before
1984 * reaching here. Force next thread selection to give tmo threads or
1985 * other threads woken before this block a first chance. */
1986 block = NULL;
1988 else
1989 #endif
1991 /* Blocking task is the old one */
1992 thread = block;
1996 #ifdef RB_PROFILE
1997 profile_thread_stopped(thread->id & THREAD_ID_SLOT_MASK);
1998 #endif
2000 /* Begin task switching by saving our current context so that we can
2001 * restore the state of the current thread later to the point prior
2002 * to this call. */
2003 store_context(&thread->context);
2005 /* Check if the current thread stack is overflown */
2006 if (UNLIKELY(thread->stack[0] != DEADBEEF))
2007 thread_stkov(thread);
2009 #if NUM_CORES > 1
2010 /* Run any blocking operations requested before switching/sleeping */
2011 run_blocking_ops(core, thread);
2012 #endif
2014 #ifdef HAVE_PRIORITY_SCHEDULING
2015 IF_NO_SKIP_YIELD( if (thread->skip_count != -1) )
2016 /* Reset the value of thread's skip count */
2017 thread->skip_count = 0;
2018 #endif
2020 for (;;)
2022 /* If there are threads on a timeout and the earliest wakeup is due,
2023 * check the list and wake any threads that need to start running
2024 * again. */
2025 if (!TIME_BEFORE(current_tick, cores[core].next_tmo_check))
2027 check_tmo_threads();
2030 disable_irq();
2031 RTR_LOCK(core);
2033 thread = cores[core].running;
2035 if (UNLIKELY(thread == NULL))
2037 /* Enter sleep mode to reduce power usage - woken up on interrupt
2038 * or wakeup request from another core - expected to enable
2039 * interrupts. */
2040 RTR_UNLOCK(core);
2041 core_sleep(IF_COP(core));
2043 else
2045 #ifdef HAVE_PRIORITY_SCHEDULING
2046 /* Select the new task based on priorities and the last time a
2047 * process got CPU time relative to the highest priority runnable
2048 * task. */
2049 struct priority_distribution *pd = &cores[core].rtr;
2050 int max = find_first_set_bit(pd->mask);
2052 if (block == NULL)
2054 /* Not switching on a block, tentatively select next thread */
2055 thread = thread->l.next;
2058 for (;;)
2060 int priority = thread->priority;
2061 int diff;
2063 /* This ridiculously simple method of aging seems to work
2064 * suspiciously well. It does tend to reward CPU hogs (under
2065 * yielding) but that's generally not desirable at all. On
2066 * the plus side, it, relatively to other threads, penalizes
2067 * excess yielding which is good if some high priority thread
2068 * is performing no useful work such as polling for a device
2069 * to be ready. Of course, aging is only employed when higher
2070 * and lower priority threads are runnable. The highest
2071 * priority runnable thread(s) are never skipped unless a
2072 * lower-priority process has aged sufficiently. Priorities
2073 * of REALTIME class are run strictly according to priority
2074 * thus are not subject to switchout due to lower-priority
2075 * processes aging; they must give up the processor by going
2076 * off the run list. */
2077 if (LIKELY(priority <= max) ||
2078 IF_NO_SKIP_YIELD( thread->skip_count == -1 || )
2079 (priority > PRIORITY_REALTIME &&
2080 (diff = priority - max,
2081 ++thread->skip_count > diff*diff)))
2083 cores[core].running = thread;
2084 break;
2087 thread = thread->l.next;
2089 #else
2090 /* Without priority use a simple FCFS algorithm */
2091 if (block == NULL)
2093 /* Not switching on a block, select next thread */
2094 thread = thread->l.next;
2095 cores[core].running = thread;
2097 #endif /* HAVE_PRIORITY_SCHEDULING */
2099 RTR_UNLOCK(core);
2100 enable_irq();
2101 break;
2105 /* And finally give control to the next thread. */
2106 load_context(&thread->context);
2108 #ifdef RB_PROFILE
2109 profile_thread_started(thread->id & THREAD_ID_SLOT_MASK);
2110 #endif
2114 /*---------------------------------------------------------------------------
2115 * Sleeps a thread for at least a specified number of ticks with zero being
2116 * a wait until the next tick.
2118 * INTERNAL: Intended for use by kernel and not for programs.
2119 *---------------------------------------------------------------------------
2121 void sleep_thread(int ticks)
2123 struct thread_entry *current = cores[CURRENT_CORE].running;
2125 LOCK_THREAD(current);
2127 /* Set our timeout, remove from run list and join timeout list. */
2128 current->tmo_tick = current_tick + ticks + 1;
2129 block_thread_on_l(current, STATE_SLEEPING);
2131 UNLOCK_THREAD(current);
2134 /*---------------------------------------------------------------------------
2135 * Indefinitely block a thread on a blocking queue for explicit wakeup.
2137 * INTERNAL: Intended for use by kernel objects and not for programs.
2138 *---------------------------------------------------------------------------
2140 void block_thread(struct thread_entry *current)
2142 /* Set the state to blocked and take us off of the run queue until we
2143 * are explicitly woken */
2144 LOCK_THREAD(current);
2146 /* Set the list for explicit wakeup */
2147 block_thread_on_l(current, STATE_BLOCKED);
2149 #ifdef HAVE_PRIORITY_SCHEDULING
2150 if (current->blocker != NULL)
2152 /* Object supports PIP */
2153 current = blocker_inherit_priority(current);
2155 #endif
2157 UNLOCK_THREAD(current);
2160 /*---------------------------------------------------------------------------
2161 * Block a thread on a blocking queue for a specified time interval or until
2162 * explicitly woken - whichever happens first.
2164 * INTERNAL: Intended for use by kernel objects and not for programs.
2165 *---------------------------------------------------------------------------
2167 void block_thread_w_tmo(struct thread_entry *current, int timeout)
2169 /* Get the entry for the current running thread. */
2170 LOCK_THREAD(current);
2172 /* Set the state to blocked with the specified timeout */
2173 current->tmo_tick = current_tick + timeout;
2175 /* Set the list for explicit wakeup */
2176 block_thread_on_l(current, STATE_BLOCKED_W_TMO);
2178 #ifdef HAVE_PRIORITY_SCHEDULING
2179 if (current->blocker != NULL)
2181 /* Object supports PIP */
2182 current = blocker_inherit_priority(current);
2184 #endif
2186 UNLOCK_THREAD(current);
2189 /*---------------------------------------------------------------------------
2190 * Explicitly wakeup a thread on a blocking queue. Only effects threads of
2191 * STATE_BLOCKED and STATE_BLOCKED_W_TMO.
2193 * This code should be considered a critical section by the caller meaning
2194 * that the object's corelock should be held.
2196 * INTERNAL: Intended for use by kernel objects and not for programs.
2197 *---------------------------------------------------------------------------
2199 unsigned int wakeup_thread(struct thread_entry **list)
2201 struct thread_entry *thread = *list;
2202 unsigned int result = THREAD_NONE;
2204 /* Check if there is a blocked thread at all. */
2205 if (thread == NULL)
2206 return result;
2208 LOCK_THREAD(thread);
2210 /* Determine thread's current state. */
2211 switch (thread->state)
2213 case STATE_BLOCKED:
2214 case STATE_BLOCKED_W_TMO:
2215 remove_from_list_l(list, thread);
2217 result = THREAD_OK;
2219 #ifdef HAVE_PRIORITY_SCHEDULING
2220 struct thread_entry *current;
2221 struct blocker *bl = thread->blocker;
2223 if (bl == NULL)
2225 /* No inheritance - just boost the thread by aging */
2226 IF_NO_SKIP_YIELD( if (thread->skip_count != -1) )
2227 thread->skip_count = thread->priority;
2228 current = cores[CURRENT_CORE].running;
2230 else
2232 /* Call the specified unblocking PIP */
2233 current = bl->wakeup_protocol(thread);
2236 if (current != NULL &&
2237 find_first_set_bit(cores[IF_COP_CORE(current->core)].rtr.mask)
2238 < current->priority)
2240 /* There is a thread ready to run of higher or same priority on
2241 * the same core as the current one; recommend a task switch.
2242 * Knowing if this is an interrupt call would be helpful here. */
2243 result |= THREAD_SWITCH;
2245 #endif /* HAVE_PRIORITY_SCHEDULING */
2247 core_schedule_wakeup(thread);
2248 break;
2250 /* Nothing to do. State is not blocked. */
2251 #if THREAD_EXTRA_CHECKS
2252 default:
2253 THREAD_PANICF("wakeup_thread->block invalid", thread);
2254 case STATE_RUNNING:
2255 case STATE_KILLED:
2256 break;
2257 #endif
2260 UNLOCK_THREAD(thread);
2261 return result;
2264 /*---------------------------------------------------------------------------
2265 * Wakeup an entire queue of threads - returns bitwise-or of return bitmask
2266 * from each operation or THREAD_NONE of nothing was awakened. Object owning
2267 * the queue must be locked first.
2269 * INTERNAL: Intended for use by kernel objects and not for programs.
2270 *---------------------------------------------------------------------------
2272 unsigned int thread_queue_wake(struct thread_entry **list)
2274 unsigned result = THREAD_NONE;
2276 for (;;)
2278 unsigned int rc = wakeup_thread(list);
2280 if (rc == THREAD_NONE)
2281 break; /* No more threads */
2283 result |= rc;
2286 return result;
2289 /*---------------------------------------------------------------------------
2290 * Assign the thread slot a new ID. Version is 1-255.
2291 *---------------------------------------------------------------------------
2293 static void new_thread_id(unsigned int slot_num,
2294 struct thread_entry *thread)
2296 unsigned int version =
2297 (thread->id + (1u << THREAD_ID_VERSION_SHIFT))
2298 & THREAD_ID_VERSION_MASK;
2300 /* If wrapped to 0, make it 1 */
2301 if (version == 0)
2302 version = 1u << THREAD_ID_VERSION_SHIFT;
2304 thread->id = version | (slot_num & THREAD_ID_SLOT_MASK);
2307 /*---------------------------------------------------------------------------
2308 * Find an empty thread slot or MAXTHREADS if none found. The slot returned
2309 * will be locked on multicore.
2310 *---------------------------------------------------------------------------
2312 static struct thread_entry * find_empty_thread_slot(void)
2314 /* Any slot could be on an interrupt-accessible list */
2315 IF_COP( int oldlevel = disable_irq_save(); )
2316 struct thread_entry *thread = NULL;
2317 int n;
2319 for (n = 0; n < MAXTHREADS; n++)
2321 /* Obtain current slot state - lock it on multicore */
2322 struct thread_entry *t = &threads[n];
2323 LOCK_THREAD(t);
2325 if (t->state == STATE_KILLED IF_COP( && t->name != THREAD_DESTRUCT ))
2327 /* Slot is empty - leave it locked and caller will unlock */
2328 thread = t;
2329 break;
2332 /* Finished examining slot - no longer busy - unlock on multicore */
2333 UNLOCK_THREAD(t);
2336 IF_COP( restore_irq(oldlevel); ) /* Reenable interrups - this slot is
2337 not accesible to them yet */
2338 return thread;
2341 /*---------------------------------------------------------------------------
2342 * Return the thread_entry pointer for a thread_id. Return the current
2343 * thread if the ID is 0 (alias for current).
2344 *---------------------------------------------------------------------------
2346 struct thread_entry * thread_id_entry(unsigned int thread_id)
2348 return (thread_id == THREAD_ID_CURRENT) ?
2349 cores[CURRENT_CORE].running :
2350 &threads[thread_id & THREAD_ID_SLOT_MASK];
2353 /*---------------------------------------------------------------------------
2354 * Place the current core in idle mode - woken up on interrupt or wake
2355 * request from another core.
2356 *---------------------------------------------------------------------------
2358 void core_idle(void)
2360 IF_COP( const unsigned int core = CURRENT_CORE; )
2361 disable_irq();
2362 core_sleep(IF_COP(core));
2365 /*---------------------------------------------------------------------------
2366 * Create a thread. If using a dual core architecture, specify which core to
2367 * start the thread on.
2369 * Return ID if context area could be allocated, else NULL.
2370 *---------------------------------------------------------------------------
2372 unsigned int create_thread(void (*function)(void),
2373 void* stack, size_t stack_size,
2374 unsigned flags, const char *name
2375 IF_PRIO(, int priority)
2376 IF_COP(, unsigned int core))
2378 unsigned int i;
2379 unsigned int stack_words;
2380 uintptr_t stackptr, stackend;
2381 struct thread_entry *thread;
2382 unsigned state;
2383 int oldlevel;
2385 thread = find_empty_thread_slot();
2386 if (thread == NULL)
2388 return 0;
2391 oldlevel = disable_irq_save();
2393 /* Munge the stack to make it easy to spot stack overflows */
2394 stackptr = ALIGN_UP((uintptr_t)stack, sizeof (uintptr_t));
2395 stackend = ALIGN_DOWN((uintptr_t)stack + stack_size, sizeof (uintptr_t));
2396 stack_size = stackend - stackptr;
2397 stack_words = stack_size / sizeof (uintptr_t);
2399 for (i = 0; i < stack_words; i++)
2401 ((uintptr_t *)stackptr)[i] = DEADBEEF;
2404 /* Store interesting information */
2405 thread->name = name;
2406 thread->stack = (uintptr_t *)stackptr;
2407 thread->stack_size = stack_size;
2408 thread->queue = NULL;
2409 #ifdef HAVE_WAKEUP_EXT_CB
2410 thread->wakeup_ext_cb = NULL;
2411 #endif
2412 #ifdef HAVE_SCHEDULER_BOOSTCTRL
2413 thread->cpu_boost = 0;
2414 #endif
2415 #ifdef HAVE_PRIORITY_SCHEDULING
2416 memset(&thread->pdist, 0, sizeof(thread->pdist));
2417 thread->blocker = NULL;
2418 thread->base_priority = priority;
2419 thread->priority = priority;
2420 thread->skip_count = priority;
2421 prio_add_entry(&thread->pdist, priority);
2422 #endif
2424 #ifdef HAVE_IO_PRIORITY
2425 /* Default to high (foreground) priority */
2426 thread->io_priority = IO_PRIORITY_IMMEDIATE;
2427 #endif
2429 #if NUM_CORES > 1
2430 thread->core = core;
2432 /* Writeback stack munging or anything else before starting */
2433 if (core != CURRENT_CORE)
2435 cpucache_flush();
2437 #endif
2439 /* Thread is not on any timeout list but be a bit paranoid */
2440 thread->tmo.prev = NULL;
2442 state = (flags & CREATE_THREAD_FROZEN) ?
2443 STATE_FROZEN : STATE_RUNNING;
2445 thread->context.sp = (typeof (thread->context.sp))stackend;
2447 /* Load the thread's context structure with needed startup information */
2448 THREAD_STARTUP_INIT(core, thread, function);
2450 thread->state = state;
2451 i = thread->id; /* Snapshot while locked */
2453 if (state == STATE_RUNNING)
2454 core_schedule_wakeup(thread);
2456 UNLOCK_THREAD(thread);
2457 restore_irq(oldlevel);
2459 return i;
2462 #ifdef HAVE_SCHEDULER_BOOSTCTRL
2463 /*---------------------------------------------------------------------------
2464 * Change the boost state of a thread boosting or unboosting the CPU
2465 * as required.
2466 *---------------------------------------------------------------------------
2468 static inline void boost_thread(struct thread_entry *thread, bool boost)
2470 if ((thread->cpu_boost != 0) != boost)
2472 thread->cpu_boost = boost;
2473 cpu_boost(boost);
2477 void trigger_cpu_boost(void)
2479 struct thread_entry *current = cores[CURRENT_CORE].running;
2480 boost_thread(current, true);
2483 void cancel_cpu_boost(void)
2485 struct thread_entry *current = cores[CURRENT_CORE].running;
2486 boost_thread(current, false);
2488 #endif /* HAVE_SCHEDULER_BOOSTCTRL */
2490 /*---------------------------------------------------------------------------
2491 * Block the current thread until another thread terminates. A thread may
2492 * wait on itself to terminate which prevents it from running again and it
2493 * will need to be killed externally.
2494 * Parameter is the ID as returned from create_thread().
2495 *---------------------------------------------------------------------------
2497 void thread_wait(unsigned int thread_id)
2499 struct thread_entry *current = cores[CURRENT_CORE].running;
2500 struct thread_entry *thread = thread_id_entry(thread_id);
2502 /* Lock thread-as-waitable-object lock */
2503 corelock_lock(&thread->waiter_cl);
2505 /* Be sure it hasn't been killed yet */
2506 if (thread_id == THREAD_ID_CURRENT ||
2507 (thread->id == thread_id && thread->state != STATE_KILLED))
2509 IF_COP( current->obj_cl = &thread->waiter_cl; )
2510 current->bqp = &thread->queue;
2512 disable_irq();
2513 block_thread(current);
2515 corelock_unlock(&thread->waiter_cl);
2517 switch_thread();
2518 return;
2521 corelock_unlock(&thread->waiter_cl);
2524 /*---------------------------------------------------------------------------
2525 * Exit the current thread. The Right Way to Do Things (TM).
2526 *---------------------------------------------------------------------------
2528 void thread_exit(void)
2530 const unsigned int core = CURRENT_CORE;
2531 struct thread_entry *current = cores[core].running;
2533 /* Cancel CPU boost if any */
2534 cancel_cpu_boost();
2536 disable_irq();
2538 corelock_lock(&current->waiter_cl);
2539 LOCK_THREAD(current);
2541 #if defined (ALLOW_REMOVE_THREAD) && NUM_CORES > 1
2542 if (current->name == THREAD_DESTRUCT)
2544 /* Thread being killed - become a waiter */
2545 unsigned int id = current->id;
2546 UNLOCK_THREAD(current);
2547 corelock_unlock(&current->waiter_cl);
2548 thread_wait(id);
2549 THREAD_PANICF("thread_exit->WK:*R", current);
2551 #endif
2553 #ifdef HAVE_PRIORITY_SCHEDULING
2554 check_for_obj_waiters("thread_exit", current);
2555 #endif
2557 if (current->tmo.prev != NULL)
2559 /* Cancel pending timeout list removal */
2560 remove_from_list_tmo(current);
2563 /* Switch tasks and never return */
2564 block_thread_on_l(current, STATE_KILLED);
2566 #if NUM_CORES > 1
2567 /* Switch to the idle stack if not on the main core (where "main"
2568 * runs) - we can hope gcc doesn't need the old stack beyond this
2569 * point. */
2570 if (core != CPU)
2572 switch_to_idle_stack(core);
2575 cpucache_flush();
2577 /* At this point, this thread isn't using resources allocated for
2578 * execution except the slot itself. */
2579 #endif
2581 /* Update ID for this slot */
2582 new_thread_id(current->id, current);
2583 current->name = NULL;
2585 /* Signal this thread */
2586 thread_queue_wake(&current->queue);
2587 corelock_unlock(&current->waiter_cl);
2588 /* Slot must be unusable until thread is really gone */
2589 UNLOCK_THREAD_AT_TASK_SWITCH(current);
2590 switch_thread();
2591 /* This should never and must never be reached - if it is, the
2592 * state is corrupted */
2593 THREAD_PANICF("thread_exit->K:*R", current);
2596 #ifdef ALLOW_REMOVE_THREAD
2597 /*---------------------------------------------------------------------------
2598 * Remove a thread from the scheduler. Not The Right Way to Do Things in
2599 * normal programs.
2601 * Parameter is the ID as returned from create_thread().
2603 * Use with care on threads that are not under careful control as this may
2604 * leave various objects in an undefined state.
2605 *---------------------------------------------------------------------------
2607 void remove_thread(unsigned int thread_id)
2609 #if NUM_CORES > 1
2610 /* core is not constant here because of core switching */
2611 unsigned int core = CURRENT_CORE;
2612 unsigned int old_core = NUM_CORES;
2613 struct corelock *ocl = NULL;
2614 #else
2615 const unsigned int core = CURRENT_CORE;
2616 #endif
2617 struct thread_entry *current = cores[core].running;
2618 struct thread_entry *thread = thread_id_entry(thread_id);
2620 unsigned state;
2621 int oldlevel;
2623 if (thread == current)
2624 thread_exit(); /* Current thread - do normal exit */
2626 oldlevel = disable_irq_save();
2628 corelock_lock(&thread->waiter_cl);
2629 LOCK_THREAD(thread);
2631 state = thread->state;
2633 if (thread->id != thread_id || state == STATE_KILLED)
2634 goto thread_killed;
2636 #if NUM_CORES > 1
2637 if (thread->name == THREAD_DESTRUCT)
2639 /* Thread being killed - become a waiter */
2640 UNLOCK_THREAD(thread);
2641 corelock_unlock(&thread->waiter_cl);
2642 restore_irq(oldlevel);
2643 thread_wait(thread_id);
2644 return;
2647 thread->name = THREAD_DESTRUCT; /* Slot can't be used for now */
2649 #ifdef HAVE_PRIORITY_SCHEDULING
2650 check_for_obj_waiters("remove_thread", thread);
2651 #endif
2653 if (thread->core != core)
2655 /* Switch cores and safely extract the thread there */
2656 /* Slot HAS to be unlocked or a deadlock could occur which means other
2657 * threads have to be guided into becoming thread waiters if they
2658 * attempt to remove it. */
2659 unsigned int new_core = thread->core;
2661 corelock_unlock(&thread->waiter_cl);
2663 UNLOCK_THREAD(thread);
2664 restore_irq(oldlevel);
2666 old_core = switch_core(new_core);
2668 oldlevel = disable_irq_save();
2670 corelock_lock(&thread->waiter_cl);
2671 LOCK_THREAD(thread);
2673 state = thread->state;
2674 core = new_core;
2675 /* Perform the extraction and switch ourselves back to the original
2676 processor */
2678 #endif /* NUM_CORES > 1 */
2680 if (thread->tmo.prev != NULL)
2682 /* Clean thread off the timeout list if a timeout check hasn't
2683 * run yet */
2684 remove_from_list_tmo(thread);
2687 #ifdef HAVE_SCHEDULER_BOOSTCTRL
2688 /* Cancel CPU boost if any */
2689 boost_thread(thread, false);
2690 #endif
2692 IF_COP( retry_state: )
2694 switch (state)
2696 case STATE_RUNNING:
2697 RTR_LOCK(core);
2698 /* Remove thread from ready to run tasks */
2699 remove_from_list_l(&cores[core].running, thread);
2700 rtr_subtract_entry(core, thread->priority);
2701 RTR_UNLOCK(core);
2702 break;
2703 case STATE_BLOCKED:
2704 case STATE_BLOCKED_W_TMO:
2705 /* Remove thread from the queue it's blocked on - including its
2706 * own if waiting there */
2707 #if NUM_CORES > 1
2708 if (&thread->waiter_cl != thread->obj_cl)
2710 ocl = thread->obj_cl;
2712 if (UNLIKELY(corelock_try_lock(ocl) == 0))
2714 UNLOCK_THREAD(thread);
2715 corelock_lock(ocl);
2716 LOCK_THREAD(thread);
2718 if (UNLIKELY(thread->state != state))
2720 /* Something woke the thread */
2721 state = thread->state;
2722 corelock_unlock(ocl);
2723 goto retry_state;
2727 #endif
2728 remove_from_list_l(thread->bqp, thread);
2730 #ifdef HAVE_WAKEUP_EXT_CB
2731 if (thread->wakeup_ext_cb != NULL)
2732 thread->wakeup_ext_cb(thread);
2733 #endif
2735 #ifdef HAVE_PRIORITY_SCHEDULING
2736 if (thread->blocker != NULL)
2738 /* Remove thread's priority influence from its chain */
2739 wakeup_priority_protocol_release(thread);
2741 #endif
2743 #if NUM_CORES > 1
2744 if (ocl != NULL)
2745 corelock_unlock(ocl);
2746 #endif
2747 break;
2748 /* Otherwise thread is frozen and hasn't run yet */
2751 new_thread_id(thread_id, thread);
2752 thread->state = STATE_KILLED;
2754 /* If thread was waiting on itself, it will have been removed above.
2755 * The wrong order would result in waking the thread first and deadlocking
2756 * since the slot is already locked. */
2757 thread_queue_wake(&thread->queue);
2759 thread->name = NULL;
2761 thread_killed: /* Thread was already killed */
2762 /* Removal complete - safe to unlock and reenable interrupts */
2763 corelock_unlock(&thread->waiter_cl);
2764 UNLOCK_THREAD(thread);
2765 restore_irq(oldlevel);
2767 #if NUM_CORES > 1
2768 if (old_core < NUM_CORES)
2770 /* Did a removal on another processor's thread - switch back to
2771 native core */
2772 switch_core(old_core);
2774 #endif
2776 #endif /* ALLOW_REMOVE_THREAD */
2778 #ifdef HAVE_PRIORITY_SCHEDULING
2779 /*---------------------------------------------------------------------------
2780 * Sets the thread's relative base priority for the core it runs on. Any
2781 * needed inheritance changes also may happen.
2782 *---------------------------------------------------------------------------
2784 int thread_set_priority(unsigned int thread_id, int priority)
2786 int old_base_priority = -1;
2787 struct thread_entry *thread = thread_id_entry(thread_id);
2789 /* A little safety measure */
2790 if (priority < HIGHEST_PRIORITY || priority > LOWEST_PRIORITY)
2791 return -1;
2793 /* Thread could be on any list and therefore on an interrupt accessible
2794 one - disable interrupts */
2795 int oldlevel = disable_irq_save();
2797 LOCK_THREAD(thread);
2799 /* Make sure it's not killed */
2800 if (thread_id == THREAD_ID_CURRENT ||
2801 (thread->id == thread_id && thread->state != STATE_KILLED))
2803 int old_priority = thread->priority;
2805 old_base_priority = thread->base_priority;
2806 thread->base_priority = priority;
2808 prio_move_entry(&thread->pdist, old_base_priority, priority);
2809 priority = find_first_set_bit(thread->pdist.mask);
2811 if (old_priority == priority)
2813 /* No priority change - do nothing */
2815 else if (thread->state == STATE_RUNNING)
2817 /* This thread is running - change location on the run
2818 * queue. No transitive inheritance needed. */
2819 set_running_thread_priority(thread, priority);
2821 else
2823 thread->priority = priority;
2825 if (thread->blocker != NULL)
2827 /* Bubble new priority down the chain */
2828 struct blocker *bl = thread->blocker; /* Blocker struct */
2829 struct thread_entry *bl_t = bl->thread; /* Blocking thread */
2830 struct thread_entry * const tstart = thread; /* Initial thread */
2831 const int highest = MIN(priority, old_priority); /* Higher of new or old */
2833 for (;;)
2835 struct thread_entry *next; /* Next thread to check */
2836 int bl_pr; /* Highest blocked thread */
2837 int queue_pr; /* New highest blocked thread */
2838 #if NUM_CORES > 1
2839 /* Owner can change but thread cannot be dislodged - thread
2840 * may not be the first in the queue which allows other
2841 * threads ahead in the list to be given ownership during the
2842 * operation. If thread is next then the waker will have to
2843 * wait for us and the owner of the object will remain fixed.
2844 * If we successfully grab the owner -- which at some point
2845 * is guaranteed -- then the queue remains fixed until we
2846 * pass by. */
2847 for (;;)
2849 LOCK_THREAD(bl_t);
2851 /* Double-check the owner - retry if it changed */
2852 if (LIKELY(bl->thread == bl_t))
2853 break;
2855 UNLOCK_THREAD(bl_t);
2856 bl_t = bl->thread;
2858 #endif
2859 bl_pr = bl->priority;
2861 if (highest > bl_pr)
2862 break; /* Object priority won't change */
2864 /* This will include the thread being set */
2865 queue_pr = find_highest_priority_in_list_l(*thread->bqp);
2867 if (queue_pr == bl_pr)
2868 break; /* Object priority not changing */
2870 /* Update thread boost for this object */
2871 bl->priority = queue_pr;
2872 prio_move_entry(&bl_t->pdist, bl_pr, queue_pr);
2873 bl_pr = find_first_set_bit(bl_t->pdist.mask);
2875 if (bl_t->priority == bl_pr)
2876 break; /* Blocking thread priority not changing */
2878 if (bl_t->state == STATE_RUNNING)
2880 /* Thread not blocked - we're done */
2881 set_running_thread_priority(bl_t, bl_pr);
2882 break;
2885 bl_t->priority = bl_pr;
2886 bl = bl_t->blocker; /* Blocking thread has a blocker? */
2888 if (bl == NULL)
2889 break; /* End of chain */
2891 next = bl->thread;
2893 if (UNLIKELY(next == tstart))
2894 break; /* Full-circle */
2896 UNLOCK_THREAD(thread);
2898 thread = bl_t;
2899 bl_t = next;
2900 } /* for (;;) */
2902 UNLOCK_THREAD(bl_t);
2907 UNLOCK_THREAD(thread);
2909 restore_irq(oldlevel);
2911 return old_base_priority;
2914 /*---------------------------------------------------------------------------
2915 * Returns the current base priority for a thread.
2916 *---------------------------------------------------------------------------
2918 int thread_get_priority(unsigned int thread_id)
2920 struct thread_entry *thread = thread_id_entry(thread_id);
2921 int base_priority = thread->base_priority;
2923 /* Simply check without locking slot. It may or may not be valid by the
2924 * time the function returns anyway. If all tests pass, it is the
2925 * correct value for when it was valid. */
2926 if (thread_id != THREAD_ID_CURRENT &&
2927 (thread->id != thread_id || thread->state == STATE_KILLED))
2928 base_priority = -1;
2930 return base_priority;
2932 #endif /* HAVE_PRIORITY_SCHEDULING */
2934 #ifdef HAVE_IO_PRIORITY
2935 int thread_get_io_priority(unsigned int thread_id)
2937 struct thread_entry *thread = thread_id_entry(thread_id);
2938 return thread->io_priority;
2941 void thread_set_io_priority(unsigned int thread_id,int io_priority)
2943 struct thread_entry *thread = thread_id_entry(thread_id);
2944 thread->io_priority = io_priority;
2946 #endif
2948 /*---------------------------------------------------------------------------
2949 * Starts a frozen thread - similar semantics to wakeup_thread except that
2950 * the thread is on no scheduler or wakeup queue at all. It exists simply by
2951 * virtue of the slot having a state of STATE_FROZEN.
2952 *---------------------------------------------------------------------------
2954 void thread_thaw(unsigned int thread_id)
2956 struct thread_entry *thread = thread_id_entry(thread_id);
2957 int oldlevel = disable_irq_save();
2959 LOCK_THREAD(thread);
2961 /* If thread is the current one, it cannot be frozen, therefore
2962 * there is no need to check that. */
2963 if (thread->id == thread_id && thread->state == STATE_FROZEN)
2964 core_schedule_wakeup(thread);
2966 UNLOCK_THREAD(thread);
2967 restore_irq(oldlevel);
2970 /*---------------------------------------------------------------------------
2971 * Return the ID of the currently executing thread.
2972 *---------------------------------------------------------------------------
2974 unsigned int thread_get_current(void)
2976 return cores[CURRENT_CORE].running->id;
2979 #if NUM_CORES > 1
2980 /*---------------------------------------------------------------------------
2981 * Switch the processor that the currently executing thread runs on.
2982 *---------------------------------------------------------------------------
2984 unsigned int switch_core(unsigned int new_core)
2986 const unsigned int core = CURRENT_CORE;
2987 struct thread_entry *current = cores[core].running;
2989 if (core == new_core)
2991 /* No change - just return same core */
2992 return core;
2995 int oldlevel = disable_irq_save();
2996 LOCK_THREAD(current);
2998 if (current->name == THREAD_DESTRUCT)
3000 /* Thread being killed - deactivate and let process complete */
3001 unsigned int id = current->id;
3002 UNLOCK_THREAD(current);
3003 restore_irq(oldlevel);
3004 thread_wait(id);
3005 /* Should never be reached */
3006 THREAD_PANICF("switch_core->D:*R", current);
3009 /* Get us off the running list for the current core */
3010 RTR_LOCK(core);
3011 remove_from_list_l(&cores[core].running, current);
3012 rtr_subtract_entry(core, current->priority);
3013 RTR_UNLOCK(core);
3015 /* Stash return value (old core) in a safe place */
3016 current->retval = core;
3018 /* If a timeout hadn't yet been cleaned-up it must be removed now or
3019 * the other core will likely attempt a removal from the wrong list! */
3020 if (current->tmo.prev != NULL)
3022 remove_from_list_tmo(current);
3025 /* Change the core number for this thread slot */
3026 current->core = new_core;
3028 /* Do not use core_schedule_wakeup here since this will result in
3029 * the thread starting to run on the other core before being finished on
3030 * this one. Delay the list unlock to keep the other core stuck
3031 * until this thread is ready. */
3032 RTR_LOCK(new_core);
3034 rtr_add_entry(new_core, current->priority);
3035 add_to_list_l(&cores[new_core].running, current);
3037 /* Make a callback into device-specific code, unlock the wakeup list so
3038 * that execution may resume on the new core, unlock our slot and finally
3039 * restore the interrupt level */
3040 cores[core].blk_ops.flags = TBOP_SWITCH_CORE;
3041 cores[core].blk_ops.cl_p = &cores[new_core].rtr_cl;
3042 cores[core].block_task = current;
3044 UNLOCK_THREAD(current);
3046 /* Alert other core to activity */
3047 core_wake(new_core);
3049 /* Do the stack switching, cache_maintenence and switch_thread call -
3050 requires native code */
3051 switch_thread_core(core, current);
3053 /* Finally return the old core to caller */
3054 return current->retval;
3056 #endif /* NUM_CORES > 1 */
3058 /*---------------------------------------------------------------------------
3059 * Initialize threading API. This assumes interrupts are not yet enabled. On
3060 * multicore setups, no core is allowed to proceed until create_thread calls
3061 * are safe to perform.
3062 *---------------------------------------------------------------------------
3064 void init_threads(void)
3066 const unsigned int core = CURRENT_CORE;
3067 struct thread_entry *thread;
3069 if (core == CPU)
3071 /* Initialize core locks and IDs in all slots */
3072 int n;
3073 for (n = 0; n < MAXTHREADS; n++)
3075 thread = &threads[n];
3076 corelock_init(&thread->waiter_cl);
3077 corelock_init(&thread->slot_cl);
3078 thread->id = THREAD_ID_INIT(n);
3082 /* CPU will initialize first and then sleep */
3083 thread = find_empty_thread_slot();
3085 if (thread == NULL)
3087 /* WTF? There really must be a slot available at this stage.
3088 * This can fail if, for example, .bss isn't zero'ed out by the loader
3089 * or threads is in the wrong section. */
3090 THREAD_PANICF("init_threads->no slot", NULL);
3093 /* Initialize initially non-zero members of core */
3094 cores[core].next_tmo_check = current_tick; /* Something not in the past */
3096 /* Initialize initially non-zero members of slot */
3097 UNLOCK_THREAD(thread); /* No sync worries yet */
3098 thread->name = main_thread_name;
3099 thread->state = STATE_RUNNING;
3100 IF_COP( thread->core = core; )
3101 #ifdef HAVE_PRIORITY_SCHEDULING
3102 corelock_init(&cores[core].rtr_cl);
3103 thread->base_priority = PRIORITY_USER_INTERFACE;
3104 prio_add_entry(&thread->pdist, PRIORITY_USER_INTERFACE);
3105 thread->priority = PRIORITY_USER_INTERFACE;
3106 rtr_add_entry(core, PRIORITY_USER_INTERFACE);
3107 #endif
3109 add_to_list_l(&cores[core].running, thread);
3111 if (core == CPU)
3113 thread->stack = stackbegin;
3114 thread->stack_size = (uintptr_t)stackend - (uintptr_t)stackbegin;
3115 #if NUM_CORES > 1 /* This code path will not be run on single core targets */
3116 /* Wait for other processors to finish their inits since create_thread
3117 * isn't safe to call until the kernel inits are done. The first
3118 * threads created in the system must of course be created by CPU.
3119 * Another possible approach is to initialize all cores and slots
3120 * for each core by CPU, let the remainder proceed in parallel and
3121 * signal CPU when all are finished. */
3122 core_thread_init(CPU);
3124 else
3126 /* Initial stack is the idle stack */
3127 thread->stack = idle_stacks[core];
3128 thread->stack_size = IDLE_STACK_SIZE;
3129 /* After last processor completes, it should signal all others to
3130 * proceed or may signal the next and call thread_exit(). The last one
3131 * to finish will signal CPU. */
3132 core_thread_init(core);
3133 /* Other cores do not have a main thread - go idle inside switch_thread
3134 * until a thread can run on the core. */
3135 thread_exit();
3136 #endif /* NUM_CORES */
3140 /* Shared stack scan helper for thread_stack_usage and idle_stack_usage */
3141 #if NUM_CORES == 1
3142 static inline int stack_usage(uintptr_t *stackptr, size_t stack_size)
3143 #else
3144 static int stack_usage(uintptr_t *stackptr, size_t stack_size)
3145 #endif
3147 unsigned int stack_words = stack_size / sizeof (uintptr_t);
3148 unsigned int i;
3149 int usage = 0;
3151 for (i = 0; i < stack_words; i++)
3153 if (stackptr[i] != DEADBEEF)
3155 usage = ((stack_words - i) * 100) / stack_words;
3156 break;
3160 return usage;
3163 /*---------------------------------------------------------------------------
3164 * Returns the maximum percentage of stack a thread ever used while running.
3165 * NOTE: Some large buffer allocations that don't use enough the buffer to
3166 * overwrite stackptr[0] will not be seen.
3167 *---------------------------------------------------------------------------
3169 int thread_stack_usage(const struct thread_entry *thread)
3171 return stack_usage(thread->stack, thread->stack_size);
3174 #if NUM_CORES > 1
3175 /*---------------------------------------------------------------------------
3176 * Returns the maximum percentage of the core's idle stack ever used during
3177 * runtime.
3178 *---------------------------------------------------------------------------
3180 int idle_stack_usage(unsigned int core)
3182 return stack_usage(idle_stacks[core], IDLE_STACK_SIZE);
3184 #endif
3186 /*---------------------------------------------------------------------------
3187 * Fills in the buffer with the specified thread's name. If the name is NULL,
3188 * empty, or the thread is in destruct state a formatted ID is written
3189 * instead.
3190 *---------------------------------------------------------------------------
3192 void thread_get_name(char *buffer, int size,
3193 struct thread_entry *thread)
3195 if (size <= 0)
3196 return;
3198 *buffer = '\0';
3200 if (thread)
3202 /* Display thread name if one or ID if none */
3203 const char *name = thread->name;
3204 const char *fmt = "%s";
3205 if (name == NULL IF_COP(|| name == THREAD_DESTRUCT) || *name == '\0')
3207 name = (const char *)thread;
3208 fmt = "%08lX";
3210 snprintf(buffer, size, fmt, name);