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