]> git.gag.com Git - fw/altos/blob - src/kernel/ao_task.c
altoslib: Protect has_monitor_battery from unset product
[fw/altos] / src / kernel / ao_task.c
1 /*
2  * Copyright © 2009 Keith Packard <keithp@keithp.com>
3  *
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.
8  *
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.
13  *
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.
17  */
18
19 #include <ao.h>
20 #include <ao_task.h>
21 #if HAS_SAMPLE_PROFILE
22 #include <ao_sample_profile.h>
23 #endif
24 #if HAS_STACK_GUARD
25 #include <ao_mpu.h>
26 #endif
27 #include <picotls.h>
28
29 #define DEBUG   0
30
31 struct ao_task * ao_tasks[AO_NUM_TASKS];
32 uint8_t ao_num_tasks;
33 struct ao_task *ao_cur_task;
34
35 #ifdef ao_arch_task_globals
36 ao_arch_task_globals
37 #endif
38
39 #define AO_CHECK_STACK  0
40
41 #if AO_CHECK_STACK
42 static uint8_t  in_yield;
43
44 static inline void ao_check_stack(void) {
45         uint8_t q;
46         if (!in_yield && ao_cur_task && &q < &ao_cur_task->stack[0])
47                 ao_panic(AO_PANIC_STACK);
48 }
49 #else
50 #define ao_check_stack()
51 #endif
52
53 #if DEBUG
54 #define ao_task_irq_check()     ao_arch_irq_check()
55 #else
56 #define ao_task_irq_check()
57 #endif
58
59 #ifndef SLEEP_HASH_SIZE
60 #define SLEEP_HASH_SIZE 17
61 #endif
62
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];
66
67 static void
68 _ao_task_to_run_queue(struct ao_task *task)
69 {
70         ao_task_irq_check();
71         ao_list_del(&task->queue);
72         ao_list_append(&task->queue, &run_queue);
73 }
74
75 static struct ao_list *
76 ao_task_sleep_queue(void *wchan)
77 {
78         return &ao_sleep_queue[(uintptr_t) wchan % SLEEP_HASH_SIZE];
79 }
80
81 static void
82 _ao_task_to_sleep_queue(struct ao_task *task, void *wchan)
83 {
84         ao_task_irq_check();
85         ao_list_del(&task->queue);
86         ao_list_append(&task->queue, ao_task_sleep_queue(wchan));
87 }
88
89 #if DEBUG
90 static void
91 ao_task_validate_alarm_queue(void)
92 {
93         struct ao_task  *alarm, *prev = NULL;
94         int             i;
95
96         if (ao_list_is_empty(&alarm_queue))
97                 return;
98         ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) {
99                 if (prev) {
100                         if ((int16_t) (alarm->alarm - prev->alarm) < 0) {
101                                 ao_panic(1);
102                         }
103                 }
104                 prev = alarm;
105         }
106         for (i = 0; i < ao_num_tasks; i++) {
107                 alarm = ao_tasks[i];
108                 if (alarm->alarm) {
109                         if (ao_list_is_empty(&alarm->alarm_queue))
110                                 ao_panic(2);
111                 } else {
112                         if (!ao_list_is_empty(&alarm->alarm_queue))
113                                 ao_panic(3);
114                 }
115         }
116         if (ao_task_alarm_tick != ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm)
117                 ao_panic(4);
118 }
119 #else
120 #define ao_task_validate_alarm_queue()
121 #endif
122
123 AO_TICK_TYPE    ao_task_alarm_tick;
124
125 static void
126 _ao_task_to_alarm_queue(struct ao_task *task)
127 {
128         struct ao_task  *alarm;
129         ao_task_irq_check();
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();
135                         return;
136                 }
137         }
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();
141 }
142
143 static void
144 _ao_task_from_alarm_queue(struct ao_task *task)
145 {
146         ao_task_irq_check();
147         ao_list_del(&task->alarm_queue);
148         if (ao_list_is_empty(&alarm_queue))
149                 ao_task_alarm_tick = 0;
150         else
151                 ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
152         ao_task_validate_alarm_queue();
153 }
154
155 static void
156 ao_task_init_queue(struct ao_task *task)
157 {
158         ao_list_init(&task->queue);
159         ao_list_init(&task->alarm_queue);
160 }
161
162 static void
163 ao_task_exit_queue(struct ao_task *task)
164 {
165         ao_task_irq_check();
166         ao_list_del(&task->queue);
167         ao_list_del(&task->alarm_queue);
168 }
169
170 void
171 ao_task_alarm(AO_TICK_TYPE tick)
172 {
173         struct ao_task  *alarm, *next;
174
175         ao_arch_critical(
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)
178                                 break;
179                         alarm->alarm = 0;
180                         _ao_task_from_alarm_queue(alarm);
181                         _ao_task_to_run_queue(alarm);
182                 });
183 }
184
185 void
186 ao_task_init(void)
187 {
188         uint8_t i;
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]);
194 }
195
196 #if DEBUG
197 static uint8_t
198 ao_task_validate_queue(struct ao_task *task)
199 {
200         uint32_t flags;
201         struct ao_task *m;
202         uint8_t ret = 0;
203         struct ao_list *queue;
204
205         flags = ao_arch_irqsave();
206         if (task->wchan) {
207                 queue = ao_task_sleep_queue(task->wchan);
208                 ret |= 2;
209         } else {
210                 queue = &run_queue;
211                 ret |= 4;
212         }
213         ao_list_for_each_entry(m, queue, struct ao_task, queue) {
214                 if (m == task) {
215                         ret |= 1;
216                         break;
217                 }
218         }
219         ao_arch_irqrestore(flags);
220         return ret;
221 }
222
223 static uint8_t
224 ao_task_validate_alarm(struct ao_task *task)
225 {
226         uint32_t        flags;
227         struct ao_task  *m;
228         uint8_t         ret = 0;
229
230         flags = ao_arch_irqsave();
231         if (task->alarm == 0)
232                 return 0xff;
233         ao_list_for_each_entry(m, &alarm_queue, struct ao_task, alarm_queue) {
234                 if (m == task)
235                         ret |= 1;
236                 else {
237                         if (!(ret&1)) {
238                                 if ((int16_t) (m->alarm - task->alarm) > 0)
239                                         ret |= 2;
240                         } else {
241                                 if ((int16_t) (task->alarm - m->alarm) > 0)
242                                         ret |= 4;
243                         }
244                 }
245         }
246         ao_arch_irqrestore(flags);
247         return ret;
248 }
249
250
251 static void
252 ao_task_validate(void)
253 {
254         uint8_t         i;
255         struct ao_task  *task;
256         uint8_t         ret;
257
258         for (i = 0; i < ao_num_tasks; i++) {
259                 task = ao_tasks[i];
260                 ret = ao_task_validate_queue(task);
261                 if (!(ret & 1)) {
262                         if (ret & 2)
263                                 printf ("sleeping task not on sleep queue %s %08x\n",
264                                         task->name, task->wchan);
265                         else
266                                 printf ("running task not on run queue %s\n",
267                                         task->name);
268                 }
269                 ret = ao_task_validate_alarm(task);
270                 if (ret != 0xff) {
271                         if (!(ret & 1))
272                                 printf ("alarm task not on alarm queue %s %d\n",
273                                         task->name, task->alarm);
274                         if (ret & 2)
275                                 printf ("alarm queue has sooner entries after %s %d\n",
276                                         task->name, task->alarm);
277                         if (ret & 4)
278                                 printf ("alarm queue has later entries before %s %d\n",
279                                         task->name, task->alarm);
280                 }
281         }
282 }
283 #endif /* DEBUG */
284
285 static inline void *
286 ao_stack_top(struct ao_task *task)
287 {
288         uint8_t *top = &task->stack8[AO_STACK_SIZE];
289
290         /* Subtract off the TLS space, but keep the resulting
291          * stack 8-byte aligned
292          */
293 #if USE_TLS
294         return top - ((_tls_size() + 7) & ~3);
295 #else
296         return top;
297 #endif
298 }
299
300 void
301 ao_add_task(struct ao_task * task, void (*task_func)(void), const char *name) 
302 {
303         uint8_t task_id;
304         uint8_t t;
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)
310                                 break;
311                 if (t == ao_num_tasks)
312                         break;
313         }
314         task->task_id = task_id;
315         task->name = name;
316         task->wchan = NULL;
317         /*
318          * Construct a stack frame so that it will 'return'
319          * to the start of the task
320          */
321         uint32_t *sp = ao_stack_top(task);
322 #if USE_TLS
323         _init_tls(sp);
324 #endif
325         ao_arch_init_stack(task, sp, task_func);
326         ao_task_init_queue(task);
327         ao_arch_critical(
328                 _ao_task_to_run_queue(task);
329                 ao_tasks[ao_num_tasks] = task;
330                 ao_num_tasks++;
331                 );
332 }
333
334 uint8_t ao_task_minimize_latency;
335
336 /* Task switching function. */
337 void
338 ao_yield(void)
339 {
340         if (ao_cur_task)
341         {
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;
348 #endif
349                 ao_arch_save_regs();
350                 ao_arch_save_stack();
351         }
352
353         ao_arch_isr_stack();
354         ao_arch_block_interrupts();
355
356 #if AO_CHECK_STACK
357         in_yield = 1;
358 #endif
359         /* Find a task to run. If there isn't any runnable task,
360          * this loop will run forever, which is just fine
361          */
362         /* If the current task is running, move it to the
363          * end of the queue to allow other tasks a chance
364          */
365         if (ao_cur_task && ao_cur_task->wchan == NULL)
366                 _ao_task_to_run_queue(ao_cur_task);
367         for (;;) {
368                 ao_arch_memory_barrier();
369                 if (!ao_list_is_empty(&run_queue))
370                         break;
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();
375                 } else
376                         ao_arch_wait_interrupt();
377         }
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();
381 #endif
382 #if HAS_STACK_GUARD
383         ao_mpu_stack_guard(ao_cur_task->stack);
384 #endif
385 #if AO_CHECK_STACK
386         in_yield = 0;
387 #endif
388 #if USE_TLS
389         _set_tls(ao_stack_top(ao_cur_task));
390 #endif
391         ao_arch_restore_stack();
392 }
393
394 uint8_t
395 ao_sleep(void *wchan)
396 {
397         ao_arch_critical(
398                 ao_cur_task->wchan = wchan;
399                 _ao_task_to_sleep_queue(ao_cur_task, wchan);
400                 );
401         ao_yield();
402         if (ao_cur_task->wchan) {
403                 ao_cur_task->wchan = NULL;
404                 ao_cur_task->alarm = 0;
405                 return 1;
406         }
407         return 0;
408 }
409
410 void
411 ao_wakeup(void *wchan) 
412 {
413         ao_validate_cur_stack();
414         struct ao_task  *sleep, *next;
415         struct ao_list  *sleep_queue;
416         uint32_t        flags;
417
418         if (ao_num_tasks == 0)
419                 return;
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) {
424                         sleep->wchan = NULL;
425                         _ao_task_to_run_queue(sleep);
426                 }
427         }
428         ao_arch_irqrestore(flags);
429         ao_check_stack();
430 }
431
432 uint8_t
433 ao_sleep_for(void *wchan, AO_TICK_TYPE timeout)
434 {
435         uint8_t ret;
436         if (timeout) {
437                 ao_arch_critical(
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
440                          */
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);
444                         );
445         }
446         ret = ao_sleep(wchan);
447         if (timeout) {
448                 ao_arch_critical(
449                         ao_cur_task->alarm = 0;
450                         _ao_task_from_alarm_queue(ao_cur_task);
451                         );
452         }
453         return ret;
454 }
455
456 static uint8_t ao_forever;
457
458 void
459 ao_delay(AO_TICK_TYPE ticks)
460 {
461         if (!ticks)
462                 ticks = 1;
463         ao_sleep_for(&ao_forever, ticks);
464 }
465
466 void
467 ao_exit(void)
468 {
469         uint8_t i;
470         ao_arch_block_interrupts();
471         for (i = 0; i < ao_num_tasks; i++)
472                 if (ao_tasks[i] == ao_cur_task)
473                         break;
474         ao_task_exit_queue(ao_cur_task);
475         /* Remove task from list */
476         ao_num_tasks--;
477         for (; i < ao_num_tasks; i++)
478                 ao_tasks[i] = ao_tasks[i+1];
479         ao_cur_task = NULL;
480         ao_yield();
481         __builtin_unreachable();
482 }
483
484 #if HAS_TASK_INFO
485 void
486 ao_task_info(void)
487 {
488         uint8_t         i;
489         struct ao_task *task;
490         AO_TICK_TYPE    now = ao_time();
491
492         for (i = 0; i < ao_num_tasks; i++) {
493                 task = ao_tasks[i];
494                 printf("%2d: wchan %08x alarm %5d %s\n",
495                        task->task_id,
496                        (int) task->wchan,
497                        task->alarm ? (int16_t) (task->alarm - now) : 9999,
498                        task->name);
499         }
500 #if DEBUG
501         ao_task_validate();
502 #endif
503 }
504 #endif
505
506 void
507 ao_start_scheduler(void)
508 {
509         ao_cur_task = NULL;
510 #if HAS_ARCH_START_SCHEDULER
511         ao_arch_start_scheduler();
512 #endif
513         ao_yield();
514         __builtin_unreachable();
515 }