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);
310 #define AO_STACK_CANARY_VALUE 0xbaadf00dU
313 ao_add_task(struct ao_task * task, void (*task_func)(void), const char *name)
317 if (ao_num_tasks == AO_NUM_TASKS)
318 ao_panic(AO_PANIC_NO_TASK);
319 for (task_id = 1; task_id != 0; task_id++) {
320 for (t = 0; t < ao_num_tasks; t++)
321 if (ao_tasks[t]->task_id == task_id)
323 if (t == ao_num_tasks)
326 task->task_id = task_id;
329 #ifdef AO_STACK_CANARY
330 task->bottom_canary = AO_STACK_CANARY_VALUE;
331 task->top_canary = AO_STACK_CANARY_VALUE;
334 * Construct a stack frame so that it will 'return'
335 * to the start of the task
337 uint32_t *sp = ao_stack_top(task);
341 ao_arch_init_stack(task, sp, task_func);
342 ao_task_init_queue(task);
344 _ao_task_to_run_queue(task);
345 ao_tasks[ao_num_tasks] = task;
350 #ifdef AO_STACK_CANARY
352 ao_check_stack_canary(void)
354 if (ao_cur_task->bottom_canary != AO_STACK_CANARY_VALUE)
355 ao_panic(AO_PANIC_STACK);
356 if (ao_cur_task->top_canary != AO_STACK_CANARY_VALUE)
357 ao_panic(AO_PANIC_STACK);
360 #define ao_check_stack_canary()
363 uint8_t ao_task_minimize_latency;
365 /* Task switching function. */
371 #if HAS_SAMPLE_PROFILE
372 AO_TICK_TYPE tick = ao_sample_profile_timer_value();
373 AO_TICK_TYPE run = tick - ao_cur_task->start;
374 if (run > ao_cur_task->max_run)
375 ao_cur_task->max_run = run;
376 ++ao_cur_task->yields;
378 ao_check_stack_canary();
380 ao_arch_save_stack();
384 ao_arch_block_interrupts();
389 /* Find a task to run. If there isn't any runnable task,
390 * this loop will run forever, which is just fine
392 /* If the current task is running, move it to the
393 * end of the queue to allow other tasks a chance
395 if (ao_cur_task && ao_cur_task->wchan == NULL)
396 _ao_task_to_run_queue(ao_cur_task);
398 ao_arch_memory_barrier();
399 if (!ao_list_is_empty(&run_queue))
401 /* Wait for interrupts when there's nothing ready */
402 if (ao_task_minimize_latency) {
403 ao_arch_release_interrupts();
404 ao_arch_block_interrupts();
406 ao_arch_wait_interrupt();
408 ao_cur_task = ao_list_first_entry(&run_queue, struct ao_task, queue);
409 #if HAS_SAMPLE_PROFILE
410 ao_cur_task->start = ao_sample_profile_timer_value();
413 ao_mpu_stack_guard(ao_cur_task->stack);
419 _set_tls(ao_stack_top(ao_cur_task));
421 ao_check_stack_canary();
422 ao_arch_restore_stack();
426 ao_sleep(void *wchan)
429 ao_cur_task->wchan = wchan;
430 _ao_task_to_sleep_queue(ao_cur_task, wchan);
433 if (ao_cur_task->wchan) {
434 ao_cur_task->wchan = NULL;
435 ao_cur_task->alarm = 0;
442 ao_wakeup(void *wchan)
444 ao_validate_cur_stack();
445 struct ao_task *sleep, *next;
446 struct ao_list *sleep_queue;
449 if (ao_num_tasks == 0)
451 sleep_queue = ao_task_sleep_queue(wchan);
452 flags = ao_arch_irqsave();
453 ao_list_for_each_entry_safe(sleep, next, sleep_queue, struct ao_task, queue) {
454 if (sleep->wchan == wchan) {
456 _ao_task_to_run_queue(sleep);
459 ao_arch_irqrestore(flags);
464 ao_sleep_for(void *wchan, AO_TICK_TYPE timeout)
469 /* Make sure we sleep *at least* delay ticks, which means adding
470 * one to account for the fact that we may be close to the next tick
472 if (!(ao_cur_task->alarm = ao_time() + timeout + 1))
473 ao_cur_task->alarm = 1;
474 _ao_task_to_alarm_queue(ao_cur_task);
477 ret = ao_sleep(wchan);
480 ao_cur_task->alarm = 0;
481 _ao_task_from_alarm_queue(ao_cur_task);
487 static uint8_t ao_forever;
490 ao_delay(AO_TICK_TYPE ticks)
494 ao_sleep_for(&ao_forever, ticks);
501 ao_arch_block_interrupts();
502 for (i = 0; i < ao_num_tasks; i++)
503 if (ao_tasks[i] == ao_cur_task)
505 ao_task_exit_queue(ao_cur_task);
506 /* Remove task from list */
508 for (; i < ao_num_tasks; i++)
509 ao_tasks[i] = ao_tasks[i+1];
512 __builtin_unreachable();
520 struct ao_task *task;
521 AO_TICK_TYPE now = ao_time();
523 for (i = 0; i < ao_num_tasks; i++) {
525 printf("%2d: wchan %08x alarm %5d %s\n",
528 task->alarm ? (int16_t) (task->alarm - now) : 9999,
538 ao_start_scheduler(void)
541 #if HAS_ARCH_START_SCHEDULER
542 ao_arch_start_scheduler();
545 __builtin_unreachable();