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 #define SLEEP_HASH_SIZE 17
63 static struct ao_list run_queue;
64 static struct ao_list alarm_queue;
65 static struct ao_list ao_sleep_queue[SLEEP_HASH_SIZE];
68 _ao_task_to_run_queue(struct ao_task *task)
71 ao_list_del(&task->queue);
72 ao_list_append(&task->queue, &run_queue);
75 static struct ao_list *
76 ao_task_sleep_queue(void *wchan)
78 return &ao_sleep_queue[(uintptr_t) wchan % SLEEP_HASH_SIZE];
82 _ao_task_to_sleep_queue(struct ao_task *task, void *wchan)
85 ao_list_del(&task->queue);
86 ao_list_append(&task->queue, ao_task_sleep_queue(wchan));
91 ao_task_validate_alarm_queue(void)
93 struct ao_task *alarm, *prev = NULL;
96 if (ao_list_is_empty(&alarm_queue))
98 ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) {
100 if ((int16_t) (alarm->alarm - prev->alarm) < 0) {
106 for (i = 0; i < ao_num_tasks; i++) {
109 if (ao_list_is_empty(&alarm->alarm_queue))
112 if (!ao_list_is_empty(&alarm->alarm_queue))
116 if (ao_task_alarm_tick != ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm)
120 #define ao_task_validate_alarm_queue()
123 AO_TICK_TYPE ao_task_alarm_tick;
126 _ao_task_to_alarm_queue(struct ao_task *task)
128 struct ao_task *alarm;
130 ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) {
131 if ((int16_t) (alarm->alarm - task->alarm) >= 0) {
132 ao_list_insert(&task->alarm_queue, alarm->alarm_queue.prev);
133 ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
134 ao_task_validate_alarm_queue();
138 ao_list_append(&task->alarm_queue, &alarm_queue);
139 ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
140 ao_task_validate_alarm_queue();
144 _ao_task_from_alarm_queue(struct ao_task *task)
147 ao_list_del(&task->alarm_queue);
148 if (ao_list_is_empty(&alarm_queue))
149 ao_task_alarm_tick = 0;
151 ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
152 ao_task_validate_alarm_queue();
156 ao_task_init_queue(struct ao_task *task)
158 ao_list_init(&task->queue);
159 ao_list_init(&task->alarm_queue);
163 ao_task_exit_queue(struct ao_task *task)
166 ao_list_del(&task->queue);
167 ao_list_del(&task->alarm_queue);
171 ao_task_alarm(AO_TICK_TYPE tick)
173 struct ao_task *alarm, *next;
176 ao_list_for_each_entry_safe(alarm, next, &alarm_queue, struct ao_task, alarm_queue) {
177 if ((AO_TICK_SIGNED) (tick - alarm->alarm) < 0)
180 _ao_task_from_alarm_queue(alarm);
181 _ao_task_to_run_queue(alarm);
189 ao_list_init(&run_queue);
190 ao_list_init(&alarm_queue);
191 ao_task_alarm_tick = 0;
192 for (i = 0; i < SLEEP_HASH_SIZE; i++)
193 ao_list_init(&ao_sleep_queue[i]);
198 ao_task_validate_queue(struct ao_task *task)
203 struct ao_list *queue;
205 flags = ao_arch_irqsave();
207 queue = ao_task_sleep_queue(task->wchan);
213 ao_list_for_each_entry(m, queue, struct ao_task, queue) {
219 ao_arch_irqrestore(flags);
224 ao_task_validate_alarm(struct ao_task *task)
230 flags = ao_arch_irqsave();
231 if (task->alarm == 0)
233 ao_list_for_each_entry(m, &alarm_queue, struct ao_task, alarm_queue) {
238 if ((int16_t) (m->alarm - task->alarm) > 0)
241 if ((int16_t) (task->alarm - m->alarm) > 0)
246 ao_arch_irqrestore(flags);
252 ao_task_validate(void)
255 struct ao_task *task;
258 for (i = 0; i < ao_num_tasks; i++) {
260 ret = ao_task_validate_queue(task);
263 printf ("sleeping task not on sleep queue %s %08x\n",
264 task->name, task->wchan);
266 printf ("running task not on run queue %s\n",
269 ret = ao_task_validate_alarm(task);
272 printf ("alarm task not on alarm queue %s %d\n",
273 task->name, task->alarm);
275 printf ("alarm queue has sooner entries after %s %d\n",
276 task->name, task->alarm);
278 printf ("alarm queue has later entries before %s %d\n",
279 task->name, task->alarm);
286 ao_stack_top(struct ao_task *task)
288 uint8_t *top = &task->stack8[AO_STACK_SIZE];
290 /* Subtract off the TLS space, but keep the resulting
291 * stack 8-byte aligned
294 return top - ((_tls_size() + 7) & ~3);
301 ao_add_task(struct ao_task * task, void (*task_func)(void), const char *name)
305 if (ao_num_tasks == AO_NUM_TASKS)
306 ao_panic(AO_PANIC_NO_TASK);
307 for (task_id = 1; task_id != 0; task_id++) {
308 for (t = 0; t < ao_num_tasks; t++)
309 if (ao_tasks[t]->task_id == task_id)
311 if (t == ao_num_tasks)
314 task->task_id = task_id;
318 * Construct a stack frame so that it will 'return'
319 * to the start of the task
321 uint32_t *sp = ao_stack_top(task);
325 ao_arch_init_stack(task, sp, task_func);
326 ao_task_init_queue(task);
328 _ao_task_to_run_queue(task);
329 ao_tasks[ao_num_tasks] = task;
334 uint8_t ao_task_minimize_latency;
336 /* Task switching function. */
342 #if HAS_SAMPLE_PROFILE
343 AO_TICK_TYPE tick = ao_sample_profile_timer_value();
344 AO_TICK_TYPE run = tick - ao_cur_task->start;
345 if (run > ao_cur_task->max_run)
346 ao_cur_task->max_run = run;
347 ++ao_cur_task->yields;
350 ao_arch_save_stack();
354 ao_arch_block_interrupts();
359 /* Find a task to run. If there isn't any runnable task,
360 * this loop will run forever, which is just fine
362 /* If the current task is running, move it to the
363 * end of the queue to allow other tasks a chance
365 if (ao_cur_task && ao_cur_task->wchan == NULL)
366 _ao_task_to_run_queue(ao_cur_task);
368 ao_arch_memory_barrier();
369 if (!ao_list_is_empty(&run_queue))
371 /* Wait for interrupts when there's nothing ready */
372 if (ao_task_minimize_latency) {
373 ao_arch_release_interrupts();
374 ao_arch_block_interrupts();
376 ao_arch_wait_interrupt();
378 ao_cur_task = ao_list_first_entry(&run_queue, struct ao_task, queue);
379 #if HAS_SAMPLE_PROFILE
380 ao_cur_task->start = ao_sample_profile_timer_value();
383 ao_mpu_stack_guard(ao_cur_task->stack);
389 _set_tls(ao_stack_top(ao_cur_task));
391 ao_arch_restore_stack();
395 ao_sleep(void *wchan)
398 ao_cur_task->wchan = wchan;
399 _ao_task_to_sleep_queue(ao_cur_task, wchan);
402 if (ao_cur_task->wchan) {
403 ao_cur_task->wchan = NULL;
404 ao_cur_task->alarm = 0;
411 ao_wakeup(void *wchan)
413 ao_validate_cur_stack();
414 struct ao_task *sleep, *next;
415 struct ao_list *sleep_queue;
418 if (ao_num_tasks == 0)
420 sleep_queue = ao_task_sleep_queue(wchan);
421 flags = ao_arch_irqsave();
422 ao_list_for_each_entry_safe(sleep, next, sleep_queue, struct ao_task, queue) {
423 if (sleep->wchan == wchan) {
425 _ao_task_to_run_queue(sleep);
428 ao_arch_irqrestore(flags);
433 ao_sleep_for(void *wchan, AO_TICK_TYPE timeout)
438 /* Make sure we sleep *at least* delay ticks, which means adding
439 * one to account for the fact that we may be close to the next tick
441 if (!(ao_cur_task->alarm = ao_time() + timeout + 1))
442 ao_cur_task->alarm = 1;
443 _ao_task_to_alarm_queue(ao_cur_task);
446 ret = ao_sleep(wchan);
449 ao_cur_task->alarm = 0;
450 _ao_task_from_alarm_queue(ao_cur_task);
456 static uint8_t ao_forever;
459 ao_delay(AO_TICK_TYPE ticks)
463 ao_sleep_for(&ao_forever, ticks);
470 ao_arch_block_interrupts();
471 for (i = 0; i < ao_num_tasks; i++)
472 if (ao_tasks[i] == ao_cur_task)
474 ao_task_exit_queue(ao_cur_task);
475 /* Remove task from list */
477 for (; i < ao_num_tasks; i++)
478 ao_tasks[i] = ao_tasks[i+1];
481 __builtin_unreachable();
489 struct ao_task *task;
490 AO_TICK_TYPE now = ao_time();
492 for (i = 0; i < ao_num_tasks; i++) {
494 printf("%2d: wchan %08x alarm %5d %s\n",
497 task->alarm ? (int16_t) (task->alarm - now) : 9999,
507 ao_start_scheduler(void)
510 #if HAS_ARCH_START_SCHEDULER
511 ao_arch_start_scheduler();
514 __builtin_unreachable();