diff options
Diffstat (limited to 'firmware/thread.c')
-rw-r--r-- | firmware/thread.c | 2535 |
1 files changed, 1436 insertions, 1099 deletions
diff --git a/firmware/thread.c b/firmware/thread.c index 8bebfedbf5..259a66a652 100644 --- a/firmware/thread.c +++ b/firmware/thread.c | |||
@@ -28,6 +28,10 @@ | |||
28 | #ifdef RB_PROFILE | 28 | #ifdef RB_PROFILE |
29 | #include <profile.h> | 29 | #include <profile.h> |
30 | #endif | 30 | #endif |
31 | /**************************************************************************** | ||
32 | * ATTENTION!! * | ||
33 | * See notes below on implementing processor-specific portions! * | ||
34 | ***************************************************************************/ | ||
31 | 35 | ||
32 | /* Define THREAD_EXTRA_CHECKS as 1 to enable additional state checks */ | 36 | /* Define THREAD_EXTRA_CHECKS as 1 to enable additional state checks */ |
33 | #ifdef DEBUG | 37 | #ifdef DEBUG |
@@ -59,9 +63,7 @@ | |||
59 | * event queues. The kernel object must have a scheme to protect itself from | 63 | * event queues. The kernel object must have a scheme to protect itself from |
60 | * access by another processor and is responsible for serializing the calls | 64 | * access by another processor and is responsible for serializing the calls |
61 | * to block_thread(_w_tmo) and wakeup_thread both to themselves and to each | 65 | * to block_thread(_w_tmo) and wakeup_thread both to themselves and to each |
62 | * other. If a thread blocks on an object it must fill-in the blk_ops members | 66 | * other. Objects' queues are also protected here. |
63 | * for its core to unlock _after_ the thread's context has been saved and the | ||
64 | * unlocking will be done in reverse from this heirarchy. | ||
65 | * | 67 | * |
66 | * 3) Thread Slot | 68 | * 3) Thread Slot |
67 | * This locks access to the thread's slot such that its state cannot be | 69 | * This locks access to the thread's slot such that its state cannot be |
@@ -70,70 +72,66 @@ | |||
70 | * a thread while it is still blocking will likely desync its state with | 72 | * a thread while it is still blocking will likely desync its state with |
71 | * the other resources used for that state. | 73 | * the other resources used for that state. |
72 | * | 74 | * |
73 | * 4) Lists | 75 | * 4) Core Lists |
74 | * Usually referring to a list (aka. queue) that a thread will be blocking | ||
75 | * on that belongs to some object and is shareable amongst multiple | ||
76 | * processors. Parts of the scheduler may have access to them without actually | ||
77 | * locking the kernel object such as when a thread is blocked with a timeout | ||
78 | * (such as calling queue_wait_w_tmo). Of course the kernel object also gets | ||
79 | * it lists locked when the thread blocks so that all object list access is | ||
80 | * synchronized. Failure to do so would corrupt the list links. | ||
81 | * | ||
82 | * 5) Core Lists | ||
83 | * These lists are specific to a particular processor core and are accessible | 76 | * These lists are specific to a particular processor core and are accessible |
84 | * by all processor cores and interrupt handlers. They are used when an | 77 | * by all processor cores and interrupt handlers. The running (rtr) list is |
85 | * operation may only be performed by the thread's own core in a normal | 78 | * the prime example where a thread may be added by any means. |
86 | * execution context. The wakeup list is the prime example where a thread | 79 | */ |
87 | * may be added by any means and the thread's own core will remove it from | ||
88 | * the wakeup list and put it on the running list (which is only ever | ||
89 | * accessible by its own processor). | ||
90 | */ | ||
91 | #define DEADBEEF ((unsigned int)0xdeadbeef) | ||
92 | /* Cast to the the machine int type, whose size could be < 4. */ | ||
93 | struct core_entry cores[NUM_CORES] IBSS_ATTR; | ||
94 | struct thread_entry threads[MAXTHREADS] IBSS_ATTR; | ||
95 | |||
96 | static const char main_thread_name[] = "main"; | ||
97 | extern int stackbegin[]; | ||
98 | extern int stackend[]; | ||
99 | 80 | ||
100 | /* core_sleep procedure to implement for any CPU to ensure an asychronous wakup | 81 | /*--------------------------------------------------------------------------- |
101 | * never results in requiring a wait until the next tick (up to 10000uS!). May | 82 | * Processor specific: core_sleep/core_wake/misc. notes |
102 | * require assembly and careful instruction ordering. | 83 | * |
84 | * ARM notes: | ||
85 | * FIQ is not dealt with by the scheduler code and is simply restored if it | ||
86 | * must by masked for some reason - because threading modifies a register | ||
87 | * that FIQ may also modify and there's no way to accomplish it atomically. | ||
88 | * s3c2440 is such a case. | ||
89 | * | ||
90 | * Audio interrupts are generally treated at a higher priority than others | ||
91 | * usage of scheduler code with interrupts higher than HIGHEST_IRQ_LEVEL | ||
92 | * are not in general safe. Special cases may be constructed on a per- | ||
93 | * source basis and blocking operations are not available. | ||
94 | * | ||
95 | * core_sleep procedure to implement for any CPU to ensure an asychronous | ||
96 | * wakup never results in requiring a wait until the next tick (up to | ||
97 | * 10000uS!). May require assembly and careful instruction ordering. | ||
103 | * | 98 | * |
104 | * 1) On multicore, stay awake if directed to do so by another. If so, goto step 4. | 99 | * 1) On multicore, stay awake if directed to do so by another. If so, goto |
105 | * 2) If processor requires, atomically reenable interrupts and perform step 3. | 100 | * step 4. |
106 | * 3) Sleep the CPU core. If wakeup itself enables interrupts (stop #0x2000 on Coldfire) | 101 | * 2) If processor requires, atomically reenable interrupts and perform step |
107 | * goto step 5. | 102 | * 3. |
103 | * 3) Sleep the CPU core. If wakeup itself enables interrupts (stop #0x2000 | ||
104 | * on Coldfire) goto step 5. | ||
108 | * 4) Enable interrupts. | 105 | * 4) Enable interrupts. |
109 | * 5) Exit procedure. | 106 | * 5) Exit procedure. |
107 | * | ||
108 | * core_wake and multprocessor notes for sleep/wake coordination: | ||
109 | * If possible, to wake up another processor, the forcing of an interrupt on | ||
110 | * the woken core by the waker core is the easiest way to ensure a non- | ||
111 | * delayed wake and immediate execution of any woken threads. If that isn't | ||
112 | * available then some careful non-blocking synchonization is needed (as on | ||
113 | * PP targets at the moment). | ||
114 | *--------------------------------------------------------------------------- | ||
110 | */ | 115 | */ |
111 | static inline void core_sleep(IF_COP_VOID(unsigned int core)) | ||
112 | __attribute__((always_inline)); | ||
113 | |||
114 | static void check_tmo_threads(void) | ||
115 | __attribute__((noinline)); | ||
116 | 116 | ||
117 | static inline void block_thread_on_l( | 117 | /* Cast to the the machine pointer size, whose size could be < 4 or > 32 |
118 | struct thread_queue *list, struct thread_entry *thread, unsigned state) | 118 | * (someday :). */ |
119 | __attribute__((always_inline)); | 119 | #define DEADBEEF ((uintptr_t)0xdeadbeefdeadbeefull) |
120 | struct core_entry cores[NUM_CORES] IBSS_ATTR; | ||
121 | struct thread_entry threads[MAXTHREADS] IBSS_ATTR; | ||
120 | 122 | ||
121 | static inline void block_thread_on_l_no_listlock( | 123 | static const char main_thread_name[] = "main"; |
122 | struct thread_entry **list, struct thread_entry *thread, unsigned state) | 124 | extern uintptr_t stackbegin[]; |
123 | __attribute__((always_inline)); | 125 | extern uintptr_t stackend[]; |
124 | 126 | ||
125 | static inline void _block_thread_on_l( | 127 | static inline void core_sleep(IF_COP_VOID(unsigned int core)) |
126 | struct thread_queue *list, struct thread_entry *thread, | ||
127 | unsigned state IF_SWCL(, const bool single)) | ||
128 | __attribute__((always_inline)); | 128 | __attribute__((always_inline)); |
129 | 129 | ||
130 | IF_SWCL(static inline) struct thread_entry * _wakeup_thread( | 130 | void check_tmo_threads(void) |
131 | struct thread_queue *list IF_SWCL(, const bool nolock)) | 131 | __attribute__((noinline)); |
132 | __attribute__((IFN_SWCL(noinline) IF_SWCL(always_inline))); | ||
133 | 132 | ||
134 | IF_SWCL(static inline) void _block_thread( | 133 | static inline void block_thread_on_l(struct thread_entry *thread, unsigned state) |
135 | struct thread_queue *list IF_SWCL(, const bool nolock)) | 134 | __attribute__((always_inline)); |
136 | __attribute__((IFN_SWCL(noinline) IF_SWCL(always_inline))); | ||
137 | 135 | ||
138 | static void add_to_list_tmo(struct thread_entry *thread) | 136 | static void add_to_list_tmo(struct thread_entry *thread) |
139 | __attribute__((noinline)); | 137 | __attribute__((noinline)); |
@@ -141,9 +139,6 @@ static void add_to_list_tmo(struct thread_entry *thread) | |||
141 | static void core_schedule_wakeup(struct thread_entry *thread) | 139 | static void core_schedule_wakeup(struct thread_entry *thread) |
142 | __attribute__((noinline)); | 140 | __attribute__((noinline)); |
143 | 141 | ||
144 | static inline void core_perform_wakeup(IF_COP_VOID(unsigned int core)) | ||
145 | __attribute__((always_inline)); | ||
146 | |||
147 | #if NUM_CORES > 1 | 142 | #if NUM_CORES > 1 |
148 | static inline void run_blocking_ops( | 143 | static inline void run_blocking_ops( |
149 | unsigned int core, struct thread_entry *thread) | 144 | unsigned int core, struct thread_entry *thread) |
@@ -159,10 +154,9 @@ static inline void store_context(void* addr) | |||
159 | static inline void load_context(const void* addr) | 154 | static inline void load_context(const void* addr) |
160 | __attribute__((always_inline)); | 155 | __attribute__((always_inline)); |
161 | 156 | ||
162 | void switch_thread(struct thread_entry *old) | 157 | void switch_thread(void) |
163 | __attribute__((noinline)); | 158 | __attribute__((noinline)); |
164 | 159 | ||
165 | |||
166 | /**************************************************************************** | 160 | /**************************************************************************** |
167 | * Processor-specific section | 161 | * Processor-specific section |
168 | */ | 162 | */ |
@@ -172,8 +166,7 @@ void switch_thread(struct thread_entry *old) | |||
172 | * Start the thread running and terminate it if it returns | 166 | * Start the thread running and terminate it if it returns |
173 | *--------------------------------------------------------------------------- | 167 | *--------------------------------------------------------------------------- |
174 | */ | 168 | */ |
175 | static void start_thread(void) __attribute__((naked,used)); | 169 | static void __attribute__((naked,used)) start_thread(void) |
176 | static void start_thread(void) | ||
177 | { | 170 | { |
178 | /* r0 = context */ | 171 | /* r0 = context */ |
179 | asm volatile ( | 172 | asm volatile ( |
@@ -188,19 +181,18 @@ static void start_thread(void) | |||
188 | #endif | 181 | #endif |
189 | "mov lr, pc \n" /* Call thread function */ | 182 | "mov lr, pc \n" /* Call thread function */ |
190 | "bx r4 \n" | 183 | "bx r4 \n" |
191 | "mov r0, #0 \n" /* remove_thread(NULL) */ | ||
192 | "ldr pc, =remove_thread \n" | ||
193 | ".ltorg \n" /* Dump constant pool */ | ||
194 | ); /* No clobber list - new thread doesn't care */ | 184 | ); /* No clobber list - new thread doesn't care */ |
185 | thread_exit(); | ||
186 | //asm volatile (".ltorg"); /* Dump constant pool */ | ||
195 | } | 187 | } |
196 | 188 | ||
197 | /* For startup, place context pointer in r4 slot, start_thread pointer in r5 | 189 | /* For startup, place context pointer in r4 slot, start_thread pointer in r5 |
198 | * slot, and thread function pointer in context.start. See load_context for | 190 | * slot, and thread function pointer in context.start. See load_context for |
199 | * what happens when thread is initially going to run. */ | 191 | * what happens when thread is initially going to run. */ |
200 | #define THREAD_STARTUP_INIT(core, thread, function) \ | 192 | #define THREAD_STARTUP_INIT(core, thread, function) \ |
201 | ({ (thread)->context.r[0] = (unsigned int)&(thread)->context, \ | 193 | ({ (thread)->context.r[0] = (uint32_t)&(thread)->context, \ |
202 | (thread)->context.r[1] = (unsigned int)start_thread, \ | 194 | (thread)->context.r[1] = (uint32_t)start_thread, \ |
203 | (thread)->context.start = (void *)function; }) | 195 | (thread)->context.start = (uint32_t)function; }) |
204 | 196 | ||
205 | /*--------------------------------------------------------------------------- | 197 | /*--------------------------------------------------------------------------- |
206 | * Store non-volatile context. | 198 | * Store non-volatile context. |
@@ -232,11 +224,11 @@ static inline void load_context(const void* addr) | |||
232 | #if defined (CPU_PP) | 224 | #if defined (CPU_PP) |
233 | 225 | ||
234 | #if NUM_CORES > 1 | 226 | #if NUM_CORES > 1 |
235 | extern int cpu_idlestackbegin[]; | 227 | extern uintptr_t cpu_idlestackbegin[]; |
236 | extern int cpu_idlestackend[]; | 228 | extern uintptr_t cpu_idlestackend[]; |
237 | extern int cop_idlestackbegin[]; | 229 | extern uintptr_t cop_idlestackbegin[]; |
238 | extern int cop_idlestackend[]; | 230 | extern uintptr_t cop_idlestackend[]; |
239 | static int * const idle_stacks[NUM_CORES] NOCACHEDATA_ATTR = | 231 | static uintptr_t * const idle_stacks[NUM_CORES] NOCACHEDATA_ATTR = |
240 | { | 232 | { |
241 | [CPU] = cpu_idlestackbegin, | 233 | [CPU] = cpu_idlestackbegin, |
242 | [COP] = cop_idlestackbegin | 234 | [COP] = cop_idlestackbegin |
@@ -253,7 +245,7 @@ struct core_semaphores | |||
253 | }; | 245 | }; |
254 | 246 | ||
255 | static struct core_semaphores core_semaphores[NUM_CORES] NOCACHEBSS_ATTR; | 247 | static struct core_semaphores core_semaphores[NUM_CORES] NOCACHEBSS_ATTR; |
256 | #endif | 248 | #endif /* CONFIG_CPU == PP5002 */ |
257 | 249 | ||
258 | #endif /* NUM_CORES */ | 250 | #endif /* NUM_CORES */ |
259 | 251 | ||
@@ -401,15 +393,15 @@ void corelock_unlock(struct corelock *cl) | |||
401 | * no other core requested a wakeup for it to perform a task. | 393 | * no other core requested a wakeup for it to perform a task. |
402 | *--------------------------------------------------------------------------- | 394 | *--------------------------------------------------------------------------- |
403 | */ | 395 | */ |
396 | #ifdef CPU_PP502x | ||
404 | #if NUM_CORES == 1 | 397 | #if NUM_CORES == 1 |
405 | /* Shared single-core build debugging version */ | ||
406 | static inline void core_sleep(void) | 398 | static inline void core_sleep(void) |
407 | { | 399 | { |
408 | PROC_CTL(CURRENT_CORE) = PROC_SLEEP; | 400 | PROC_CTL(CURRENT_CORE) = PROC_SLEEP; |
409 | nop; nop; nop; | 401 | nop; nop; nop; |
410 | set_interrupt_status(IRQ_FIQ_ENABLED, IRQ_FIQ_STATUS); | 402 | set_irq_level(IRQ_ENABLED); |
411 | } | 403 | } |
412 | #elif defined (CPU_PP502x) | 404 | #else |
413 | static inline void core_sleep(unsigned int core) | 405 | static inline void core_sleep(unsigned int core) |
414 | { | 406 | { |
415 | #if 1 | 407 | #if 1 |
@@ -429,8 +421,8 @@ static inline void core_sleep(unsigned int core) | |||
429 | "ldr r1, [%[mbx], #0] \n" | 421 | "ldr r1, [%[mbx], #0] \n" |
430 | "tst r1, r0, lsr #2 \n" | 422 | "tst r1, r0, lsr #2 \n" |
431 | "bne 1b \n" | 423 | "bne 1b \n" |
432 | "mrs r1, cpsr \n" /* Enable interrupts */ | 424 | "mrs r1, cpsr \n" /* Enable IRQ */ |
433 | "bic r1, r1, #0xc0 \n" | 425 | "bic r1, r1, #0x80 \n" |
434 | "msr cpsr_c, r1 \n" | 426 | "msr cpsr_c, r1 \n" |
435 | : | 427 | : |
436 | : [ctl]"r"(&PROC_CTL(CPU)), [mbx]"r"(MBX_BASE), [c]"r"(core) | 428 | : [ctl]"r"(&PROC_CTL(CPU)), [mbx]"r"(MBX_BASE), [c]"r"(core) |
@@ -452,11 +444,36 @@ static inline void core_sleep(unsigned int core) | |||
452 | /* Wait for other processor to finish wake procedure */ | 444 | /* Wait for other processor to finish wake procedure */ |
453 | while (MBX_MSG_STAT & (0x1 << core)); | 445 | while (MBX_MSG_STAT & (0x1 << core)); |
454 | 446 | ||
455 | /* Enable IRQ, FIQ */ | 447 | /* Enable IRQ */ |
456 | set_interrupt_status(IRQ_FIQ_ENABLED, IRQ_FIQ_STATUS); | 448 | set_irq_level(IRQ_ENABLED); |
457 | #endif /* ASM/C selection */ | 449 | #endif /* ASM/C selection */ |
458 | } | 450 | } |
451 | #endif /* NUM_CORES */ | ||
459 | #elif CONFIG_CPU == PP5002 | 452 | #elif CONFIG_CPU == PP5002 |
453 | #if NUM_CORES == 1 | ||
454 | static inline void core_sleep(void) | ||
455 | { | ||
456 | asm volatile ( | ||
457 | /* Sleep: PP5002 crashes if the instruction that puts it to sleep is | ||
458 | * located at 0xNNNNNNN0. 4/8/C works. This sequence makes sure | ||
459 | * that the correct alternative is executed. Don't change the order | ||
460 | * of the next 4 instructions! */ | ||
461 | "tst pc, #0x0c \n" | ||
462 | "mov r0, #0xca \n" | ||
463 | "strne r0, [%[ctl]] \n" | ||
464 | "streq r0, [%[ctl]] \n" | ||
465 | "nop \n" /* nop's needed because of pipeline */ | ||
466 | "nop \n" | ||
467 | "nop \n" | ||
468 | "mrs r0, cpsr \n" /* Enable IRQ */ | ||
469 | "bic r0, r0, #0x80 \n" | ||
470 | "msr cpsr_c, r0 \n" | ||
471 | : | ||
472 | : [ctl]"r"(&PROC_CTL(CURRENT_CORE)) | ||
473 | : "r0" | ||
474 | ); | ||
475 | } | ||
476 | #else | ||
460 | /* PP5002 has no mailboxes - emulate using bytes */ | 477 | /* PP5002 has no mailboxes - emulate using bytes */ |
461 | static inline void core_sleep(unsigned int core) | 478 | static inline void core_sleep(unsigned int core) |
462 | { | 479 | { |
@@ -486,8 +503,8 @@ static inline void core_sleep(unsigned int core) | |||
486 | "ldrb r0, [%[sem], #0] \n" | 503 | "ldrb r0, [%[sem], #0] \n" |
487 | "cmp r0, #0 \n" | 504 | "cmp r0, #0 \n" |
488 | "bne 1b \n" | 505 | "bne 1b \n" |
489 | "mrs r0, cpsr \n" /* Enable interrupts */ | 506 | "mrs r0, cpsr \n" /* Enable IRQ */ |
490 | "bic r0, r0, #0xc0 \n" | 507 | "bic r0, r0, #0x80 \n" |
491 | "msr cpsr_c, r0 \n" | 508 | "msr cpsr_c, r0 \n" |
492 | : | 509 | : |
493 | : [sem]"r"(&core_semaphores[core]), [c]"r"(core), | 510 | : [sem]"r"(&core_semaphores[core]), [c]"r"(core), |
@@ -512,11 +529,12 @@ static inline void core_sleep(unsigned int core) | |||
512 | /* Wait for other processor to finish wake procedure */ | 529 | /* Wait for other processor to finish wake procedure */ |
513 | while (core_semaphores[core].intend_wake != 0); | 530 | while (core_semaphores[core].intend_wake != 0); |
514 | 531 | ||
515 | /* Enable IRQ, FIQ */ | 532 | /* Enable IRQ */ |
516 | set_interrupt_status(IRQ_FIQ_ENABLED, IRQ_FIQ_STATUS); | 533 | set_irq_level(IRQ_ENABLED); |
517 | #endif /* ASM/C selection */ | 534 | #endif /* ASM/C selection */ |
518 | } | 535 | } |
519 | #endif /* CPU type */ | 536 | #endif /* NUM_CORES */ |
537 | #endif /* PP CPU type */ | ||
520 | 538 | ||
521 | /*--------------------------------------------------------------------------- | 539 | /*--------------------------------------------------------------------------- |
522 | * Wake another processor core that is sleeping or prevent it from doing so | 540 | * Wake another processor core that is sleeping or prevent it from doing so |
@@ -553,7 +571,7 @@ void core_wake(unsigned int othercore) | |||
553 | "strne r1, [%[ctl], %[oc], lsl #2] \n" | 571 | "strne r1, [%[ctl], %[oc], lsl #2] \n" |
554 | "mov r1, r2, lsr #4 \n" | 572 | "mov r1, r2, lsr #4 \n" |
555 | "str r1, [%[mbx], #8] \n" /* Done with wake procedure */ | 573 | "str r1, [%[mbx], #8] \n" /* Done with wake procedure */ |
556 | "msr cpsr_c, r3 \n" /* Restore int status */ | 574 | "msr cpsr_c, r3 \n" /* Restore IRQ */ |
557 | : | 575 | : |
558 | : [ctl]"r"(&PROC_CTL(CPU)), [mbx]"r"(MBX_BASE), | 576 | : [ctl]"r"(&PROC_CTL(CPU)), [mbx]"r"(MBX_BASE), |
559 | [oc]"r"(othercore) | 577 | [oc]"r"(othercore) |
@@ -604,7 +622,7 @@ void core_wake(unsigned int othercore) | |||
604 | "strne r1, [r2, %[oc], lsl #2] \n" | 622 | "strne r1, [r2, %[oc], lsl #2] \n" |
605 | "mov r1, #0 \n" /* Done with wake procedure */ | 623 | "mov r1, #0 \n" /* Done with wake procedure */ |
606 | "strb r1, [%[sem], #0] \n" | 624 | "strb r1, [%[sem], #0] \n" |
607 | "msr cpsr_c, r3 \n" /* Restore int status */ | 625 | "msr cpsr_c, r3 \n" /* Restore IRQ */ |
608 | : | 626 | : |
609 | : [sem]"r"(&core_semaphores[othercore]), | 627 | : [sem]"r"(&core_semaphores[othercore]), |
610 | [st]"r"(&PROC_STAT), | 628 | [st]"r"(&PROC_STAT), |
@@ -640,8 +658,8 @@ void core_wake(unsigned int othercore) | |||
640 | * | 658 | * |
641 | * Needed when a thread suicides on a core other than the main CPU since the | 659 | * 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 | 660 | * stack used when idling is the stack of the last thread to run. This stack |
643 | * may not reside in the core in which case the core will continue to use a | 661 | * may not reside in the core firmware in which case the core will continue |
644 | * stack from an unloaded module until another thread runs on it. | 662 | * to use a stack from an unloaded module until another thread runs on it. |
645 | *--------------------------------------------------------------------------- | 663 | *--------------------------------------------------------------------------- |
646 | */ | 664 | */ |
647 | static inline void switch_to_idle_stack(const unsigned int core) | 665 | static inline void switch_to_idle_stack(const unsigned int core) |
@@ -670,11 +688,11 @@ static void core_switch_blk_op(unsigned int core, struct thread_entry *thread) | |||
670 | /* Flush our data to ram */ | 688 | /* Flush our data to ram */ |
671 | flush_icache(); | 689 | flush_icache(); |
672 | /* Stash thread in r4 slot */ | 690 | /* Stash thread in r4 slot */ |
673 | thread->context.r[0] = (unsigned int)thread; | 691 | thread->context.r[0] = (uint32_t)thread; |
674 | /* Stash restart address in r5 slot */ | 692 | /* Stash restart address in r5 slot */ |
675 | thread->context.r[1] = (unsigned int)thread->context.start; | 693 | thread->context.r[1] = thread->context.start; |
676 | /* Save sp in context.sp while still running on old core */ | 694 | /* Save sp in context.sp while still running on old core */ |
677 | thread->context.sp = (void*)idle_stacks[core][IDLE_STACK_WORDS-1]; | 695 | thread->context.sp = idle_stacks[core][IDLE_STACK_WORDS-1]; |
678 | } | 696 | } |
679 | 697 | ||
680 | /*--------------------------------------------------------------------------- | 698 | /*--------------------------------------------------------------------------- |
@@ -689,9 +707,8 @@ static void core_switch_blk_op(unsigned int core, struct thread_entry *thread) | |||
689 | /*--------------------------------------------------------------------------- | 707 | /*--------------------------------------------------------------------------- |
690 | * This actually performs the core switch. | 708 | * This actually performs the core switch. |
691 | */ | 709 | */ |
692 | static void switch_thread_core(unsigned int core, struct thread_entry *thread) | 710 | static void __attribute__((naked)) |
693 | __attribute__((naked)); | 711 | switch_thread_core(unsigned int core, struct thread_entry *thread) |
694 | static void switch_thread_core(unsigned int core, struct thread_entry *thread) | ||
695 | { | 712 | { |
696 | /* Pure asm for this because compiler behavior isn't sufficiently predictable. | 713 | /* Pure asm for this because compiler behavior isn't sufficiently predictable. |
697 | * Stack access also isn't permitted until restoring the original stack and | 714 | * Stack access also isn't permitted until restoring the original stack and |
@@ -705,7 +722,6 @@ static void switch_thread_core(unsigned int core, struct thread_entry *thread) | |||
705 | "mov sp, r2 \n" /* switch stacks */ | 722 | "mov sp, r2 \n" /* switch stacks */ |
706 | "adr r2, 1f \n" /* r2 = new core restart address */ | 723 | "adr r2, 1f \n" /* r2 = new core restart address */ |
707 | "str r2, [r1, #40] \n" /* thread->context.start = r2 */ | 724 | "str r2, [r1, #40] \n" /* thread->context.start = r2 */ |
708 | "mov r0, r1 \n" /* switch_thread(thread) */ | ||
709 | "ldr pc, =switch_thread \n" /* r0 = thread after call - see load_context */ | 725 | "ldr pc, =switch_thread \n" /* r0 = thread after call - see load_context */ |
710 | "1: \n" | 726 | "1: \n" |
711 | "ldr sp, [r0, #32] \n" /* Reload original sp from context structure */ | 727 | "ldr sp, [r0, #32] \n" /* Reload original sp from context structure */ |
@@ -733,13 +749,15 @@ static inline void core_sleep(void) | |||
733 | /* FIQ also changes the CLKCON register so FIQ must be disabled | 749 | /* FIQ also changes the CLKCON register so FIQ must be disabled |
734 | when changing it here */ | 750 | when changing it here */ |
735 | asm volatile ( | 751 | asm volatile ( |
736 | "mrs r0, cpsr \n" /* Prepare IRQ, FIQ enable */ | 752 | "mrs r0, cpsr \n" |
737 | "bic r0, r0, #0xc0 \n" | 753 | "orr r2, r0, #0x40 \n" /* Disable FIQ */ |
754 | "bic r0, r0, #0x80 \n" /* Prepare IRQ enable */ | ||
755 | "msr cpsr_c, r2 \n" | ||
738 | "mov r1, #0x4c000000 \n" /* CLKCON = 0x4c00000c */ | 756 | "mov r1, #0x4c000000 \n" /* CLKCON = 0x4c00000c */ |
739 | "ldr r2, [r1, #0xc] \n" /* Set IDLE bit */ | 757 | "ldr r2, [r1, #0xc] \n" /* Set IDLE bit */ |
740 | "orr r2, r2, #4 \n" | 758 | "orr r2, r2, #4 \n" |
741 | "str r2, [r1, #0xc] \n" | 759 | "str r2, [r1, #0xc] \n" |
742 | "msr cpsr_c, r0 \n" /* Enable IRQ, FIQ */ | 760 | "msr cpsr_c, r0 \n" /* Enable IRQ, restore FIQ */ |
743 | "mov r2, #0 \n" /* wait for IDLE */ | 761 | "mov r2, #0 \n" /* wait for IDLE */ |
744 | "1: \n" | 762 | "1: \n" |
745 | "add r2, r2, #1 \n" | 763 | "add r2, r2, #1 \n" |
@@ -750,13 +768,14 @@ static inline void core_sleep(void) | |||
750 | "ldr r2, [r1, #0xc] \n" /* Reset IDLE bit */ | 768 | "ldr r2, [r1, #0xc] \n" /* Reset IDLE bit */ |
751 | "bic r2, r2, #4 \n" | 769 | "bic r2, r2, #4 \n" |
752 | "str r2, [r1, #0xc] \n" | 770 | "str r2, [r1, #0xc] \n" |
753 | "msr cpsr_c, r0 \n" /* Enable IRQ, FIQ */ | 771 | "msr cpsr_c, r0 \n" /* Enable IRQ, restore FIQ */ |
754 | : : : "r0", "r1", "r2"); | 772 | : : : "r0", "r1", "r2"); |
755 | } | 773 | } |
756 | #elif defined(CPU_TCC77X) | 774 | #elif defined(CPU_TCC77X) |
757 | static inline void core_sleep(void) | 775 | static inline void core_sleep(void) |
758 | { | 776 | { |
759 | #warning TODO: Implement core_sleep | 777 | #warning TODO: Implement core_sleep |
778 | set_irq_level(IRQ_ENABLED); | ||
760 | } | 779 | } |
761 | #elif defined(CPU_TCC780X) | 780 | #elif defined(CPU_TCC780X) |
762 | static inline void core_sleep(void) | 781 | static inline void core_sleep(void) |
@@ -765,8 +784,8 @@ static inline void core_sleep(void) | |||
765 | asm volatile ( | 784 | asm volatile ( |
766 | "mov r0, #0 \n" | 785 | "mov r0, #0 \n" |
767 | "mcr p15, 0, r0, c7, c0, 4 \n" /* Wait for interrupt */ | 786 | "mcr p15, 0, r0, c7, c0, 4 \n" /* Wait for interrupt */ |
768 | "mrs r0, cpsr \n" /* Unmask IRQ/FIQ at core level */ | 787 | "mrs r0, cpsr \n" /* Unmask IRQ at core level */ |
769 | "bic r0, r0, #0xc0 \n" | 788 | "bic r0, r0, #0x80 \n" |
770 | "msr cpsr_c, r0 \n" | 789 | "msr cpsr_c, r0 \n" |
771 | : : : "r0" | 790 | : : : "r0" |
772 | ); | 791 | ); |
@@ -777,8 +796,8 @@ static inline void core_sleep(void) | |||
777 | asm volatile ( | 796 | asm volatile ( |
778 | "mov r0, #0 \n" | 797 | "mov r0, #0 \n" |
779 | "mcr p15, 0, r0, c7, c0, 4 \n" /* Wait for interrupt */ | 798 | "mcr p15, 0, r0, c7, c0, 4 \n" /* Wait for interrupt */ |
780 | "mrs r0, cpsr \n" /* Unmask IRQ/FIQ at core level */ | 799 | "mrs r0, cpsr \n" /* Unmask IRQ at core level */ |
781 | "bic r0, r0, #0xc0 \n" | 800 | "bic r0, r0, #0x80 \n" |
782 | "msr cpsr_c, r0 \n" | 801 | "msr cpsr_c, r0 \n" |
783 | : : : "r0" | 802 | : : : "r0" |
784 | ); | 803 | ); |
@@ -787,6 +806,7 @@ static inline void core_sleep(void) | |||
787 | static inline void core_sleep(void) | 806 | static inline void core_sleep(void) |
788 | { | 807 | { |
789 | #warning core_sleep not implemented, battery life will be decreased | 808 | #warning core_sleep not implemented, battery life will be decreased |
809 | set_irq_level(0); | ||
790 | } | 810 | } |
791 | #endif /* CONFIG_CPU == */ | 811 | #endif /* CONFIG_CPU == */ |
792 | 812 | ||
@@ -796,8 +816,7 @@ static inline void core_sleep(void) | |||
796 | *--------------------------------------------------------------------------- | 816 | *--------------------------------------------------------------------------- |
797 | */ | 817 | */ |
798 | void start_thread(void); /* Provide C access to ASM label */ | 818 | void start_thread(void); /* Provide C access to ASM label */ |
799 | static void __start_thread(void) __attribute__((used)); | 819 | static void __attribute__((used)) __start_thread(void) |
800 | static void __start_thread(void) | ||
801 | { | 820 | { |
802 | /* a0=macsr, a1=context */ | 821 | /* a0=macsr, a1=context */ |
803 | asm volatile ( | 822 | asm volatile ( |
@@ -808,9 +827,8 @@ static void __start_thread(void) | |||
808 | "move.l (%a1), %a2 \n" /* Fetch thread function pointer */ | 827 | "move.l (%a1), %a2 \n" /* Fetch thread function pointer */ |
809 | "clr.l (%a1) \n" /* Mark thread running */ | 828 | "clr.l (%a1) \n" /* Mark thread running */ |
810 | "jsr (%a2) \n" /* Call thread function */ | 829 | "jsr (%a2) \n" /* Call thread function */ |
811 | "clr.l -(%sp) \n" /* remove_thread(NULL) */ | ||
812 | "jsr remove_thread \n" | ||
813 | ); | 830 | ); |
831 | thread_exit(); | ||
814 | } | 832 | } |
815 | 833 | ||
816 | /* Set EMAC unit to fractional mode with saturation for each new thread, | 834 | /* Set EMAC unit to fractional mode with saturation for each new thread, |
@@ -823,9 +841,9 @@ static void __start_thread(void) | |||
823 | */ | 841 | */ |
824 | #define THREAD_STARTUP_INIT(core, thread, function) \ | 842 | #define THREAD_STARTUP_INIT(core, thread, function) \ |
825 | ({ (thread)->context.macsr = EMAC_FRACTIONAL | EMAC_SATURATE, \ | 843 | ({ (thread)->context.macsr = EMAC_FRACTIONAL | EMAC_SATURATE, \ |
826 | (thread)->context.d[0] = (unsigned int)&(thread)->context, \ | 844 | (thread)->context.d[0] = (uint32_t)&(thread)->context, \ |
827 | (thread)->context.d[1] = (unsigned int)start_thread, \ | 845 | (thread)->context.d[1] = (uint32_t)start_thread, \ |
828 | (thread)->context.start = (void *)(function); }) | 846 | (thread)->context.start = (uint32_t)(function); }) |
829 | 847 | ||
830 | /*--------------------------------------------------------------------------- | 848 | /*--------------------------------------------------------------------------- |
831 | * Store non-volatile context. | 849 | * Store non-volatile context. |
@@ -874,8 +892,7 @@ static inline void core_sleep(void) | |||
874 | *--------------------------------------------------------------------------- | 892 | *--------------------------------------------------------------------------- |
875 | */ | 893 | */ |
876 | void start_thread(void); /* Provide C access to ASM label */ | 894 | void start_thread(void); /* Provide C access to ASM label */ |
877 | static void __start_thread(void) __attribute__((used)); | 895 | static void __attribute__((used)) __start_thread(void) |
878 | static void __start_thread(void) | ||
879 | { | 896 | { |
880 | /* r8 = context */ | 897 | /* r8 = context */ |
881 | asm volatile ( | 898 | asm volatile ( |
@@ -885,20 +902,16 @@ static void __start_thread(void) | |||
885 | "mov #0, r1 \n" /* Start the thread */ | 902 | "mov #0, r1 \n" /* Start the thread */ |
886 | "jsr @r0 \n" | 903 | "jsr @r0 \n" |
887 | "mov.l r1, @(36, r8) \n" /* Clear start address */ | 904 | "mov.l r1, @(36, r8) \n" /* Clear start address */ |
888 | "mov.l 1f, r0 \n" /* remove_thread(NULL) */ | ||
889 | "jmp @r0 \n" | ||
890 | "mov #0, r4 \n" | ||
891 | "1: \n" | ||
892 | ".long _remove_thread \n" | ||
893 | ); | 905 | ); |
906 | thread_exit(); | ||
894 | } | 907 | } |
895 | 908 | ||
896 | /* Place context pointer in r8 slot, function pointer in r9 slot, and | 909 | /* Place context pointer in r8 slot, function pointer in r9 slot, and |
897 | * start_thread pointer in context_start */ | 910 | * start_thread pointer in context_start */ |
898 | #define THREAD_STARTUP_INIT(core, thread, function) \ | 911 | #define THREAD_STARTUP_INIT(core, thread, function) \ |
899 | ({ (thread)->context.r[0] = (unsigned int)&(thread)->context, \ | 912 | ({ (thread)->context.r[0] = (uint32_t)&(thread)->context, \ |
900 | (thread)->context.r[1] = (unsigned int)(function), \ | 913 | (thread)->context.r[1] = (uint32_t)(function), \ |
901 | (thread)->context.start = (void*)start_thread; }) | 914 | (thread)->context.start = (uint32_t)start_thread; }) |
902 | 915 | ||
903 | /*--------------------------------------------------------------------------- | 916 | /*--------------------------------------------------------------------------- |
904 | * Store non-volatile context. | 917 | * Store non-volatile context. |
@@ -947,7 +960,7 @@ static inline void load_context(const void* addr) | |||
947 | } | 960 | } |
948 | 961 | ||
949 | /*--------------------------------------------------------------------------- | 962 | /*--------------------------------------------------------------------------- |
950 | * Put core in a power-saving state if waking list wasn't repopulated. | 963 | * Put core in a power-saving state. |
951 | *--------------------------------------------------------------------------- | 964 | *--------------------------------------------------------------------------- |
952 | */ | 965 | */ |
953 | static inline void core_sleep(void) | 966 | static inline void core_sleep(void) |
@@ -969,9 +982,7 @@ static inline void core_sleep(void) | |||
969 | #if THREAD_EXTRA_CHECKS | 982 | #if THREAD_EXTRA_CHECKS |
970 | static void thread_panicf(const char *msg, struct thread_entry *thread) | 983 | static void thread_panicf(const char *msg, struct thread_entry *thread) |
971 | { | 984 | { |
972 | #if NUM_CORES > 1 | 985 | IF_COP( const unsigned int core = thread->core; ) |
973 | const unsigned int core = thread->core; | ||
974 | #endif | ||
975 | static char name[32]; | 986 | static char name[32]; |
976 | thread_get_name(name, 32, thread); | 987 | thread_get_name(name, 32, thread); |
977 | panicf ("%s %s" IF_COP(" (%d)"), msg, name IF_COP(, core)); | 988 | panicf ("%s %s" IF_COP(" (%d)"), msg, name IF_COP(, core)); |
@@ -987,9 +998,7 @@ static void thread_stkov(struct thread_entry *thread) | |||
987 | #else | 998 | #else |
988 | static void thread_stkov(struct thread_entry *thread) | 999 | static void thread_stkov(struct thread_entry *thread) |
989 | { | 1000 | { |
990 | #if NUM_CORES > 1 | 1001 | IF_COP( const unsigned int core = thread->core; ) |
991 | const unsigned int core = thread->core; | ||
992 | #endif | ||
993 | static char name[32]; | 1002 | static char name[32]; |
994 | thread_get_name(name, 32, thread); | 1003 | thread_get_name(name, 32, thread); |
995 | panicf("Stkov %s" IF_COP(" (%d)"), name IF_COP(, core)); | 1004 | panicf("Stkov %s" IF_COP(" (%d)"), name IF_COP(, core)); |
@@ -998,111 +1007,67 @@ static void thread_stkov(struct thread_entry *thread) | |||
998 | #define THREAD_ASSERT(exp, msg, thread) | 1007 | #define THREAD_ASSERT(exp, msg, thread) |
999 | #endif /* THREAD_EXTRA_CHECKS */ | 1008 | #endif /* THREAD_EXTRA_CHECKS */ |
1000 | 1009 | ||
1001 | /*--------------------------------------------------------------------------- | ||
1002 | * Lock a list pointer and returns its value | ||
1003 | *--------------------------------------------------------------------------- | ||
1004 | */ | ||
1005 | #if CONFIG_CORELOCK == SW_CORELOCK | ||
1006 | /* Separate locking function versions */ | ||
1007 | |||
1008 | /* Thread locking */ | 1010 | /* Thread locking */ |
1009 | #define GET_THREAD_STATE(thread) \ | 1011 | #if NUM_CORES > 1 |
1010 | ({ corelock_lock(&(thread)->cl); (thread)->state; }) | 1012 | #define LOCK_THREAD(thread) \ |
1011 | #define TRY_GET_THREAD_STATE(thread) \ | 1013 | ({ corelock_lock(&(thread)->slot_cl); }) |
1012 | ({ corelock_try_lock(&thread->cl) ? thread->state : STATE_BUSY; }) | 1014 | #define TRY_LOCK_THREAD(thread) \ |
1013 | #define UNLOCK_THREAD(thread, state) \ | 1015 | ({ corelock_try_lock(&thread->slot_cl); }) |
1014 | ({ corelock_unlock(&(thread)->cl); }) | 1016 | #define UNLOCK_THREAD(thread) \ |
1015 | #define UNLOCK_THREAD_SET_STATE(thread, _state) \ | 1017 | ({ corelock_unlock(&(thread)->slot_cl); }) |
1016 | ({ (thread)->state = (_state); corelock_unlock(&(thread)->cl); }) | 1018 | #define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \ |
1017 | 1019 | ({ unsigned int _core = (thread)->core; \ | |
1018 | /* List locking */ | 1020 | cores[_core].blk_ops.flags |= TBOP_UNLOCK_CORELOCK; \ |
1019 | #define LOCK_LIST(tqp) \ | 1021 | cores[_core].blk_ops.cl_p = &(thread)->slot_cl; }) |
1020 | ({ corelock_lock(&(tqp)->cl); (tqp)->queue; }) | 1022 | #else |
1021 | #define UNLOCK_LIST(tqp, mod) \ | 1023 | #define LOCK_THREAD(thread) \ |
1022 | ({ corelock_unlock(&(tqp)->cl); }) | 1024 | ({ }) |
1023 | #define UNLOCK_LIST_SET_PTR(tqp, mod) \ | 1025 | #define TRY_LOCK_THREAD(thread) \ |
1024 | ({ (tqp)->queue = (mod); corelock_unlock(&(tqp)->cl); }) | 1026 | ({ }) |
1025 | 1027 | #define UNLOCK_THREAD(thread) \ | |
1026 | /* Select the queue pointer directly */ | 1028 | ({ }) |
1027 | #define ADD_TO_LIST_L_SELECT(tc, tqp, thread) \ | 1029 | #define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \ |
1028 | ({ add_to_list_l(&(tqp)->queue, (thread)); }) | 1030 | ({ }) |
1029 | #define REMOVE_FROM_LIST_L_SELECT(tc, tqp, thread) \ | 1031 | #endif |
1030 | ({ remove_from_list_l(&(tqp)->queue, (thread)); }) | 1032 | |
1031 | 1033 | /* RTR list */ | |
1032 | #elif CONFIG_CORELOCK == CORELOCK_SWAP | 1034 | #define RTR_LOCK(core) \ |
1033 | /* Native swap/exchange versions */ | 1035 | ({ corelock_lock(&cores[core].rtr_cl); }) |
1036 | #define RTR_UNLOCK(core) \ | ||
1037 | ({ corelock_unlock(&cores[core].rtr_cl); }) | ||
1034 | 1038 | ||
1035 | /* Thread locking */ | 1039 | #ifdef HAVE_PRIORITY_SCHEDULING |
1036 | #define GET_THREAD_STATE(thread) \ | 1040 | #define rtr_add_entry(core, priority) \ |
1037 | ({ unsigned _s; \ | 1041 | prio_add_entry(&cores[core].rtr, (priority)) |
1038 | while ((_s = xchg8(&(thread)->state, STATE_BUSY)) == STATE_BUSY); \ | ||
1039 | _s; }) | ||
1040 | #define TRY_GET_THREAD_STATE(thread) \ | ||
1041 | ({ xchg8(&(thread)->state, STATE_BUSY); }) | ||
1042 | #define UNLOCK_THREAD(thread, _state) \ | ||
1043 | ({ (thread)->state = (_state); }) | ||
1044 | #define UNLOCK_THREAD_SET_STATE(thread, _state) \ | ||
1045 | ({ (thread)->state = (_state); }) | ||
1046 | |||
1047 | /* List locking */ | ||
1048 | #define LOCK_LIST(tqp) \ | ||
1049 | ({ struct thread_entry *_l; \ | ||
1050 | while((_l = xchgptr(&(tqp)->queue, STATE_BUSYuptr)) == STATE_BUSYuptr); \ | ||
1051 | _l; }) | ||
1052 | #define UNLOCK_LIST(tqp, mod) \ | ||
1053 | ({ (tqp)->queue = (mod); }) | ||
1054 | #define UNLOCK_LIST_SET_PTR(tqp, mod) \ | ||
1055 | ({ (tqp)->queue = (mod); }) | ||
1056 | |||
1057 | /* Select the local queue pointer copy returned from LOCK_LIST */ | ||
1058 | #define ADD_TO_LIST_L_SELECT(tc, tqp, thread) \ | ||
1059 | ({ add_to_list_l(&(tc), (thread)); }) | ||
1060 | #define REMOVE_FROM_LIST_L_SELECT(tc, tqp, thread) \ | ||
1061 | ({ remove_from_list_l(&(tc), (thread)); }) | ||
1062 | 1042 | ||
1043 | #define rtr_subtract_entry(core, priority) \ | ||
1044 | prio_subtract_entry(&cores[core].rtr, (priority)) | ||
1045 | |||
1046 | #define rtr_move_entry(core, from, to) \ | ||
1047 | prio_move_entry(&cores[core].rtr, (from), (to)) | ||
1063 | #else | 1048 | #else |
1064 | /* Single-core/non-locked versions */ | 1049 | #define rtr_add_entry(core, priority) |
1065 | 1050 | #define rtr_add_entry_inl(core, priority) | |
1066 | /* Threads */ | 1051 | #define rtr_subtract_entry(core, priority) |
1067 | #define GET_THREAD_STATE(thread) \ | 1052 | #define rtr_subtract_entry_inl(core, priotity) |
1068 | ({ (thread)->state; }) | 1053 | #define rtr_move_entry(core, from, to) |
1069 | #define UNLOCK_THREAD(thread, _state) | 1054 | #define rtr_move_entry_inl(core, from, to) |
1070 | #define UNLOCK_THREAD_SET_STATE(thread, _state) \ | 1055 | #endif |
1071 | ({ (thread)->state = (_state); }) | ||
1072 | |||
1073 | /* Lists */ | ||
1074 | #define LOCK_LIST(tqp) \ | ||
1075 | ({ (tqp)->queue; }) | ||
1076 | #define UNLOCK_LIST(tqp, mod) | ||
1077 | #define UNLOCK_LIST_SET_PTR(tqp, mod) \ | ||
1078 | ({ (tqp)->queue = (mod); }) | ||
1079 | |||
1080 | /* Select the queue pointer directly */ | ||
1081 | #define ADD_TO_LIST_L_SELECT(tc, tqp, thread) \ | ||
1082 | ({ add_to_list_l(&(tqp)->queue, (thread)); }) | ||
1083 | #define REMOVE_FROM_LIST_L_SELECT(tc, tqp, thread) \ | ||
1084 | ({ remove_from_list_l(&(tqp)->queue, (thread)); }) | ||
1085 | |||
1086 | #endif /* locking selection */ | ||
1087 | 1056 | ||
1088 | #if THREAD_EXTRA_CHECKS | ||
1089 | /*--------------------------------------------------------------------------- | 1057 | /*--------------------------------------------------------------------------- |
1090 | * Lock the thread slot to obtain the state and then unlock it. Waits for | 1058 | * Thread list structure - circular: |
1091 | * it not to be busy. Used for debugging. | 1059 | * +------------------------------+ |
1060 | * | | | ||
1061 | * +--+---+<-+---+<-+---+<-+---+<-+ | ||
1062 | * Head->| T | | T | | T | | T | | ||
1063 | * +->+---+->+---+->+---+->+---+--+ | ||
1064 | * | | | ||
1065 | * +------------------------------+ | ||
1092 | *--------------------------------------------------------------------------- | 1066 | *--------------------------------------------------------------------------- |
1093 | */ | 1067 | */ |
1094 | static unsigned peek_thread_state(struct thread_entry *thread) | ||
1095 | { | ||
1096 | int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); | ||
1097 | unsigned state = GET_THREAD_STATE(thread); | ||
1098 | UNLOCK_THREAD(thread, state); | ||
1099 | set_irq_level(oldlevel); | ||
1100 | return state; | ||
1101 | } | ||
1102 | #endif /* THREAD_EXTRA_CHECKS */ | ||
1103 | 1068 | ||
1104 | /*--------------------------------------------------------------------------- | 1069 | /*--------------------------------------------------------------------------- |
1105 | * Adds a thread to a list of threads using "intert last". Uses the "l" | 1070 | * Adds a thread to a list of threads using "insert last". Uses the "l" |
1106 | * links. | 1071 | * links. |
1107 | *--------------------------------------------------------------------------- | 1072 | *--------------------------------------------------------------------------- |
1108 | */ | 1073 | */ |
@@ -1114,44 +1079,18 @@ static void add_to_list_l(struct thread_entry **list, | |||
1114 | if (l == NULL) | 1079 | if (l == NULL) |
1115 | { | 1080 | { |
1116 | /* Insert into unoccupied list */ | 1081 | /* Insert into unoccupied list */ |
1117 | thread->l.next = thread; | ||
1118 | thread->l.prev = thread; | 1082 | thread->l.prev = thread; |
1083 | thread->l.next = thread; | ||
1119 | *list = thread; | 1084 | *list = thread; |
1120 | return; | 1085 | return; |
1121 | } | 1086 | } |
1122 | 1087 | ||
1123 | /* Insert last */ | 1088 | /* Insert last */ |
1124 | thread->l.next = l; | ||
1125 | thread->l.prev = l->l.prev; | 1089 | thread->l.prev = l->l.prev; |
1126 | thread->l.prev->l.next = thread; | 1090 | thread->l.next = l; |
1091 | l->l.prev->l.next = thread; | ||
1127 | l->l.prev = thread; | 1092 | l->l.prev = thread; |
1128 | |||
1129 | /* Insert next | ||
1130 | thread->l.next = l->l.next; | ||
1131 | thread->l.prev = l; | ||
1132 | thread->l.next->l.prev = thread; | ||
1133 | l->l.next = thread; | ||
1134 | */ | ||
1135 | } | ||
1136 | |||
1137 | /*--------------------------------------------------------------------------- | ||
1138 | * Locks a list, adds the thread entry and unlocks the list on multicore. | ||
1139 | * Defined as add_to_list_l on single-core. | ||
1140 | *--------------------------------------------------------------------------- | ||
1141 | */ | ||
1142 | #if NUM_CORES > 1 | ||
1143 | static void add_to_list_l_locked(struct thread_queue *tq, | ||
1144 | struct thread_entry *thread) | ||
1145 | { | ||
1146 | struct thread_entry *t = LOCK_LIST(tq); | ||
1147 | ADD_TO_LIST_L_SELECT(t, tq, thread); | ||
1148 | UNLOCK_LIST(tq, t); | ||
1149 | (void)t; | ||
1150 | } | 1093 | } |
1151 | #else | ||
1152 | #define add_to_list_l_locked(tq, thread) \ | ||
1153 | add_to_list_l(&(tq)->queue, (thread)) | ||
1154 | #endif | ||
1155 | 1094 | ||
1156 | /*--------------------------------------------------------------------------- | 1095 | /*--------------------------------------------------------------------------- |
1157 | * Removes a thread from a list of threads. Uses the "l" links. | 1096 | * Removes a thread from a list of threads. Uses the "l" links. |
@@ -1180,28 +1119,20 @@ static void remove_from_list_l(struct thread_entry **list, | |||
1180 | prev = thread->l.prev; | 1119 | prev = thread->l.prev; |
1181 | 1120 | ||
1182 | /* Fix links to jump over the removed entry. */ | 1121 | /* Fix links to jump over the removed entry. */ |
1183 | prev->l.next = next; | ||
1184 | next->l.prev = prev; | 1122 | next->l.prev = prev; |
1123 | prev->l.next = next; | ||
1185 | } | 1124 | } |
1186 | 1125 | ||
1187 | /*--------------------------------------------------------------------------- | 1126 | /*--------------------------------------------------------------------------- |
1188 | * Locks a list, removes the thread entry and unlocks the list on multicore. | 1127 | * Timeout list structure - circular reverse (to make "remove item" O(1)), |
1189 | * Defined as remove_from_list_l on single-core. | 1128 | * NULL-terminated forward (to ease the far more common forward traversal): |
1129 | * +------------------------------+ | ||
1130 | * | | | ||
1131 | * +--+---+<-+---+<-+---+<-+---+<-+ | ||
1132 | * Head->| T | | T | | T | | T | | ||
1133 | * +---+->+---+->+---+->+---+-X | ||
1190 | *--------------------------------------------------------------------------- | 1134 | *--------------------------------------------------------------------------- |
1191 | */ | 1135 | */ |
1192 | #if NUM_CORES > 1 | ||
1193 | static void remove_from_list_l_locked(struct thread_queue *tq, | ||
1194 | struct thread_entry *thread) | ||
1195 | { | ||
1196 | struct thread_entry *t = LOCK_LIST(tq); | ||
1197 | REMOVE_FROM_LIST_L_SELECT(t, tq, thread); | ||
1198 | UNLOCK_LIST(tq, t); | ||
1199 | (void)t; | ||
1200 | } | ||
1201 | #else | ||
1202 | #define remove_from_list_l_locked(tq, thread) \ | ||
1203 | remove_from_list_l(&(tq)->queue, (thread)) | ||
1204 | #endif | ||
1205 | 1136 | ||
1206 | /*--------------------------------------------------------------------------- | 1137 | /*--------------------------------------------------------------------------- |
1207 | * Add a thread from the core's timout list by linking the pointers in its | 1138 | * Add a thread from the core's timout list by linking the pointers in its |
@@ -1210,19 +1141,24 @@ static void remove_from_list_l_locked(struct thread_queue *tq, | |||
1210 | */ | 1141 | */ |
1211 | static void add_to_list_tmo(struct thread_entry *thread) | 1142 | static void add_to_list_tmo(struct thread_entry *thread) |
1212 | { | 1143 | { |
1213 | /* Insert first */ | 1144 | struct thread_entry *tmo = cores[IF_COP_CORE(thread->core)].timeout; |
1214 | struct thread_entry *t = cores[IF_COP_CORE(thread->core)].timeout; | 1145 | THREAD_ASSERT(thread->tmo.prev == NULL, |
1146 | "add_to_list_tmo->already listed", thread); | ||
1215 | 1147 | ||
1216 | thread->tmo.prev = thread; | 1148 | thread->tmo.next = NULL; |
1217 | thread->tmo.next = t; | ||
1218 | 1149 | ||
1219 | if (t != NULL) | 1150 | if (tmo == NULL) |
1220 | { | 1151 | { |
1221 | /* Fix second item's prev pointer to point to this thread */ | 1152 | /* Insert into unoccupied list */ |
1222 | t->tmo.prev = thread; | 1153 | thread->tmo.prev = thread; |
1154 | cores[IF_COP_CORE(thread->core)].timeout = thread; | ||
1155 | return; | ||
1223 | } | 1156 | } |
1224 | 1157 | ||
1225 | cores[IF_COP_CORE(thread->core)].timeout = thread; | 1158 | /* Insert Last */ |
1159 | thread->tmo.prev = tmo->tmo.prev; | ||
1160 | tmo->tmo.prev->tmo.next = thread; | ||
1161 | tmo->tmo.prev = thread; | ||
1226 | } | 1162 | } |
1227 | 1163 | ||
1228 | /*--------------------------------------------------------------------------- | 1164 | /*--------------------------------------------------------------------------- |
@@ -1233,91 +1169,520 @@ static void add_to_list_tmo(struct thread_entry *thread) | |||
1233 | */ | 1169 | */ |
1234 | static void remove_from_list_tmo(struct thread_entry *thread) | 1170 | static void remove_from_list_tmo(struct thread_entry *thread) |
1235 | { | 1171 | { |
1172 | struct thread_entry **list = &cores[IF_COP_CORE(thread->core)].timeout; | ||
1173 | struct thread_entry *prev = thread->tmo.prev; | ||
1236 | struct thread_entry *next = thread->tmo.next; | 1174 | struct thread_entry *next = thread->tmo.next; |
1237 | struct thread_entry *prev; | ||
1238 | 1175 | ||
1239 | if (thread == cores[IF_COP_CORE(thread->core)].timeout) | 1176 | THREAD_ASSERT(prev != NULL, "remove_from_list_tmo->not listed", thread); |
1177 | |||
1178 | if (next != NULL) | ||
1179 | next->tmo.prev = prev; | ||
1180 | |||
1181 | if (thread == *list) | ||
1182 | { | ||
1183 | /* List becomes next item and empty if next == NULL */ | ||
1184 | *list = next; | ||
1185 | /* Mark as unlisted */ | ||
1186 | thread->tmo.prev = NULL; | ||
1187 | } | ||
1188 | else | ||
1189 | { | ||
1190 | if (next == NULL) | ||
1191 | (*list)->tmo.prev = prev; | ||
1192 | prev->tmo.next = next; | ||
1193 | /* Mark as unlisted */ | ||
1194 | thread->tmo.prev = NULL; | ||
1195 | } | ||
1196 | } | ||
1197 | |||
1198 | |||
1199 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
1200 | /*--------------------------------------------------------------------------- | ||
1201 | * Priority distribution structure (one category for each possible priority): | ||
1202 | * | ||
1203 | * +----+----+----+ ... +-----+ | ||
1204 | * hist: | F0 | F1 | F2 | | F31 | | ||
1205 | * +----+----+----+ ... +-----+ | ||
1206 | * mask: | b0 | b1 | b2 | | b31 | | ||
1207 | * +----+----+----+ ... +-----+ | ||
1208 | * | ||
1209 | * F = count of threads at priority category n (frequency) | ||
1210 | * b = bitmask of non-zero priority categories (occupancy) | ||
1211 | * | ||
1212 | * / if H[n] != 0 : 1 | ||
1213 | * b[n] = | | ||
1214 | * \ else : 0 | ||
1215 | * | ||
1216 | *--------------------------------------------------------------------------- | ||
1217 | * Basic priority inheritance priotocol (PIP): | ||
1218 | * | ||
1219 | * Mn = mutex n, Tn = thread n | ||
1220 | * | ||
1221 | * A lower priority thread inherits the priority of the highest priority | ||
1222 | * thread blocked waiting for it to complete an action (such as release a | ||
1223 | * mutex or respond to a message via queue_send): | ||
1224 | * | ||
1225 | * 1) T2->M1->T1 | ||
1226 | * | ||
1227 | * T1 owns M1, T2 is waiting for M1 to realease M1. If T2 has a higher | ||
1228 | * priority than T1 then T1 inherits the priority of T2. | ||
1229 | * | ||
1230 | * 2) T3 | ||
1231 | * \/ | ||
1232 | * T2->M1->T1 | ||
1233 | * | ||
1234 | * Situation is like 1) but T2 and T3 are both queued waiting for M1 and so | ||
1235 | * T1 inherits the higher of T2 and T3. | ||
1236 | * | ||
1237 | * 3) T3->M2->T2->M1->T1 | ||
1238 | * | ||
1239 | * T1 owns M1, T2 owns M2. If T3 has a higher priority than both T1 and T2, | ||
1240 | * then T1 inherits the priority of T3 through T2. | ||
1241 | * | ||
1242 | * Blocking chains can grow arbitrarily complex (though it's best that they | ||
1243 | * not form at all very often :) and build-up from these units. | ||
1244 | *--------------------------------------------------------------------------- | ||
1245 | */ | ||
1246 | |||
1247 | /*--------------------------------------------------------------------------- | ||
1248 | * Increment frequency at category "priority" | ||
1249 | *--------------------------------------------------------------------------- | ||
1250 | */ | ||
1251 | static inline unsigned int prio_add_entry( | ||
1252 | struct priority_distribution *pd, int priority) | ||
1253 | { | ||
1254 | unsigned int count; | ||
1255 | /* Enough size/instruction count difference for ARM makes it worth it to | ||
1256 | * use different code (192 bytes for ARM). Only thing better is ASM. */ | ||
1257 | #ifdef CPU_ARM | ||
1258 | count = pd->hist[priority]; | ||
1259 | if (++count == 1) | ||
1260 | pd->mask |= 1 << priority; | ||
1261 | pd->hist[priority] = count; | ||
1262 | #else /* This one's better for Coldfire */ | ||
1263 | if ((count = ++pd->hist[priority]) == 1) | ||
1264 | pd->mask |= 1 << priority; | ||
1265 | #endif | ||
1266 | |||
1267 | return count; | ||
1268 | } | ||
1269 | |||
1270 | /*--------------------------------------------------------------------------- | ||
1271 | * Decrement frequency at category "priority" | ||
1272 | *--------------------------------------------------------------------------- | ||
1273 | */ | ||
1274 | static inline unsigned int prio_subtract_entry( | ||
1275 | struct priority_distribution *pd, int priority) | ||
1276 | { | ||
1277 | unsigned int count; | ||
1278 | |||
1279 | #ifdef CPU_ARM | ||
1280 | count = pd->hist[priority]; | ||
1281 | if (--count == 0) | ||
1282 | pd->mask &= ~(1 << priority); | ||
1283 | pd->hist[priority] = count; | ||
1284 | #else | ||
1285 | if ((count = --pd->hist[priority]) == 0) | ||
1286 | pd->mask &= ~(1 << priority); | ||
1287 | #endif | ||
1288 | |||
1289 | return count; | ||
1290 | } | ||
1291 | |||
1292 | /*--------------------------------------------------------------------------- | ||
1293 | * Remove from one category and add to another | ||
1294 | *--------------------------------------------------------------------------- | ||
1295 | */ | ||
1296 | static inline void prio_move_entry( | ||
1297 | struct priority_distribution *pd, int from, int to) | ||
1298 | { | ||
1299 | uint32_t mask = pd->mask; | ||
1300 | |||
1301 | #ifdef CPU_ARM | ||
1302 | unsigned int count; | ||
1303 | |||
1304 | count = pd->hist[from]; | ||
1305 | if (--count == 0) | ||
1306 | mask &= ~(1 << from); | ||
1307 | pd->hist[from] = count; | ||
1308 | |||
1309 | count = pd->hist[to]; | ||
1310 | if (++count == 1) | ||
1311 | mask |= 1 << to; | ||
1312 | pd->hist[to] = count; | ||
1313 | #else | ||
1314 | if (--pd->hist[from] == 0) | ||
1315 | mask &= ~(1 << from); | ||
1316 | |||
1317 | if (++pd->hist[to] == 1) | ||
1318 | mask |= 1 << to; | ||
1319 | #endif | ||
1320 | |||
1321 | pd->mask = mask; | ||
1322 | } | ||
1323 | |||
1324 | /*--------------------------------------------------------------------------- | ||
1325 | * Change the priority and rtr entry for a running thread | ||
1326 | *--------------------------------------------------------------------------- | ||
1327 | */ | ||
1328 | static inline void set_running_thread_priority( | ||
1329 | struct thread_entry *thread, int priority) | ||
1330 | { | ||
1331 | const unsigned int core = IF_COP_CORE(thread->core); | ||
1332 | RTR_LOCK(core); | ||
1333 | rtr_move_entry(core, thread->priority, priority); | ||
1334 | thread->priority = priority; | ||
1335 | RTR_UNLOCK(core); | ||
1336 | } | ||
1337 | |||
1338 | /*--------------------------------------------------------------------------- | ||
1339 | * Finds the highest priority thread in a list of threads. If the list is | ||
1340 | * empty, the PRIORITY_IDLE is returned. | ||
1341 | * | ||
1342 | * It is possible to use the struct priority_distribution within an object | ||
1343 | * instead of scanning the remaining threads in the list but as a compromise, | ||
1344 | * the resulting per-object memory overhead is saved at a slight speed | ||
1345 | * penalty under high contention. | ||
1346 | *--------------------------------------------------------------------------- | ||
1347 | */ | ||
1348 | static int find_highest_priority_in_list_l( | ||
1349 | struct thread_entry * const thread) | ||
1350 | { | ||
1351 | if (thread != NULL) | ||
1240 | { | 1352 | { |
1241 | /* Next item becomes list head */ | 1353 | /* Go though list until the ending up at the initial thread */ |
1242 | cores[IF_COP_CORE(thread->core)].timeout = next; | 1354 | int highest_priority = thread->priority; |
1355 | struct thread_entry *curr = thread; | ||
1243 | 1356 | ||
1244 | if (next != NULL) | 1357 | do |
1245 | { | 1358 | { |
1246 | /* Fix new list head's prev to point to itself. */ | 1359 | int priority = curr->priority; |
1247 | next->tmo.prev = next; | 1360 | |
1361 | if (priority < highest_priority) | ||
1362 | highest_priority = priority; | ||
1363 | |||
1364 | curr = curr->l.next; | ||
1248 | } | 1365 | } |
1366 | while (curr != thread); | ||
1249 | 1367 | ||
1250 | thread->tmo.prev = NULL; | 1368 | return highest_priority; |
1251 | return; | ||
1252 | } | 1369 | } |
1253 | 1370 | ||
1254 | prev = thread->tmo.prev; | 1371 | return PRIORITY_IDLE; |
1372 | } | ||
1255 | 1373 | ||
1256 | if (next != NULL) | 1374 | /*--------------------------------------------------------------------------- |
1375 | * Register priority with blocking system and bubble it down the chain if | ||
1376 | * any until we reach the end or something is already equal or higher. | ||
1377 | * | ||
1378 | * NOTE: A simultaneous circular wait could spin deadlock on multiprocessor | ||
1379 | * targets but that same action also guarantees a circular block anyway and | ||
1380 | * those are prevented, right? :-) | ||
1381 | *--------------------------------------------------------------------------- | ||
1382 | */ | ||
1383 | static struct thread_entry * | ||
1384 | blocker_inherit_priority(struct thread_entry *current) | ||
1385 | { | ||
1386 | const int priority = current->priority; | ||
1387 | struct blocker *bl = current->blocker; | ||
1388 | struct thread_entry * const tstart = current; | ||
1389 | struct thread_entry *bl_t = bl->thread; | ||
1390 | |||
1391 | /* Blocker cannot change since the object protection is held */ | ||
1392 | LOCK_THREAD(bl_t); | ||
1393 | |||
1394 | for (;;) | ||
1257 | { | 1395 | { |
1258 | next->tmo.prev = prev; | 1396 | struct thread_entry *next; |
1397 | int bl_pr = bl->priority; | ||
1398 | |||
1399 | if (priority >= bl_pr) | ||
1400 | break; /* Object priority already high enough */ | ||
1401 | |||
1402 | bl->priority = priority; | ||
1403 | |||
1404 | /* Add this one */ | ||
1405 | prio_add_entry(&bl_t->pdist, priority); | ||
1406 | |||
1407 | if (bl_pr < PRIORITY_IDLE) | ||
1408 | { | ||
1409 | /* Not first waiter - subtract old one */ | ||
1410 | prio_subtract_entry(&bl_t->pdist, bl_pr); | ||
1411 | } | ||
1412 | |||
1413 | if (priority >= bl_t->priority) | ||
1414 | break; /* Thread priority high enough */ | ||
1415 | |||
1416 | if (bl_t->state == STATE_RUNNING) | ||
1417 | { | ||
1418 | /* Blocking thread is a running thread therefore there are no | ||
1419 | * further blockers. Change the "run queue" on which it | ||
1420 | * resides. */ | ||
1421 | set_running_thread_priority(bl_t, priority); | ||
1422 | break; | ||
1423 | } | ||
1424 | |||
1425 | bl_t->priority = priority; | ||
1426 | |||
1427 | /* If blocking thread has a blocker, apply transitive inheritance */ | ||
1428 | bl = bl_t->blocker; | ||
1429 | |||
1430 | if (bl == NULL) | ||
1431 | break; /* End of chain or object doesn't support inheritance */ | ||
1432 | |||
1433 | next = bl->thread; | ||
1434 | |||
1435 | if (next == tstart) | ||
1436 | break; /* Full-circle - deadlock! */ | ||
1437 | |||
1438 | UNLOCK_THREAD(current); | ||
1439 | |||
1440 | #if NUM_CORES > 1 | ||
1441 | for (;;) | ||
1442 | { | ||
1443 | LOCK_THREAD(next); | ||
1444 | |||
1445 | /* Blocker could change - retest condition */ | ||
1446 | if (bl->thread == next) | ||
1447 | break; | ||
1448 | |||
1449 | UNLOCK_THREAD(next); | ||
1450 | next = bl->thread; | ||
1451 | } | ||
1452 | #endif | ||
1453 | current = bl_t; | ||
1454 | bl_t = next; | ||
1259 | } | 1455 | } |
1260 | 1456 | ||
1261 | prev->tmo.next = next; | 1457 | UNLOCK_THREAD(bl_t); |
1262 | thread->tmo.prev = NULL; | 1458 | |
1459 | return current; | ||
1263 | } | 1460 | } |
1264 | 1461 | ||
1265 | /*--------------------------------------------------------------------------- | 1462 | /*--------------------------------------------------------------------------- |
1266 | * Schedules a thread wakeup on the specified core. Threads will be made | 1463 | * Readjust priorities when waking a thread blocked waiting for another |
1267 | * ready to run when the next task switch occurs. Note that this does not | 1464 | * in essence "releasing" the thread's effect on the object owner. Can be |
1268 | * introduce an on-core delay since the soonest the next thread may run is | 1465 | * performed from any context. |
1269 | * no sooner than that. Other cores and on-core interrupts may only ever | ||
1270 | * add to the list. | ||
1271 | *--------------------------------------------------------------------------- | 1466 | *--------------------------------------------------------------------------- |
1272 | */ | 1467 | */ |
1273 | static void core_schedule_wakeup(struct thread_entry *thread) | 1468 | struct thread_entry * |
1469 | wakeup_priority_protocol_release(struct thread_entry *thread) | ||
1274 | { | 1470 | { |
1275 | int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); | 1471 | const int priority = thread->priority; |
1276 | const unsigned int core = IF_COP_CORE(thread->core); | 1472 | struct blocker *bl = thread->blocker; |
1277 | add_to_list_l_locked(&cores[core].waking, thread); | 1473 | struct thread_entry * const tstart = thread; |
1474 | struct thread_entry *bl_t = bl->thread; | ||
1475 | |||
1476 | /* Blocker cannot change since object will be locked */ | ||
1477 | LOCK_THREAD(bl_t); | ||
1478 | |||
1479 | thread->blocker = NULL; /* Thread not blocked */ | ||
1480 | |||
1481 | for (;;) | ||
1482 | { | ||
1483 | struct thread_entry *next; | ||
1484 | int bl_pr = bl->priority; | ||
1485 | |||
1486 | if (priority > bl_pr) | ||
1487 | break; /* Object priority higher */ | ||
1488 | |||
1489 | next = *thread->bqp; | ||
1490 | |||
1491 | if (next == NULL) | ||
1492 | { | ||
1493 | /* No more threads in queue */ | ||
1494 | prio_subtract_entry(&bl_t->pdist, bl_pr); | ||
1495 | bl->priority = PRIORITY_IDLE; | ||
1496 | } | ||
1497 | else | ||
1498 | { | ||
1499 | /* Check list for highest remaining priority */ | ||
1500 | int queue_pr = find_highest_priority_in_list_l(next); | ||
1501 | |||
1502 | if (queue_pr == bl_pr) | ||
1503 | break; /* Object priority not changing */ | ||
1504 | |||
1505 | /* Change queue priority */ | ||
1506 | prio_move_entry(&bl_t->pdist, bl_pr, queue_pr); | ||
1507 | bl->priority = queue_pr; | ||
1508 | } | ||
1509 | |||
1510 | if (bl_pr > bl_t->priority) | ||
1511 | break; /* thread priority is higher */ | ||
1512 | |||
1513 | bl_pr = find_first_set_bit(bl_t->pdist.mask); | ||
1514 | |||
1515 | if (bl_pr == bl_t->priority) | ||
1516 | break; /* Thread priority not changing */ | ||
1517 | |||
1518 | if (bl_t->state == STATE_RUNNING) | ||
1519 | { | ||
1520 | /* No further blockers */ | ||
1521 | set_running_thread_priority(bl_t, bl_pr); | ||
1522 | break; | ||
1523 | } | ||
1524 | |||
1525 | bl_t->priority = bl_pr; | ||
1526 | |||
1527 | /* If blocking thread has a blocker, apply transitive inheritance */ | ||
1528 | bl = bl_t->blocker; | ||
1529 | |||
1530 | if (bl == NULL) | ||
1531 | break; /* End of chain or object doesn't support inheritance */ | ||
1532 | |||
1533 | next = bl->thread; | ||
1534 | |||
1535 | if (next == tstart) | ||
1536 | break; /* Full-circle - deadlock! */ | ||
1537 | |||
1538 | UNLOCK_THREAD(thread); | ||
1539 | |||
1278 | #if NUM_CORES > 1 | 1540 | #if NUM_CORES > 1 |
1279 | if (core != CURRENT_CORE) | 1541 | for (;;) |
1542 | { | ||
1543 | LOCK_THREAD(next); | ||
1544 | |||
1545 | /* Blocker could change - retest condition */ | ||
1546 | if (bl->thread == next) | ||
1547 | break; | ||
1548 | |||
1549 | UNLOCK_THREAD(next); | ||
1550 | next = bl->thread; | ||
1551 | } | ||
1552 | #endif | ||
1553 | thread = bl_t; | ||
1554 | bl_t = next; | ||
1555 | } | ||
1556 | |||
1557 | UNLOCK_THREAD(bl_t); | ||
1558 | |||
1559 | #if NUM_CORES > 1 | ||
1560 | if (thread != tstart) | ||
1280 | { | 1561 | { |
1281 | core_wake(core); | 1562 | /* Relock original if it changed */ |
1563 | LOCK_THREAD(tstart); | ||
1282 | } | 1564 | } |
1283 | #endif | 1565 | #endif |
1284 | set_irq_level(oldlevel); | 1566 | |
1567 | return cores[CURRENT_CORE].running; | ||
1285 | } | 1568 | } |
1286 | 1569 | ||
1287 | /*--------------------------------------------------------------------------- | 1570 | /*--------------------------------------------------------------------------- |
1288 | * If the waking list was populated, move all threads on it onto the running | 1571 | * Transfer ownership to a thread waiting for an objects and transfer |
1289 | * list so they may be run ASAP. | 1572 | * inherited priority boost from other waiters. This algorithm knows that |
1573 | * blocking chains may only unblock from the very end. | ||
1574 | * | ||
1575 | * Only the owning thread itself may call this and so the assumption that | ||
1576 | * it is the running thread is made. | ||
1290 | *--------------------------------------------------------------------------- | 1577 | *--------------------------------------------------------------------------- |
1291 | */ | 1578 | */ |
1292 | static inline void core_perform_wakeup(IF_COP_VOID(unsigned int core)) | 1579 | struct thread_entry * |
1580 | wakeup_priority_protocol_transfer(struct thread_entry *thread) | ||
1293 | { | 1581 | { |
1294 | struct thread_entry *w = LOCK_LIST(&cores[IF_COP_CORE(core)].waking); | 1582 | /* Waking thread inherits priority boost from object owner */ |
1295 | struct thread_entry *r = cores[IF_COP_CORE(core)].running; | 1583 | struct blocker *bl = thread->blocker; |
1584 | struct thread_entry *bl_t = bl->thread; | ||
1585 | struct thread_entry *next; | ||
1586 | int bl_pr; | ||
1296 | 1587 | ||
1297 | /* Tranfer all threads on waking list to running list in one | 1588 | THREAD_ASSERT(thread_get_current() == bl_t, |
1298 | swoop */ | 1589 | "UPPT->wrong thread", thread_get_current()); |
1299 | if (r != NULL) | 1590 | |
1591 | LOCK_THREAD(bl_t); | ||
1592 | |||
1593 | bl_pr = bl->priority; | ||
1594 | |||
1595 | /* Remove the object's boost from the owning thread */ | ||
1596 | if (prio_subtract_entry(&bl_t->pdist, bl_pr) == 0 && | ||
1597 | bl_pr <= bl_t->priority) | ||
1300 | { | 1598 | { |
1301 | /* Place waking threads at the end of the running list. */ | 1599 | /* No more threads at this priority are waiting and the old level is |
1302 | struct thread_entry *tmp; | 1600 | * at least the thread level */ |
1303 | w->l.prev->l.next = r; | 1601 | int priority = find_first_set_bit(bl_t->pdist.mask); |
1304 | r->l.prev->l.next = w; | 1602 | |
1305 | tmp = r->l.prev; | 1603 | if (priority != bl_t->priority) |
1306 | r->l.prev = w->l.prev; | 1604 | { |
1307 | w->l.prev = tmp; | 1605 | /* Adjust this thread's priority */ |
1606 | set_running_thread_priority(bl_t, priority); | ||
1607 | } | ||
1608 | } | ||
1609 | |||
1610 | next = *thread->bqp; | ||
1611 | |||
1612 | if (next == NULL) | ||
1613 | { | ||
1614 | /* Expected shortcut - no more waiters */ | ||
1615 | bl_pr = PRIORITY_IDLE; | ||
1308 | } | 1616 | } |
1309 | else | 1617 | else |
1310 | { | 1618 | { |
1311 | /* Just transfer the list as-is */ | 1619 | if (thread->priority <= bl_pr) |
1312 | cores[IF_COP_CORE(core)].running = w; | 1620 | { |
1621 | /* Need to scan threads remaining in queue */ | ||
1622 | bl_pr = find_highest_priority_in_list_l(next); | ||
1623 | } | ||
1624 | |||
1625 | if (prio_add_entry(&thread->pdist, bl_pr) == 1 && | ||
1626 | bl_pr < thread->priority) | ||
1627 | { | ||
1628 | /* Thread priority must be raised */ | ||
1629 | thread->priority = bl_pr; | ||
1630 | } | ||
1631 | } | ||
1632 | |||
1633 | bl->thread = thread; /* This thread pwns */ | ||
1634 | bl->priority = bl_pr; /* Save highest blocked priority */ | ||
1635 | thread->blocker = NULL; /* Thread not blocked */ | ||
1636 | |||
1637 | UNLOCK_THREAD(bl_t); | ||
1638 | |||
1639 | return bl_t; | ||
1640 | } | ||
1641 | |||
1642 | /*--------------------------------------------------------------------------- | ||
1643 | * No threads must be blocked waiting for this thread except for it to exit. | ||
1644 | * The alternative is more elaborate cleanup and object registration code. | ||
1645 | * Check this for risk of silent data corruption when objects with | ||
1646 | * inheritable blocking are abandoned by the owner - not precise but may | ||
1647 | * catch something. | ||
1648 | *--------------------------------------------------------------------------- | ||
1649 | */ | ||
1650 | void check_for_obj_waiters(const char *function, struct thread_entry *thread) | ||
1651 | { | ||
1652 | /* Only one bit in the mask should be set with a frequency on 1 which | ||
1653 | * represents the thread's own base priority */ | ||
1654 | uint32_t mask = thread->pdist.mask; | ||
1655 | if ((mask & (mask - 1)) != 0 || | ||
1656 | thread->pdist.hist[find_first_set_bit(mask)] > 1) | ||
1657 | { | ||
1658 | unsigned char name[32]; | ||
1659 | thread_get_name(name, 32, thread); | ||
1660 | panicf("%s->%s with obj. waiters", function, name); | ||
1313 | } | 1661 | } |
1314 | /* Just leave any timeout threads on the timeout list. If a timeout check | 1662 | } |
1315 | * is due, they will be removed there. If they do a timeout again before | 1663 | #endif /* HAVE_PRIORITY_SCHEDULING */ |
1316 | * being removed, they will just stay on the list with a new expiration | 1664 | |
1317 | * tick. */ | 1665 | /*--------------------------------------------------------------------------- |
1666 | * Move a thread back to a running state on its core. | ||
1667 | *--------------------------------------------------------------------------- | ||
1668 | */ | ||
1669 | static void core_schedule_wakeup(struct thread_entry *thread) | ||
1670 | { | ||
1671 | const unsigned int core = IF_COP_CORE(thread->core); | ||
1672 | |||
1673 | RTR_LOCK(core); | ||
1674 | |||
1675 | thread->state = STATE_RUNNING; | ||
1676 | |||
1677 | add_to_list_l(&cores[core].running, thread); | ||
1678 | rtr_add_entry(core, thread->priority); | ||
1679 | |||
1680 | RTR_UNLOCK(core); | ||
1318 | 1681 | ||
1319 | /* Waking list is clear - NULL and unlock it */ | 1682 | #if NUM_CORES > 1 |
1320 | UNLOCK_LIST_SET_PTR(&cores[IF_COP_CORE(core)].waking, NULL); | 1683 | if (core != CURRENT_CORE) |
1684 | core_wake(core); | ||
1685 | #endif | ||
1321 | } | 1686 | } |
1322 | 1687 | ||
1323 | /*--------------------------------------------------------------------------- | 1688 | /*--------------------------------------------------------------------------- |
@@ -1326,7 +1691,7 @@ static inline void core_perform_wakeup(IF_COP_VOID(unsigned int core)) | |||
1326 | * tick when the next check will occur. | 1691 | * tick when the next check will occur. |
1327 | *--------------------------------------------------------------------------- | 1692 | *--------------------------------------------------------------------------- |
1328 | */ | 1693 | */ |
1329 | static void check_tmo_threads(void) | 1694 | void check_tmo_threads(void) |
1330 | { | 1695 | { |
1331 | const unsigned int core = CURRENT_CORE; | 1696 | const unsigned int core = CURRENT_CORE; |
1332 | const long tick = current_tick; /* snapshot the current tick */ | 1697 | const long tick = current_tick; /* snapshot the current tick */ |
@@ -1335,54 +1700,98 @@ static void check_tmo_threads(void) | |||
1335 | 1700 | ||
1336 | /* If there are no processes waiting for a timeout, just keep the check | 1701 | /* If there are no processes waiting for a timeout, just keep the check |
1337 | tick from falling into the past. */ | 1702 | tick from falling into the past. */ |
1338 | if (next != NULL) | 1703 | |
1704 | /* Break the loop once we have walked through the list of all | ||
1705 | * sleeping processes or have removed them all. */ | ||
1706 | while (next != NULL) | ||
1339 | { | 1707 | { |
1340 | /* Check sleeping threads. */ | 1708 | /* Check sleeping threads. Allow interrupts between checks. */ |
1341 | do | 1709 | set_irq_level(0); |
1342 | { | ||
1343 | /* Must make sure noone else is examining the state, wait until | ||
1344 | slot is no longer busy */ | ||
1345 | struct thread_entry *curr = next; | ||
1346 | next = curr->tmo.next; | ||
1347 | 1710 | ||
1348 | unsigned state = GET_THREAD_STATE(curr); | 1711 | struct thread_entry *curr = next; |
1349 | 1712 | ||
1350 | if (state < TIMEOUT_STATE_FIRST) | 1713 | next = curr->tmo.next; |
1351 | { | 1714 | |
1352 | /* Cleanup threads no longer on a timeout but still on the | 1715 | /* Lock thread slot against explicit wakeup */ |
1353 | * list. */ | 1716 | set_irq_level(HIGHEST_IRQ_LEVEL); |
1354 | remove_from_list_tmo(curr); | 1717 | LOCK_THREAD(curr); |
1355 | UNLOCK_THREAD(curr, state); /* Unlock thread slot */ | 1718 | |
1356 | } | 1719 | unsigned state = curr->state; |
1357 | else if (TIME_BEFORE(tick, curr->tmo_tick)) | 1720 | |
1721 | if (state < TIMEOUT_STATE_FIRST) | ||
1722 | { | ||
1723 | /* Cleanup threads no longer on a timeout but still on the | ||
1724 | * list. */ | ||
1725 | remove_from_list_tmo(curr); | ||
1726 | } | ||
1727 | else if (TIME_BEFORE(tick, curr->tmo_tick)) | ||
1728 | { | ||
1729 | /* Timeout still pending - this will be the usual case */ | ||
1730 | if (TIME_BEFORE(curr->tmo_tick, next_tmo_check)) | ||
1358 | { | 1731 | { |
1359 | /* Timeout still pending - this will be the usual case */ | 1732 | /* Earliest timeout found so far - move the next check up |
1360 | if (TIME_BEFORE(curr->tmo_tick, next_tmo_check)) | 1733 | to its time */ |
1361 | { | 1734 | next_tmo_check = curr->tmo_tick; |
1362 | /* Earliest timeout found so far - move the next check up | ||
1363 | to its time */ | ||
1364 | next_tmo_check = curr->tmo_tick; | ||
1365 | } | ||
1366 | UNLOCK_THREAD(curr, state); /* Unlock thread slot */ | ||
1367 | } | 1735 | } |
1368 | else | 1736 | } |
1737 | else | ||
1738 | { | ||
1739 | /* Sleep timeout has been reached so bring the thread back to | ||
1740 | * life again. */ | ||
1741 | if (state == STATE_BLOCKED_W_TMO) | ||
1369 | { | 1742 | { |
1370 | /* Sleep timeout has been reached so bring the thread back to | 1743 | #if NUM_CORES > 1 |
1371 | * life again. */ | 1744 | /* Lock the waiting thread's kernel object */ |
1372 | if (state == STATE_BLOCKED_W_TMO) | 1745 | struct corelock *ocl = curr->obj_cl; |
1746 | |||
1747 | if (corelock_try_lock(ocl) == 0) | ||
1373 | { | 1748 | { |
1374 | remove_from_list_l_locked(curr->bqp, curr); | 1749 | /* Need to retry in the correct order though the need is |
1750 | * unlikely */ | ||
1751 | UNLOCK_THREAD(curr); | ||
1752 | corelock_lock(ocl); | ||
1753 | LOCK_THREAD(curr); | ||
1754 | |||
1755 | if (curr->state != STATE_BLOCKED_W_TMO) | ||
1756 | { | ||
1757 | /* Thread was woken or removed explicitely while slot | ||
1758 | * was unlocked */ | ||
1759 | corelock_unlock(ocl); | ||
1760 | remove_from_list_tmo(curr); | ||
1761 | UNLOCK_THREAD(curr); | ||
1762 | continue; | ||
1763 | } | ||
1375 | } | 1764 | } |
1765 | #endif /* NUM_CORES */ | ||
1766 | |||
1767 | remove_from_list_l(curr->bqp, curr); | ||
1768 | |||
1769 | #ifdef HAVE_WAKEUP_EXT_CB | ||
1770 | if (curr->wakeup_ext_cb != NULL) | ||
1771 | curr->wakeup_ext_cb(curr); | ||
1772 | #endif | ||
1376 | 1773 | ||
1377 | remove_from_list_tmo(curr); | 1774 | #ifdef HAVE_PRIORITY_SCHEDULING |
1378 | add_to_list_l(&cores[core].running, curr); | 1775 | if (curr->blocker != NULL) |
1379 | UNLOCK_THREAD_SET_STATE(curr, STATE_RUNNING); | 1776 | wakeup_priority_protocol_release(curr); |
1777 | #endif | ||
1778 | corelock_unlock(ocl); | ||
1380 | } | 1779 | } |
1780 | /* else state == STATE_SLEEPING */ | ||
1781 | |||
1782 | remove_from_list_tmo(curr); | ||
1783 | |||
1784 | RTR_LOCK(core); | ||
1381 | 1785 | ||
1382 | /* Break the loop once we have walked through the list of all | 1786 | curr->state = STATE_RUNNING; |
1383 | * sleeping processes or have removed them all. */ | 1787 | |
1788 | add_to_list_l(&cores[core].running, curr); | ||
1789 | rtr_add_entry(core, curr->priority); | ||
1790 | |||
1791 | RTR_UNLOCK(core); | ||
1384 | } | 1792 | } |
1385 | while (next != NULL); | 1793 | |
1794 | UNLOCK_THREAD(curr); | ||
1386 | } | 1795 | } |
1387 | 1796 | ||
1388 | cores[core].next_tmo_check = next_tmo_check; | 1797 | cores[core].next_tmo_check = next_tmo_check; |
@@ -1390,109 +1799,33 @@ static void check_tmo_threads(void) | |||
1390 | 1799 | ||
1391 | /*--------------------------------------------------------------------------- | 1800 | /*--------------------------------------------------------------------------- |
1392 | * Performs operations that must be done before blocking a thread but after | 1801 | * Performs operations that must be done before blocking a thread but after |
1393 | * the state is saved - follows reverse of locking order. blk_ops.flags is | 1802 | * the state is saved. |
1394 | * assumed to be nonzero. | ||
1395 | *--------------------------------------------------------------------------- | 1803 | *--------------------------------------------------------------------------- |
1396 | */ | 1804 | */ |
1397 | #if NUM_CORES > 1 | 1805 | #if NUM_CORES > 1 |
1398 | static inline void run_blocking_ops( | 1806 | static inline void run_blocking_ops( |
1399 | unsigned int core, struct thread_entry *thread) | 1807 | unsigned int core, struct thread_entry *thread) |
1400 | { | 1808 | { |
1401 | struct thread_blk_ops *ops = &cores[IF_COP_CORE(core)].blk_ops; | 1809 | struct thread_blk_ops *ops = &cores[core].blk_ops; |
1402 | const unsigned flags = ops->flags; | 1810 | const unsigned flags = ops->flags; |
1403 | 1811 | ||
1404 | if (flags == 0) | 1812 | if (flags == TBOP_CLEAR) |
1405 | return; | 1813 | return; |
1406 | 1814 | ||
1407 | if (flags & TBOP_SWITCH_CORE) | 1815 | switch (flags) |
1408 | { | 1816 | { |
1817 | case TBOP_SWITCH_CORE: | ||
1409 | core_switch_blk_op(core, thread); | 1818 | core_switch_blk_op(core, thread); |
1410 | } | 1819 | /* Fall-through */ |
1411 | 1820 | case TBOP_UNLOCK_CORELOCK: | |
1412 | #if CONFIG_CORELOCK == SW_CORELOCK | ||
1413 | if (flags & TBOP_UNLOCK_LIST) | ||
1414 | { | ||
1415 | UNLOCK_LIST(ops->list_p, NULL); | ||
1416 | } | ||
1417 | |||
1418 | if (flags & TBOP_UNLOCK_CORELOCK) | ||
1419 | { | ||
1420 | corelock_unlock(ops->cl_p); | 1821 | corelock_unlock(ops->cl_p); |
1421 | } | ||
1422 | |||
1423 | if (flags & TBOP_UNLOCK_THREAD) | ||
1424 | { | ||
1425 | UNLOCK_THREAD(ops->thread, 0); | ||
1426 | } | ||
1427 | #elif CONFIG_CORELOCK == CORELOCK_SWAP | ||
1428 | /* Write updated variable value into memory location */ | ||
1429 | switch (flags & TBOP_VAR_TYPE_MASK) | ||
1430 | { | ||
1431 | case TBOP_UNLOCK_LIST: | ||
1432 | UNLOCK_LIST(ops->list_p, ops->list_v); | ||
1433 | break; | ||
1434 | case TBOP_SET_VARi: | ||
1435 | *ops->var_ip = ops->var_iv; | ||
1436 | break; | ||
1437 | case TBOP_SET_VARu8: | ||
1438 | *ops->var_u8p = ops->var_u8v; | ||
1439 | break; | 1822 | break; |
1440 | } | 1823 | } |
1441 | #endif /* CONFIG_CORELOCK == */ | ||
1442 | 1824 | ||
1443 | /* Unlock thread's slot */ | 1825 | ops->flags = TBOP_CLEAR; |
1444 | if (flags & TBOP_UNLOCK_CURRENT) | ||
1445 | { | ||
1446 | UNLOCK_THREAD(thread, ops->state); | ||
1447 | } | ||
1448 | |||
1449 | ops->flags = 0; | ||
1450 | } | 1826 | } |
1451 | #endif /* NUM_CORES > 1 */ | 1827 | #endif /* NUM_CORES > 1 */ |
1452 | 1828 | ||
1453 | |||
1454 | /*--------------------------------------------------------------------------- | ||
1455 | * Runs any operations that may cause threads to be ready to run and then | ||
1456 | * sleeps the processor core until the next interrupt if none are. | ||
1457 | *--------------------------------------------------------------------------- | ||
1458 | */ | ||
1459 | static inline struct thread_entry * sleep_core(IF_COP_VOID(unsigned int core)) | ||
1460 | { | ||
1461 | for (;;) | ||
1462 | { | ||
1463 | set_irq_level(HIGHEST_IRQ_LEVEL); | ||
1464 | /* We want to do these ASAP as it may change the decision to sleep | ||
1465 | * the core or a core has woken because an interrupt occurred | ||
1466 | * and posted a message to a queue. */ | ||
1467 | if (cores[IF_COP_CORE(core)].waking.queue != NULL) | ||
1468 | { | ||
1469 | core_perform_wakeup(IF_COP(core)); | ||
1470 | } | ||
1471 | |||
1472 | /* If there are threads on a timeout and the earliest wakeup is due, | ||
1473 | * check the list and wake any threads that need to start running | ||
1474 | * again. */ | ||
1475 | if (!TIME_BEFORE(current_tick, cores[IF_COP_CORE(core)].next_tmo_check)) | ||
1476 | { | ||
1477 | check_tmo_threads(); | ||
1478 | } | ||
1479 | |||
1480 | /* If there is a ready to run task, return its ID and keep core | ||
1481 | * awake. */ | ||
1482 | if (cores[IF_COP_CORE(core)].running == NULL) | ||
1483 | { | ||
1484 | /* Enter sleep mode to reduce power usage - woken up on interrupt | ||
1485 | * or wakeup request from another core - expected to enable all | ||
1486 | * interrupts. */ | ||
1487 | core_sleep(IF_COP(core)); | ||
1488 | continue; | ||
1489 | } | ||
1490 | |||
1491 | set_irq_level(0); | ||
1492 | return cores[IF_COP_CORE(core)].running; | ||
1493 | } | ||
1494 | } | ||
1495 | |||
1496 | #ifdef RB_PROFILE | 1829 | #ifdef RB_PROFILE |
1497 | void profile_thread(void) | 1830 | void profile_thread(void) |
1498 | { | 1831 | { |
@@ -1502,55 +1835,34 @@ void profile_thread(void) | |||
1502 | 1835 | ||
1503 | /*--------------------------------------------------------------------------- | 1836 | /*--------------------------------------------------------------------------- |
1504 | * Prepares a thread to block on an object's list and/or for a specified | 1837 | * Prepares a thread to block on an object's list and/or for a specified |
1505 | * duration - expects object and slot to be appropriately locked if needed. | 1838 | * duration - expects object and slot to be appropriately locked if needed |
1839 | * and interrupts to be masked. | ||
1506 | *--------------------------------------------------------------------------- | 1840 | *--------------------------------------------------------------------------- |
1507 | */ | 1841 | */ |
1508 | static inline void _block_thread_on_l(struct thread_queue *list, | 1842 | static inline void block_thread_on_l(struct thread_entry *thread, |
1509 | struct thread_entry *thread, | 1843 | unsigned state) |
1510 | unsigned state | ||
1511 | IF_SWCL(, const bool nolock)) | ||
1512 | { | 1844 | { |
1513 | /* If inlined, unreachable branches will be pruned with no size penalty | 1845 | /* If inlined, unreachable branches will be pruned with no size penalty |
1514 | because constant params are used for state and nolock. */ | 1846 | because state is passed as a constant parameter. */ |
1515 | const unsigned int core = IF_COP_CORE(thread->core); | 1847 | const unsigned int core = IF_COP_CORE(thread->core); |
1516 | 1848 | ||
1517 | /* Remove the thread from the list of running threads. */ | 1849 | /* Remove the thread from the list of running threads. */ |
1850 | RTR_LOCK(core); | ||
1518 | remove_from_list_l(&cores[core].running, thread); | 1851 | remove_from_list_l(&cores[core].running, thread); |
1852 | rtr_subtract_entry(core, thread->priority); | ||
1853 | RTR_UNLOCK(core); | ||
1519 | 1854 | ||
1520 | /* Add a timeout to the block if not infinite */ | 1855 | /* Add a timeout to the block if not infinite */ |
1521 | switch (state) | 1856 | switch (state) |
1522 | { | 1857 | { |
1523 | case STATE_BLOCKED: | 1858 | case STATE_BLOCKED: |
1524 | /* Put the thread into a new list of inactive threads. */ | ||
1525 | #if CONFIG_CORELOCK == SW_CORELOCK | ||
1526 | if (nolock) | ||
1527 | { | ||
1528 | thread->bqp = NULL; /* Indicate nolock list */ | ||
1529 | thread->bqnlp = (struct thread_entry **)list; | ||
1530 | add_to_list_l((struct thread_entry **)list, thread); | ||
1531 | } | ||
1532 | else | ||
1533 | #endif | ||
1534 | { | ||
1535 | thread->bqp = list; | ||
1536 | add_to_list_l_locked(list, thread); | ||
1537 | } | ||
1538 | break; | ||
1539 | case STATE_BLOCKED_W_TMO: | 1859 | case STATE_BLOCKED_W_TMO: |
1540 | /* Put the thread into a new list of inactive threads. */ | 1860 | /* Put the thread into a new list of inactive threads. */ |
1541 | #if CONFIG_CORELOCK == SW_CORELOCK | 1861 | add_to_list_l(thread->bqp, thread); |
1542 | if (nolock) | 1862 | |
1543 | { | 1863 | if (state == STATE_BLOCKED) |
1544 | thread->bqp = NULL; /* Indicate nolock list */ | 1864 | break; |
1545 | thread->bqnlp = (struct thread_entry **)list; | 1865 | |
1546 | add_to_list_l((struct thread_entry **)list, thread); | ||
1547 | } | ||
1548 | else | ||
1549 | #endif | ||
1550 | { | ||
1551 | thread->bqp = list; | ||
1552 | add_to_list_l_locked(list, thread); | ||
1553 | } | ||
1554 | /* Fall-through */ | 1866 | /* Fall-through */ |
1555 | case STATE_SLEEPING: | 1867 | case STATE_SLEEPING: |
1556 | /* If this thread times out sooner than any other thread, update | 1868 | /* If this thread times out sooner than any other thread, update |
@@ -1568,35 +1880,11 @@ static inline void _block_thread_on_l(struct thread_queue *list, | |||
1568 | break; | 1880 | break; |
1569 | } | 1881 | } |
1570 | 1882 | ||
1571 | #ifdef HAVE_PRIORITY_SCHEDULING | 1883 | /* Remember the the next thread about to block. */ |
1572 | /* Reset priorities */ | 1884 | cores[core].block_task = thread; |
1573 | if (thread->priority == cores[core].highest_priority) | ||
1574 | cores[core].highest_priority = LOWEST_PRIORITY; | ||
1575 | #endif | ||
1576 | 1885 | ||
1577 | #if NUM_CORES == 1 || CONFIG_CORELOCK == SW_CORELOCK | 1886 | /* Report new state. */ |
1578 | /* Safe to set state now */ | ||
1579 | thread->state = state; | 1887 | thread->state = state; |
1580 | #elif CONFIG_CORELOCK == CORELOCK_SWAP | ||
1581 | cores[core].blk_ops.state = state; | ||
1582 | #endif | ||
1583 | |||
1584 | #if NUM_CORES > 1 | ||
1585 | /* Delay slot unlock until task switch */ | ||
1586 | cores[core].blk_ops.flags |= TBOP_UNLOCK_CURRENT; | ||
1587 | #endif | ||
1588 | } | ||
1589 | |||
1590 | static inline void block_thread_on_l( | ||
1591 | struct thread_queue *list, struct thread_entry *thread, unsigned state) | ||
1592 | { | ||
1593 | _block_thread_on_l(list, thread, state IF_SWCL(, false)); | ||
1594 | } | ||
1595 | |||
1596 | static inline void block_thread_on_l_no_listlock( | ||
1597 | struct thread_entry **list, struct thread_entry *thread, unsigned state) | ||
1598 | { | ||
1599 | _block_thread_on_l((struct thread_queue *)list, thread, state IF_SWCL(, true)); | ||
1600 | } | 1888 | } |
1601 | 1889 | ||
1602 | /*--------------------------------------------------------------------------- | 1890 | /*--------------------------------------------------------------------------- |
@@ -1607,72 +1895,134 @@ static inline void block_thread_on_l_no_listlock( | |||
1607 | * INTERNAL: Intended for use by kernel and not for programs. | 1895 | * INTERNAL: Intended for use by kernel and not for programs. |
1608 | *--------------------------------------------------------------------------- | 1896 | *--------------------------------------------------------------------------- |
1609 | */ | 1897 | */ |
1610 | void switch_thread(struct thread_entry *old) | 1898 | void switch_thread(void) |
1611 | { | 1899 | { |
1612 | const unsigned int core = CURRENT_CORE; | 1900 | const unsigned int core = CURRENT_CORE; |
1901 | struct thread_entry *block = cores[core].block_task; | ||
1613 | struct thread_entry *thread = cores[core].running; | 1902 | struct thread_entry *thread = cores[core].running; |
1614 | struct thread_entry *block = old; | ||
1615 | 1903 | ||
1616 | if (block == NULL) | 1904 | /* Get context to save - next thread to run is unknown until all wakeups |
1617 | old = thread; | 1905 | * are evaluated */ |
1906 | if (block != NULL) | ||
1907 | { | ||
1908 | cores[core].block_task = NULL; | ||
1909 | |||
1910 | #if NUM_CORES > 1 | ||
1911 | if (thread == block) | ||
1912 | { | ||
1913 | /* This was the last thread running and another core woke us before | ||
1914 | * reaching here. Force next thread selection to give tmo threads or | ||
1915 | * other threads woken before this block a first chance. */ | ||
1916 | block = NULL; | ||
1917 | } | ||
1918 | else | ||
1919 | #endif | ||
1920 | { | ||
1921 | /* Blocking task is the old one */ | ||
1922 | thread = block; | ||
1923 | } | ||
1924 | } | ||
1618 | 1925 | ||
1619 | #ifdef RB_PROFILE | 1926 | #ifdef RB_PROFILE |
1620 | profile_thread_stopped(old - threads); | 1927 | profile_thread_stopped(thread - threads); |
1621 | #endif | 1928 | #endif |
1622 | 1929 | ||
1623 | /* Begin task switching by saving our current context so that we can | 1930 | /* Begin task switching by saving our current context so that we can |
1624 | * restore the state of the current thread later to the point prior | 1931 | * restore the state of the current thread later to the point prior |
1625 | * to this call. */ | 1932 | * to this call. */ |
1626 | store_context(&old->context); | 1933 | store_context(&thread->context); |
1627 | 1934 | ||
1628 | /* Check if the current thread stack is overflown */ | 1935 | /* Check if the current thread stack is overflown */ |
1629 | if(((unsigned int *)old->stack)[0] != DEADBEEF) | 1936 | if (thread->stack[0] != DEADBEEF) |
1630 | thread_stkov(old); | 1937 | thread_stkov(thread); |
1631 | 1938 | ||
1632 | #if NUM_CORES > 1 | 1939 | #if NUM_CORES > 1 |
1633 | /* Run any blocking operations requested before switching/sleeping */ | 1940 | /* Run any blocking operations requested before switching/sleeping */ |
1634 | run_blocking_ops(core, old); | 1941 | run_blocking_ops(core, thread); |
1635 | #endif | 1942 | #endif |
1636 | 1943 | ||
1637 | /* Go through the list of sleeping task to check if we need to wake up | ||
1638 | * any of them due to timeout. Also puts core into sleep state until | ||
1639 | * there is at least one running process again. */ | ||
1640 | thread = sleep_core(IF_COP(core)); | ||
1641 | |||
1642 | #ifdef HAVE_PRIORITY_SCHEDULING | 1944 | #ifdef HAVE_PRIORITY_SCHEDULING |
1643 | /* Select the new task based on priorities and the last time a process | 1945 | /* Reset the value of thread's skip count */ |
1644 | * got CPU time. */ | 1946 | thread->skip_count = 0; |
1645 | if (block == NULL) | 1947 | #endif |
1646 | thread = thread->l.next; | ||
1647 | 1948 | ||
1648 | for (;;) | 1949 | for (;;) |
1649 | { | 1950 | { |
1650 | int priority = thread->priority; | 1951 | /* If there are threads on a timeout and the earliest wakeup is due, |
1952 | * check the list and wake any threads that need to start running | ||
1953 | * again. */ | ||
1954 | if (!TIME_BEFORE(current_tick, cores[core].next_tmo_check)) | ||
1955 | { | ||
1956 | check_tmo_threads(); | ||
1957 | } | ||
1958 | |||
1959 | set_irq_level(HIGHEST_IRQ_LEVEL); | ||
1960 | RTR_LOCK(core); | ||
1651 | 1961 | ||
1652 | if (priority < cores[core].highest_priority) | 1962 | thread = cores[core].running; |
1653 | cores[core].highest_priority = priority; | ||
1654 | 1963 | ||
1655 | if (priority == cores[core].highest_priority || | 1964 | if (thread == NULL) |
1656 | thread->priority_x < cores[core].highest_priority || | ||
1657 | (current_tick - thread->last_run > priority * 8)) | ||
1658 | { | 1965 | { |
1659 | cores[core].running = thread; | 1966 | /* Enter sleep mode to reduce power usage - woken up on interrupt |
1660 | break; | 1967 | * or wakeup request from another core - expected to enable |
1968 | * interrupts. */ | ||
1969 | RTR_UNLOCK(core); | ||
1970 | core_sleep(IF_COP(core)); | ||
1661 | } | 1971 | } |
1972 | else | ||
1973 | { | ||
1974 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
1975 | /* Select the new task based on priorities and the last time a | ||
1976 | * process got CPU time relative to the highest priority runnable | ||
1977 | * task. */ | ||
1978 | struct priority_distribution *pd = &cores[core].rtr; | ||
1979 | int max = find_first_set_bit(pd->mask); | ||
1662 | 1980 | ||
1663 | thread = thread->l.next; | 1981 | if (block == NULL) |
1664 | } | 1982 | { |
1665 | 1983 | /* Not switching on a block, tentatively select next thread */ | |
1666 | /* Reset the value of thread's last running time to the current time. */ | 1984 | thread = thread->l.next; |
1667 | thread->last_run = current_tick; | 1985 | } |
1986 | |||
1987 | for (;;) | ||
1988 | { | ||
1989 | int priority = thread->priority; | ||
1990 | int diff; | ||
1991 | |||
1992 | /* This ridiculously simple method of aging seems to work | ||
1993 | * suspiciously well. It does tend to reward CPU hogs (under | ||
1994 | * yielding) but that's generally not desirable at all. On the | ||
1995 | * plus side, it, relatively to other threads, penalizes excess | ||
1996 | * yielding which is good if some high priority thread is | ||
1997 | * performing no useful work such as polling for a device to be | ||
1998 | * ready. Of course, aging is only employed when higher and lower | ||
1999 | * priority threads are runnable. The highest priority runnable | ||
2000 | * thread(s) are never skipped. */ | ||
2001 | if (priority <= max || | ||
2002 | (diff = priority - max, ++thread->skip_count > diff*diff)) | ||
2003 | { | ||
2004 | cores[core].running = thread; | ||
2005 | break; | ||
2006 | } | ||
2007 | |||
2008 | thread = thread->l.next; | ||
2009 | } | ||
1668 | #else | 2010 | #else |
1669 | if (block == NULL) | 2011 | /* Without priority use a simple FCFS algorithm */ |
1670 | { | 2012 | if (block == NULL) |
1671 | thread = thread->l.next; | 2013 | { |
1672 | cores[core].running = thread; | 2014 | /* Not switching on a block, select next thread */ |
1673 | } | 2015 | thread = thread->l.next; |
2016 | cores[core].running = thread; | ||
2017 | } | ||
1674 | #endif /* HAVE_PRIORITY_SCHEDULING */ | 2018 | #endif /* HAVE_PRIORITY_SCHEDULING */ |
1675 | 2019 | ||
2020 | RTR_UNLOCK(core); | ||
2021 | set_irq_level(0); | ||
2022 | break; | ||
2023 | } | ||
2024 | } | ||
2025 | |||
1676 | /* And finally give control to the next thread. */ | 2026 | /* And finally give control to the next thread. */ |
1677 | load_context(&thread->context); | 2027 | load_context(&thread->context); |
1678 | 2028 | ||
@@ -1682,314 +2032,210 @@ void switch_thread(struct thread_entry *old) | |||
1682 | } | 2032 | } |
1683 | 2033 | ||
1684 | /*--------------------------------------------------------------------------- | 2034 | /*--------------------------------------------------------------------------- |
1685 | * Change the boost state of a thread boosting or unboosting the CPU | 2035 | * Sleeps a thread for at least a specified number of ticks with zero being |
1686 | * as required. Require thread slot to be locked first. | 2036 | * a wait until the next tick. |
1687 | *--------------------------------------------------------------------------- | ||
1688 | */ | ||
1689 | static inline void boost_thread(struct thread_entry *thread, bool boost) | ||
1690 | { | ||
1691 | #ifdef HAVE_SCHEDULER_BOOSTCTRL | ||
1692 | if ((thread->boosted != 0) != boost) | ||
1693 | { | ||
1694 | thread->boosted = boost; | ||
1695 | cpu_boost(boost); | ||
1696 | } | ||
1697 | #endif | ||
1698 | (void)thread; (void)boost; | ||
1699 | } | ||
1700 | |||
1701 | /*--------------------------------------------------------------------------- | ||
1702 | * Sleeps a thread for a specified number of ticks and unboost the thread if | ||
1703 | * if it is boosted. If ticks is zero, it does not delay but instead switches | ||
1704 | * tasks. | ||
1705 | * | 2037 | * |
1706 | * INTERNAL: Intended for use by kernel and not for programs. | 2038 | * INTERNAL: Intended for use by kernel and not for programs. |
1707 | *--------------------------------------------------------------------------- | 2039 | *--------------------------------------------------------------------------- |
1708 | */ | 2040 | */ |
1709 | void sleep_thread(int ticks) | 2041 | void sleep_thread(int ticks) |
1710 | { | 2042 | { |
1711 | /* Get the entry for the current running thread. */ | ||
1712 | struct thread_entry *current = cores[CURRENT_CORE].running; | 2043 | struct thread_entry *current = cores[CURRENT_CORE].running; |
1713 | 2044 | ||
1714 | #if NUM_CORES > 1 | 2045 | LOCK_THREAD(current); |
1715 | /* Lock thread slot */ | ||
1716 | GET_THREAD_STATE(current); | ||
1717 | #endif | ||
1718 | 2046 | ||
1719 | /* Set our timeout, change lists, and finally switch threads. | 2047 | /* Set our timeout, remove from run list and join timeout list. */ |
1720 | * Unlock during switch on mulicore. */ | ||
1721 | current->tmo_tick = current_tick + ticks + 1; | 2048 | current->tmo_tick = current_tick + ticks + 1; |
1722 | block_thread_on_l(NULL, current, STATE_SLEEPING); | 2049 | block_thread_on_l(current, STATE_SLEEPING); |
1723 | switch_thread(current); | ||
1724 | 2050 | ||
1725 | /* Our status should be STATE_RUNNING */ | 2051 | UNLOCK_THREAD(current); |
1726 | THREAD_ASSERT(peek_thread_state(current) == STATE_RUNNING, | ||
1727 | "S:R->!*R", current); | ||
1728 | } | 2052 | } |
1729 | 2053 | ||
1730 | /*--------------------------------------------------------------------------- | 2054 | /*--------------------------------------------------------------------------- |
1731 | * Indefinitely block a thread on a blocking queue for explicit wakeup. | 2055 | * Indefinitely block a thread on a blocking queue for explicit wakeup. |
1732 | * Caller with interrupt-accessible lists should disable interrupts first | ||
1733 | * and request a BOP_IRQ_LEVEL blocking operation to reset it. | ||
1734 | * | 2056 | * |
1735 | * INTERNAL: Intended for use by kernel objects and not for programs. | 2057 | * INTERNAL: Intended for use by kernel objects and not for programs. |
1736 | *--------------------------------------------------------------------------- | 2058 | *--------------------------------------------------------------------------- |
1737 | */ | 2059 | */ |
1738 | IF_SWCL(static inline) void _block_thread(struct thread_queue *list | 2060 | void block_thread(struct thread_entry *current) |
1739 | IF_SWCL(, const bool nolock)) | ||
1740 | { | 2061 | { |
1741 | /* Get the entry for the current running thread. */ | 2062 | /* Set the state to blocked and take us off of the run queue until we |
1742 | struct thread_entry *current = cores[CURRENT_CORE].running; | 2063 | * are explicitly woken */ |
1743 | 2064 | LOCK_THREAD(current); | |
1744 | /* Set the state to blocked and ask the scheduler to switch tasks, | ||
1745 | * this takes us off of the run queue until we are explicitly woken */ | ||
1746 | 2065 | ||
1747 | #if NUM_CORES > 1 | 2066 | /* Set the list for explicit wakeup */ |
1748 | /* Lock thread slot */ | 2067 | block_thread_on_l(current, STATE_BLOCKED); |
1749 | GET_THREAD_STATE(current); | ||
1750 | #endif | ||
1751 | 2068 | ||
1752 | #if CONFIG_CORELOCK == SW_CORELOCK | 2069 | #ifdef HAVE_PRIORITY_SCHEDULING |
1753 | /* One branch optimized away during inlining */ | 2070 | if (current->blocker != NULL) |
1754 | if (nolock) | ||
1755 | { | 2071 | { |
1756 | block_thread_on_l_no_listlock((struct thread_entry **)list, | 2072 | /* Object supports PIP */ |
1757 | current, STATE_BLOCKED); | 2073 | current = blocker_inherit_priority(current); |
1758 | } | 2074 | } |
1759 | else | ||
1760 | #endif | 2075 | #endif |
1761 | { | ||
1762 | block_thread_on_l(list, current, STATE_BLOCKED); | ||
1763 | } | ||
1764 | |||
1765 | switch_thread(current); | ||
1766 | |||
1767 | /* Our status should be STATE_RUNNING */ | ||
1768 | THREAD_ASSERT(peek_thread_state(current) == STATE_RUNNING, | ||
1769 | "B:R->!*R", current); | ||
1770 | } | ||
1771 | |||
1772 | #if CONFIG_CORELOCK == SW_CORELOCK | ||
1773 | /* Inline lock/nolock version of _block_thread into these functions */ | ||
1774 | void block_thread(struct thread_queue *tq) | ||
1775 | { | ||
1776 | _block_thread(tq, false); | ||
1777 | } | ||
1778 | 2076 | ||
1779 | void block_thread_no_listlock(struct thread_entry **list) | 2077 | UNLOCK_THREAD(current); |
1780 | { | ||
1781 | _block_thread((struct thread_queue *)list, true); | ||
1782 | } | 2078 | } |
1783 | #endif /* CONFIG_CORELOCK */ | ||
1784 | 2079 | ||
1785 | /*--------------------------------------------------------------------------- | 2080 | /*--------------------------------------------------------------------------- |
1786 | * Block a thread on a blocking queue for a specified time interval or until | 2081 | * Block a thread on a blocking queue for a specified time interval or until |
1787 | * explicitly woken - whichever happens first. | 2082 | * explicitly woken - whichever happens first. |
1788 | * Caller with interrupt-accessible lists should disable interrupts first | ||
1789 | * and request that interrupt level be restored after switching out the | ||
1790 | * current thread. | ||
1791 | * | 2083 | * |
1792 | * INTERNAL: Intended for use by kernel objects and not for programs. | 2084 | * INTERNAL: Intended for use by kernel objects and not for programs. |
1793 | *--------------------------------------------------------------------------- | 2085 | *--------------------------------------------------------------------------- |
1794 | */ | 2086 | */ |
1795 | void block_thread_w_tmo(struct thread_queue *list, int timeout) | 2087 | void block_thread_w_tmo(struct thread_entry *current, int timeout) |
1796 | { | 2088 | { |
1797 | /* Get the entry for the current running thread. */ | 2089 | /* Get the entry for the current running thread. */ |
1798 | struct thread_entry *current = cores[CURRENT_CORE].running; | 2090 | LOCK_THREAD(current); |
1799 | |||
1800 | #if NUM_CORES > 1 | ||
1801 | /* Lock thread slot */ | ||
1802 | GET_THREAD_STATE(current); | ||
1803 | #endif | ||
1804 | 2091 | ||
1805 | /* Set the state to blocked with the specified timeout */ | 2092 | /* Set the state to blocked with the specified timeout */ |
1806 | current->tmo_tick = current_tick + timeout; | 2093 | current->tmo_tick = current_tick + timeout; |
2094 | |||
1807 | /* Set the list for explicit wakeup */ | 2095 | /* Set the list for explicit wakeup */ |
1808 | block_thread_on_l(list, current, STATE_BLOCKED_W_TMO); | 2096 | block_thread_on_l(current, STATE_BLOCKED_W_TMO); |
1809 | 2097 | ||
1810 | /* Now force a task switch and block until we have been woken up | 2098 | #ifdef HAVE_PRIORITY_SCHEDULING |
1811 | * by another thread or timeout is reached - whichever happens first */ | 2099 | if (current->blocker != NULL) |
1812 | switch_thread(current); | 2100 | { |
2101 | /* Object supports PIP */ | ||
2102 | current = blocker_inherit_priority(current); | ||
2103 | } | ||
2104 | #endif | ||
1813 | 2105 | ||
1814 | /* Our status should be STATE_RUNNING */ | 2106 | UNLOCK_THREAD(current); |
1815 | THREAD_ASSERT(peek_thread_state(current) == STATE_RUNNING, | ||
1816 | "T:R->!*R", current); | ||
1817 | } | 2107 | } |
1818 | 2108 | ||
1819 | /*--------------------------------------------------------------------------- | 2109 | /*--------------------------------------------------------------------------- |
1820 | * Explicitly wakeup a thread on a blocking queue. Has no effect on threads | 2110 | * Explicitly wakeup a thread on a blocking queue. Only effects threads of |
1821 | * that called sleep(). | 2111 | * STATE_BLOCKED and STATE_BLOCKED_W_TMO. |
1822 | * Caller with interrupt-accessible lists should disable interrupts first. | 2112 | * |
1823 | * This code should be considered a critical section by the caller. | 2113 | * This code should be considered a critical section by the caller meaning |
2114 | * that the object's corelock should be held. | ||
1824 | * | 2115 | * |
1825 | * INTERNAL: Intended for use by kernel objects and not for programs. | 2116 | * INTERNAL: Intended for use by kernel objects and not for programs. |
1826 | *--------------------------------------------------------------------------- | 2117 | *--------------------------------------------------------------------------- |
1827 | */ | 2118 | */ |
1828 | IF_SWCL(static inline) struct thread_entry * _wakeup_thread( | 2119 | unsigned int wakeup_thread(struct thread_entry **list) |
1829 | struct thread_queue *list IF_SWCL(, const bool nolock)) | ||
1830 | { | 2120 | { |
1831 | struct thread_entry *t; | 2121 | struct thread_entry *thread = *list; |
1832 | struct thread_entry *thread; | 2122 | unsigned int result = THREAD_NONE; |
1833 | unsigned state; | ||
1834 | |||
1835 | /* Wake up the last thread first. */ | ||
1836 | #if CONFIG_CORELOCK == SW_CORELOCK | ||
1837 | /* One branch optimized away during inlining */ | ||
1838 | if (nolock) | ||
1839 | { | ||
1840 | t = list->queue; | ||
1841 | } | ||
1842 | else | ||
1843 | #endif | ||
1844 | { | ||
1845 | t = LOCK_LIST(list); | ||
1846 | } | ||
1847 | 2123 | ||
1848 | /* Check if there is a blocked thread at all. */ | 2124 | /* Check if there is a blocked thread at all. */ |
1849 | if (t == NULL) | 2125 | if (thread == NULL) |
1850 | { | 2126 | return result; |
1851 | #if CONFIG_CORELOCK == SW_CORELOCK | ||
1852 | if (!nolock) | ||
1853 | #endif | ||
1854 | { | ||
1855 | UNLOCK_LIST(list, NULL); | ||
1856 | } | ||
1857 | return NULL; | ||
1858 | } | ||
1859 | 2127 | ||
1860 | thread = t; | 2128 | LOCK_THREAD(thread); |
1861 | |||
1862 | #if NUM_CORES > 1 | ||
1863 | #if CONFIG_CORELOCK == SW_CORELOCK | ||
1864 | if (nolock) | ||
1865 | { | ||
1866 | /* Lock thread only, not list */ | ||
1867 | state = GET_THREAD_STATE(thread); | ||
1868 | } | ||
1869 | else | ||
1870 | #endif | ||
1871 | { | ||
1872 | /* This locks in reverse order from other routines so a retry in the | ||
1873 | correct order may be needed */ | ||
1874 | state = TRY_GET_THREAD_STATE(thread); | ||
1875 | if (state == STATE_BUSY) | ||
1876 | { | ||
1877 | /* Unlock list and retry slot, then list */ | ||
1878 | UNLOCK_LIST(list, t); | ||
1879 | state = GET_THREAD_STATE(thread); | ||
1880 | t = LOCK_LIST(list); | ||
1881 | /* Be sure thread still exists here - it couldn't have re-added | ||
1882 | itself if it was woken elsewhere because this function is | ||
1883 | serialized within the object that owns the list. */ | ||
1884 | if (thread != t) | ||
1885 | { | ||
1886 | /* Thread disappeared :( */ | ||
1887 | UNLOCK_LIST(list, t); | ||
1888 | UNLOCK_THREAD(thread, state); | ||
1889 | return THREAD_WAKEUP_MISSING; /* Indicate disappearance */ | ||
1890 | } | ||
1891 | } | ||
1892 | } | ||
1893 | #else /* NUM_CORES == 1 */ | ||
1894 | state = GET_THREAD_STATE(thread); | ||
1895 | #endif /* NUM_CORES */ | ||
1896 | 2129 | ||
1897 | /* Determine thread's current state. */ | 2130 | /* Determine thread's current state. */ |
1898 | switch (state) | 2131 | switch (thread->state) |
1899 | { | 2132 | { |
1900 | case STATE_BLOCKED: | 2133 | case STATE_BLOCKED: |
1901 | case STATE_BLOCKED_W_TMO: | 2134 | case STATE_BLOCKED_W_TMO: |
1902 | /* Remove thread from object's blocked list - select t or list depending | 2135 | remove_from_list_l(list, thread); |
1903 | on locking type at compile time */ | 2136 | |
1904 | REMOVE_FROM_LIST_L_SELECT(t, list, thread); | 2137 | result = THREAD_OK; |
1905 | #if CONFIG_CORELOCK == SW_CORELOCK | 2138 | |
1906 | /* Statment optimized away during inlining if nolock != false */ | 2139 | #ifdef HAVE_PRIORITY_SCHEDULING |
1907 | if (!nolock) | 2140 | struct thread_entry *current; |
1908 | #endif | 2141 | struct blocker *bl = thread->blocker; |
2142 | |||
2143 | if (bl == NULL) | ||
1909 | { | 2144 | { |
1910 | UNLOCK_LIST(list, t); /* Unlock list - removal complete */ | 2145 | /* No inheritance - just boost the thread by aging */ |
2146 | thread->skip_count = thread->priority; | ||
2147 | current = cores[CURRENT_CORE].running; | ||
2148 | } | ||
2149 | else | ||
2150 | { | ||
2151 | /* Call the specified unblocking PIP */ | ||
2152 | current = bl->wakeup_protocol(thread); | ||
1911 | } | 2153 | } |
1912 | 2154 | ||
1913 | #ifdef HAVE_PRIORITY_SCHEDULING | 2155 | if (current != NULL && thread->priority < current->priority |
1914 | /* Give the task a kick to avoid a stall after wakeup. | 2156 | IF_COP( && thread->core == current->core )) |
1915 | Not really proper treatment - TODO later. */ | 2157 | { |
1916 | thread->last_run = current_tick - 8*LOWEST_PRIORITY; | 2158 | /* Woken thread is higher priority and exists on the same CPU core; |
1917 | #endif | 2159 | * recommend a task switch. Knowing if this is an interrupt call |
2160 | * would be helpful here. */ | ||
2161 | result |= THREAD_SWITCH; | ||
2162 | } | ||
2163 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
2164 | |||
1918 | core_schedule_wakeup(thread); | 2165 | core_schedule_wakeup(thread); |
1919 | UNLOCK_THREAD_SET_STATE(thread, STATE_RUNNING); | 2166 | break; |
1920 | return thread; | 2167 | |
1921 | default: | 2168 | /* Nothing to do. State is not blocked. */ |
1922 | /* Nothing to do. State is not blocked. */ | ||
1923 | #if THREAD_EXTRA_CHECKS | 2169 | #if THREAD_EXTRA_CHECKS |
2170 | default: | ||
1924 | THREAD_PANICF("wakeup_thread->block invalid", thread); | 2171 | THREAD_PANICF("wakeup_thread->block invalid", thread); |
1925 | case STATE_RUNNING: | 2172 | case STATE_RUNNING: |
1926 | case STATE_KILLED: | 2173 | case STATE_KILLED: |
2174 | break; | ||
1927 | #endif | 2175 | #endif |
1928 | #if CONFIG_CORELOCK == SW_CORELOCK | ||
1929 | /* Statement optimized away during inlining if nolock != false */ | ||
1930 | if (!nolock) | ||
1931 | #endif | ||
1932 | { | ||
1933 | UNLOCK_LIST(list, t); /* Unlock the object's list */ | ||
1934 | } | ||
1935 | UNLOCK_THREAD(thread, state); /* Unlock thread slot */ | ||
1936 | return NULL; | ||
1937 | } | 2176 | } |
1938 | } | ||
1939 | 2177 | ||
1940 | #if CONFIG_CORELOCK == SW_CORELOCK | 2178 | UNLOCK_THREAD(thread); |
1941 | /* Inline lock/nolock version of _wakeup_thread into these functions */ | 2179 | return result; |
1942 | struct thread_entry * wakeup_thread(struct thread_queue *tq) | ||
1943 | { | ||
1944 | return _wakeup_thread(tq, false); | ||
1945 | } | 2180 | } |
1946 | 2181 | ||
1947 | struct thread_entry * wakeup_thread_no_listlock(struct thread_entry **list) | 2182 | /*--------------------------------------------------------------------------- |
2183 | * Wakeup an entire queue of threads - returns bitwise-or of return bitmask | ||
2184 | * from each operation or THREAD_NONE of nothing was awakened. Object owning | ||
2185 | * the queue must be locked first. | ||
2186 | * | ||
2187 | * INTERNAL: Intended for use by kernel objects and not for programs. | ||
2188 | *--------------------------------------------------------------------------- | ||
2189 | */ | ||
2190 | unsigned int thread_queue_wake(struct thread_entry **list) | ||
1948 | { | 2191 | { |
1949 | return _wakeup_thread((struct thread_queue *)list, true); | 2192 | unsigned result = THREAD_NONE; |
2193 | |||
2194 | for (;;) | ||
2195 | { | ||
2196 | unsigned int rc = wakeup_thread(list); | ||
2197 | |||
2198 | if (rc == THREAD_NONE) | ||
2199 | break; /* No more threads */ | ||
2200 | |||
2201 | result |= rc; | ||
2202 | } | ||
2203 | |||
2204 | return result; | ||
1950 | } | 2205 | } |
1951 | #endif /* CONFIG_CORELOCK */ | ||
1952 | 2206 | ||
1953 | /*--------------------------------------------------------------------------- | 2207 | /*--------------------------------------------------------------------------- |
1954 | * Find an empty thread slot or MAXTHREADS if none found. The slot returned | 2208 | * Find an empty thread slot or MAXTHREADS if none found. The slot returned |
1955 | * will be locked on multicore. | 2209 | * will be locked on multicore. |
1956 | *--------------------------------------------------------------------------- | 2210 | *--------------------------------------------------------------------------- |
1957 | */ | 2211 | */ |
1958 | static int find_empty_thread_slot(void) | 2212 | static struct thread_entry * find_empty_thread_slot(void) |
1959 | { | 2213 | { |
1960 | #if NUM_CORES > 1 | 2214 | /* Any slot could be on an interrupt-accessible list */ |
1961 | /* Any slot could be on an IRQ-accessible list */ | 2215 | IF_COP( int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); ) |
1962 | int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); | 2216 | struct thread_entry *thread = NULL; |
1963 | #endif | ||
1964 | /* Thread slots are not locked on single core */ | ||
1965 | |||
1966 | int n; | 2217 | int n; |
1967 | 2218 | ||
1968 | for (n = 0; n < MAXTHREADS; n++) | 2219 | for (n = 0; n < MAXTHREADS; n++) |
1969 | { | 2220 | { |
1970 | /* Obtain current slot state - lock it on multicore */ | 2221 | /* Obtain current slot state - lock it on multicore */ |
1971 | unsigned state = GET_THREAD_STATE(&threads[n]); | 2222 | struct thread_entry *t = &threads[n]; |
2223 | LOCK_THREAD(t); | ||
1972 | 2224 | ||
1973 | if (state == STATE_KILLED | 2225 | if (t->state == STATE_KILLED IF_COP( && t->name != THREAD_DESTRUCT )) |
1974 | #if NUM_CORES > 1 | ||
1975 | && threads[n].name != THREAD_DESTRUCT | ||
1976 | #endif | ||
1977 | ) | ||
1978 | { | 2226 | { |
1979 | /* Slot is empty - leave it locked and caller will unlock */ | 2227 | /* Slot is empty - leave it locked and caller will unlock */ |
2228 | thread = t; | ||
1980 | break; | 2229 | break; |
1981 | } | 2230 | } |
1982 | 2231 | ||
1983 | /* Finished examining slot - no longer busy - unlock on multicore */ | 2232 | /* Finished examining slot - no longer busy - unlock on multicore */ |
1984 | UNLOCK_THREAD(&threads[n], state); | 2233 | UNLOCK_THREAD(t); |
1985 | } | 2234 | } |
1986 | 2235 | ||
1987 | #if NUM_CORES > 1 | 2236 | IF_COP( set_irq_level(oldlevel); ) /* Reenable interrups - this slot is |
1988 | set_irq_level(oldlevel); /* Reenable interrups - this slot is | 2237 | not accesible to them yet */ |
1989 | not accesible to them yet */ | 2238 | return thread; |
1990 | #endif | ||
1991 | |||
1992 | return n; | ||
1993 | } | 2239 | } |
1994 | 2240 | ||
1995 | 2241 | ||
@@ -2000,65 +2246,68 @@ static int find_empty_thread_slot(void) | |||
2000 | */ | 2246 | */ |
2001 | void core_idle(void) | 2247 | void core_idle(void) |
2002 | { | 2248 | { |
2003 | #if NUM_CORES > 1 | 2249 | IF_COP( const unsigned int core = CURRENT_CORE; ) |
2004 | const unsigned int core = CURRENT_CORE; | ||
2005 | #endif | ||
2006 | set_irq_level(HIGHEST_IRQ_LEVEL); | 2250 | set_irq_level(HIGHEST_IRQ_LEVEL); |
2007 | core_sleep(IF_COP(core)); | 2251 | core_sleep(IF_COP(core)); |
2008 | } | 2252 | } |
2009 | 2253 | ||
2010 | /*--------------------------------------------------------------------------- | 2254 | /*--------------------------------------------------------------------------- |
2011 | * Create a thread | 2255 | * Create a thread. If using a dual core architecture, specify which core to |
2012 | * If using a dual core architecture, specify which core to start the thread | 2256 | * start the thread on. |
2013 | * on, and whether to fall back to the other core if it can't be created | 2257 | * |
2014 | * Return ID if context area could be allocated, else NULL. | 2258 | * Return ID if context area could be allocated, else NULL. |
2015 | *--------------------------------------------------------------------------- | 2259 | *--------------------------------------------------------------------------- |
2016 | */ | 2260 | */ |
2017 | struct thread_entry* | 2261 | struct thread_entry* |
2018 | create_thread(void (*function)(void), void* stack, int stack_size, | 2262 | create_thread(void (*function)(void), void* stack, size_t stack_size, |
2019 | unsigned flags, const char *name | 2263 | unsigned flags, const char *name |
2020 | IF_PRIO(, int priority) | 2264 | IF_PRIO(, int priority) |
2021 | IF_COP(, unsigned int core)) | 2265 | IF_COP(, unsigned int core)) |
2022 | { | 2266 | { |
2023 | unsigned int i; | 2267 | unsigned int i; |
2024 | unsigned int stacklen; | 2268 | unsigned int stack_words; |
2025 | unsigned int *stackptr; | 2269 | uintptr_t stackptr, stackend; |
2026 | int slot; | ||
2027 | struct thread_entry *thread; | 2270 | struct thread_entry *thread; |
2028 | unsigned state; | 2271 | unsigned state; |
2272 | int oldlevel; | ||
2029 | 2273 | ||
2030 | slot = find_empty_thread_slot(); | 2274 | thread = find_empty_thread_slot(); |
2031 | if (slot >= MAXTHREADS) | 2275 | if (thread == NULL) |
2032 | { | 2276 | { |
2033 | return NULL; | 2277 | return NULL; |
2034 | } | 2278 | } |
2035 | 2279 | ||
2280 | oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); | ||
2281 | |||
2036 | /* Munge the stack to make it easy to spot stack overflows */ | 2282 | /* Munge the stack to make it easy to spot stack overflows */ |
2037 | stacklen = stack_size / sizeof(int); | 2283 | stackptr = ALIGN_UP((uintptr_t)stack, sizeof (uintptr_t)); |
2038 | stackptr = stack; | 2284 | stackend = ALIGN_DOWN((uintptr_t)stack + stack_size, sizeof (uintptr_t)); |
2039 | for(i = 0;i < stacklen;i++) | 2285 | stack_size = stackend - stackptr; |
2286 | stack_words = stack_size / sizeof (uintptr_t); | ||
2287 | |||
2288 | for (i = 0; i < stack_words; i++) | ||
2040 | { | 2289 | { |
2041 | stackptr[i] = DEADBEEF; | 2290 | ((uintptr_t *)stackptr)[i] = DEADBEEF; |
2042 | } | 2291 | } |
2043 | 2292 | ||
2044 | /* Store interesting information */ | 2293 | /* Store interesting information */ |
2045 | thread = &threads[slot]; | ||
2046 | thread->name = name; | 2294 | thread->name = name; |
2047 | thread->stack = stack; | 2295 | thread->stack = (uintptr_t *)stackptr; |
2048 | thread->stack_size = stack_size; | 2296 | thread->stack_size = stack_size; |
2049 | thread->bqp = NULL; | ||
2050 | #if CONFIG_CORELOCK == SW_CORELOCK | ||
2051 | thread->bqnlp = NULL; | ||
2052 | #endif | ||
2053 | thread->queue = NULL; | 2297 | thread->queue = NULL; |
2298 | #ifdef HAVE_WAKEUP_EXT_CB | ||
2299 | thread->wakeup_ext_cb = NULL; | ||
2300 | #endif | ||
2054 | #ifdef HAVE_SCHEDULER_BOOSTCTRL | 2301 | #ifdef HAVE_SCHEDULER_BOOSTCTRL |
2055 | thread->boosted = 0; | 2302 | thread->cpu_boost = 0; |
2056 | #endif | 2303 | #endif |
2057 | #ifdef HAVE_PRIORITY_SCHEDULING | 2304 | #ifdef HAVE_PRIORITY_SCHEDULING |
2058 | thread->priority_x = LOWEST_PRIORITY; | 2305 | memset(&thread->pdist, 0, sizeof(thread->pdist)); |
2306 | thread->blocker = NULL; | ||
2307 | thread->base_priority = priority; | ||
2059 | thread->priority = priority; | 2308 | thread->priority = priority; |
2060 | thread->last_run = current_tick - priority * 8; | 2309 | thread->skip_count = priority; |
2061 | cores[IF_COP_CORE(core)].highest_priority = LOWEST_PRIORITY; | 2310 | prio_add_entry(&thread->pdist, priority); |
2062 | #endif | 2311 | #endif |
2063 | 2312 | ||
2064 | #if NUM_CORES > 1 | 2313 | #if NUM_CORES > 1 |
@@ -2077,70 +2326,160 @@ struct thread_entry* | |||
2077 | state = (flags & CREATE_THREAD_FROZEN) ? | 2326 | state = (flags & CREATE_THREAD_FROZEN) ? |
2078 | STATE_FROZEN : STATE_RUNNING; | 2327 | STATE_FROZEN : STATE_RUNNING; |
2079 | 2328 | ||
2080 | /* Align stack to an even 32 bit boundary */ | 2329 | thread->context.sp = (typeof (thread->context.sp))stackend; |
2081 | thread->context.sp = (void*)(((unsigned int)stack + stack_size) & ~3); | ||
2082 | 2330 | ||
2083 | /* Load the thread's context structure with needed startup information */ | 2331 | /* Load the thread's context structure with needed startup information */ |
2084 | THREAD_STARTUP_INIT(core, thread, function); | 2332 | THREAD_STARTUP_INIT(core, thread, function); |
2085 | 2333 | ||
2334 | thread->state = state; | ||
2335 | |||
2086 | if (state == STATE_RUNNING) | 2336 | if (state == STATE_RUNNING) |
2087 | { | 2337 | core_schedule_wakeup(thread); |
2088 | #if NUM_CORES > 1 | 2338 | |
2089 | if (core != CURRENT_CORE) | 2339 | UNLOCK_THREAD(thread); |
2090 | { | 2340 | |
2091 | /* Next task switch on other core moves thread to running list */ | 2341 | set_irq_level(oldlevel); |
2092 | core_schedule_wakeup(thread); | ||
2093 | } | ||
2094 | else | ||
2095 | #endif | ||
2096 | { | ||
2097 | /* Place on running list immediately */ | ||
2098 | add_to_list_l(&cores[IF_COP_CORE(core)].running, thread); | ||
2099 | } | ||
2100 | } | ||
2101 | 2342 | ||
2102 | /* remove lock and set state */ | ||
2103 | UNLOCK_THREAD_SET_STATE(thread, state); | ||
2104 | |||
2105 | return thread; | 2343 | return thread; |
2106 | } | 2344 | } |
2107 | 2345 | ||
2108 | #ifdef HAVE_SCHEDULER_BOOSTCTRL | 2346 | #ifdef HAVE_SCHEDULER_BOOSTCTRL |
2347 | /*--------------------------------------------------------------------------- | ||
2348 | * Change the boost state of a thread boosting or unboosting the CPU | ||
2349 | * as required. | ||
2350 | *--------------------------------------------------------------------------- | ||
2351 | */ | ||
2352 | static inline void boost_thread(struct thread_entry *thread, bool boost) | ||
2353 | { | ||
2354 | if ((thread->cpu_boost != 0) != boost) | ||
2355 | { | ||
2356 | thread->cpu_boost = boost; | ||
2357 | cpu_boost(boost); | ||
2358 | } | ||
2359 | } | ||
2360 | |||
2109 | void trigger_cpu_boost(void) | 2361 | void trigger_cpu_boost(void) |
2110 | { | 2362 | { |
2111 | /* No IRQ disable nescessary since the current thread cannot be blocked | ||
2112 | on an IRQ-accessible list */ | ||
2113 | struct thread_entry *current = cores[CURRENT_CORE].running; | 2363 | struct thread_entry *current = cores[CURRENT_CORE].running; |
2114 | unsigned state; | ||
2115 | |||
2116 | state = GET_THREAD_STATE(current); | ||
2117 | boost_thread(current, true); | 2364 | boost_thread(current, true); |
2118 | UNLOCK_THREAD(current, state); | ||
2119 | |||
2120 | (void)state; | ||
2121 | } | 2365 | } |
2122 | 2366 | ||
2123 | void cancel_cpu_boost(void) | 2367 | void cancel_cpu_boost(void) |
2124 | { | 2368 | { |
2125 | struct thread_entry *current = cores[CURRENT_CORE].running; | 2369 | struct thread_entry *current = cores[CURRENT_CORE].running; |
2126 | unsigned state; | ||
2127 | |||
2128 | state = GET_THREAD_STATE(current); | ||
2129 | boost_thread(current, false); | 2370 | boost_thread(current, false); |
2130 | UNLOCK_THREAD(current, state); | ||
2131 | |||
2132 | (void)state; | ||
2133 | } | 2371 | } |
2134 | #endif /* HAVE_SCHEDULER_BOOSTCTRL */ | 2372 | #endif /* HAVE_SCHEDULER_BOOSTCTRL */ |
2135 | 2373 | ||
2136 | /*--------------------------------------------------------------------------- | 2374 | /*--------------------------------------------------------------------------- |
2137 | * Remove a thread from the scheduler. | 2375 | * Block the current thread until another thread terminates. A thread may |
2376 | * wait on itself to terminate which prevents it from running again and it | ||
2377 | * will need to be killed externally. | ||
2378 | * Parameter is the ID as returned from create_thread(). | ||
2379 | *--------------------------------------------------------------------------- | ||
2380 | */ | ||
2381 | void thread_wait(struct thread_entry *thread) | ||
2382 | { | ||
2383 | struct thread_entry *current = cores[CURRENT_CORE].running; | ||
2384 | |||
2385 | if (thread == NULL) | ||
2386 | thread = current; | ||
2387 | |||
2388 | /* Lock thread-as-waitable-object lock */ | ||
2389 | corelock_lock(&thread->waiter_cl); | ||
2390 | |||
2391 | /* Be sure it hasn't been killed yet */ | ||
2392 | if (thread->state != STATE_KILLED) | ||
2393 | { | ||
2394 | IF_COP( current->obj_cl = &thread->waiter_cl; ) | ||
2395 | current->bqp = &thread->queue; | ||
2396 | |||
2397 | set_irq_level(HIGHEST_IRQ_LEVEL); | ||
2398 | block_thread(current); | ||
2399 | |||
2400 | corelock_unlock(&thread->waiter_cl); | ||
2401 | |||
2402 | switch_thread(); | ||
2403 | return; | ||
2404 | } | ||
2405 | |||
2406 | corelock_unlock(&thread->waiter_cl); | ||
2407 | } | ||
2408 | |||
2409 | /*--------------------------------------------------------------------------- | ||
2410 | * Exit the current thread. The Right Way to Do Things (TM). | ||
2411 | *--------------------------------------------------------------------------- | ||
2412 | */ | ||
2413 | void thread_exit(void) | ||
2414 | { | ||
2415 | const unsigned int core = CURRENT_CORE; | ||
2416 | struct thread_entry *current = cores[core].running; | ||
2417 | |||
2418 | /* Cancel CPU boost if any */ | ||
2419 | cancel_cpu_boost(); | ||
2420 | |||
2421 | set_irq_level(HIGHEST_IRQ_LEVEL); | ||
2422 | |||
2423 | corelock_lock(¤t->waiter_cl); | ||
2424 | LOCK_THREAD(current); | ||
2425 | |||
2426 | #if defined (ALLOW_REMOVE_THREAD) && NUM_CORES > 1 | ||
2427 | if (current->name == THREAD_DESTRUCT) | ||
2428 | { | ||
2429 | /* Thread being killed - become a waiter */ | ||
2430 | UNLOCK_THREAD(current); | ||
2431 | corelock_unlock(¤t->waiter_cl); | ||
2432 | thread_wait(current); | ||
2433 | THREAD_PANICF("thread_exit->WK:*R", current); | ||
2434 | } | ||
2435 | #endif | ||
2436 | |||
2437 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
2438 | check_for_obj_waiters("thread_exit", current); | ||
2439 | #endif | ||
2440 | |||
2441 | if (current->tmo.prev != NULL) | ||
2442 | { | ||
2443 | /* Cancel pending timeout list removal */ | ||
2444 | remove_from_list_tmo(current); | ||
2445 | } | ||
2446 | |||
2447 | /* Switch tasks and never return */ | ||
2448 | block_thread_on_l(current, STATE_KILLED); | ||
2449 | |||
2450 | #if NUM_CORES > 1 | ||
2451 | /* Switch to the idle stack if not on the main core (where "main" | ||
2452 | * runs) - we can hope gcc doesn't need the old stack beyond this | ||
2453 | * point. */ | ||
2454 | if (core != CPU) | ||
2455 | { | ||
2456 | switch_to_idle_stack(core); | ||
2457 | } | ||
2458 | |||
2459 | flush_icache(); | ||
2460 | #endif | ||
2461 | current->name = NULL; | ||
2462 | |||
2463 | /* Signal this thread */ | ||
2464 | thread_queue_wake(¤t->queue); | ||
2465 | corelock_unlock(¤t->waiter_cl); | ||
2466 | /* Slot must be unusable until thread is really gone */ | ||
2467 | UNLOCK_THREAD_AT_TASK_SWITCH(current); | ||
2468 | switch_thread(); | ||
2469 | /* This should never and must never be reached - if it is, the | ||
2470 | * state is corrupted */ | ||
2471 | THREAD_PANICF("thread_exit->K:*R", current); | ||
2472 | } | ||
2473 | |||
2474 | #ifdef ALLOW_REMOVE_THREAD | ||
2475 | /*--------------------------------------------------------------------------- | ||
2476 | * Remove a thread from the scheduler. Not The Right Way to Do Things in | ||
2477 | * normal programs. | ||
2478 | * | ||
2138 | * Parameter is the ID as returned from create_thread(). | 2479 | * Parameter is the ID as returned from create_thread(). |
2139 | * | 2480 | * |
2140 | * Use with care on threads that are not under careful control as this may | 2481 | * Use with care on threads that are not under careful control as this may |
2141 | * leave various objects in an undefined state. When trying to kill a thread | 2482 | * leave various objects in an undefined state. |
2142 | * on another processor, be sure you know what it's doing and won't be | ||
2143 | * switching around itself. | ||
2144 | *--------------------------------------------------------------------------- | 2483 | *--------------------------------------------------------------------------- |
2145 | */ | 2484 | */ |
2146 | void remove_thread(struct thread_entry *thread) | 2485 | void remove_thread(struct thread_entry *thread) |
@@ -2149,17 +2488,27 @@ void remove_thread(struct thread_entry *thread) | |||
2149 | /* core is not constant here because of core switching */ | 2488 | /* core is not constant here because of core switching */ |
2150 | unsigned int core = CURRENT_CORE; | 2489 | unsigned int core = CURRENT_CORE; |
2151 | unsigned int old_core = NUM_CORES; | 2490 | unsigned int old_core = NUM_CORES; |
2491 | struct corelock *ocl = NULL; | ||
2152 | #else | 2492 | #else |
2153 | const unsigned int core = CURRENT_CORE; | 2493 | const unsigned int core = CURRENT_CORE; |
2154 | #endif | 2494 | #endif |
2495 | struct thread_entry *current = cores[core].running; | ||
2496 | |||
2155 | unsigned state; | 2497 | unsigned state; |
2156 | int oldlevel; | 2498 | int oldlevel; |
2157 | 2499 | ||
2158 | if (thread == NULL) | 2500 | if (thread == NULL) |
2159 | thread = cores[core].running; | 2501 | thread = current; |
2502 | |||
2503 | if (thread == current) | ||
2504 | thread_exit(); /* Current thread - do normal exit */ | ||
2160 | 2505 | ||
2161 | oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); | 2506 | oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); |
2162 | state = GET_THREAD_STATE(thread); | 2507 | |
2508 | corelock_lock(&thread->waiter_cl); | ||
2509 | LOCK_THREAD(thread); | ||
2510 | |||
2511 | state = thread->state; | ||
2163 | 2512 | ||
2164 | if (state == STATE_KILLED) | 2513 | if (state == STATE_KILLED) |
2165 | { | 2514 | { |
@@ -2167,50 +2516,49 @@ void remove_thread(struct thread_entry *thread) | |||
2167 | } | 2516 | } |
2168 | 2517 | ||
2169 | #if NUM_CORES > 1 | 2518 | #if NUM_CORES > 1 |
2519 | if (thread->name == THREAD_DESTRUCT) | ||
2520 | { | ||
2521 | /* Thread being killed - become a waiter */ | ||
2522 | UNLOCK_THREAD(thread); | ||
2523 | corelock_unlock(&thread->waiter_cl); | ||
2524 | set_irq_level(oldlevel); | ||
2525 | thread_wait(thread); | ||
2526 | return; | ||
2527 | } | ||
2528 | |||
2529 | thread->name = THREAD_DESTRUCT; /* Slot can't be used for now */ | ||
2530 | |||
2531 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
2532 | check_for_obj_waiters("remove_thread", thread); | ||
2533 | #endif | ||
2534 | |||
2170 | if (thread->core != core) | 2535 | if (thread->core != core) |
2171 | { | 2536 | { |
2172 | /* Switch cores and safely extract the thread there */ | 2537 | /* Switch cores and safely extract the thread there */ |
2173 | /* Slot HAS to be unlocked or a deadlock could occur - potential livelock | 2538 | /* Slot HAS to be unlocked or a deadlock could occur which means other |
2174 | condition if the thread runs away to another processor. */ | 2539 | * threads have to be guided into becoming thread waiters if they |
2540 | * attempt to remove it. */ | ||
2175 | unsigned int new_core = thread->core; | 2541 | unsigned int new_core = thread->core; |
2176 | const char *old_name = thread->name; | ||
2177 | 2542 | ||
2178 | thread->name = THREAD_DESTRUCT; /* Slot can't be used for now */ | 2543 | corelock_unlock(&thread->waiter_cl); |
2179 | UNLOCK_THREAD(thread, state); | 2544 | |
2545 | UNLOCK_THREAD(thread); | ||
2180 | set_irq_level(oldlevel); | 2546 | set_irq_level(oldlevel); |
2181 | 2547 | ||
2182 | old_core = switch_core(new_core); | 2548 | old_core = switch_core(new_core); |
2183 | 2549 | ||
2184 | oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); | 2550 | oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); |
2185 | state = GET_THREAD_STATE(thread); | ||
2186 | |||
2187 | core = new_core; | ||
2188 | |||
2189 | if (state == STATE_KILLED) | ||
2190 | { | ||
2191 | /* Thread suicided before we could kill it */ | ||
2192 | goto thread_killed; | ||
2193 | } | ||
2194 | |||
2195 | /* Reopen slot - it's locked again anyway */ | ||
2196 | thread->name = old_name; | ||
2197 | 2551 | ||
2198 | if (thread->core != core) | 2552 | corelock_lock(&thread->waiter_cl); |
2199 | { | 2553 | LOCK_THREAD(thread); |
2200 | /* We won't play thread tag - just forget it */ | ||
2201 | UNLOCK_THREAD(thread, state); | ||
2202 | set_irq_level(oldlevel); | ||
2203 | goto thread_kill_abort; | ||
2204 | } | ||
2205 | 2554 | ||
2555 | state = thread->state; | ||
2556 | core = new_core; | ||
2206 | /* Perform the extraction and switch ourselves back to the original | 2557 | /* Perform the extraction and switch ourselves back to the original |
2207 | processor */ | 2558 | processor */ |
2208 | } | 2559 | } |
2209 | #endif /* NUM_CORES > 1 */ | 2560 | #endif /* NUM_CORES > 1 */ |
2210 | 2561 | ||
2211 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
2212 | cores[IF_COP_CORE(core)].highest_priority = LOWEST_PRIORITY; | ||
2213 | #endif | ||
2214 | if (thread->tmo.prev != NULL) | 2562 | if (thread->tmo.prev != NULL) |
2215 | { | 2563 | { |
2216 | /* Clean thread off the timeout list if a timeout check hasn't | 2564 | /* Clean thread off the timeout list if a timeout check hasn't |
@@ -2218,87 +2566,86 @@ void remove_thread(struct thread_entry *thread) | |||
2218 | remove_from_list_tmo(thread); | 2566 | remove_from_list_tmo(thread); |
2219 | } | 2567 | } |
2220 | 2568 | ||
2569 | #ifdef HAVE_SCHEDULER_BOOSTCTRL | ||
2570 | /* Cancel CPU boost if any */ | ||
2221 | boost_thread(thread, false); | 2571 | boost_thread(thread, false); |
2222 | |||
2223 | if (thread == cores[core].running) | ||
2224 | { | ||
2225 | /* Suicide - thread has unconditional rights to do this */ | ||
2226 | /* Maintain locks until switch-out */ | ||
2227 | block_thread_on_l(NULL, thread, STATE_KILLED); | ||
2228 | |||
2229 | #if NUM_CORES > 1 | ||
2230 | /* Switch to the idle stack if not on the main core (where "main" | ||
2231 | * runs) */ | ||
2232 | if (core != CPU) | ||
2233 | { | ||
2234 | switch_to_idle_stack(core); | ||
2235 | } | ||
2236 | |||
2237 | flush_icache(); | ||
2238 | #endif | 2572 | #endif |
2239 | /* Signal this thread */ | ||
2240 | thread_queue_wake_no_listlock(&thread->queue); | ||
2241 | /* Switch tasks and never return */ | ||
2242 | switch_thread(thread); | ||
2243 | /* This should never and must never be reached - if it is, the | ||
2244 | * state is corrupted */ | ||
2245 | THREAD_PANICF("remove_thread->K:*R", thread); | ||
2246 | } | ||
2247 | 2573 | ||
2248 | #if NUM_CORES > 1 | 2574 | IF_COP( retry_state: ) |
2249 | if (thread->name == THREAD_DESTRUCT) | ||
2250 | { | ||
2251 | /* Another core is doing this operation already */ | ||
2252 | UNLOCK_THREAD(thread, state); | ||
2253 | set_irq_level(oldlevel); | ||
2254 | return; | ||
2255 | } | ||
2256 | #endif | ||
2257 | if (cores[core].waking.queue != NULL) | ||
2258 | { | ||
2259 | /* Get any threads off the waking list and onto the running | ||
2260 | * list first - waking and running cannot be distinguished by | ||
2261 | * state */ | ||
2262 | core_perform_wakeup(IF_COP(core)); | ||
2263 | } | ||
2264 | 2575 | ||
2265 | switch (state) | 2576 | switch (state) |
2266 | { | 2577 | { |
2267 | case STATE_RUNNING: | 2578 | case STATE_RUNNING: |
2579 | RTR_LOCK(core); | ||
2268 | /* Remove thread from ready to run tasks */ | 2580 | /* Remove thread from ready to run tasks */ |
2269 | remove_from_list_l(&cores[core].running, thread); | 2581 | remove_from_list_l(&cores[core].running, thread); |
2582 | rtr_subtract_entry(core, thread->priority); | ||
2583 | RTR_UNLOCK(core); | ||
2270 | break; | 2584 | break; |
2271 | case STATE_BLOCKED: | 2585 | case STATE_BLOCKED: |
2272 | case STATE_BLOCKED_W_TMO: | 2586 | case STATE_BLOCKED_W_TMO: |
2273 | /* Remove thread from the queue it's blocked on - including its | 2587 | /* Remove thread from the queue it's blocked on - including its |
2274 | * own if waiting there */ | 2588 | * own if waiting there */ |
2275 | #if CONFIG_CORELOCK == SW_CORELOCK | 2589 | #if NUM_CORES > 1 |
2276 | /* One or the other will be valid */ | 2590 | if (&thread->waiter_cl != thread->obj_cl) |
2277 | if (thread->bqp == NULL) | ||
2278 | { | 2591 | { |
2279 | remove_from_list_l(thread->bqnlp, thread); | 2592 | ocl = thread->obj_cl; |
2593 | |||
2594 | if (corelock_try_lock(ocl) == 0) | ||
2595 | { | ||
2596 | UNLOCK_THREAD(thread); | ||
2597 | corelock_lock(ocl); | ||
2598 | LOCK_THREAD(thread); | ||
2599 | |||
2600 | if (thread->state != state) | ||
2601 | { | ||
2602 | /* Something woke the thread */ | ||
2603 | state = thread->state; | ||
2604 | corelock_unlock(ocl); | ||
2605 | goto retry_state; | ||
2606 | } | ||
2607 | } | ||
2280 | } | 2608 | } |
2281 | else | 2609 | #endif |
2282 | #endif /* CONFIG_CORELOCK */ | 2610 | remove_from_list_l(thread->bqp, thread); |
2611 | |||
2612 | #ifdef HAVE_WAKEUP_EXT_CB | ||
2613 | if (thread->wakeup_ext_cb != NULL) | ||
2614 | thread->wakeup_ext_cb(thread); | ||
2615 | #endif | ||
2616 | |||
2617 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
2618 | if (thread->blocker != NULL) | ||
2283 | { | 2619 | { |
2284 | remove_from_list_l_locked(thread->bqp, thread); | 2620 | /* Remove thread's priority influence from its chain */ |
2621 | wakeup_priority_protocol_release(thread); | ||
2285 | } | 2622 | } |
2623 | #endif | ||
2624 | |||
2625 | #if NUM_CORES > 1 | ||
2626 | if (ocl != NULL) | ||
2627 | corelock_unlock(ocl); | ||
2628 | #endif | ||
2286 | break; | 2629 | break; |
2287 | /* Otherwise thread is killed or is frozen and hasn't run yet */ | 2630 | /* Otherwise thread is frozen and hasn't run yet */ |
2288 | } | 2631 | } |
2289 | 2632 | ||
2633 | thread->state = STATE_KILLED; | ||
2634 | |||
2290 | /* If thread was waiting on itself, it will have been removed above. | 2635 | /* If thread was waiting on itself, it will have been removed above. |
2291 | * The wrong order would result in waking the thread first and deadlocking | 2636 | * The wrong order would result in waking the thread first and deadlocking |
2292 | * since the slot is already locked. */ | 2637 | * since the slot is already locked. */ |
2293 | thread_queue_wake_no_listlock(&thread->queue); | 2638 | thread_queue_wake(&thread->queue); |
2639 | |||
2640 | thread->name = NULL; | ||
2294 | 2641 | ||
2295 | thread_killed: /* Thread was already killed */ | 2642 | thread_killed: /* Thread was already killed */ |
2296 | /* Removal complete - safe to unlock state and reenable interrupts */ | 2643 | /* Removal complete - safe to unlock and reenable interrupts */ |
2297 | UNLOCK_THREAD_SET_STATE(thread, STATE_KILLED); | 2644 | corelock_unlock(&thread->waiter_cl); |
2645 | UNLOCK_THREAD(thread); | ||
2298 | set_irq_level(oldlevel); | 2646 | set_irq_level(oldlevel); |
2299 | 2647 | ||
2300 | #if NUM_CORES > 1 | 2648 | #if NUM_CORES > 1 |
2301 | thread_kill_abort: /* Something stopped us from killing the thread */ | ||
2302 | if (old_core < NUM_CORES) | 2649 | if (old_core < NUM_CORES) |
2303 | { | 2650 | { |
2304 | /* Did a removal on another processor's thread - switch back to | 2651 | /* Did a removal on another processor's thread - switch back to |
@@ -2307,114 +2654,147 @@ thread_kill_abort: /* Something stopped us from killing the thread */ | |||
2307 | } | 2654 | } |
2308 | #endif | 2655 | #endif |
2309 | } | 2656 | } |
2657 | #endif /* ALLOW_REMOVE_THREAD */ | ||
2310 | 2658 | ||
2659 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
2311 | /*--------------------------------------------------------------------------- | 2660 | /*--------------------------------------------------------------------------- |
2312 | * Block the current thread until another thread terminates. A thread may | 2661 | * Sets the thread's relative base priority for the core it runs on. Any |
2313 | * wait on itself to terminate which prevents it from running again and it | 2662 | * needed inheritance changes also may happen. |
2314 | * will need to be killed externally. | ||
2315 | * Parameter is the ID as returned from create_thread(). | ||
2316 | *--------------------------------------------------------------------------- | 2663 | *--------------------------------------------------------------------------- |
2317 | */ | 2664 | */ |
2318 | void thread_wait(struct thread_entry *thread) | 2665 | int thread_set_priority(struct thread_entry *thread, int priority) |
2319 | { | 2666 | { |
2320 | const unsigned int core = CURRENT_CORE; | 2667 | int old_base_priority = -1; |
2321 | struct thread_entry *current = cores[core].running; | 2668 | |
2322 | unsigned thread_state; | 2669 | /* A little safety measure */ |
2323 | #if NUM_CORES > 1 | 2670 | if (priority < HIGHEST_PRIORITY || priority > LOWEST_PRIORITY) |
2324 | int oldlevel; | 2671 | return -1; |
2325 | unsigned current_state; | ||
2326 | #endif | ||
2327 | 2672 | ||
2328 | if (thread == NULL) | 2673 | if (thread == NULL) |
2329 | thread = current; | 2674 | thread = cores[CURRENT_CORE].running; |
2330 | 2675 | ||
2331 | #if NUM_CORES > 1 | 2676 | /* Thread could be on any list and therefore on an interrupt accessible |
2332 | oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); | 2677 | one - disable interrupts */ |
2333 | #endif | 2678 | int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); |
2334 | 2679 | ||
2335 | thread_state = GET_THREAD_STATE(thread); | 2680 | LOCK_THREAD(thread); |
2336 | 2681 | ||
2337 | #if NUM_CORES > 1 | 2682 | /* Make sure it's not killed */ |
2338 | /* We can't lock the same slot twice. The waitee will also lock itself | 2683 | if (thread->state != STATE_KILLED) |
2339 | first then the thread slots that will be locked and woken in turn. | ||
2340 | The same order must be observed here as well. */ | ||
2341 | if (thread == current) | ||
2342 | { | ||
2343 | current_state = thread_state; | ||
2344 | } | ||
2345 | else | ||
2346 | { | 2684 | { |
2347 | current_state = GET_THREAD_STATE(current); | 2685 | int old_priority = thread->priority; |
2348 | } | ||
2349 | #endif | ||
2350 | 2686 | ||
2351 | if (thread_state != STATE_KILLED) | 2687 | old_base_priority = thread->base_priority; |
2352 | { | 2688 | thread->base_priority = priority; |
2353 | /* Unlock the waitee state at task switch - not done for self-wait | 2689 | |
2354 | because the would double-unlock the state and potentially | 2690 | prio_move_entry(&thread->pdist, old_base_priority, priority); |
2355 | corrupt another's busy assert on the slot */ | 2691 | priority = find_first_set_bit(thread->pdist.mask); |
2356 | if (thread != current) | 2692 | |
2693 | if (old_priority == priority) | ||
2357 | { | 2694 | { |
2358 | #if CONFIG_CORELOCK == SW_CORELOCK | 2695 | /* No priority change - do nothing */ |
2359 | cores[core].blk_ops.flags |= TBOP_UNLOCK_THREAD; | ||
2360 | cores[core].blk_ops.thread = thread; | ||
2361 | #elif CONFIG_CORELOCK == CORELOCK_SWAP | ||
2362 | cores[core].blk_ops.flags |= TBOP_SET_VARu8; | ||
2363 | cores[core].blk_ops.var_u8p = &thread->state; | ||
2364 | cores[core].blk_ops.var_u8v = thread_state; | ||
2365 | #endif | ||
2366 | } | 2696 | } |
2367 | block_thread_on_l_no_listlock(&thread->queue, current, STATE_BLOCKED); | 2697 | else if (thread->state == STATE_RUNNING) |
2368 | switch_thread(current); | 2698 | { |
2369 | return; | 2699 | /* This thread is running - change location on the run |
2370 | } | 2700 | * queue. No transitive inheritance needed. */ |
2701 | set_running_thread_priority(thread, priority); | ||
2702 | } | ||
2703 | else | ||
2704 | { | ||
2705 | thread->priority = priority; | ||
2706 | |||
2707 | if (thread->blocker != NULL) | ||
2708 | { | ||
2709 | /* Bubble new priority down the chain */ | ||
2710 | struct blocker *bl = thread->blocker; /* Blocker struct */ | ||
2711 | struct thread_entry *bl_t = bl->thread; /* Blocking thread */ | ||
2712 | struct thread_entry * const tstart = thread; /* Initial thread */ | ||
2713 | const int highest = MIN(priority, old_priority); /* Higher of new or old */ | ||
2371 | 2714 | ||
2372 | /* Unlock both slots - obviously the current thread can't have | 2715 | for (;;) |
2373 | STATE_KILLED so the above if clause will always catch a thread | 2716 | { |
2374 | waiting on itself */ | 2717 | struct thread_entry *next; /* Next thread to check */ |
2718 | int bl_pr; /* Highest blocked thread */ | ||
2719 | int queue_pr; /* New highest blocked thread */ | ||
2375 | #if NUM_CORES > 1 | 2720 | #if NUM_CORES > 1 |
2376 | UNLOCK_THREAD(current, current_state); | 2721 | /* Owner can change but thread cannot be dislodged - thread |
2377 | UNLOCK_THREAD(thread, thread_state); | 2722 | * may not be the first in the queue which allows other |
2378 | set_irq_level(oldlevel); | 2723 | * threads ahead in the list to be given ownership during the |
2379 | #endif | 2724 | * operation. If thread is next then the waker will have to |
2380 | } | 2725 | * wait for us and the owner of the object will remain fixed. |
2726 | * If we successfully grab the owner -- which at some point | ||
2727 | * is guaranteed -- then the queue remains fixed until we | ||
2728 | * pass by. */ | ||
2729 | for (;;) | ||
2730 | { | ||
2731 | LOCK_THREAD(bl_t); | ||
2381 | 2732 | ||
2382 | #ifdef HAVE_PRIORITY_SCHEDULING | 2733 | /* Double-check the owner - retry if it changed */ |
2383 | /*--------------------------------------------------------------------------- | 2734 | if (bl->thread == bl_t) |
2384 | * Sets the thread's relative priority for the core it runs on. | 2735 | break; |
2385 | *--------------------------------------------------------------------------- | ||
2386 | */ | ||
2387 | int thread_set_priority(struct thread_entry *thread, int priority) | ||
2388 | { | ||
2389 | unsigned old_priority = (unsigned)-1; | ||
2390 | |||
2391 | if (thread == NULL) | ||
2392 | thread = cores[CURRENT_CORE].running; | ||
2393 | 2736 | ||
2394 | #if NUM_CORES > 1 | 2737 | UNLOCK_THREAD(bl_t); |
2395 | /* Thread could be on any list and therefore on an interrupt accessible | 2738 | bl_t = bl->thread; |
2396 | one - disable interrupts */ | 2739 | } |
2397 | int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); | ||
2398 | #endif | 2740 | #endif |
2399 | unsigned state = GET_THREAD_STATE(thread); | 2741 | bl_pr = bl->priority; |
2400 | 2742 | ||
2401 | /* Make sure it's not killed */ | 2743 | if (highest > bl_pr) |
2402 | if (state != STATE_KILLED) | 2744 | break; /* Object priority won't change */ |
2403 | { | 2745 | |
2404 | old_priority = thread->priority; | 2746 | /* This will include the thread being set */ |
2405 | thread->priority = priority; | 2747 | queue_pr = find_highest_priority_in_list_l(*thread->bqp); |
2406 | cores[IF_COP_CORE(thread->core)].highest_priority = LOWEST_PRIORITY; | 2748 | |
2749 | if (queue_pr == bl_pr) | ||
2750 | break; /* Object priority not changing */ | ||
2751 | |||
2752 | /* Update thread boost for this object */ | ||
2753 | bl->priority = queue_pr; | ||
2754 | prio_move_entry(&bl_t->pdist, bl_pr, queue_pr); | ||
2755 | bl_pr = find_first_set_bit(bl_t->pdist.mask); | ||
2756 | |||
2757 | if (bl_t->priority == bl_pr) | ||
2758 | break; /* Blocking thread priority not changing */ | ||
2759 | |||
2760 | if (bl_t->state == STATE_RUNNING) | ||
2761 | { | ||
2762 | /* Thread not blocked - we're done */ | ||
2763 | set_running_thread_priority(bl_t, bl_pr); | ||
2764 | break; | ||
2765 | } | ||
2766 | |||
2767 | bl_t->priority = bl_pr; | ||
2768 | bl = bl_t->blocker; /* Blocking thread has a blocker? */ | ||
2769 | |||
2770 | if (bl == NULL) | ||
2771 | break; /* End of chain */ | ||
2772 | |||
2773 | next = bl->thread; | ||
2774 | |||
2775 | if (next == tstart) | ||
2776 | break; /* Full-circle */ | ||
2777 | |||
2778 | UNLOCK_THREAD(thread); | ||
2779 | |||
2780 | thread = bl_t; | ||
2781 | bl_t = next; | ||
2782 | } /* for (;;) */ | ||
2783 | |||
2784 | UNLOCK_THREAD(bl_t); | ||
2785 | } | ||
2786 | } | ||
2407 | } | 2787 | } |
2408 | 2788 | ||
2409 | #if NUM_CORES > 1 | 2789 | UNLOCK_THREAD(thread); |
2410 | UNLOCK_THREAD(thread, state); | 2790 | |
2411 | set_irq_level(oldlevel); | 2791 | set_irq_level(oldlevel); |
2412 | #endif | 2792 | |
2413 | return old_priority; | 2793 | return old_base_priority; |
2414 | } | 2794 | } |
2415 | 2795 | ||
2416 | /*--------------------------------------------------------------------------- | 2796 | /*--------------------------------------------------------------------------- |
2417 | * Returns the current priority for a thread. | 2797 | * Returns the current base priority for a thread. |
2418 | *--------------------------------------------------------------------------- | 2798 | *--------------------------------------------------------------------------- |
2419 | */ | 2799 | */ |
2420 | int thread_get_priority(struct thread_entry *thread) | 2800 | int thread_get_priority(struct thread_entry *thread) |
@@ -2423,64 +2803,26 @@ int thread_get_priority(struct thread_entry *thread) | |||
2423 | if (thread == NULL) | 2803 | if (thread == NULL) |
2424 | thread = cores[CURRENT_CORE].running; | 2804 | thread = cores[CURRENT_CORE].running; |
2425 | 2805 | ||
2426 | return (unsigned)thread->priority; | 2806 | return thread->base_priority; |
2427 | } | 2807 | } |
2808 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
2428 | 2809 | ||
2429 | /*--------------------------------------------------------------------------- | 2810 | /*--------------------------------------------------------------------------- |
2430 | * Yield that guarantees thread execution once per round regardless of | 2811 | * Starts a frozen thread - similar semantics to wakeup_thread except that |
2431 | * thread's scheduler priority - basically a transient realtime boost | 2812 | * the thread is on no scheduler or wakeup queue at all. It exists simply by |
2432 | * without altering the scheduler's thread precedence. | 2813 | * virtue of the slot having a state of STATE_FROZEN. |
2433 | * | ||
2434 | * HACK ALERT! Search for "priority inheritance" for proper treatment. | ||
2435 | *--------------------------------------------------------------------------- | 2814 | *--------------------------------------------------------------------------- |
2436 | */ | 2815 | */ |
2437 | void priority_yield(void) | ||
2438 | { | ||
2439 | const unsigned int core = CURRENT_CORE; | ||
2440 | struct thread_entry *thread = cores[core].running; | ||
2441 | thread->priority_x = HIGHEST_PRIORITY; | ||
2442 | switch_thread(NULL); | ||
2443 | thread->priority_x = LOWEST_PRIORITY; | ||
2444 | } | ||
2445 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
2446 | |||
2447 | /* Resumes a frozen thread - similar logic to wakeup_thread except that | ||
2448 | the thread is on no scheduler list at all. It exists simply by virtue of | ||
2449 | the slot having a state of STATE_FROZEN. */ | ||
2450 | void thread_thaw(struct thread_entry *thread) | 2816 | void thread_thaw(struct thread_entry *thread) |
2451 | { | 2817 | { |
2452 | #if NUM_CORES > 1 | ||
2453 | /* Thread could be on any list and therefore on an interrupt accessible | ||
2454 | one - disable interrupts */ | ||
2455 | int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); | 2818 | int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); |
2456 | #endif | 2819 | LOCK_THREAD(thread); |
2457 | unsigned state = GET_THREAD_STATE(thread); | ||
2458 | 2820 | ||
2459 | if (state == STATE_FROZEN) | 2821 | if (thread->state == STATE_FROZEN) |
2460 | { | 2822 | core_schedule_wakeup(thread); |
2461 | const unsigned int core = CURRENT_CORE; | ||
2462 | #if NUM_CORES > 1 | ||
2463 | if (thread->core != core) | ||
2464 | { | ||
2465 | core_schedule_wakeup(thread); | ||
2466 | } | ||
2467 | else | ||
2468 | #endif | ||
2469 | { | ||
2470 | add_to_list_l(&cores[core].running, thread); | ||
2471 | } | ||
2472 | |||
2473 | UNLOCK_THREAD_SET_STATE(thread, STATE_RUNNING); | ||
2474 | #if NUM_CORES > 1 | ||
2475 | set_irq_level(oldlevel); | ||
2476 | #endif | ||
2477 | return; | ||
2478 | } | ||
2479 | 2823 | ||
2480 | #if NUM_CORES > 1 | 2824 | UNLOCK_THREAD(thread); |
2481 | UNLOCK_THREAD(thread, state); | ||
2482 | set_irq_level(oldlevel); | 2825 | set_irq_level(oldlevel); |
2483 | #endif | ||
2484 | } | 2826 | } |
2485 | 2827 | ||
2486 | /*--------------------------------------------------------------------------- | 2828 | /*--------------------------------------------------------------------------- |
@@ -2501,21 +2843,31 @@ unsigned int switch_core(unsigned int new_core) | |||
2501 | { | 2843 | { |
2502 | const unsigned int core = CURRENT_CORE; | 2844 | const unsigned int core = CURRENT_CORE; |
2503 | struct thread_entry *current = cores[core].running; | 2845 | struct thread_entry *current = cores[core].running; |
2504 | struct thread_entry *w; | ||
2505 | int oldlevel; | ||
2506 | |||
2507 | /* Interrupts can access the lists that will be used - disable them */ | ||
2508 | unsigned state = GET_THREAD_STATE(current); | ||
2509 | 2846 | ||
2510 | if (core == new_core) | 2847 | if (core == new_core) |
2511 | { | 2848 | { |
2512 | /* No change - just unlock everything and return same core */ | 2849 | /* No change - just return same core */ |
2513 | UNLOCK_THREAD(current, state); | ||
2514 | return core; | 2850 | return core; |
2515 | } | 2851 | } |
2516 | 2852 | ||
2853 | int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); | ||
2854 | LOCK_THREAD(current); | ||
2855 | |||
2856 | if (current->name == THREAD_DESTRUCT) | ||
2857 | { | ||
2858 | /* Thread being killed - deactivate and let process complete */ | ||
2859 | UNLOCK_THREAD(current); | ||
2860 | set_irq_level(oldlevel); | ||
2861 | thread_wait(current); | ||
2862 | /* Should never be reached */ | ||
2863 | THREAD_PANICF("switch_core->D:*R", current); | ||
2864 | } | ||
2865 | |||
2517 | /* Get us off the running list for the current core */ | 2866 | /* Get us off the running list for the current core */ |
2867 | RTR_LOCK(core); | ||
2518 | remove_from_list_l(&cores[core].running, current); | 2868 | remove_from_list_l(&cores[core].running, current); |
2869 | rtr_subtract_entry(core, current->priority); | ||
2870 | RTR_UNLOCK(core); | ||
2519 | 2871 | ||
2520 | /* Stash return value (old core) in a safe place */ | 2872 | /* Stash return value (old core) in a safe place */ |
2521 | current->retval = core; | 2873 | current->retval = core; |
@@ -2532,39 +2884,31 @@ unsigned int switch_core(unsigned int new_core) | |||
2532 | 2884 | ||
2533 | /* Do not use core_schedule_wakeup here since this will result in | 2885 | /* Do not use core_schedule_wakeup here since this will result in |
2534 | * the thread starting to run on the other core before being finished on | 2886 | * the thread starting to run on the other core before being finished on |
2535 | * this one. Delay the wakeup list unlock to keep the other core stuck | 2887 | * this one. Delay the list unlock to keep the other core stuck |
2536 | * until this thread is ready. */ | 2888 | * until this thread is ready. */ |
2537 | oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); | 2889 | RTR_LOCK(new_core); |
2538 | w = LOCK_LIST(&cores[new_core].waking); | 2890 | |
2539 | ADD_TO_LIST_L_SELECT(w, &cores[new_core].waking, current); | 2891 | rtr_add_entry(new_core, current->priority); |
2892 | add_to_list_l(&cores[new_core].running, current); | ||
2540 | 2893 | ||
2541 | /* Make a callback into device-specific code, unlock the wakeup list so | 2894 | /* Make a callback into device-specific code, unlock the wakeup list so |
2542 | * that execution may resume on the new core, unlock our slot and finally | 2895 | * that execution may resume on the new core, unlock our slot and finally |
2543 | * restore the interrupt level */ | 2896 | * restore the interrupt level */ |
2544 | cores[core].blk_ops.flags = TBOP_SWITCH_CORE | TBOP_UNLOCK_CURRENT | | 2897 | cores[core].blk_ops.flags = TBOP_SWITCH_CORE; |
2545 | TBOP_UNLOCK_LIST; | 2898 | cores[core].blk_ops.cl_p = &cores[new_core].rtr_cl; |
2546 | cores[core].blk_ops.list_p = &cores[new_core].waking; | 2899 | cores[core].block_task = current; |
2547 | #if CONFIG_CORELOCK == CORELOCK_SWAP | 2900 | |
2548 | cores[core].blk_ops.state = STATE_RUNNING; | 2901 | UNLOCK_THREAD(current); |
2549 | cores[core].blk_ops.list_v = w; | 2902 | |
2550 | #endif | 2903 | /* Alert other core to activity */ |
2904 | core_wake(new_core); | ||
2551 | 2905 | ||
2552 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
2553 | current->priority_x = HIGHEST_PRIORITY; | ||
2554 | cores[core].highest_priority = LOWEST_PRIORITY; | ||
2555 | #endif | ||
2556 | /* Do the stack switching, cache_maintenence and switch_thread call - | 2906 | /* Do the stack switching, cache_maintenence and switch_thread call - |
2557 | requires native code */ | 2907 | requires native code */ |
2558 | switch_thread_core(core, current); | 2908 | switch_thread_core(core, current); |
2559 | 2909 | ||
2560 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
2561 | current->priority_x = LOWEST_PRIORITY; | ||
2562 | cores[current->core].highest_priority = LOWEST_PRIORITY; | ||
2563 | #endif | ||
2564 | |||
2565 | /* Finally return the old core to caller */ | 2910 | /* Finally return the old core to caller */ |
2566 | return current->retval; | 2911 | return current->retval; |
2567 | (void)state; | ||
2568 | } | 2912 | } |
2569 | #endif /* NUM_CORES > 1 */ | 2913 | #endif /* NUM_CORES > 1 */ |
2570 | 2914 | ||
@@ -2578,12 +2922,11 @@ void init_threads(void) | |||
2578 | { | 2922 | { |
2579 | const unsigned int core = CURRENT_CORE; | 2923 | const unsigned int core = CURRENT_CORE; |
2580 | struct thread_entry *thread; | 2924 | struct thread_entry *thread; |
2581 | int slot; | ||
2582 | 2925 | ||
2583 | /* CPU will initialize first and then sleep */ | 2926 | /* CPU will initialize first and then sleep */ |
2584 | slot = find_empty_thread_slot(); | 2927 | thread = find_empty_thread_slot(); |
2585 | 2928 | ||
2586 | if (slot >= MAXTHREADS) | 2929 | if (thread == NULL) |
2587 | { | 2930 | { |
2588 | /* WTF? There really must be a slot available at this stage. | 2931 | /* WTF? There really must be a slot available at this stage. |
2589 | * This can fail if, for example, .bss isn't zero'ed out by the loader | 2932 | * This can fail if, for example, .bss isn't zero'ed out by the loader |
@@ -2592,33 +2935,29 @@ void init_threads(void) | |||
2592 | } | 2935 | } |
2593 | 2936 | ||
2594 | /* Initialize initially non-zero members of core */ | 2937 | /* Initialize initially non-zero members of core */ |
2595 | thread_queue_init(&cores[core].waking); | ||
2596 | cores[core].next_tmo_check = current_tick; /* Something not in the past */ | 2938 | cores[core].next_tmo_check = current_tick; /* Something not in the past */ |
2597 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
2598 | cores[core].highest_priority = LOWEST_PRIORITY; | ||
2599 | #endif | ||
2600 | 2939 | ||
2601 | /* Initialize initially non-zero members of slot */ | 2940 | /* Initialize initially non-zero members of slot */ |
2602 | thread = &threads[slot]; | 2941 | UNLOCK_THREAD(thread); /* No sync worries yet */ |
2603 | thread->name = main_thread_name; | 2942 | thread->name = main_thread_name; |
2604 | UNLOCK_THREAD_SET_STATE(thread, STATE_RUNNING); /* No sync worries yet */ | 2943 | thread->state = STATE_RUNNING; |
2605 | #if NUM_CORES > 1 | 2944 | IF_COP( thread->core = core; ) |
2606 | thread->core = core; | ||
2607 | #endif | ||
2608 | #ifdef HAVE_PRIORITY_SCHEDULING | 2945 | #ifdef HAVE_PRIORITY_SCHEDULING |
2946 | corelock_init(&cores[core].rtr_cl); | ||
2947 | thread->base_priority = PRIORITY_USER_INTERFACE; | ||
2948 | prio_add_entry(&thread->pdist, PRIORITY_USER_INTERFACE); | ||
2609 | thread->priority = PRIORITY_USER_INTERFACE; | 2949 | thread->priority = PRIORITY_USER_INTERFACE; |
2610 | thread->priority_x = LOWEST_PRIORITY; | 2950 | rtr_add_entry(core, PRIORITY_USER_INTERFACE); |
2611 | #endif | ||
2612 | #if CONFIG_CORELOCK == SW_CORELOCK | ||
2613 | corelock_init(&thread->cl); | ||
2614 | #endif | 2951 | #endif |
2952 | corelock_init(&thread->waiter_cl); | ||
2953 | corelock_init(&thread->slot_cl); | ||
2615 | 2954 | ||
2616 | add_to_list_l(&cores[core].running, thread); | 2955 | add_to_list_l(&cores[core].running, thread); |
2617 | 2956 | ||
2618 | if (core == CPU) | 2957 | if (core == CPU) |
2619 | { | 2958 | { |
2620 | thread->stack = stackbegin; | 2959 | thread->stack = stackbegin; |
2621 | thread->stack_size = (int)stackend - (int)stackbegin; | 2960 | thread->stack_size = (uintptr_t)stackend - (uintptr_t)stackbegin; |
2622 | #if NUM_CORES > 1 /* This code path will not be run on single core targets */ | 2961 | #if NUM_CORES > 1 /* This code path will not be run on single core targets */ |
2623 | /* TODO: HAL interface for this */ | 2962 | /* TODO: HAL interface for this */ |
2624 | /* Wake up coprocessor and let it initialize kernel and threads */ | 2963 | /* Wake up coprocessor and let it initialize kernel and threads */ |
@@ -2638,22 +2977,21 @@ void init_threads(void) | |||
2638 | /* Get COP safely primed inside switch_thread where it will remain | 2977 | /* Get COP safely primed inside switch_thread where it will remain |
2639 | * until a thread actually exists on it */ | 2978 | * until a thread actually exists on it */ |
2640 | CPU_CTL = PROC_WAKE; | 2979 | CPU_CTL = PROC_WAKE; |
2641 | remove_thread(NULL); | 2980 | thread_exit(); |
2642 | #endif /* NUM_CORES */ | 2981 | #endif /* NUM_CORES */ |
2643 | } | 2982 | } |
2644 | } | 2983 | } |
2645 | 2984 | ||
2646 | /*--------------------------------------------------------------------------- | 2985 | /* Shared stack scan helper for thread_stack_usage and idle_stack_usage */ |
2647 | * Returns the maximum percentage of stack a thread ever used while running. | 2986 | #if NUM_CORES == 1 |
2648 | * NOTE: Some large buffer allocations that don't use enough the buffer to | 2987 | static inline int stack_usage(uintptr_t *stackptr, size_t stack_size) |
2649 | * overwrite stackptr[0] will not be seen. | 2988 | #else |
2650 | *--------------------------------------------------------------------------- | 2989 | static int stack_usage(uintptr_t *stackptr, size_t stack_size) |
2651 | */ | 2990 | #endif |
2652 | int thread_stack_usage(const struct thread_entry *thread) | ||
2653 | { | 2991 | { |
2654 | unsigned int *stackptr = thread->stack; | 2992 | unsigned int stack_words = stack_size / sizeof (uintptr_t); |
2655 | int stack_words = thread->stack_size / sizeof (int); | 2993 | unsigned int i; |
2656 | int i, usage = 0; | 2994 | int usage = 0; |
2657 | 2995 | ||
2658 | for (i = 0; i < stack_words; i++) | 2996 | for (i = 0; i < stack_words; i++) |
2659 | { | 2997 | { |
@@ -2667,6 +3005,17 @@ int thread_stack_usage(const struct thread_entry *thread) | |||
2667 | return usage; | 3005 | return usage; |
2668 | } | 3006 | } |
2669 | 3007 | ||
3008 | /*--------------------------------------------------------------------------- | ||
3009 | * Returns the maximum percentage of stack a thread ever used while running. | ||
3010 | * NOTE: Some large buffer allocations that don't use enough the buffer to | ||
3011 | * overwrite stackptr[0] will not be seen. | ||
3012 | *--------------------------------------------------------------------------- | ||
3013 | */ | ||
3014 | int thread_stack_usage(const struct thread_entry *thread) | ||
3015 | { | ||
3016 | return stack_usage(thread->stack, thread->stack_size); | ||
3017 | } | ||
3018 | |||
2670 | #if NUM_CORES > 1 | 3019 | #if NUM_CORES > 1 |
2671 | /*--------------------------------------------------------------------------- | 3020 | /*--------------------------------------------------------------------------- |
2672 | * Returns the maximum percentage of the core's idle stack ever used during | 3021 | * Returns the maximum percentage of the core's idle stack ever used during |
@@ -2675,19 +3024,7 @@ int thread_stack_usage(const struct thread_entry *thread) | |||
2675 | */ | 3024 | */ |
2676 | int idle_stack_usage(unsigned int core) | 3025 | int idle_stack_usage(unsigned int core) |
2677 | { | 3026 | { |
2678 | unsigned int *stackptr = idle_stacks[core]; | 3027 | return stack_usage(idle_stacks[core], IDLE_STACK_SIZE); |
2679 | int i, usage = 0; | ||
2680 | |||
2681 | for (i = 0; i < IDLE_STACK_WORDS; i++) | ||
2682 | { | ||
2683 | if (stackptr[i] != DEADBEEF) | ||
2684 | { | ||
2685 | usage = ((IDLE_STACK_WORDS - i) * 100) / IDLE_STACK_WORDS; | ||
2686 | break; | ||
2687 | } | ||
2688 | } | ||
2689 | |||
2690 | return usage; | ||
2691 | } | 3028 | } |
2692 | #endif | 3029 | #endif |
2693 | 3030 | ||