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