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