diff options
Diffstat (limited to 'firmware/thread.c')
| -rw-r--r-- | firmware/thread.c | 43 |
1 files changed, 26 insertions, 17 deletions
diff --git a/firmware/thread.c b/firmware/thread.c index 614286c422..8022d94862 100644 --- a/firmware/thread.c +++ b/firmware/thread.c @@ -39,11 +39,6 @@ static unsigned short highest_priority IBSS_ATTR; static int boosted_threads IBSS_ATTR; #endif -#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME -#define STAY_IRQ_LEVEL -1 -static int switch_to_irq_level = STAY_IRQ_LEVEL; -#endif - /* Define to enable additional checks for blocking violations etc. */ #define THREAD_EXTRA_CHECKS @@ -136,11 +131,11 @@ static inline void load_context(const void* addr) "movem.l (%0),%%d0/%%d2-%%d7/%%a2-%%a7 \n" /* Load context */ "move.l %%d0,%%macsr \n" "move.l (52,%0),%%d0 \n" /* Get start address */ - "beq.b .running \n" /* NULL -> already running */ + "beq.b 1f \n" /* NULL -> already running */ "clr.l (52,%0) \n" /* Clear start address.. */ "move.l %%d0,%0 \n" "jmp (%0) \n" /* ..and start the thread */ - ".running: \n" + "1: \n" : : "a" (addr) : "d0" /* only! */ ); } @@ -422,10 +417,10 @@ void switch_thread(bool save_context, struct thread_entry **blocked_list) /* This has to be done after the scheduler is finished with the blocked_list pointer so that an IRQ can't kill us by attempting a wake but before attempting any core sleep. */ - if (switch_to_irq_level != STAY_IRQ_LEVEL) + if (cores[CURRENT_CORE].switch_to_irq_level != STAY_IRQ_LEVEL) { - int level = switch_to_irq_level; - switch_to_irq_level = STAY_IRQ_LEVEL; + int level = cores[CURRENT_CORE].switch_to_irq_level; + cores[CURRENT_CORE].switch_to_irq_level = STAY_IRQ_LEVEL; set_irq_level(level); } #endif @@ -442,13 +437,14 @@ void switch_thread(bool save_context, struct thread_entry **blocked_list) for (;;) { int priority = cores[CURRENT_CORE].running->priority; - + if (priority < highest_priority) highest_priority = priority; - + if (priority == highest_priority || (current_tick - cores[CURRENT_CORE].running->last_run > - priority * 8)) + priority * 8) || + cores[CURRENT_CORE].running->priority_x != 0) break; cores[CURRENT_CORE].running = cores[CURRENT_CORE].running->next; @@ -567,7 +563,7 @@ void block_thread_w_tmo(struct thread_entry **list, int timeout) #if defined(HAVE_EXTENDED_MESSAGING_AND_NAME) && !defined(SIMULATOR) void set_irq_level_and_block_thread(struct thread_entry **list, int level) { - switch_to_irq_level = level; + cores[CURRENT_CORE].switch_to_irq_level = level; block_thread(list); } @@ -575,7 +571,7 @@ void set_irq_level_and_block_thread(struct thread_entry **list, int level) void set_irq_level_and_block_thread_w_tmo(struct thread_entry **list, int timeout, int level) { - switch_to_irq_level = level; + cores[CURRENT_CORE].switch_to_irq_level = level; block_thread_w_tmo(list, timeout); } #endif @@ -688,6 +684,7 @@ struct thread_entry* thread->stack_size = stack_size; thread->statearg = 0; #ifdef HAVE_PRIORITY_SCHEDULING + thread->priority_x = 0; thread->priority = priority; highest_priority = 100; #endif @@ -759,7 +756,7 @@ int thread_set_priority(struct thread_entry *thread, int priority) if (thread == NULL) thread = cores[CURRENT_CORE].running; - + old_priority = thread->priority; thread->priority = priority; highest_priority = 100; @@ -774,7 +771,15 @@ int thread_get_priority(struct thread_entry *thread) return thread->priority; } -#endif + +void priority_yield(void) +{ + struct thread_entry *thread = cores[CURRENT_CORE].running; + thread->priority_x = 1; + switch_thread(true, NULL); + thread->priority_x = 0; +} +#endif /* HAVE_PRIORITY_SCHEDULING */ struct thread_entry * thread_get_current(void) { @@ -789,10 +794,14 @@ void init_threads(void) memset(cores, 0, sizeof cores); cores[core].sleeping = NULL; cores[core].running = NULL; +#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME + cores[core].switch_to_irq_level = STAY_IRQ_LEVEL; +#endif cores[core].threads[0].name = main_thread_name; cores[core].threads[0].statearg = 0; #ifdef HAVE_PRIORITY_SCHEDULING cores[core].threads[0].priority = PRIORITY_USER_INTERFACE; + cores[core].threads[0].priority_x = 0; highest_priority = 100; #endif #ifdef HAVE_SCHEDULER_BOOSTCTRL |