2 * Copyright © 2009 Keith Packard <keithp@keithp.com>
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * General Public License for more details.
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free Software Foundation, Inc.,
16 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
21 #if HAS_SAMPLE_PROFILE
22 #include <ao_sample_profile.h>
31 struct ao_task * ao_tasks[AO_NUM_TASKS];
33 struct ao_task *ao_cur_task;
35 #ifdef ao_arch_task_globals
39 #define AO_CHECK_STACK 0
42 static uint8_t in_yield;
44 static inline void ao_check_stack(void) {
46 if (!in_yield && ao_cur_task && &q < &ao_cur_task->stack[0])
47 ao_panic(AO_PANIC_STACK);
50 #define ao_check_stack()
54 #define ao_task_irq_check() ao_arch_irq_check()
56 #define ao_task_irq_check()
59 #ifndef SLEEP_HASH_SIZE
60 #ifdef __ARM_FEATURE_IDIV__
61 #define SLEEP_HASH_SIZE 17
63 #define SLEEP_HASH_SIZE 16
67 #if SLEEP_HASH_SIZE & (SLEEP_HASH_SIZE - 1)
68 #define SLEEP_HASH_SHIFT 0
70 #define SLEEP_HASH_SHIFT 2
73 static struct ao_list run_queue;
74 static struct ao_list alarm_queue;
75 static struct ao_list ao_sleep_queue[SLEEP_HASH_SIZE];
78 _ao_task_to_run_queue(struct ao_task *task)
81 ao_list_del(&task->queue);
82 ao_list_append(&task->queue, &run_queue);
85 static struct ao_list *
86 ao_task_sleep_queue(void *wchan)
88 return &ao_sleep_queue[(((uintptr_t) wchan) >> SLEEP_HASH_SHIFT) % SLEEP_HASH_SIZE];
92 _ao_task_to_sleep_queue(struct ao_task *task, void *wchan)
95 ao_list_del(&task->queue);
96 ao_list_append(&task->queue, ao_task_sleep_queue(wchan));
101 ao_task_validate_alarm_queue(void)
103 struct ao_task *alarm, *prev = NULL;
106 if (ao_list_is_empty(&alarm_queue))
108 ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) {
110 if ((int16_t) (alarm->alarm - prev->alarm) < 0) {
116 for (i = 0; i < ao_num_tasks; i++) {
119 if (ao_list_is_empty(&alarm->alarm_queue))
122 if (!ao_list_is_empty(&alarm->alarm_queue))
126 if (ao_task_alarm_tick != ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm)
130 #define ao_task_validate_alarm_queue()
133 AO_TICK_TYPE ao_task_alarm_tick;
136 _ao_task_to_alarm_queue(struct ao_task *task)
138 struct ao_task *alarm;
140 ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) {
141 if ((int16_t) (alarm->alarm - task->alarm) >= 0) {
142 ao_list_insert(&task->alarm_queue, alarm->alarm_queue.prev);
143 ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
144 ao_task_validate_alarm_queue();
148 ao_list_append(&task->alarm_queue, &alarm_queue);
149 ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
150 ao_task_validate_alarm_queue();
154 _ao_task_from_alarm_queue(struct ao_task *task)
157 ao_list_del(&task->alarm_queue);
158 if (ao_list_is_empty(&alarm_queue))
159 ao_task_alarm_tick = 0;
161 ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
162 ao_task_validate_alarm_queue();
166 ao_task_init_queue(struct ao_task *task)
168 ao_list_init(&task->queue);
169 ao_list_init(&task->alarm_queue);
173 ao_task_exit_queue(struct ao_task *task)
176 ao_list_del(&task->queue);
177 ao_list_del(&task->alarm_queue);
181 ao_task_alarm(AO_TICK_TYPE tick)
183 struct ao_task *alarm, *next;
186 ao_list_for_each_entry_safe(alarm, next, &alarm_queue, struct ao_task, alarm_queue) {
187 if ((AO_TICK_SIGNED) (tick - alarm->alarm) < 0)
190 _ao_task_from_alarm_queue(alarm);
191 _ao_task_to_run_queue(alarm);
199 ao_list_init(&run_queue);
200 ao_list_init(&alarm_queue);
201 ao_task_alarm_tick = 0;
202 for (i = 0; i < SLEEP_HASH_SIZE; i++)
203 ao_list_init(&ao_sleep_queue[i]);
208 ao_task_validate_queue(struct ao_task *task)
213 struct ao_list *queue;
215 flags = ao_arch_irqsave();
217 queue = ao_task_sleep_queue(task->wchan);
223 ao_list_for_each_entry(m, queue, struct ao_task, queue) {
229 ao_arch_irqrestore(flags);
234 ao_task_validate_alarm(struct ao_task *task)
240 flags = ao_arch_irqsave();
241 if (task->alarm == 0)
243 ao_list_for_each_entry(m, &alarm_queue, struct ao_task, alarm_queue) {
248 if ((int16_t) (m->alarm - task->alarm) > 0)
251 if ((int16_t) (task->alarm - m->alarm) > 0)
256 ao_arch_irqrestore(flags);
262 ao_task_validate(void)
265 struct ao_task *task;
268 for (i = 0; i < ao_num_tasks; i++) {
270 ret = ao_task_validate_queue(task);
273 printf ("sleeping task not on sleep queue %s %08x\n",
274 task->name, task->wchan);
276 printf ("running task not on run queue %s\n",
279 ret = ao_task_validate_alarm(task);
282 printf ("alarm task not on alarm queue %s %d\n",
283 task->name, task->alarm);
285 printf ("alarm queue has sooner entries after %s %d\n",
286 task->name, task->alarm);
288 printf ("alarm queue has later entries before %s %d\n",
289 task->name, task->alarm);
296 ao_stack_top(struct ao_task *task)
298 uint8_t *top = &task->stack8[AO_STACK_SIZE];
300 /* Subtract off the TLS space, but keep the resulting
301 * stack 8-byte aligned
304 return top - ((_tls_size() + 7) & ~3);
311 ao_add_task(struct ao_task * task, void (*task_func)(void), const char *name)
315 if (ao_num_tasks == AO_NUM_TASKS)
316 ao_panic(AO_PANIC_NO_TASK);
317 for (task_id = 1; task_id != 0; task_id++) {
318 for (t = 0; t < ao_num_tasks; t++)
319 if (ao_tasks[t]->task_id == task_id)
321 if (t == ao_num_tasks)
324 task->task_id = task_id;
328 * Construct a stack frame so that it will 'return'
329 * to the start of the task
331 uint32_t *sp = ao_stack_top(task);
335 ao_arch_init_stack(task, sp, task_func);
336 ao_task_init_queue(task);
338 _ao_task_to_run_queue(task);
339 ao_tasks[ao_num_tasks] = task;
344 uint8_t ao_task_minimize_latency;
346 /* Task switching function. */
352 #if HAS_SAMPLE_PROFILE
353 AO_TICK_TYPE tick = ao_sample_profile_timer_value();
354 AO_TICK_TYPE run = tick - ao_cur_task->start;
355 if (run > ao_cur_task->max_run)
356 ao_cur_task->max_run = run;
357 ++ao_cur_task->yields;
360 ao_arch_save_stack();
364 ao_arch_block_interrupts();
369 /* Find a task to run. If there isn't any runnable task,
370 * this loop will run forever, which is just fine
372 /* If the current task is running, move it to the
373 * end of the queue to allow other tasks a chance
375 if (ao_cur_task && ao_cur_task->wchan == NULL)
376 _ao_task_to_run_queue(ao_cur_task);
378 ao_arch_memory_barrier();
379 if (!ao_list_is_empty(&run_queue))
381 /* Wait for interrupts when there's nothing ready */
382 if (ao_task_minimize_latency) {
383 ao_arch_release_interrupts();
384 ao_arch_block_interrupts();
386 ao_arch_wait_interrupt();
388 ao_cur_task = ao_list_first_entry(&run_queue, struct ao_task, queue);
389 #if HAS_SAMPLE_PROFILE
390 ao_cur_task->start = ao_sample_profile_timer_value();
393 ao_mpu_stack_guard(ao_cur_task->stack);
399 _set_tls(ao_stack_top(ao_cur_task));
401 ao_arch_restore_stack();
405 ao_sleep(void *wchan)
408 ao_cur_task->wchan = wchan;
409 _ao_task_to_sleep_queue(ao_cur_task, wchan);
412 if (ao_cur_task->wchan) {
413 ao_cur_task->wchan = NULL;
414 ao_cur_task->alarm = 0;
421 ao_wakeup(void *wchan)
423 ao_validate_cur_stack();
424 struct ao_task *sleep, *next;
425 struct ao_list *sleep_queue;
428 if (ao_num_tasks == 0)
430 sleep_queue = ao_task_sleep_queue(wchan);
431 flags = ao_arch_irqsave();
432 ao_list_for_each_entry_safe(sleep, next, sleep_queue, struct ao_task, queue) {
433 if (sleep->wchan == wchan) {
435 _ao_task_to_run_queue(sleep);
438 ao_arch_irqrestore(flags);
443 ao_sleep_for(void *wchan, AO_TICK_TYPE timeout)
448 /* Make sure we sleep *at least* delay ticks, which means adding
449 * one to account for the fact that we may be close to the next tick
451 if (!(ao_cur_task->alarm = ao_time() + timeout + 1))
452 ao_cur_task->alarm = 1;
453 _ao_task_to_alarm_queue(ao_cur_task);
456 ret = ao_sleep(wchan);
459 ao_cur_task->alarm = 0;
460 _ao_task_from_alarm_queue(ao_cur_task);
466 static uint8_t ao_forever;
469 ao_delay(AO_TICK_TYPE ticks)
473 ao_sleep_for(&ao_forever, ticks);
480 ao_arch_block_interrupts();
481 for (i = 0; i < ao_num_tasks; i++)
482 if (ao_tasks[i] == ao_cur_task)
484 ao_task_exit_queue(ao_cur_task);
485 /* Remove task from list */
487 for (; i < ao_num_tasks; i++)
488 ao_tasks[i] = ao_tasks[i+1];
491 __builtin_unreachable();
499 struct ao_task *task;
500 AO_TICK_TYPE now = ao_time();
502 for (i = 0; i < ao_num_tasks; i++) {
504 printf("%2d: wchan %08x alarm %5d %s\n",
507 task->alarm ? (int16_t) (task->alarm - now) : 9999,
517 ao_start_scheduler(void)
520 #if HAS_ARCH_START_SCHEDULER
521 ao_arch_start_scheduler();
524 __builtin_unreachable();