1 /***************************************************************************
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
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 ****************************************************************************/
33 /****************************************************************************
35 * See notes below on implementing processor-specific portions! *
36 ***************************************************************************/
38 /* Define THREAD_EXTRA_CHECKS as 1 to enable additional state checks */
40 #define THREAD_EXTRA_CHECKS 1 /* Always 1 for DEBUG */
42 #define THREAD_EXTRA_CHECKS 0
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
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
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.
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.
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
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
103 * 2) If processor requires, atomically reenable interrupts and perform step
105 * 3) Sleep the CPU core. If wakeup itself enables interrupts (stop #0x2000
106 * on Coldfire) goto step 5.
107 * 4) Enable interrupts.
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
121 #define DEADBEEF ((uintptr_t)0xdeadbeefdeadbeefull)
122 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
));
145 static inline void run_blocking_ops(
146 unsigned int core
, struct thread_entry
*thread
)
147 __attribute__((always_inline
));
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__
170 #define IF_NO_SKIP_YIELD(...)
174 /*---------------------------------------------------------------------------
175 * Start the thread running and terminate it if it returns
176 *---------------------------------------------------------------------------
178 static void __attribute__((naked
,used
)) start_thread(void)
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"
187 "ldr r0, =invalidate_icache \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. */
191 "mov lr, pc \n" /* Call thread function */
193 ); /* No clobber list - new thread doesn't care */
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
)
213 "stmia %0, { r4-r11, sp, lr } \n"
218 /*---------------------------------------------------------------------------
219 * Load non-volatile context.
220 *---------------------------------------------------------------------------
222 static inline void load_context(const void* addr
)
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! */
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) */
283 "mov r1, %0 \n" /* r1 = PROCESSOR_ID */
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 */
289 "ldrb r3, [r0, r2, lsr #7] \n" /* cl->myl[othercore] == 0 ? */
290 "cmp r3, #0 \n" /* yes? lock acquired */
292 "ldrb r3, [r0, #2] \n" /* || cl->turn == core ? */
294 "bxeq lr \n" /* yes? lock acquired */
295 "b 1b \n" /* keep trying */
296 : : "i"(&PROCESSOR_ID
)
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) */
310 "mov r1, %0 \n" /* r1 = PROCESSOR_ID */
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 */
319 "ldrb r0, [r3, #2] \n" /* || cl->turn == core? */
321 "streqb r0, [r3, r1, lsr #7] \n" /* if not, cl->myl[core] = 0 */
322 "bx lr \n" /* return result */
323 : : "i"(&PROCESSOR_ID
)
330 /*---------------------------------------------------------------------------
331 * Release ownership of the corelock
332 *---------------------------------------------------------------------------
334 void corelock_unlock(struct corelock
*cl
) __attribute__((naked
));
335 void corelock_unlock(struct corelock
*cl
)
338 "mov r1, %0 \n" /* r1 = PROCESSOR_ID */
340 "mov r2, #0 \n" /* cl->myl[core] = 0 */
341 "strb r2, [r0, r1, lsr #7] \n"
343 : : "i"(&PROCESSOR_ID
)
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
;
362 if (cl
->myl
[othercore
] == 0 || cl
->turn
== core
)
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
)
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 *---------------------------------------------------------------------------
407 static inline void core_sleep(void)
409 sleep_core(CURRENT_CORE
);
413 static inline void core_sleep(unsigned int core
)
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"
433 : [ctl
]"r"(&CPU_CTL
), [mbx
]"r"(MBX_BASE
), [c
]"r"(core
)
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)
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 */
454 #endif /* NUM_CORES */
455 #elif CONFIG_CPU == PP5002
457 static inline void core_sleep(void)
459 sleep_core(CURRENT_CORE
);
463 /* PP5002 has no mailboxes - emulate using bytes */
464 static inline void core_sleep(unsigned int core
)
468 "mov r0, #1 \n" /* Signal intent to sleep */
469 "strb r0, [%[sem], #2] \n"
470 "ldrb r0, [%[sem], #1] \n" /* && stay_awake == 0? */
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! */
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 */
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"
493 : [sem
]"r"(&core_semaphores
[core
]), [c
]"r"(core
),
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)
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);
515 #endif /* ASM/C selection */
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 *---------------------------------------------------------------------------
527 /* Shared single-core build debugging version */
530 /* No wakey - core already wakey */
532 #elif defined (CPU_PP502x)
533 void core_wake(unsigned int othercore
)
536 /* avoid r0 since that contains othercore */
538 "mrs r3, cpsr \n" /* Disable IRQ */
539 "orr r1, r3, #0x80 \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 */
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
),
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
)
586 /* avoid r0 since that contains othercore */
588 "mrs r3, cpsr \n" /* Disable IRQ */
589 "orr r1, r3, #0x80 \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"
595 "1: \n" /* If it intends to sleep, let it first */
596 "ldrb r1, [%[sem], #2] \n" /* intend_sleep != 0 ? */
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 */
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
]),
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 */
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
)
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]));
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 */
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
699 "stmfd sp!, { r4-r12, 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 */
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, =invalidate_icache \n" /* Invalidate new core's cache */
715 "ldmfd sp!, { r4-r12, 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
725 *---------------------------------------------------------------------------
727 static void core_thread_init(unsigned int core
)
731 /* Wake up coprocessor and let it initialize kernel and threads */
736 /* Sleep until COP has finished */
741 /* Wake the CPU and return */
745 #endif /* NUM_CORES */
747 #elif CONFIG_CPU == S3C2440
749 /*---------------------------------------------------------------------------
750 * Put core in a power-saving state if waking list wasn't repopulated.
751 *---------------------------------------------------------------------------
753 static inline void core_sleep(void)
755 /* FIQ also changes the CLKCON register so FIQ must be disabled
756 when changing it here */
759 "orr r2, r0, #0x40 \n" /* Disable FIQ */
760 "bic r0, r0, #0x80 \n" /* Prepare IRQ enable */
762 "mov r1, #0x4c000000 \n" /* CLKCON = 0x4c00000c */
763 "ldr r2, [r1, #0xc] \n" /* Set IDLE bit */
765 "str r2, [r1, #0xc] \n"
766 "msr cpsr_c, r0 \n" /* Enable IRQ, restore FIQ */
767 "mov r2, #0 \n" /* wait for IDLE */
772 "orr r2, r0, #0xc0 \n" /* Disable IRQ, FIQ */
774 "ldr r2, [r1, #0xc] \n" /* Reset IDLE bit */
776 "str r2, [r1, #0xc] \n"
777 "msr cpsr_c, r0 \n" /* Enable IRQ, restore FIQ */
778 : : : "r0", "r1", "r2");
780 #elif defined(CPU_TCC77X)
781 static inline void core_sleep(void)
783 #warning TODO: Implement core_sleep
786 #elif defined(CPU_TCC780X)
787 static inline void core_sleep(void)
789 /* Single core only for now. Use the generic ARMv5 wait for IRQ */
792 "mcr p15, 0, r0, c7, c0, 4 \n" /* Wait for interrupt */
797 #elif CONFIG_CPU == IMX31L
798 static inline void core_sleep(void)
802 "mcr p15, 0, r0, c7, c0, 4 \n" /* Wait for interrupt */
807 #elif CONFIG_CPU == DM320
808 static inline void core_sleep(void)
812 "mcr p15, 0, r0, c7, c0, 4 \n" /* Wait for interrupt */
818 static inline void core_sleep(void)
820 #warning core_sleep not implemented, battery life will be decreased
823 #endif /* CONFIG_CPU == */
825 #elif defined(CPU_COLDFIRE)
826 /*---------------------------------------------------------------------------
827 * Start the thread running and terminate it if it returns
828 *---------------------------------------------------------------------------
830 void start_thread(void); /* Provide C access to ASM label */
831 static void __attribute__((used
)) __start_thread(void)
833 /* a0=macsr, a1=context */
835 "start_thread: \n" /* Start here - no naked attribute */
836 "move.l %a0, %macsr \n" /* Set initial mac status reg */
837 "lea.l 48(%a1), %a1 \n"
838 "move.l (%a1)+, %sp \n" /* Set initial stack */
839 "move.l (%a1), %a2 \n" /* Fetch thread function pointer */
840 "clr.l (%a1) \n" /* Mark thread running */
841 "jsr (%a2) \n" /* Call thread function */
846 /* Set EMAC unit to fractional mode with saturation for each new thread,
847 * since that's what'll be the most useful for most things which the dsp
848 * will do. Codecs should still initialize their preferred modes
849 * explicitly. Context pointer is placed in d2 slot and start_thread
850 * pointer in d3 slot. thread function pointer is placed in context.start.
851 * See load_context for what happens when thread is initially going to
854 #define THREAD_STARTUP_INIT(core, thread, function) \
855 ({ (thread)->context.macsr = EMAC_FRACTIONAL | EMAC_SATURATE, \
856 (thread)->context.d[0] = (uint32_t)&(thread)->context, \
857 (thread)->context.d[1] = (uint32_t)start_thread, \
858 (thread)->context.start = (uint32_t)(function); })
860 /*---------------------------------------------------------------------------
861 * Store non-volatile context.
862 *---------------------------------------------------------------------------
864 static inline void store_context(void* addr
)
867 "move.l %%macsr,%%d0 \n"
868 "movem.l %%d0/%%d2-%%d7/%%a2-%%a7,(%0) \n"
869 : : "a" (addr
) : "d0" /* only! */
873 /*---------------------------------------------------------------------------
874 * Load non-volatile context.
875 *---------------------------------------------------------------------------
877 static inline void load_context(const void* addr
)
880 "move.l 52(%0), %%d0 \n" /* Get start address */
881 "beq.b 1f \n" /* NULL -> already running */
882 "movem.l (%0), %%a0-%%a2 \n" /* a0=macsr, a1=context, a2=start_thread */
883 "jmp (%%a2) \n" /* Start the thread */
885 "movem.l (%0), %%d0/%%d2-%%d7/%%a2-%%a7 \n" /* Load context */
886 "move.l %%d0, %%macsr \n"
887 : : "a" (addr
) : "d0" /* only! */
891 /*---------------------------------------------------------------------------
892 * Put core in a power-saving state if waking list wasn't repopulated.
893 *---------------------------------------------------------------------------
895 static inline void core_sleep(void)
897 /* Supervisor mode, interrupts enabled upon wakeup */
898 asm volatile ("stop #0x2000");
901 #elif CONFIG_CPU == SH7034
902 /*---------------------------------------------------------------------------
903 * Start the thread running and terminate it if it returns
904 *---------------------------------------------------------------------------
906 void start_thread(void); /* Provide C access to ASM label */
907 static void __attribute__((used
)) __start_thread(void)
911 "_start_thread: \n" /* Start here - no naked attribute */
912 "mov.l @(4, r8), r0 \n" /* Fetch thread function pointer */
913 "mov.l @(28, r8), r15 \n" /* Set initial sp */
914 "mov #0, r1 \n" /* Start the thread */
916 "mov.l r1, @(36, r8) \n" /* Clear start address */
921 /* Place context pointer in r8 slot, function pointer in r9 slot, and
922 * start_thread pointer in context_start */
923 #define THREAD_STARTUP_INIT(core, thread, function) \
924 ({ (thread)->context.r[0] = (uint32_t)&(thread)->context, \
925 (thread)->context.r[1] = (uint32_t)(function), \
926 (thread)->context.start = (uint32_t)start_thread; })
928 /*---------------------------------------------------------------------------
929 * Store non-volatile context.
930 *---------------------------------------------------------------------------
932 static inline void store_context(void* addr
)
935 "add #36, %0 \n" /* Start at last reg. By the time routine */
936 "sts.l pr, @-%0 \n" /* is done, %0 will have the original value */
949 /*---------------------------------------------------------------------------
950 * Load non-volatile context.
951 *---------------------------------------------------------------------------
953 static inline void load_context(const void* addr
)
956 "mov.l @(36, %0), r0 \n" /* Get start address */
958 "bt .running \n" /* NULL -> already running */
959 "jmp @r0 \n" /* r8 = context */
961 "mov.l @%0+, r8 \n" /* Executes in delay slot and outside it */
970 : : "r" (addr
) : "r0" /* only! */
974 /*---------------------------------------------------------------------------
975 * Put core in a power-saving state.
976 *---------------------------------------------------------------------------
978 static inline void core_sleep(void)
981 "and.b #0x7f, @(r0, gbr) \n" /* Clear SBY (bit 7) in SBYCR */
982 "mov #0, r1 \n" /* Enable interrupts */
983 "ldc r1, sr \n" /* Following instruction cannot be interrupted */
984 "sleep \n" /* Execute standby */
985 : : "z"(&SBYCR
-GBR
) : "r1");
990 /*---------------------------------------------------------------------------
991 * Start the thread running and terminate it if it returns
992 *---------------------------------------------------------------------------
994 void start_thread(void); /* Provide C access to ASM label */
996 static void __attribute__((used
)) __start_thread(void)
1002 "_start_thread: \n" /* Start here - no naked attribute */
1003 "lw $8, (4)$2 \n" /* Fetch thread function pointer ($8 = $t0, $2 = $v0) */
1004 "lw $29, (108)$2 \n" /* Set initial sp(=$29) */
1005 "jalr $8 \n" /* Start the thread ($8 = $t0,)*/
1006 "sw $0, (116)$2 \n" /* Clear start address ($2 = $v0) */
1013 void start_thread(void)
1019 /* Place context pointer in $v0 slot, function pointer in $v1 slot, and
1020 * start_thread pointer in context_start */
1021 #define THREAD_STARTUP_INIT(core, thread, function) \
1022 ({ (thread)->context.r[0] = (uint32_t)&(thread)->context, \
1023 (thread)->context.r[1] = (uint32_t)(function), \
1024 (thread)->context.start = (uint32_t)start_thread; })
1026 /*---------------------------------------------------------------------------
1027 * Store non-volatile context.
1028 *---------------------------------------------------------------------------
1030 static inline void store_context(void* addr
)
1037 "sw $2,(4)%0 \n" /* $v0 */
1038 "sw $3,(8)%0 \n" /* $v1 */
1039 "sw $4,(12)%0 \n" /* $a0 */
1040 "sw $5,(16)%0 \n" /* $a1 */
1041 "sw $6,(20)%0 \n" /* $a2 */
1042 "sw $7,(24)%0 \n" /* $a3 */
1043 "sw $8,(28)%0 \n" /* $t0 */
1044 "sw $9,(32)%0 \n" /* $t1 */
1045 "sw $10,(36)%0 \n" /* $t2 */
1046 "sw $11,(40)%0 \n" /* $t3 */
1047 "sw $12,(44)%0 \n" /* $t4 */
1048 "sw $13,(48)%0 \n" /* $t5 */
1049 "sw $14,(52)%0 \n" /* $t6 */
1050 "sw $15,(56)%0 \n" /* $t7 */
1051 "sw $24,(60)%0 \n" /* $t8 */
1052 "sw $25,(64)%0 \n" /* $t9 */
1053 "sw $16,(68)%0 \n" /* $s0 */
1054 "sw $17,(72)%0 \n" /* $s1 */
1055 "sw $18,(76)%0 \n" /* $s2 */
1056 "sw $19,(80)%0 \n" /* $s3 */
1057 "sw $20,(84)%0 \n" /* $s4 */
1058 "sw $21,(88)%0 \n" /* $s5 */
1059 "sw $22,(92)%0 \n" /* $s6 */
1060 "sw $23,(96)%0 \n" /* $s7 */
1061 "sw $28,(100)%0 \n" /* gp */
1062 "sw $30,(104)%0 \n" /* fp */
1063 "sw $29,(108)%0 \n" /* sp */
1064 "sw $31,(112)%0 \n" /* ra */
1071 /*---------------------------------------------------------------------------
1072 * Load non-volatile context.
1073 *---------------------------------------------------------------------------
1075 static inline void load_context(const void* addr
)
1081 "lw $8, 116(%0) \n" /* Get start address ($8 = $t0) */
1083 "j .running \n" /* NULL -> already running */
1084 "jr $8 \n" /* $t0 = $8 = context */
1087 "lw $2,(4)%0 \n" /* $v0 */
1088 "lw $3,(8)%0 \n" /* $v1 */
1089 "lw $4,(12)%0 \n" /* $a0 */
1090 "lw $5,(16)%0 \n" /* $a1 */
1091 "lw $6,(20)%0 \n" /* $a2 */
1092 "lw $7,(24)%0 \n" /* $a3 */
1093 "lw $8,(28)%0 \n" /* $t0 */
1094 "lw $9,(32)%0 \n" /* $t1 */
1095 "lw $10,(36)%0 \n" /* $t2 */
1096 "lw $11,(40)%0 \n" /* $t3 */
1097 "lw $12,(44)%0 \n" /* $t4 */
1098 "lw $13,(48)%0 \n" /* $t5 */
1099 "lw $14,(52)%0 \n" /* $t6 */
1100 "lw $15,(56)%0 \n" /* $t7 */
1101 "lw $24,(60)%0 \n" /* $t8 */
1102 "lw $25,(64)%0 \n" /* $t9 */
1103 "lw $16,(68)%0 \n" /* $s0 */
1104 "lw $17,(72)%0 \n" /* $s1 */
1105 "lw $18,(76)%0 \n" /* $s2 */
1106 "lw $19,(80)%0 \n" /* $s3 */
1107 "lw $20,(84)%0 \n" /* $s4 */
1108 "lw $21,(88)%0 \n" /* $s5 */
1109 "lw $22,(92)%0 \n" /* $s6 */
1110 "lw $23,(96)%0 \n" /* $s7 */
1111 "lw $28,(100)%0 \n" /* gp */
1112 "lw $30,(104)%0 \n" /* fp */
1113 "lw $29,(108)%0 \n" /* sp */
1114 "lw $31,(112)%0 \n" /* ra */
1116 : : "r" (addr
) : "v0" /* only! */
1121 /*---------------------------------------------------------------------------
1122 * Put core in a power-saving state.
1123 *---------------------------------------------------------------------------
1125 static inline void core_sleep(void)
1128 REG_CPM_LCR &= ~CPM_LCR_LPM_MASK;
1129 REG_CPM_LCR |= CPM_LCR_LPM_SLEEP;
1132 asm volatile(".set mips32 \n"
1135 "ori t0, t0, 0x8000000 \n" /* Enable reduced power mode */
1144 REG_CPM_LCR &= ~CPM_LCR_LPM_MASK;
1145 REG_CPM_LCR |= CPM_LCR_LPM_IDLE;
1150 #endif /* CONFIG_CPU == */
1153 * End Processor-specific section
1154 ***************************************************************************/
1156 #if THREAD_EXTRA_CHECKS
1157 static void thread_panicf(const char *msg
, struct thread_entry
*thread
)
1159 IF_COP( const unsigned int core
= thread
->core
; )
1160 static char name
[32];
1161 thread_get_name(name
, 32, thread
);
1162 panicf ("%s %s" IF_COP(" (%d)"), msg
, name
IF_COP(, core
));
1164 static void thread_stkov(struct thread_entry
*thread
)
1166 thread_panicf("Stkov", thread
);
1168 #define THREAD_PANICF(msg, thread) \
1169 thread_panicf(msg, thread)
1170 #define THREAD_ASSERT(exp, msg, thread) \
1171 ({ if (!({ exp; })) thread_panicf((msg), (thread)); })
1173 static void thread_stkov(struct thread_entry
*thread
)
1175 IF_COP( const unsigned int core
= thread
->core
; )
1176 static char name
[32];
1177 thread_get_name(name
, 32, thread
);
1178 panicf("Stkov %s" IF_COP(" (%d)"), name
IF_COP(, core
));
1180 #define THREAD_PANICF(msg, thread)
1181 #define THREAD_ASSERT(exp, msg, thread)
1182 #endif /* THREAD_EXTRA_CHECKS */
1184 /* Thread locking */
1186 #define LOCK_THREAD(thread) \
1187 ({ corelock_lock(&(thread)->slot_cl); })
1188 #define TRY_LOCK_THREAD(thread) \
1189 ({ corelock_try_lock(&thread->slot_cl); })
1190 #define UNLOCK_THREAD(thread) \
1191 ({ corelock_unlock(&(thread)->slot_cl); })
1192 #define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \
1193 ({ unsigned int _core = (thread)->core; \
1194 cores[_core].blk_ops.flags |= TBOP_UNLOCK_CORELOCK; \
1195 cores[_core].blk_ops.cl_p = &(thread)->slot_cl; })
1197 #define LOCK_THREAD(thread) \
1199 #define TRY_LOCK_THREAD(thread) \
1201 #define UNLOCK_THREAD(thread) \
1203 #define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \
1208 #define RTR_LOCK(core) \
1209 ({ corelock_lock(&cores[core].rtr_cl); })
1210 #define RTR_UNLOCK(core) \
1211 ({ corelock_unlock(&cores[core].rtr_cl); })
1213 #ifdef HAVE_PRIORITY_SCHEDULING
1214 #define rtr_add_entry(core, priority) \
1215 prio_add_entry(&cores[core].rtr, (priority))
1217 #define rtr_subtract_entry(core, priority) \
1218 prio_subtract_entry(&cores[core].rtr, (priority))
1220 #define rtr_move_entry(core, from, to) \
1221 prio_move_entry(&cores[core].rtr, (from), (to))
1223 #define rtr_add_entry(core, priority)
1224 #define rtr_add_entry_inl(core, priority)
1225 #define rtr_subtract_entry(core, priority)
1226 #define rtr_subtract_entry_inl(core, priotity)
1227 #define rtr_move_entry(core, from, to)
1228 #define rtr_move_entry_inl(core, from, to)
1231 /*---------------------------------------------------------------------------
1232 * Thread list structure - circular:
1233 * +------------------------------+
1235 * +--+---+<-+---+<-+---+<-+---+<-+
1236 * Head->| T | | T | | T | | T |
1237 * +->+---+->+---+->+---+->+---+--+
1239 * +------------------------------+
1240 *---------------------------------------------------------------------------
1243 /*---------------------------------------------------------------------------
1244 * Adds a thread to a list of threads using "insert last". Uses the "l"
1246 *---------------------------------------------------------------------------
1248 static void add_to_list_l(struct thread_entry
**list
,
1249 struct thread_entry
*thread
)
1251 struct thread_entry
*l
= *list
;
1255 /* Insert into unoccupied list */
1256 thread
->l
.prev
= thread
;
1257 thread
->l
.next
= thread
;
1263 thread
->l
.prev
= l
->l
.prev
;
1265 l
->l
.prev
->l
.next
= thread
;
1269 /*---------------------------------------------------------------------------
1270 * Removes a thread from a list of threads. Uses the "l" links.
1271 *---------------------------------------------------------------------------
1273 static void remove_from_list_l(struct thread_entry
**list
,
1274 struct thread_entry
*thread
)
1276 struct thread_entry
*prev
, *next
;
1278 next
= thread
->l
.next
;
1287 if (thread
== *list
)
1289 /* List becomes next item */
1293 prev
= thread
->l
.prev
;
1295 /* Fix links to jump over the removed entry. */
1296 next
->l
.prev
= prev
;
1297 prev
->l
.next
= next
;
1300 /*---------------------------------------------------------------------------
1301 * Timeout list structure - circular reverse (to make "remove item" O(1)),
1302 * NULL-terminated forward (to ease the far more common forward traversal):
1303 * +------------------------------+
1305 * +--+---+<-+---+<-+---+<-+---+<-+
1306 * Head->| T | | T | | T | | T |
1307 * +---+->+---+->+---+->+---+-X
1308 *---------------------------------------------------------------------------
1311 /*---------------------------------------------------------------------------
1312 * Add a thread from the core's timout list by linking the pointers in its
1314 *---------------------------------------------------------------------------
1316 static void add_to_list_tmo(struct thread_entry
*thread
)
1318 struct thread_entry
*tmo
= cores
[IF_COP_CORE(thread
->core
)].timeout
;
1319 THREAD_ASSERT(thread
->tmo
.prev
== NULL
,
1320 "add_to_list_tmo->already listed", thread
);
1322 thread
->tmo
.next
= NULL
;
1326 /* Insert into unoccupied list */
1327 thread
->tmo
.prev
= thread
;
1328 cores
[IF_COP_CORE(thread
->core
)].timeout
= thread
;
1333 thread
->tmo
.prev
= tmo
->tmo
.prev
;
1334 tmo
->tmo
.prev
->tmo
.next
= thread
;
1335 tmo
->tmo
.prev
= thread
;
1338 /*---------------------------------------------------------------------------
1339 * Remove a thread from the core's timout list by unlinking the pointers in
1340 * its tmo structure. Sets thread->tmo.prev to NULL to indicate the timeout
1342 *---------------------------------------------------------------------------
1344 static void remove_from_list_tmo(struct thread_entry
*thread
)
1346 struct thread_entry
**list
= &cores
[IF_COP_CORE(thread
->core
)].timeout
;
1347 struct thread_entry
*prev
= thread
->tmo
.prev
;
1348 struct thread_entry
*next
= thread
->tmo
.next
;
1350 THREAD_ASSERT(prev
!= NULL
, "remove_from_list_tmo->not listed", thread
);
1353 next
->tmo
.prev
= prev
;
1355 if (thread
== *list
)
1357 /* List becomes next item and empty if next == NULL */
1359 /* Mark as unlisted */
1360 thread
->tmo
.prev
= NULL
;
1365 (*list
)->tmo
.prev
= prev
;
1366 prev
->tmo
.next
= next
;
1367 /* Mark as unlisted */
1368 thread
->tmo
.prev
= NULL
;
1373 #ifdef HAVE_PRIORITY_SCHEDULING
1374 /*---------------------------------------------------------------------------
1375 * Priority distribution structure (one category for each possible priority):
1377 * +----+----+----+ ... +-----+
1378 * hist: | F0 | F1 | F2 | | F31 |
1379 * +----+----+----+ ... +-----+
1380 * mask: | b0 | b1 | b2 | | b31 |
1381 * +----+----+----+ ... +-----+
1383 * F = count of threads at priority category n (frequency)
1384 * b = bitmask of non-zero priority categories (occupancy)
1386 * / if H[n] != 0 : 1
1390 *---------------------------------------------------------------------------
1391 * Basic priority inheritance priotocol (PIP):
1393 * Mn = mutex n, Tn = thread n
1395 * A lower priority thread inherits the priority of the highest priority
1396 * thread blocked waiting for it to complete an action (such as release a
1397 * mutex or respond to a message via queue_send):
1401 * T1 owns M1, T2 is waiting for M1 to realease M1. If T2 has a higher
1402 * priority than T1 then T1 inherits the priority of T2.
1408 * Situation is like 1) but T2 and T3 are both queued waiting for M1 and so
1409 * T1 inherits the higher of T2 and T3.
1411 * 3) T3->M2->T2->M1->T1
1413 * T1 owns M1, T2 owns M2. If T3 has a higher priority than both T1 and T2,
1414 * then T1 inherits the priority of T3 through T2.
1416 * Blocking chains can grow arbitrarily complex (though it's best that they
1417 * not form at all very often :) and build-up from these units.
1418 *---------------------------------------------------------------------------
1421 /*---------------------------------------------------------------------------
1422 * Increment frequency at category "priority"
1423 *---------------------------------------------------------------------------
1425 static inline unsigned int prio_add_entry(
1426 struct priority_distribution
*pd
, int priority
)
1429 /* Enough size/instruction count difference for ARM makes it worth it to
1430 * use different code (192 bytes for ARM). Only thing better is ASM. */
1432 count
= pd
->hist
[priority
];
1434 pd
->mask
|= 1 << priority
;
1435 pd
->hist
[priority
] = count
;
1436 #else /* This one's better for Coldfire */
1437 if ((count
= ++pd
->hist
[priority
]) == 1)
1438 pd
->mask
|= 1 << priority
;
1444 /*---------------------------------------------------------------------------
1445 * Decrement frequency at category "priority"
1446 *---------------------------------------------------------------------------
1448 static inline unsigned int prio_subtract_entry(
1449 struct priority_distribution
*pd
, int priority
)
1454 count
= pd
->hist
[priority
];
1456 pd
->mask
&= ~(1 << priority
);
1457 pd
->hist
[priority
] = count
;
1459 if ((count
= --pd
->hist
[priority
]) == 0)
1460 pd
->mask
&= ~(1 << priority
);
1466 /*---------------------------------------------------------------------------
1467 * Remove from one category and add to another
1468 *---------------------------------------------------------------------------
1470 static inline void prio_move_entry(
1471 struct priority_distribution
*pd
, int from
, int to
)
1473 uint32_t mask
= pd
->mask
;
1478 count
= pd
->hist
[from
];
1480 mask
&= ~(1 << from
);
1481 pd
->hist
[from
] = count
;
1483 count
= pd
->hist
[to
];
1486 pd
->hist
[to
] = count
;
1488 if (--pd
->hist
[from
] == 0)
1489 mask
&= ~(1 << from
);
1491 if (++pd
->hist
[to
] == 1)
1498 /*---------------------------------------------------------------------------
1499 * Change the priority and rtr entry for a running thread
1500 *---------------------------------------------------------------------------
1502 static inline void set_running_thread_priority(
1503 struct thread_entry
*thread
, int priority
)
1505 const unsigned int core
= IF_COP_CORE(thread
->core
);
1507 rtr_move_entry(core
, thread
->priority
, priority
);
1508 thread
->priority
= priority
;
1512 /*---------------------------------------------------------------------------
1513 * Finds the highest priority thread in a list of threads. If the list is
1514 * empty, the PRIORITY_IDLE is returned.
1516 * It is possible to use the struct priority_distribution within an object
1517 * instead of scanning the remaining threads in the list but as a compromise,
1518 * the resulting per-object memory overhead is saved at a slight speed
1519 * penalty under high contention.
1520 *---------------------------------------------------------------------------
1522 static int find_highest_priority_in_list_l(
1523 struct thread_entry
* const thread
)
1527 /* Go though list until the ending up at the initial thread */
1528 int highest_priority
= thread
->priority
;
1529 struct thread_entry
*curr
= thread
;
1533 int priority
= curr
->priority
;
1535 if (priority
< highest_priority
)
1536 highest_priority
= priority
;
1538 curr
= curr
->l
.next
;
1540 while (curr
!= thread
);
1542 return highest_priority
;
1545 return PRIORITY_IDLE
;
1548 /*---------------------------------------------------------------------------
1549 * Register priority with blocking system and bubble it down the chain if
1550 * any until we reach the end or something is already equal or higher.
1552 * NOTE: A simultaneous circular wait could spin deadlock on multiprocessor
1553 * targets but that same action also guarantees a circular block anyway and
1554 * those are prevented, right? :-)
1555 *---------------------------------------------------------------------------
1557 static struct thread_entry
*
1558 blocker_inherit_priority(struct thread_entry
*current
)
1560 const int priority
= current
->priority
;
1561 struct blocker
*bl
= current
->blocker
;
1562 struct thread_entry
* const tstart
= current
;
1563 struct thread_entry
*bl_t
= bl
->thread
;
1565 /* Blocker cannot change since the object protection is held */
1570 struct thread_entry
*next
;
1571 int bl_pr
= bl
->priority
;
1573 if (priority
>= bl_pr
)
1574 break; /* Object priority already high enough */
1576 bl
->priority
= priority
;
1579 prio_add_entry(&bl_t
->pdist
, priority
);
1581 if (bl_pr
< PRIORITY_IDLE
)
1583 /* Not first waiter - subtract old one */
1584 prio_subtract_entry(&bl_t
->pdist
, bl_pr
);
1587 if (priority
>= bl_t
->priority
)
1588 break; /* Thread priority high enough */
1590 if (bl_t
->state
== STATE_RUNNING
)
1592 /* Blocking thread is a running thread therefore there are no
1593 * further blockers. Change the "run queue" on which it
1595 set_running_thread_priority(bl_t
, priority
);
1599 bl_t
->priority
= priority
;
1601 /* If blocking thread has a blocker, apply transitive inheritance */
1605 break; /* End of chain or object doesn't support inheritance */
1610 break; /* Full-circle - deadlock! */
1612 UNLOCK_THREAD(current
);
1619 /* Blocker could change - retest condition */
1620 if (bl
->thread
== next
)
1623 UNLOCK_THREAD(next
);
1631 UNLOCK_THREAD(bl_t
);
1636 /*---------------------------------------------------------------------------
1637 * Readjust priorities when waking a thread blocked waiting for another
1638 * in essence "releasing" the thread's effect on the object owner. Can be
1639 * performed from any context.
1640 *---------------------------------------------------------------------------
1642 struct thread_entry
*
1643 wakeup_priority_protocol_release(struct thread_entry
*thread
)
1645 const int priority
= thread
->priority
;
1646 struct blocker
*bl
= thread
->blocker
;
1647 struct thread_entry
* const tstart
= thread
;
1648 struct thread_entry
*bl_t
= bl
->thread
;
1650 /* Blocker cannot change since object will be locked */
1653 thread
->blocker
= NULL
; /* Thread not blocked */
1657 struct thread_entry
*next
;
1658 int bl_pr
= bl
->priority
;
1660 if (priority
> bl_pr
)
1661 break; /* Object priority higher */
1663 next
= *thread
->bqp
;
1667 /* No more threads in queue */
1668 prio_subtract_entry(&bl_t
->pdist
, bl_pr
);
1669 bl
->priority
= PRIORITY_IDLE
;
1673 /* Check list for highest remaining priority */
1674 int queue_pr
= find_highest_priority_in_list_l(next
);
1676 if (queue_pr
== bl_pr
)
1677 break; /* Object priority not changing */
1679 /* Change queue priority */
1680 prio_move_entry(&bl_t
->pdist
, bl_pr
, queue_pr
);
1681 bl
->priority
= queue_pr
;
1684 if (bl_pr
> bl_t
->priority
)
1685 break; /* thread priority is higher */
1687 bl_pr
= find_first_set_bit(bl_t
->pdist
.mask
);
1689 if (bl_pr
== bl_t
->priority
)
1690 break; /* Thread priority not changing */
1692 if (bl_t
->state
== STATE_RUNNING
)
1694 /* No further blockers */
1695 set_running_thread_priority(bl_t
, bl_pr
);
1699 bl_t
->priority
= bl_pr
;
1701 /* If blocking thread has a blocker, apply transitive inheritance */
1705 break; /* End of chain or object doesn't support inheritance */
1710 break; /* Full-circle - deadlock! */
1712 UNLOCK_THREAD(thread
);
1719 /* Blocker could change - retest condition */
1720 if (bl
->thread
== next
)
1723 UNLOCK_THREAD(next
);
1731 UNLOCK_THREAD(bl_t
);
1734 if (thread
!= tstart
)
1736 /* Relock original if it changed */
1737 LOCK_THREAD(tstart
);
1741 return cores
[CURRENT_CORE
].running
;
1744 /*---------------------------------------------------------------------------
1745 * Transfer ownership to a thread waiting for an objects and transfer
1746 * inherited priority boost from other waiters. This algorithm knows that
1747 * blocking chains may only unblock from the very end.
1749 * Only the owning thread itself may call this and so the assumption that
1750 * it is the running thread is made.
1751 *---------------------------------------------------------------------------
1753 struct thread_entry
*
1754 wakeup_priority_protocol_transfer(struct thread_entry
*thread
)
1756 /* Waking thread inherits priority boost from object owner */
1757 struct blocker
*bl
= thread
->blocker
;
1758 struct thread_entry
*bl_t
= bl
->thread
;
1759 struct thread_entry
*next
;
1762 THREAD_ASSERT(thread_get_current() == bl_t
,
1763 "UPPT->wrong thread", thread_get_current());
1767 bl_pr
= bl
->priority
;
1769 /* Remove the object's boost from the owning thread */
1770 if (prio_subtract_entry(&bl_t
->pdist
, bl_pr
) == 0 &&
1771 bl_pr
<= bl_t
->priority
)
1773 /* No more threads at this priority are waiting and the old level is
1774 * at least the thread level */
1775 int priority
= find_first_set_bit(bl_t
->pdist
.mask
);
1777 if (priority
!= bl_t
->priority
)
1779 /* Adjust this thread's priority */
1780 set_running_thread_priority(bl_t
, priority
);
1784 next
= *thread
->bqp
;
1788 /* Expected shortcut - no more waiters */
1789 bl_pr
= PRIORITY_IDLE
;
1793 if (thread
->priority
<= bl_pr
)
1795 /* Need to scan threads remaining in queue */
1796 bl_pr
= find_highest_priority_in_list_l(next
);
1799 if (prio_add_entry(&thread
->pdist
, bl_pr
) == 1 &&
1800 bl_pr
< thread
->priority
)
1802 /* Thread priority must be raised */
1803 thread
->priority
= bl_pr
;
1807 bl
->thread
= thread
; /* This thread pwns */
1808 bl
->priority
= bl_pr
; /* Save highest blocked priority */
1809 thread
->blocker
= NULL
; /* Thread not blocked */
1811 UNLOCK_THREAD(bl_t
);
1816 /*---------------------------------------------------------------------------
1817 * No threads must be blocked waiting for this thread except for it to exit.
1818 * The alternative is more elaborate cleanup and object registration code.
1819 * Check this for risk of silent data corruption when objects with
1820 * inheritable blocking are abandoned by the owner - not precise but may
1822 *---------------------------------------------------------------------------
1824 static void check_for_obj_waiters(const char *function
, struct thread_entry
*thread
)
1826 /* Only one bit in the mask should be set with a frequency on 1 which
1827 * represents the thread's own base priority */
1828 uint32_t mask
= thread
->pdist
.mask
;
1829 if ((mask
& (mask
- 1)) != 0 ||
1830 thread
->pdist
.hist
[find_first_set_bit(mask
)] > 1)
1832 unsigned char name
[32];
1833 thread_get_name(name
, 32, thread
);
1834 panicf("%s->%s with obj. waiters", function
, name
);
1837 #endif /* HAVE_PRIORITY_SCHEDULING */
1839 /*---------------------------------------------------------------------------
1840 * Move a thread back to a running state on its core.
1841 *---------------------------------------------------------------------------
1843 static void core_schedule_wakeup(struct thread_entry
*thread
)
1845 const unsigned int core
= IF_COP_CORE(thread
->core
);
1849 thread
->state
= STATE_RUNNING
;
1851 add_to_list_l(&cores
[core
].running
, thread
);
1852 rtr_add_entry(core
, thread
->priority
);
1857 if (core
!= CURRENT_CORE
)
1862 /*---------------------------------------------------------------------------
1863 * Check the core's timeout list when at least one thread is due to wake.
1864 * Filtering for the condition is done before making the call. Resets the
1865 * tick when the next check will occur.
1866 *---------------------------------------------------------------------------
1868 void check_tmo_threads(void)
1870 const unsigned int core
= CURRENT_CORE
;
1871 const long tick
= current_tick
; /* snapshot the current tick */
1872 long next_tmo_check
= tick
+ 60*HZ
; /* minimum duration: once/minute */
1873 struct thread_entry
*next
= cores
[core
].timeout
;
1875 /* If there are no processes waiting for a timeout, just keep the check
1876 tick from falling into the past. */
1878 /* Break the loop once we have walked through the list of all
1879 * sleeping processes or have removed them all. */
1880 while (next
!= NULL
)
1882 /* Check sleeping threads. Allow interrupts between checks. */
1885 struct thread_entry
*curr
= next
;
1887 next
= curr
->tmo
.next
;
1889 /* Lock thread slot against explicit wakeup */
1893 unsigned state
= curr
->state
;
1895 if (state
< TIMEOUT_STATE_FIRST
)
1897 /* Cleanup threads no longer on a timeout but still on the
1899 remove_from_list_tmo(curr
);
1901 else if (TIME_BEFORE(tick
, curr
->tmo_tick
))
1903 /* Timeout still pending - this will be the usual case */
1904 if (TIME_BEFORE(curr
->tmo_tick
, next_tmo_check
))
1906 /* Earliest timeout found so far - move the next check up
1908 next_tmo_check
= curr
->tmo_tick
;
1913 /* Sleep timeout has been reached so bring the thread back to
1915 if (state
== STATE_BLOCKED_W_TMO
)
1918 /* Lock the waiting thread's kernel object */
1919 struct corelock
*ocl
= curr
->obj_cl
;
1921 if (corelock_try_lock(ocl
) == 0)
1923 /* Need to retry in the correct order though the need is
1925 UNLOCK_THREAD(curr
);
1929 if (curr
->state
!= STATE_BLOCKED_W_TMO
)
1931 /* Thread was woken or removed explicitely while slot
1933 corelock_unlock(ocl
);
1934 remove_from_list_tmo(curr
);
1935 UNLOCK_THREAD(curr
);
1939 #endif /* NUM_CORES */
1941 remove_from_list_l(curr
->bqp
, curr
);
1943 #ifdef HAVE_WAKEUP_EXT_CB
1944 if (curr
->wakeup_ext_cb
!= NULL
)
1945 curr
->wakeup_ext_cb(curr
);
1948 #ifdef HAVE_PRIORITY_SCHEDULING
1949 if (curr
->blocker
!= NULL
)
1950 wakeup_priority_protocol_release(curr
);
1952 corelock_unlock(ocl
);
1954 /* else state == STATE_SLEEPING */
1956 remove_from_list_tmo(curr
);
1960 curr
->state
= STATE_RUNNING
;
1962 add_to_list_l(&cores
[core
].running
, curr
);
1963 rtr_add_entry(core
, curr
->priority
);
1968 UNLOCK_THREAD(curr
);
1971 cores
[core
].next_tmo_check
= next_tmo_check
;
1974 /*---------------------------------------------------------------------------
1975 * Performs operations that must be done before blocking a thread but after
1976 * the state is saved.
1977 *---------------------------------------------------------------------------
1980 static inline void run_blocking_ops(
1981 unsigned int core
, struct thread_entry
*thread
)
1983 struct thread_blk_ops
*ops
= &cores
[core
].blk_ops
;
1984 const unsigned flags
= ops
->flags
;
1986 if (flags
== TBOP_CLEAR
)
1991 case TBOP_SWITCH_CORE
:
1992 core_switch_blk_op(core
, thread
);
1994 case TBOP_UNLOCK_CORELOCK
:
1995 corelock_unlock(ops
->cl_p
);
1999 ops
->flags
= TBOP_CLEAR
;
2001 #endif /* NUM_CORES > 1 */
2004 void profile_thread(void)
2006 profstart(cores
[CURRENT_CORE
].running
- threads
);
2010 /*---------------------------------------------------------------------------
2011 * Prepares a thread to block on an object's list and/or for a specified
2012 * duration - expects object and slot to be appropriately locked if needed
2013 * and interrupts to be masked.
2014 *---------------------------------------------------------------------------
2016 static inline void block_thread_on_l(struct thread_entry
*thread
,
2019 /* If inlined, unreachable branches will be pruned with no size penalty
2020 because state is passed as a constant parameter. */
2021 const unsigned int core
= IF_COP_CORE(thread
->core
);
2023 /* Remove the thread from the list of running threads. */
2025 remove_from_list_l(&cores
[core
].running
, thread
);
2026 rtr_subtract_entry(core
, thread
->priority
);
2029 /* Add a timeout to the block if not infinite */
2033 case STATE_BLOCKED_W_TMO
:
2034 /* Put the thread into a new list of inactive threads. */
2035 add_to_list_l(thread
->bqp
, thread
);
2037 if (state
== STATE_BLOCKED
)
2041 case STATE_SLEEPING
:
2042 /* If this thread times out sooner than any other thread, update
2043 next_tmo_check to its timeout */
2044 if (TIME_BEFORE(thread
->tmo_tick
, cores
[core
].next_tmo_check
))
2046 cores
[core
].next_tmo_check
= thread
->tmo_tick
;
2049 if (thread
->tmo
.prev
== NULL
)
2051 add_to_list_tmo(thread
);
2053 /* else thread was never removed from list - just keep it there */
2057 /* Remember the the next thread about to block. */
2058 cores
[core
].block_task
= thread
;
2060 /* Report new state. */
2061 thread
->state
= state
;
2064 /*---------------------------------------------------------------------------
2065 * Switch thread in round robin fashion for any given priority. Any thread
2066 * that removed itself from the running list first must specify itself in
2069 * INTERNAL: Intended for use by kernel and not for programs.
2070 *---------------------------------------------------------------------------
2072 void switch_thread(void)
2076 const unsigned int core
= CURRENT_CORE
;
2077 struct thread_entry
*block
= cores
[core
].block_task
;
2078 struct thread_entry
*thread
= cores
[core
].running
;
2080 /* Get context to save - next thread to run is unknown until all wakeups
2084 cores
[core
].block_task
= NULL
;
2087 if (thread
== block
)
2089 /* This was the last thread running and another core woke us before
2090 * reaching here. Force next thread selection to give tmo threads or
2091 * other threads woken before this block a first chance. */
2097 /* Blocking task is the old one */
2103 profile_thread_stopped(thread
- threads
);
2106 /* Begin task switching by saving our current context so that we can
2107 * restore the state of the current thread later to the point prior
2109 store_context(&thread
->context
);
2111 /* Check if the current thread stack is overflown */
2112 if (thread
->stack
[0] != DEADBEEF
)
2113 thread_stkov(thread
);
2116 /* Run any blocking operations requested before switching/sleeping */
2117 run_blocking_ops(core
, thread
);
2120 #ifdef HAVE_PRIORITY_SCHEDULING
2121 IF_NO_SKIP_YIELD( if (thread
->skip_count
!= -1) )
2122 /* Reset the value of thread's skip count */
2123 thread
->skip_count
= 0;
2128 /* If there are threads on a timeout and the earliest wakeup is due,
2129 * check the list and wake any threads that need to start running
2131 if (!TIME_BEFORE(current_tick
, cores
[core
].next_tmo_check
))
2133 check_tmo_threads();
2139 thread
= cores
[core
].running
;
2143 /* Enter sleep mode to reduce power usage - woken up on interrupt
2144 * or wakeup request from another core - expected to enable
2147 core_sleep(IF_COP(core
));
2151 #ifdef HAVE_PRIORITY_SCHEDULING
2152 /* Select the new task based on priorities and the last time a
2153 * process got CPU time relative to the highest priority runnable
2155 struct priority_distribution
*pd
= &cores
[core
].rtr
;
2156 int max
= find_first_set_bit(pd
->mask
);
2160 /* Not switching on a block, tentatively select next thread */
2161 thread
= thread
->l
.next
;
2166 int priority
= thread
->priority
;
2169 /* This ridiculously simple method of aging seems to work
2170 * suspiciously well. It does tend to reward CPU hogs (under
2171 * yielding) but that's generally not desirable at all. On the
2172 * plus side, it, relatively to other threads, penalizes excess
2173 * yielding which is good if some high priority thread is
2174 * performing no useful work such as polling for a device to be
2175 * ready. Of course, aging is only employed when higher and lower
2176 * priority threads are runnable. The highest priority runnable
2177 * thread(s) are never skipped. */
2178 if (priority
<= max
||
2179 IF_NO_SKIP_YIELD( thread
->skip_count
== -1 || )
2180 (diff
= priority
- max
, ++thread
->skip_count
> diff
*diff
))
2182 cores
[core
].running
= thread
;
2186 thread
= thread
->l
.next
;
2189 /* Without priority use a simple FCFS algorithm */
2192 /* Not switching on a block, select next thread */
2193 thread
= thread
->l
.next
;
2194 cores
[core
].running
= thread
;
2196 #endif /* HAVE_PRIORITY_SCHEDULING */
2204 /* And finally give control to the next thread. */
2205 load_context(&thread
->context
);
2208 profile_thread_started(thread
- threads
);
2214 /*---------------------------------------------------------------------------
2215 * Sleeps a thread for at least a specified number of ticks with zero being
2216 * a wait until the next tick.
2218 * INTERNAL: Intended for use by kernel and not for programs.
2219 *---------------------------------------------------------------------------
2221 void sleep_thread(int ticks
)
2223 struct thread_entry
*current
= cores
[CURRENT_CORE
].running
;
2225 LOCK_THREAD(current
);
2227 /* Set our timeout, remove from run list and join timeout list. */
2228 current
->tmo_tick
= current_tick
+ ticks
+ 1;
2229 block_thread_on_l(current
, STATE_SLEEPING
);
2231 UNLOCK_THREAD(current
);
2234 /*---------------------------------------------------------------------------
2235 * Indefinitely block a thread on a blocking queue for explicit wakeup.
2237 * INTERNAL: Intended for use by kernel objects and not for programs.
2238 *---------------------------------------------------------------------------
2240 void block_thread(struct thread_entry
*current
)
2242 /* Set the state to blocked and take us off of the run queue until we
2243 * are explicitly woken */
2244 LOCK_THREAD(current
);
2246 /* Set the list for explicit wakeup */
2247 block_thread_on_l(current
, STATE_BLOCKED
);
2249 #ifdef HAVE_PRIORITY_SCHEDULING
2250 if (current
->blocker
!= NULL
)
2252 /* Object supports PIP */
2253 current
= blocker_inherit_priority(current
);
2257 UNLOCK_THREAD(current
);
2260 /*---------------------------------------------------------------------------
2261 * Block a thread on a blocking queue for a specified time interval or until
2262 * explicitly woken - whichever happens first.
2264 * INTERNAL: Intended for use by kernel objects and not for programs.
2265 *---------------------------------------------------------------------------
2267 void block_thread_w_tmo(struct thread_entry
*current
, int timeout
)
2269 /* Get the entry for the current running thread. */
2270 LOCK_THREAD(current
);
2272 /* Set the state to blocked with the specified timeout */
2273 current
->tmo_tick
= current_tick
+ timeout
;
2275 /* Set the list for explicit wakeup */
2276 block_thread_on_l(current
, STATE_BLOCKED_W_TMO
);
2278 #ifdef HAVE_PRIORITY_SCHEDULING
2279 if (current
->blocker
!= NULL
)
2281 /* Object supports PIP */
2282 current
= blocker_inherit_priority(current
);
2286 UNLOCK_THREAD(current
);
2289 /*---------------------------------------------------------------------------
2290 * Explicitly wakeup a thread on a blocking queue. Only effects threads of
2291 * STATE_BLOCKED and STATE_BLOCKED_W_TMO.
2293 * This code should be considered a critical section by the caller meaning
2294 * that the object's corelock should be held.
2296 * INTERNAL: Intended for use by kernel objects and not for programs.
2297 *---------------------------------------------------------------------------
2299 unsigned int wakeup_thread(struct thread_entry
**list
)
2301 struct thread_entry
*thread
= *list
;
2302 unsigned int result
= THREAD_NONE
;
2304 /* Check if there is a blocked thread at all. */
2308 LOCK_THREAD(thread
);
2310 /* Determine thread's current state. */
2311 switch (thread
->state
)
2314 case STATE_BLOCKED_W_TMO
:
2315 remove_from_list_l(list
, thread
);
2319 #ifdef HAVE_PRIORITY_SCHEDULING
2320 struct thread_entry
*current
;
2321 struct blocker
*bl
= thread
->blocker
;
2325 /* No inheritance - just boost the thread by aging */
2326 IF_NO_SKIP_YIELD( if (thread
->skip_count
!= -1) )
2327 thread
->skip_count
= thread
->priority
;
2328 current
= cores
[CURRENT_CORE
].running
;
2332 /* Call the specified unblocking PIP */
2333 current
= bl
->wakeup_protocol(thread
);
2336 if (current
!= NULL
&& thread
->priority
< current
->priority
2337 IF_COP( && thread
->core
== current
->core
))
2339 /* Woken thread is higher priority and exists on the same CPU core;
2340 * recommend a task switch. Knowing if this is an interrupt call
2341 * would be helpful here. */
2342 result
|= THREAD_SWITCH
;
2344 #endif /* HAVE_PRIORITY_SCHEDULING */
2346 core_schedule_wakeup(thread
);
2349 /* Nothing to do. State is not blocked. */
2350 #if THREAD_EXTRA_CHECKS
2352 THREAD_PANICF("wakeup_thread->block invalid", thread
);
2359 UNLOCK_THREAD(thread
);
2363 /*---------------------------------------------------------------------------
2364 * Wakeup an entire queue of threads - returns bitwise-or of return bitmask
2365 * from each operation or THREAD_NONE of nothing was awakened. Object owning
2366 * the queue must be locked first.
2368 * INTERNAL: Intended for use by kernel objects and not for programs.
2369 *---------------------------------------------------------------------------
2371 unsigned int thread_queue_wake(struct thread_entry
**list
)
2373 unsigned result
= THREAD_NONE
;
2377 unsigned int rc
= wakeup_thread(list
);
2379 if (rc
== THREAD_NONE
)
2380 break; /* No more threads */
2388 /*---------------------------------------------------------------------------
2389 * Find an empty thread slot or MAXTHREADS if none found. The slot returned
2390 * will be locked on multicore.
2391 *---------------------------------------------------------------------------
2393 static struct thread_entry
* find_empty_thread_slot(void)
2395 /* Any slot could be on an interrupt-accessible list */
2396 IF_COP( int oldlevel
= disable_irq_save(); )
2397 struct thread_entry
*thread
= NULL
;
2400 for (n
= 0; n
< MAXTHREADS
; n
++)
2402 /* Obtain current slot state - lock it on multicore */
2403 struct thread_entry
*t
= &threads
[n
];
2406 if (t
->state
== STATE_KILLED
IF_COP( && t
->name
!= THREAD_DESTRUCT
))
2408 /* Slot is empty - leave it locked and caller will unlock */
2413 /* Finished examining slot - no longer busy - unlock on multicore */
2417 IF_COP( restore_irq(oldlevel
); ) /* Reenable interrups - this slot is
2418 not accesible to them yet */
2423 /*---------------------------------------------------------------------------
2424 * Place the current core in idle mode - woken up on interrupt or wake
2425 * request from another core.
2426 *---------------------------------------------------------------------------
2428 void core_idle(void)
2430 IF_COP( const unsigned int core
= CURRENT_CORE
; )
2432 core_sleep(IF_COP(core
));
2435 /*---------------------------------------------------------------------------
2436 * Create a thread. If using a dual core architecture, specify which core to
2437 * start the thread on.
2439 * Return ID if context area could be allocated, else NULL.
2440 *---------------------------------------------------------------------------
2442 struct thread_entry
*
2443 create_thread(void (*function
)(void), void* stack
, size_t stack_size
,
2444 unsigned flags
, const char *name
2445 IF_PRIO(, int priority
)
2446 IF_COP(, unsigned int core
))
2449 unsigned int stack_words
;
2450 uintptr_t stackptr
, stackend
;
2451 struct thread_entry
*thread
;
2455 thread
= find_empty_thread_slot();
2461 oldlevel
= disable_irq_save();
2463 /* Munge the stack to make it easy to spot stack overflows */
2464 stackptr
= ALIGN_UP((uintptr_t)stack
, sizeof (uintptr_t));
2465 stackend
= ALIGN_DOWN((uintptr_t)stack
+ stack_size
, sizeof (uintptr_t));
2466 stack_size
= stackend
- stackptr
;
2467 stack_words
= stack_size
/ sizeof (uintptr_t);
2469 for (i
= 0; i
< stack_words
; i
++)
2471 ((uintptr_t *)stackptr
)[i
] = DEADBEEF
;
2474 /* Store interesting information */
2475 thread
->name
= name
;
2476 thread
->stack
= (uintptr_t *)stackptr
;
2477 thread
->stack_size
= stack_size
;
2478 thread
->queue
= NULL
;
2479 #ifdef HAVE_WAKEUP_EXT_CB
2480 thread
->wakeup_ext_cb
= NULL
;
2482 #ifdef HAVE_SCHEDULER_BOOSTCTRL
2483 thread
->cpu_boost
= 0;
2485 #ifdef HAVE_PRIORITY_SCHEDULING
2486 memset(&thread
->pdist
, 0, sizeof(thread
->pdist
));
2487 thread
->blocker
= NULL
;
2488 thread
->base_priority
= priority
;
2489 thread
->priority
= priority
;
2490 thread
->skip_count
= priority
;
2491 prio_add_entry(&thread
->pdist
, priority
);
2495 thread
->core
= core
;
2497 /* Writeback stack munging or anything else before starting */
2498 if (core
!= CURRENT_CORE
)
2504 /* Thread is not on any timeout list but be a bit paranoid */
2505 thread
->tmo
.prev
= NULL
;
2507 state
= (flags
& CREATE_THREAD_FROZEN
) ?
2508 STATE_FROZEN
: STATE_RUNNING
;
2510 thread
->context
.sp
= (typeof (thread
->context
.sp
))stackend
;
2512 /* Load the thread's context structure with needed startup information */
2513 THREAD_STARTUP_INIT(core
, thread
, function
);
2515 thread
->state
= state
;
2517 if (state
== STATE_RUNNING
)
2518 core_schedule_wakeup(thread
);
2520 UNLOCK_THREAD(thread
);
2522 restore_irq(oldlevel
);
2527 #ifdef HAVE_SCHEDULER_BOOSTCTRL
2528 /*---------------------------------------------------------------------------
2529 * Change the boost state of a thread boosting or unboosting the CPU
2531 *---------------------------------------------------------------------------
2533 static inline void boost_thread(struct thread_entry
*thread
, bool boost
)
2535 if ((thread
->cpu_boost
!= 0) != boost
)
2537 thread
->cpu_boost
= boost
;
2542 void trigger_cpu_boost(void)
2544 struct thread_entry
*current
= cores
[CURRENT_CORE
].running
;
2545 boost_thread(current
, true);
2548 void cancel_cpu_boost(void)
2550 struct thread_entry
*current
= cores
[CURRENT_CORE
].running
;
2551 boost_thread(current
, false);
2553 #endif /* HAVE_SCHEDULER_BOOSTCTRL */
2555 /*---------------------------------------------------------------------------
2556 * Block the current thread until another thread terminates. A thread may
2557 * wait on itself to terminate which prevents it from running again and it
2558 * will need to be killed externally.
2559 * Parameter is the ID as returned from create_thread().
2560 *---------------------------------------------------------------------------
2562 void thread_wait(struct thread_entry
*thread
)
2564 struct thread_entry
*current
= cores
[CURRENT_CORE
].running
;
2569 /* Lock thread-as-waitable-object lock */
2570 corelock_lock(&thread
->waiter_cl
);
2572 /* Be sure it hasn't been killed yet */
2573 if (thread
->state
!= STATE_KILLED
)
2575 IF_COP( current
->obj_cl
= &thread
->waiter_cl
; )
2576 current
->bqp
= &thread
->queue
;
2579 block_thread(current
);
2581 corelock_unlock(&thread
->waiter_cl
);
2587 corelock_unlock(&thread
->waiter_cl
);
2590 /*---------------------------------------------------------------------------
2591 * Exit the current thread. The Right Way to Do Things (TM).
2592 *---------------------------------------------------------------------------
2594 void thread_exit(void)
2596 const unsigned int core
= CURRENT_CORE
;
2597 struct thread_entry
*current
= cores
[core
].running
;
2599 /* Cancel CPU boost if any */
2604 corelock_lock(¤t
->waiter_cl
);
2605 LOCK_THREAD(current
);
2607 #if defined (ALLOW_REMOVE_THREAD) && NUM_CORES > 1
2608 if (current
->name
== THREAD_DESTRUCT
)
2610 /* Thread being killed - become a waiter */
2611 UNLOCK_THREAD(current
);
2612 corelock_unlock(¤t
->waiter_cl
);
2613 thread_wait(current
);
2614 THREAD_PANICF("thread_exit->WK:*R", current
);
2618 #ifdef HAVE_PRIORITY_SCHEDULING
2619 check_for_obj_waiters("thread_exit", current
);
2622 if (current
->tmo
.prev
!= NULL
)
2624 /* Cancel pending timeout list removal */
2625 remove_from_list_tmo(current
);
2628 /* Switch tasks and never return */
2629 block_thread_on_l(current
, STATE_KILLED
);
2632 /* Switch to the idle stack if not on the main core (where "main"
2633 * runs) - we can hope gcc doesn't need the old stack beyond this
2637 switch_to_idle_stack(core
);
2642 current
->name
= NULL
;
2644 /* Signal this thread */
2645 thread_queue_wake(¤t
->queue
);
2646 corelock_unlock(¤t
->waiter_cl
);
2647 /* Slot must be unusable until thread is really gone */
2648 UNLOCK_THREAD_AT_TASK_SWITCH(current
);
2650 /* This should never and must never be reached - if it is, the
2651 * state is corrupted */
2652 THREAD_PANICF("thread_exit->K:*R", current
);
2655 #ifdef ALLOW_REMOVE_THREAD
2656 /*---------------------------------------------------------------------------
2657 * Remove a thread from the scheduler. Not The Right Way to Do Things in
2660 * Parameter is the ID as returned from create_thread().
2662 * Use with care on threads that are not under careful control as this may
2663 * leave various objects in an undefined state.
2664 *---------------------------------------------------------------------------
2666 void remove_thread(struct thread_entry
*thread
)
2669 /* core is not constant here because of core switching */
2670 unsigned int core
= CURRENT_CORE
;
2671 unsigned int old_core
= NUM_CORES
;
2672 struct corelock
*ocl
= NULL
;
2674 const unsigned int core
= CURRENT_CORE
;
2676 struct thread_entry
*current
= cores
[core
].running
;
2684 if (thread
== current
)
2685 thread_exit(); /* Current thread - do normal exit */
2687 oldlevel
= disable_irq_save();
2689 corelock_lock(&thread
->waiter_cl
);
2690 LOCK_THREAD(thread
);
2692 state
= thread
->state
;
2694 if (state
== STATE_KILLED
)
2700 if (thread
->name
== THREAD_DESTRUCT
)
2702 /* Thread being killed - become a waiter */
2703 UNLOCK_THREAD(thread
);
2704 corelock_unlock(&thread
->waiter_cl
);
2705 restore_irq(oldlevel
);
2706 thread_wait(thread
);
2710 thread
->name
= THREAD_DESTRUCT
; /* Slot can't be used for now */
2712 #ifdef HAVE_PRIORITY_SCHEDULING
2713 check_for_obj_waiters("remove_thread", thread
);
2716 if (thread
->core
!= core
)
2718 /* Switch cores and safely extract the thread there */
2719 /* Slot HAS to be unlocked or a deadlock could occur which means other
2720 * threads have to be guided into becoming thread waiters if they
2721 * attempt to remove it. */
2722 unsigned int new_core
= thread
->core
;
2724 corelock_unlock(&thread
->waiter_cl
);
2726 UNLOCK_THREAD(thread
);
2727 restore_irq(oldlevel
);
2729 old_core
= switch_core(new_core
);
2731 oldlevel
= disable_irq_save();
2733 corelock_lock(&thread
->waiter_cl
);
2734 LOCK_THREAD(thread
);
2736 state
= thread
->state
;
2738 /* Perform the extraction and switch ourselves back to the original
2741 #endif /* NUM_CORES > 1 */
2743 if (thread
->tmo
.prev
!= NULL
)
2745 /* Clean thread off the timeout list if a timeout check hasn't
2747 remove_from_list_tmo(thread
);
2750 #ifdef HAVE_SCHEDULER_BOOSTCTRL
2751 /* Cancel CPU boost if any */
2752 boost_thread(thread
, false);
2755 IF_COP( retry_state
: )
2761 /* Remove thread from ready to run tasks */
2762 remove_from_list_l(&cores
[core
].running
, thread
);
2763 rtr_subtract_entry(core
, thread
->priority
);
2767 case STATE_BLOCKED_W_TMO
:
2768 /* Remove thread from the queue it's blocked on - including its
2769 * own if waiting there */
2771 if (&thread
->waiter_cl
!= thread
->obj_cl
)
2773 ocl
= thread
->obj_cl
;
2775 if (corelock_try_lock(ocl
) == 0)
2777 UNLOCK_THREAD(thread
);
2779 LOCK_THREAD(thread
);
2781 if (thread
->state
!= state
)
2783 /* Something woke the thread */
2784 state
= thread
->state
;
2785 corelock_unlock(ocl
);
2791 remove_from_list_l(thread
->bqp
, thread
);
2793 #ifdef HAVE_WAKEUP_EXT_CB
2794 if (thread
->wakeup_ext_cb
!= NULL
)
2795 thread
->wakeup_ext_cb(thread
);
2798 #ifdef HAVE_PRIORITY_SCHEDULING
2799 if (thread
->blocker
!= NULL
)
2801 /* Remove thread's priority influence from its chain */
2802 wakeup_priority_protocol_release(thread
);
2808 corelock_unlock(ocl
);
2811 /* Otherwise thread is frozen and hasn't run yet */
2814 thread
->state
= STATE_KILLED
;
2816 /* If thread was waiting on itself, it will have been removed above.
2817 * The wrong order would result in waking the thread first and deadlocking
2818 * since the slot is already locked. */
2819 thread_queue_wake(&thread
->queue
);
2821 thread
->name
= NULL
;
2823 thread_killed
: /* Thread was already killed */
2824 /* Removal complete - safe to unlock and reenable interrupts */
2825 corelock_unlock(&thread
->waiter_cl
);
2826 UNLOCK_THREAD(thread
);
2827 restore_irq(oldlevel
);
2830 if (old_core
< NUM_CORES
)
2832 /* Did a removal on another processor's thread - switch back to
2834 switch_core(old_core
);
2838 #endif /* ALLOW_REMOVE_THREAD */
2840 #ifdef HAVE_PRIORITY_SCHEDULING
2841 /*---------------------------------------------------------------------------
2842 * Sets the thread's relative base priority for the core it runs on. Any
2843 * needed inheritance changes also may happen.
2844 *---------------------------------------------------------------------------
2846 int thread_set_priority(struct thread_entry
*thread
, int priority
)
2848 int old_base_priority
= -1;
2850 /* A little safety measure */
2851 if (priority
< HIGHEST_PRIORITY
|| priority
> LOWEST_PRIORITY
)
2855 thread
= cores
[CURRENT_CORE
].running
;
2857 /* Thread could be on any list and therefore on an interrupt accessible
2858 one - disable interrupts */
2859 int oldlevel
= disable_irq_save();
2861 LOCK_THREAD(thread
);
2863 /* Make sure it's not killed */
2864 if (thread
->state
!= STATE_KILLED
)
2866 int old_priority
= thread
->priority
;
2868 old_base_priority
= thread
->base_priority
;
2869 thread
->base_priority
= priority
;
2871 prio_move_entry(&thread
->pdist
, old_base_priority
, priority
);
2872 priority
= find_first_set_bit(thread
->pdist
.mask
);
2874 if (old_priority
== priority
)
2876 /* No priority change - do nothing */
2878 else if (thread
->state
== STATE_RUNNING
)
2880 /* This thread is running - change location on the run
2881 * queue. No transitive inheritance needed. */
2882 set_running_thread_priority(thread
, priority
);
2886 thread
->priority
= priority
;
2888 if (thread
->blocker
!= NULL
)
2890 /* Bubble new priority down the chain */
2891 struct blocker
*bl
= thread
->blocker
; /* Blocker struct */
2892 struct thread_entry
*bl_t
= bl
->thread
; /* Blocking thread */
2893 struct thread_entry
* const tstart
= thread
; /* Initial thread */
2894 const int highest
= MIN(priority
, old_priority
); /* Higher of new or old */
2898 struct thread_entry
*next
; /* Next thread to check */
2899 int bl_pr
; /* Highest blocked thread */
2900 int queue_pr
; /* New highest blocked thread */
2902 /* Owner can change but thread cannot be dislodged - thread
2903 * may not be the first in the queue which allows other
2904 * threads ahead in the list to be given ownership during the
2905 * operation. If thread is next then the waker will have to
2906 * wait for us and the owner of the object will remain fixed.
2907 * If we successfully grab the owner -- which at some point
2908 * is guaranteed -- then the queue remains fixed until we
2914 /* Double-check the owner - retry if it changed */
2915 if (bl
->thread
== bl_t
)
2918 UNLOCK_THREAD(bl_t
);
2922 bl_pr
= bl
->priority
;
2924 if (highest
> bl_pr
)
2925 break; /* Object priority won't change */
2927 /* This will include the thread being set */
2928 queue_pr
= find_highest_priority_in_list_l(*thread
->bqp
);
2930 if (queue_pr
== bl_pr
)
2931 break; /* Object priority not changing */
2933 /* Update thread boost for this object */
2934 bl
->priority
= queue_pr
;
2935 prio_move_entry(&bl_t
->pdist
, bl_pr
, queue_pr
);
2936 bl_pr
= find_first_set_bit(bl_t
->pdist
.mask
);
2938 if (bl_t
->priority
== bl_pr
)
2939 break; /* Blocking thread priority not changing */
2941 if (bl_t
->state
== STATE_RUNNING
)
2943 /* Thread not blocked - we're done */
2944 set_running_thread_priority(bl_t
, bl_pr
);
2948 bl_t
->priority
= bl_pr
;
2949 bl
= bl_t
->blocker
; /* Blocking thread has a blocker? */
2952 break; /* End of chain */
2957 break; /* Full-circle */
2959 UNLOCK_THREAD(thread
);
2965 UNLOCK_THREAD(bl_t
);
2970 UNLOCK_THREAD(thread
);
2972 restore_irq(oldlevel
);
2974 return old_base_priority
;
2977 /*---------------------------------------------------------------------------
2978 * Returns the current base priority for a thread.
2979 *---------------------------------------------------------------------------
2981 int thread_get_priority(struct thread_entry
*thread
)
2983 /* Simple, quick probe. */
2985 thread
= cores
[CURRENT_CORE
].running
;
2987 return thread
->base_priority
;
2989 #endif /* HAVE_PRIORITY_SCHEDULING */
2991 /*---------------------------------------------------------------------------
2992 * Starts a frozen thread - similar semantics to wakeup_thread except that
2993 * the thread is on no scheduler or wakeup queue at all. It exists simply by
2994 * virtue of the slot having a state of STATE_FROZEN.
2995 *---------------------------------------------------------------------------
2997 void thread_thaw(struct thread_entry
*thread
)
2999 int oldlevel
= disable_irq_save();
3000 LOCK_THREAD(thread
);
3002 if (thread
->state
== STATE_FROZEN
)
3003 core_schedule_wakeup(thread
);
3005 UNLOCK_THREAD(thread
);
3006 restore_irq(oldlevel
);
3009 /*---------------------------------------------------------------------------
3010 * Return the ID of the currently executing thread.
3011 *---------------------------------------------------------------------------
3013 struct thread_entry
* thread_get_current(void)
3015 return cores
[CURRENT_CORE
].running
;
3019 /*---------------------------------------------------------------------------
3020 * Switch the processor that the currently executing thread runs on.
3021 *---------------------------------------------------------------------------
3023 unsigned int switch_core(unsigned int new_core
)
3025 const unsigned int core
= CURRENT_CORE
;
3026 struct thread_entry
*current
= cores
[core
].running
;
3028 if (core
== new_core
)
3030 /* No change - just return same core */
3034 int oldlevel
= disable_irq_save();
3035 LOCK_THREAD(current
);
3037 if (current
->name
== THREAD_DESTRUCT
)
3039 /* Thread being killed - deactivate and let process complete */
3040 UNLOCK_THREAD(current
);
3041 restore_irq(oldlevel
);
3042 thread_wait(current
);
3043 /* Should never be reached */
3044 THREAD_PANICF("switch_core->D:*R", current
);
3047 /* Get us off the running list for the current core */
3049 remove_from_list_l(&cores
[core
].running
, current
);
3050 rtr_subtract_entry(core
, current
->priority
);
3053 /* Stash return value (old core) in a safe place */
3054 current
->retval
= core
;
3056 /* If a timeout hadn't yet been cleaned-up it must be removed now or
3057 * the other core will likely attempt a removal from the wrong list! */
3058 if (current
->tmo
.prev
!= NULL
)
3060 remove_from_list_tmo(current
);
3063 /* Change the core number for this thread slot */
3064 current
->core
= new_core
;
3066 /* Do not use core_schedule_wakeup here since this will result in
3067 * the thread starting to run on the other core before being finished on
3068 * this one. Delay the list unlock to keep the other core stuck
3069 * until this thread is ready. */
3072 rtr_add_entry(new_core
, current
->priority
);
3073 add_to_list_l(&cores
[new_core
].running
, current
);
3075 /* Make a callback into device-specific code, unlock the wakeup list so
3076 * that execution may resume on the new core, unlock our slot and finally
3077 * restore the interrupt level */
3078 cores
[core
].blk_ops
.flags
= TBOP_SWITCH_CORE
;
3079 cores
[core
].blk_ops
.cl_p
= &cores
[new_core
].rtr_cl
;
3080 cores
[core
].block_task
= current
;
3082 UNLOCK_THREAD(current
);
3084 /* Alert other core to activity */
3085 core_wake(new_core
);
3087 /* Do the stack switching, cache_maintenence and switch_thread call -
3088 requires native code */
3089 switch_thread_core(core
, current
);
3091 /* Finally return the old core to caller */
3092 return current
->retval
;
3094 #endif /* NUM_CORES > 1 */
3096 /*---------------------------------------------------------------------------
3097 * Initialize threading API. This assumes interrupts are not yet enabled. On
3098 * multicore setups, no core is allowed to proceed until create_thread calls
3099 * are safe to perform.
3100 *---------------------------------------------------------------------------
3102 void init_threads(void)
3104 const unsigned int core
= CURRENT_CORE
;
3105 struct thread_entry
*thread
;
3107 /* CPU will initialize first and then sleep */
3108 thread
= find_empty_thread_slot();
3112 /* WTF? There really must be a slot available at this stage.
3113 * This can fail if, for example, .bss isn't zero'ed out by the loader
3114 * or threads is in the wrong section. */
3115 THREAD_PANICF("init_threads->no slot", NULL
);
3118 /* Initialize initially non-zero members of core */
3119 cores
[core
].next_tmo_check
= current_tick
; /* Something not in the past */
3121 /* Initialize initially non-zero members of slot */
3122 UNLOCK_THREAD(thread
); /* No sync worries yet */
3123 thread
->name
= main_thread_name
;
3124 thread
->state
= STATE_RUNNING
;
3125 IF_COP( thread
->core
= core
; )
3126 #ifdef HAVE_PRIORITY_SCHEDULING
3127 corelock_init(&cores
[core
].rtr_cl
);
3128 thread
->base_priority
= PRIORITY_USER_INTERFACE
;
3129 prio_add_entry(&thread
->pdist
, PRIORITY_USER_INTERFACE
);
3130 thread
->priority
= PRIORITY_USER_INTERFACE
;
3131 rtr_add_entry(core
, PRIORITY_USER_INTERFACE
);
3133 corelock_init(&thread
->waiter_cl
);
3134 corelock_init(&thread
->slot_cl
);
3136 add_to_list_l(&cores
[core
].running
, thread
);
3140 thread
->stack
= stackbegin
;
3141 thread
->stack_size
= (uintptr_t)stackend
- (uintptr_t)stackbegin
;
3142 #if NUM_CORES > 1 /* This code path will not be run on single core targets */
3143 /* Wait for other processors to finish their inits since create_thread
3144 * isn't safe to call until the kernel inits are done. The first
3145 * threads created in the system must of course be created by CPU. */
3146 core_thread_init(CPU
);
3150 /* Initial stack is the idle stack */
3151 thread
->stack
= idle_stacks
[core
];
3152 thread
->stack_size
= IDLE_STACK_SIZE
;
3153 /* After last processor completes, it should signal all others to
3154 * proceed or may signal the next and call thread_exit(). The last one
3155 * to finish will signal CPU. */
3156 core_thread_init(core
);
3157 /* Other cores do not have a main thread - go idle inside switch_thread
3158 * until a thread can run on the core. */
3160 #endif /* NUM_CORES */
3164 /* Shared stack scan helper for thread_stack_usage and idle_stack_usage */
3166 static inline int stack_usage(uintptr_t *stackptr
, size_t stack_size
)
3168 static int stack_usage(uintptr_t *stackptr
, size_t stack_size
)
3171 unsigned int stack_words
= stack_size
/ sizeof (uintptr_t);
3175 for (i
= 0; i
< stack_words
; i
++)
3177 if (stackptr
[i
] != DEADBEEF
)
3179 usage
= ((stack_words
- i
) * 100) / stack_words
;
3187 /*---------------------------------------------------------------------------
3188 * Returns the maximum percentage of stack a thread ever used while running.
3189 * NOTE: Some large buffer allocations that don't use enough the buffer to
3190 * overwrite stackptr[0] will not be seen.
3191 *---------------------------------------------------------------------------
3193 int thread_stack_usage(const struct thread_entry
*thread
)
3195 return stack_usage(thread
->stack
, thread
->stack_size
);
3199 /*---------------------------------------------------------------------------
3200 * Returns the maximum percentage of the core's idle stack ever used during
3202 *---------------------------------------------------------------------------
3204 int idle_stack_usage(unsigned int core
)
3206 return stack_usage(idle_stacks
[core
], IDLE_STACK_SIZE
);
3210 /*---------------------------------------------------------------------------
3211 * Fills in the buffer with the specified thread's name. If the name is NULL,
3212 * empty, or the thread is in destruct state a formatted ID is written
3214 *---------------------------------------------------------------------------
3216 void thread_get_name(char *buffer
, int size
,
3217 struct thread_entry
*thread
)
3226 /* Display thread name if one or ID if none */
3227 const char *name
= thread
->name
;
3228 const char *fmt
= "%s";
3229 if (name
== NULL
IF_COP(|| name
== THREAD_DESTRUCT
) || *name
== '\0')
3231 name
= (const char *)thread
;
3234 snprintf(buffer
, size
, fmt
, name
);