git.gag.com Git - fw/altos/blob - 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; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #include <ao.h>
  19 #include <ao_task.h>
  20 #if HAS_SAMPLE_PROFILE
  21 #include <ao_sample_profile.h>
  22 #endif
  23 #if HAS_STACK_GUARD
  24 #include <ao_mpu.h>
  25 #endif
  26
  27 #define DEBUG   0
  28
  29 #define AO_NO_TASK_INDEX        0xff
  30
  31 __xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS];
  32 __data uint8_t ao_num_tasks;
  33 __xdata struct ao_task *__data ao_cur_task;
  34
  35 #if !HAS_TASK_QUEUE
  36 static __data uint8_t ao_cur_task_index;
  37 #endif
  38
  39 #ifdef ao_arch_task_globals
  40 ao_arch_task_globals
  41 #endif
  42
  43 #define AO_CHECK_STACK  0
  44
  45 #if AO_CHECK_STACK
  46 static uint8_t  in_yield;
  47
  48 static inline void ao_check_stack(void) {
  49         uint8_t q;
  50         if (!in_yield && ao_cur_task && &q < &ao_cur_task->stack[0])
  51                 ao_panic(AO_PANIC_STACK);
  52 }
  53 #else
  54 #define ao_check_stack()
  55 #endif
  56
  57 #if DEBUG
  58 #define ao_task_irq_check()     ao_arch_irq_check()
  59 #else
  60 #define ao_task_irq_check()
  61 #endif
  62
  63 #if HAS_TASK_QUEUE
  64
  65 #define SLEEP_HASH_SIZE 17
  66
  67 static struct ao_list   run_queue;
  68 static struct ao_list   alarm_queue;
  69 static struct ao_list   sleep_queue[SLEEP_HASH_SIZE];
  70
  71 static void
  72 ao_task_to_run_queue(struct ao_task *task)
  73 {
  74         ao_task_irq_check();
  75         ao_list_del(&task->queue);
  76         ao_list_append(&task->queue, &run_queue);
  77 }
  78
  79 static struct ao_list *
  80 ao_task_sleep_queue(void *wchan)
  81 {
  82         return &sleep_queue[(uintptr_t) wchan % SLEEP_HASH_SIZE];
  83 }
  84
  85 static void
  86 ao_task_to_sleep_queue(struct ao_task *task, void *wchan)
  87 {
  88         ao_task_irq_check();
  89         ao_list_del(&task->queue);
  90         ao_list_append(&task->queue, ao_task_sleep_queue(wchan));
  91 }
  92
  93 #if DEBUG
  94 static void
  95 ao_task_validate_alarm_queue(void)
  96 {
  97         struct ao_task  *alarm, *prev = NULL;
  98         int             i;
  99
 100         if (ao_list_is_empty(&alarm_queue))
 101                 return;
 102         ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) {
 103                 if (prev) {
 104                         if ((int16_t) (alarm->alarm - prev->alarm) < 0) {
 105                                 ao_panic(1);
 106                         }
 107                 }
 108                 prev = alarm;
 109         }
 110         for (i = 0; i < ao_num_tasks; i++) {
 111                 alarm = ao_tasks[i];
 112                 if (alarm->alarm) {
 113                         if (ao_list_is_empty(&alarm->alarm_queue))
 114                                 ao_panic(2);
 115                 } else {
 116                         if (!ao_list_is_empty(&alarm->alarm_queue))
 117                                 ao_panic(3);
 118                 }
 119         }
 120         if (ao_task_alarm_tick != ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm)
 121                 ao_panic(4);
 122 }
 123 #else
 124 #define ao_task_validate_alarm_queue()
 125 #endif
 126
 127 uint16_t        ao_task_alarm_tick;
 128
 129 static void
 130 ao_task_to_alarm_queue(struct ao_task *task)
 131 {
 132         struct ao_task  *alarm;
 133         ao_task_irq_check();
 134         ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) {
 135                 if ((int16_t) (alarm->alarm - task->alarm) >= 0) {
 136                         ao_list_insert(&task->alarm_queue, alarm->alarm_queue.prev);
 137                         ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
 138                         ao_task_validate_alarm_queue();
 139                         return;
 140                 }
 141         }
 142         ao_list_append(&task->alarm_queue, &alarm_queue);
 143         ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
 144         ao_task_validate_alarm_queue();
 145 }
 146
 147 static void
 148 ao_task_from_alarm_queue(struct ao_task *task)
 149 {
 150         ao_task_irq_check();
 151         ao_list_del(&task->alarm_queue);
 152         if (ao_list_is_empty(&alarm_queue))
 153                 ao_task_alarm_tick = 0;
 154         else
 155                 ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
 156         ao_task_validate_alarm_queue();
 157 }
 158
 159 static void
 160 ao_task_init_queue(struct ao_task *task)
 161 {
 162         ao_list_init(&task->queue);
 163         ao_list_init(&task->alarm_queue);
 164 }
 165
 166 static void
 167 ao_task_exit_queue(struct ao_task *task)
 168 {
 169         ao_task_irq_check();
 170         ao_list_del(&task->queue);
 171         ao_list_del(&task->alarm_queue);
 172 }
 173
 174 void
 175 ao_task_check_alarm(uint16_t tick)
 176 {
 177         struct ao_task  *alarm, *next;
 178
 179         ao_arch_critical(
 180                 ao_list_for_each_entry_safe(alarm, next, &alarm_queue, struct ao_task, alarm_queue) {
 181                         if ((int16_t) (tick - alarm->alarm) < 0)
 182                                 break;
 183                         alarm->alarm = 0;
 184                         ao_task_from_alarm_queue(alarm);
 185                         ao_task_to_run_queue(alarm);
 186                 });
 187 }
 188
 189 void
 190 ao_task_init(void)
 191 {
 192         uint8_t i;
 193         ao_list_init(&run_queue);
 194         ao_list_init(&alarm_queue);
 195         ao_task_alarm_tick = 0;
 196         for (i = 0; i < SLEEP_HASH_SIZE; i++)
 197                 ao_list_init(&sleep_queue[i]);
 198 }
 199
 200 #if DEBUG
 201 static uint8_t
 202 ao_task_validate_queue(struct ao_task *task)
 203 {
 204         uint32_t flags;
 205         struct ao_task *m;
 206         uint8_t ret = 0;
 207         struct ao_list *queue;
 208
 209         flags = ao_arch_irqsave();
 210         if (task->wchan) {
 211                 queue = ao_task_sleep_queue(task->wchan);
 212                 ret |= 2;
 213         } else {
 214                 queue = &run_queue;
 215                 ret |= 4;
 216         }
 217         ao_list_for_each_entry(m, queue, struct ao_task, queue) {
 218                 if (m == task) {
 219                         ret |= 1;
 220                         break;
 221                 }
 222         }
 223         ao_arch_irqrestore(flags);
 224         return ret;
 225 }
 226
 227 static uint8_t
 228 ao_task_validate_alarm(struct ao_task *task)
 229 {
 230         uint32_t        flags;
 231         struct ao_task  *m;
 232         uint8_t         ret = 0;
 233
 234         flags = ao_arch_irqsave();
 235         if (task->alarm == 0)
 236                 return 0xff;
 237         ao_list_for_each_entry(m, &alarm_queue, struct ao_task, alarm_queue) {
 238                 if (m == task)
 239                         ret |= 1;
 240                 else {
 241                         if (!(ret&1)) {
 242                                 if ((int16_t) (m->alarm - task->alarm) > 0)
 243                                         ret |= 2;
 244                         } else {
 245                                 if ((int16_t) (task->alarm - m->alarm) > 0)
 246                                         ret |= 4;
 247                         }
 248                 }
 249         }
 250         ao_arch_irqrestore(flags);
 251         return ret;
 252 }
 253
 254
 255 static void
 256 ao_task_validate(void)
 257 {
 258         uint8_t         i;
 259         struct ao_task  *task;
 260         uint8_t         ret;
 261
 262         for (i = 0; i < ao_num_tasks; i++) {
 263                 task = ao_tasks[i];
 264                 ret = ao_task_validate_queue(task);
 265                 if (!(ret & 1)) {
 266                         if (ret & 2)
 267                                 printf ("sleeping task not on sleep queue %s %08x\n",
 268                                         task->name, task->wchan);
 269                         else
 270                                 printf ("running task not on run queue %s\n",
 271                                         task->name);
 272                 }
 273                 ret = ao_task_validate_alarm(task);
 274                 if (ret != 0xff) {
 275                         if (!(ret & 1))
 276                                 printf ("alarm task not on alarm queue %s %d\n",
 277                                         task->name, task->alarm);
 278                         if (ret & 2)
 279                                 printf ("alarm queue has sooner entries after %s %d\n",
 280                                         task->name, task->alarm);
 281                         if (ret & 4)
 282                                 printf ("alarm queue has later entries before %s %d\n",
 283                                         task->name, task->alarm);
 284                 }
 285         }
 286 }
 287 #endif /* DEBUG */
 288
 289 #endif /* HAS_TASK_QUEUE */
 290
 291 void
 292 ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant
 293 {
 294         uint8_t task_id;
 295         uint8_t t;
 296         if (ao_num_tasks == AO_NUM_TASKS)
 297                 ao_panic(AO_PANIC_NO_TASK);
 298         for (task_id = 1; task_id != 0; task_id++) {
 299                 for (t = 0; t < ao_num_tasks; t++)
 300                         if (ao_tasks[t]->task_id == task_id)
 301                                 break;
 302                 if (t == ao_num_tasks)
 303                         break;
 304         }
 305         task->task_id = task_id;
 306         task->name = name;
 307         task->wchan = NULL;
 308         /*
 309          * Construct a stack frame so that it will 'return'
 310          * to the start of the task
 311          */
 312         ao_arch_init_stack(task, start);
 313         ao_arch_critical(
 314 #if HAS_TASK_QUEUE
 315                 ao_task_init_queue(task);
 316                 ao_task_to_run_queue(task);
 317 #endif
 318                 ao_tasks[ao_num_tasks] = task;
 319                 ao_num_tasks++;
 320                 );
 321 }
 322
 323 __data uint8_t  ao_task_minimize_latency;
 324
 325 /* Task switching function. This must not use any stack variables */
 326 void
 327 ao_yield(void) ao_arch_naked_define
 328 {
 329         ao_arch_save_regs();
 330
 331 #if HAS_TASK_QUEUE
 332         if (ao_cur_task == NULL)
 333                 ao_cur_task = ao_tasks[ao_num_tasks-1];
 334 #else
 335         if (ao_cur_task_index == AO_NO_TASK_INDEX)
 336                 ao_cur_task_index = ao_num_tasks-1;
 337 #endif
 338         else
 339         {
 340 #if HAS_SAMPLE_PROFILE
 341                 uint16_t        tick = ao_sample_profile_timer_value();
 342                 uint16_t        run = tick - ao_cur_task->start;
 343                 if (run > ao_cur_task->max_run)
 344                         ao_cur_task->max_run = run;
 345                 ++ao_cur_task->yields;
 346 #endif
 347                 ao_arch_save_stack();
 348         }
 349
 350         ao_arch_isr_stack();
 351 #if !HAS_TASK_QUEUE
 352         if (ao_task_minimize_latency)
 353                 ao_arch_release_interrupts();
 354         else
 355 #endif
 356                 ao_arch_block_interrupts();
 357
 358 #if AO_CHECK_STACK
 359         in_yield = 1;
 360 #endif
 361         /* Find a task to run. If there isn't any runnable task,
 362          * this loop will run forever, which is just fine
 363          */
 364 #if HAS_TASK_QUEUE
 365         /* If the current task is running, move it to the
 366          * end of the queue to allow other tasks a chance
 367          */
 368         if (ao_cur_task->wchan == NULL)
 369                 ao_task_to_run_queue(ao_cur_task);
 370         for (;;) {
 371                 ao_arch_memory_barrier();
 372                 if (!ao_list_is_empty(&run_queue))
 373                         break;
 374                 /* Wait for interrupts when there's nothing ready */
 375                 ao_arch_wait_interrupt();
 376         }
 377         ao_cur_task = ao_list_first_entry(&run_queue, struct ao_task, queue);
 378 #else
 379         {
 380                 __pdata uint8_t ao_last_task_index = ao_cur_task_index;
 381                 for (;;) {
 382                         ++ao_cur_task_index;
 383                         if (ao_cur_task_index == ao_num_tasks)
 384                                 ao_cur_task_index = 0;
 385
 386                         ao_cur_task = ao_tasks[ao_cur_task_index];
 387
 388                         /* Check for ready task */
 389                         if (ao_cur_task->wchan == NULL)
 390                                 break;
 391
 392                         /* Check if the alarm is set for a time which has passed */
 393                         if (ao_cur_task->alarm &&
 394                             (int16_t) (ao_time() - ao_cur_task->alarm) >= 0)
 395                                 break;
 396
 397                         /* Wait for interrupts when there's nothing ready */
 398                         if (ao_cur_task_index == ao_last_task_index && !ao_task_minimize_latency)
 399                                 ao_arch_wait_interrupt();
 400                 }
 401         }
 402 #endif
 403 #if HAS_SAMPLE_PROFILE
 404         ao_cur_task->start = ao_sample_profile_timer_value();
 405 #endif
 406 #if HAS_STACK_GUARD
 407         ao_mpu_stack_guard(ao_cur_task->stack);
 408 #endif
 409 #if AO_CHECK_STACK
 410         in_yield = 0;
 411 #endif
 412         ao_arch_restore_stack();
 413 }
 414
 415 uint8_t
 416 ao_sleep(__xdata void *wchan)
 417 {
 418 #if HAS_TASK_QUEUE
 419         uint32_t flags;
 420         flags = ao_arch_irqsave();
 421 #endif
 422         ao_cur_task->wchan = wchan;
 423 #if HAS_TASK_QUEUE
 424         ao_task_to_sleep_queue(ao_cur_task, wchan);
 425         ao_arch_irqrestore(flags);
 426 #endif
 427         ao_yield();
 428         if (ao_cur_task->wchan) {
 429                 ao_cur_task->wchan = NULL;
 430                 ao_cur_task->alarm = 0;
 431                 return 1;
 432         }
 433         return 0;
 434 }
 435
 436 void
 437 ao_wakeup(__xdata void *wchan) __reentrant
 438 {
 439         ao_validate_cur_stack();
 440 #if HAS_TASK_QUEUE
 441         struct ao_task  *sleep, *next;
 442         struct ao_list  *sleep_queue;
 443         uint32_t        flags;
 444
 445         if (ao_num_tasks == 0)
 446                 return;
 447         sleep_queue = ao_task_sleep_queue(wchan);
 448         flags = ao_arch_irqsave();
 449         ao_list_for_each_entry_safe(sleep, next, sleep_queue, struct ao_task, queue) {
 450                 if (sleep->wchan == wchan) {
 451                         sleep->wchan = NULL;
 452                         ao_task_to_run_queue(sleep);
 453                 }
 454         }
 455         ao_arch_irqrestore(flags);
 456 #else
 457         {
 458         uint8_t i;
 459         for (i = 0; i < ao_num_tasks; i++)
 460                 if (ao_tasks[i]->wchan == wchan)
 461                         ao_tasks[i]->wchan = NULL;
 462         }
 463 #endif
 464         ao_check_stack();
 465 }
 466
 467 uint8_t
 468 ao_sleep_for(__xdata void *wchan, uint16_t timeout)
 469 {
 470         uint8_t ret;
 471         if (timeout) {
 472 #if HAS_TASK_QUEUE
 473                 uint32_t flags;
 474                 flags = ao_arch_irqsave();
 475 #endif
 476                 /* Make sure we sleep *at least* delay ticks, which means adding
 477                  * one to account for the fact that we may be close to the next tick
 478                  */
 479                 if (!(ao_cur_task->alarm = ao_time() + timeout + 1))
 480                         ao_cur_task->alarm = 1;
 481 #if HAS_TASK_QUEUE
 482                 ao_task_to_alarm_queue(ao_cur_task);
 483                 ao_arch_irqrestore(flags);
 484 #endif
 485         }
 486         ret = ao_sleep(wchan);
 487         if (timeout) {
 488 #if HAS_TASK_QUEUE
 489                 uint32_t flags;
 490
 491                 flags = ao_arch_irqsave();
 492 #endif
 493                 ao_cur_task->alarm = 0;
 494 #if HAS_TASK_QUEUE
 495                 ao_task_from_alarm_queue(ao_cur_task);
 496                 ao_arch_irqrestore(flags);
 497 #endif
 498         }
 499         return ret;
 500 }
 501
 502 static __xdata uint8_t ao_forever;
 503
 504 void
 505 ao_delay(uint16_t ticks)
 506 {
 507         ao_sleep_for(&ao_forever, ticks);
 508 }
 509
 510 void
 511 ao_exit(void)
 512 {
 513         uint8_t i;
 514         ao_arch_block_interrupts();
 515         ao_num_tasks--;
 516 #if HAS_TASK_QUEUE
 517         for (i = 0; i < ao_num_tasks; i++)
 518                 if (ao_tasks[i] == ao_cur_task)
 519                         break;
 520         ao_task_exit_queue(ao_cur_task);
 521 #else
 522         i = ao_cur_task_index;
 523         ao_cur_task_index = AO_NO_TASK_INDEX;
 524 #endif
 525         for (; i < ao_num_tasks; i++)
 526                 ao_tasks[i] = ao_tasks[i+1];
 527         ao_cur_task = NULL;
 528         ao_yield();
 529         /* we'll never get back here */
 530 }
 531
 532 #if HAS_TASK_INFO
 533 void
 534 ao_task_info(void)
 535 {
 536         uint8_t         i;
 537         __xdata struct ao_task *task;
 538
 539         for (i = 0; i < ao_num_tasks; i++) {
 540                 task = ao_tasks[i];
 541                 printf("%12s: wchan %04x\n",
 542                        task->name,
 543                        (int) task->wchan);
 544         }
 545 #if HAS_TASK_QUEUE && DEBUG
 546         ao_task_validate();
 547 #endif
 548 }
 549 #endif
 550
 551 void
 552 ao_start_scheduler(void)
 553 {
 554 #if !HAS_TASK_QUEUE
 555         ao_cur_task_index = AO_NO_TASK_INDEX;
 556 #endif
 557         ao_cur_task = NULL;
 558 #if HAS_ARCH_START_SCHEDULER
 559         ao_arch_start_scheduler();
 560 #endif
 561         ao_yield();
 562 }