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