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