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