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 /* 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 }