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 #define AO_STACK_CANARY_VALUE   0xbaadf00dU
 311
 312 void
 313 ao_add_task(struct ao_task * task, void (*task_func)(void), const char *name)
 314 {
 315         uint8_t task_id;
 316         uint8_t t;
 317         if (ao_num_tasks == AO_NUM_TASKS)
 318                 ao_panic(AO_PANIC_NO_TASK);
 319         for (task_id = 1; task_id != 0; task_id++) {
 320                 for (t = 0; t < ao_num_tasks; t++)
 321                         if (ao_tasks[t]->task_id == task_id)
 322                                 break;
 323                 if (t == ao_num_tasks)
 324                         break;
 325         }
 326         task->task_id = task_id;
 327         task->name = name;
 328         task->wchan = NULL;
 329 #ifdef AO_STACK_CANARY
 330         task->bottom_canary = AO_STACK_CANARY_VALUE;
 331         task->top_canary = AO_STACK_CANARY_VALUE;
 332 #endif
 333         /*
 334          * Construct a stack frame so that it will 'return'
 335          * to the start of the task
 336          */
 337         uint32_t *sp = ao_stack_top(task);
 338 #if USE_TLS
 339         _init_tls(sp);
 340 #endif
 341         ao_arch_init_stack(task, sp, task_func);
 342         ao_task_init_queue(task);
 343         ao_arch_critical(
 344                 _ao_task_to_run_queue(task);
 345                 ao_tasks[ao_num_tasks] = task;
 346                 ao_num_tasks++;
 347                 );
 348 }
 349
 350 #ifdef AO_STACK_CANARY
 351 static void
 352 ao_check_stack_canary(void)
 353 {
 354         if (ao_cur_task->bottom_canary != AO_STACK_CANARY_VALUE)
 355                 ao_panic(AO_PANIC_STACK);
 356         if (ao_cur_task->top_canary != AO_STACK_CANARY_VALUE)
 357                 ao_panic(AO_PANIC_STACK);
 358 }
 359 #else
 360 #define ao_check_stack_canary()
 361 #endif
 362
 363 uint8_t ao_task_minimize_latency;
 364
 365 /* Task switching function. */
 366 void
 367 ao_yield(void)
 368 {
 369         if (ao_cur_task)
 370         {
 371 #if HAS_SAMPLE_PROFILE
 372                 AO_TICK_TYPE    tick = ao_sample_profile_timer_value();
 373                 AO_TICK_TYPE    run = tick - ao_cur_task->start;
 374                 if (run > ao_cur_task->max_run)
 375                         ao_cur_task->max_run = run;
 376                 ++ao_cur_task->yields;
 377 #endif
 378                 ao_check_stack_canary();
 379                 ao_arch_save_regs();
 380                 ao_arch_save_stack();
 381         }
 382
 383         ao_arch_isr_stack();
 384         ao_arch_block_interrupts();
 385
 386 #if AO_CHECK_STACK
 387         in_yield = 1;
 388 #endif
 389         /* Find a task to run. If there isn't any runnable task,
 390          * this loop will run forever, which is just fine
 391          */
 392         /* If the current task is running, move it to the
 393          * end of the queue to allow other tasks a chance
 394          */
 395         if (ao_cur_task && ao_cur_task->wchan == NULL)
 396                 _ao_task_to_run_queue(ao_cur_task);
 397         for (;;) {
 398                 ao_arch_memory_barrier();
 399                 if (!ao_list_is_empty(&run_queue))
 400                         break;
 401                 /* Wait for interrupts when there's nothing ready */
 402                 if (ao_task_minimize_latency) {
 403                         ao_arch_release_interrupts();
 404                         ao_arch_block_interrupts();
 405                 } else
 406                         ao_arch_wait_interrupt();
 407         }
 408         ao_cur_task = ao_list_first_entry(&run_queue, struct ao_task, queue);
 409 #if HAS_SAMPLE_PROFILE
 410         ao_cur_task->start = ao_sample_profile_timer_value();
 411 #endif
 412 #if HAS_STACK_GUARD
 413         ao_mpu_stack_guard(ao_cur_task->stack);
 414 #endif
 415 #if AO_CHECK_STACK
 416         in_yield = 0;
 417 #endif
 418 #if USE_TLS
 419         _set_tls(ao_stack_top(ao_cur_task));
 420 #endif
 421         ao_check_stack_canary();
 422         ao_arch_restore_stack();
 423 }
 424
 425 uint8_t
 426 ao_sleep(void *wchan)
 427 {
 428         ao_arch_critical(
 429                 ao_cur_task->wchan = wchan;
 430                 _ao_task_to_sleep_queue(ao_cur_task, wchan);
 431                 );
 432         ao_yield();
 433         if (ao_cur_task->wchan) {
 434                 ao_cur_task->wchan = NULL;
 435                 ao_cur_task->alarm = 0;
 436                 return 1;
 437         }
 438         return 0;
 439 }
 440
 441 void
 442 ao_wakeup(void *wchan)
 443 {
 444         ao_validate_cur_stack();
 445         struct ao_task  *sleep, *next;
 446         struct ao_list  *sleep_queue;
 447         uint32_t        flags;
 448
 449         if (ao_num_tasks == 0)
 450                 return;
 451         sleep_queue = ao_task_sleep_queue(wchan);
 452         flags = ao_arch_irqsave();
 453         ao_list_for_each_entry_safe(sleep, next, sleep_queue, struct ao_task, queue) {
 454                 if (sleep->wchan == wchan) {
 455                         sleep->wchan = NULL;
 456                         _ao_task_to_run_queue(sleep);
 457                 }
 458         }
 459         ao_arch_irqrestore(flags);
 460         ao_check_stack();
 461 }
 462
 463 uint8_t
 464 ao_sleep_for(void *wchan, AO_TICK_TYPE timeout)
 465 {
 466         uint8_t ret;
 467         if (timeout) {
 468                 ao_arch_critical(
 469                         /* Make sure we sleep *at least* delay ticks, which means adding
 470                          * one to account for the fact that we may be close to the next tick
 471                          */
 472                         if (!(ao_cur_task->alarm = ao_time() + timeout + 1))
 473                                 ao_cur_task->alarm = 1;
 474                         _ao_task_to_alarm_queue(ao_cur_task);
 475                         );
 476         }
 477         ret = ao_sleep(wchan);
 478         if (timeout) {
 479                 ao_arch_critical(
 480                         ao_cur_task->alarm = 0;
 481                         _ao_task_from_alarm_queue(ao_cur_task);
 482                         );
 483         }
 484         return ret;
 485 }
 486
 487 static uint8_t ao_forever;
 488
 489 void
 490 ao_delay(AO_TICK_TYPE ticks)
 491 {
 492         if (!ticks)
 493                 ticks = 1;
 494         ao_sleep_for(&ao_forever, ticks);
 495 }
 496
 497 void
 498 ao_exit(void)
 499 {
 500         uint8_t i;
 501         ao_arch_block_interrupts();
 502         for (i = 0; i < ao_num_tasks; i++)
 503                 if (ao_tasks[i] == ao_cur_task)
 504                         break;
 505         ao_task_exit_queue(ao_cur_task);
 506         /* Remove task from list */
 507         ao_num_tasks--;
 508         for (; i < ao_num_tasks; i++)
 509                 ao_tasks[i] = ao_tasks[i+1];
 510         ao_cur_task = NULL;
 511         ao_yield();
 512         __builtin_unreachable();
 513 }
 514
 515 #if HAS_TASK_INFO
 516 void
 517 ao_task_info(void)
 518 {
 519         uint8_t         i;
 520         struct ao_task *task;
 521         AO_TICK_TYPE    now = ao_time();
 522
 523         for (i = 0; i < ao_num_tasks; i++) {
 524                 task = ao_tasks[i];
 525                 printf("%2d: wchan %08x alarm %5d %s\n",
 526                        task->task_id,
 527                        (int) task->wchan,
 528                        task->alarm ? (int16_t) (task->alarm - now) : 9999,
 529                        task->name);
 530         }
 531 #if DEBUG
 532         ao_task_validate();
 533 #endif
 534 }
 535 #endif
 536
 537 void
 538 ao_start_scheduler(void)
 539 {
 540         ao_cur_task = NULL;
 541 #if HAS_ARCH_START_SCHEDULER
 542         ao_arch_start_scheduler();
 543 #endif
 544         ao_yield();
 545         __builtin_unreachable();
 546 }