firmware/thread.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2002 by Ulf Ralberg
  11  *
  12  * All files in this archive are subject to the GNU General Public License.
  13  * See the file COPYING in the source tree root for full license agreement.
  14  *
  15  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  16  * KIND, either express or implied.
  17  *
  18  ****************************************************************************/
  19 #include "config.h"
  20 #include <stdbool.h>
  21 #include "thread.h"
  22 #include "panic.h"
  23 #include "sprintf.h"
  24 #include "system.h"
  25 #include "kernel.h"
  26 #include "cpu.h"
  27 #include "string.h"
  28 #ifdef RB_PROFILE
  29 #include <profile.h>
  30 #endif
  31
  32 #if NUM_CORES > 1
  33 # define IF_COP2(x) x
  34 #else
  35 # define IF_COP2(x) CURRENT_CORE
  36 #endif
  37
  38 #define DEADBEEF ((unsigned int)0xdeadbeef)
  39 /* Cast to the the machine int type, whose size could be < 4. */
  40
  41 struct core_entry cores[NUM_CORES] IBSS_ATTR;
  42 struct thread_entry threads[MAXTHREADS] IBSS_ATTR;
  43 #ifdef HAVE_SCHEDULER_BOOSTCTRL
  44 static int boosted_threads IBSS_ATTR;
  45 #endif
  46
  47 /* Define to enable additional checks for blocking violations etc. */
  48 #define THREAD_EXTRA_CHECKS
  49
  50 static const char main_thread_name[] = "main";
  51
  52 extern int stackbegin[];
  53 extern int stackend[];
  54
  55 #ifdef CPU_PP
  56 #ifndef BOOTLOADER
  57 extern int cop_stackbegin[];
  58 extern int cop_stackend[];
  59 #else
  60 /* The coprocessor stack is not set up in the bootloader code, but the threading
  61  * is.  No threads are run on the coprocessor, so set up some dummy stack */
  62 int *cop_stackbegin = stackbegin;
  63 int *cop_stackend = stackend;
  64 #endif
  65 #endif
  66
  67 #if NUM_CORES > 1
  68 #if 0
  69 static long cores_locked IBSS_ATTR;
  70
  71 #define LOCK(...) do { } while (test_and_set(&cores_locked, 1))
  72 #define UNLOCK(...) cores_locked = 0
  73 #endif
  74
  75 /* #warning "Core locking mechanism should be fixed on H10/4G!" */
  76
  77 inline void lock_cores(void)
  78 {
  79 #if 0
  80     if (!cores[CURRENT_CORE].lock_issued)
  81     {
  82         LOCK();
  83         cores[CURRENT_CORE].lock_issued = true;
  84     }
  85 #endif
  86 }
  87
  88 inline void unlock_cores(void)
  89 {
  90 #if 0
  91     if (cores[CURRENT_CORE].lock_issued)
  92     {
  93         cores[CURRENT_CORE].lock_issued = false;
  94         UNLOCK();
  95     }
  96 #endif
  97 }
  98
  99 #endif
 100
 101 /* Conserve IRAM
 102 static void add_to_list(struct thread_entry **list,
 103                         struct thread_entry *thread) ICODE_ATTR;
 104 static void remove_from_list(struct thread_entry **list,
 105                              struct thread_entry *thread) ICODE_ATTR;
 106 */
 107
 108 void switch_thread(bool save_context, struct thread_entry **blocked_list)
 109     ICODE_ATTR;
 110
 111 static inline void store_context(void* addr) __attribute__ ((always_inline));
 112 static inline void load_context(const void* addr)
 113     __attribute__ ((always_inline));
 114 static inline void core_sleep(void) __attribute__((always_inline));
 115
 116 #if defined(CPU_ARM)
 117 /*---------------------------------------------------------------------------
 118  * Store non-volatile context.
 119  *---------------------------------------------------------------------------
 120  */
 121 static inline void store_context(void* addr)
 122 {
 123     asm volatile(
 124         "stmia  %0, { r4-r11, sp, lr }\n"
 125         : : "r" (addr)
 126     );
 127 }
 128
 129 /*---------------------------------------------------------------------------
 130  * Load non-volatile context.
 131  *---------------------------------------------------------------------------
 132  */
 133 static void start_thread(void (*thread_func)(void), const void* addr) __attribute__((naked,used));
 134 static void start_thread(void (*thread_func)(void), const void* addr)
 135 {
 136     /* r0 = thread_func, r1 = addr */
 137 #if NUM_CORES > 1 && CONFIG_CPU != PP5002
 138     asm volatile (
 139         "mov    r2, #0           \n"
 140         "str    r2, [r1, #40]    \n"
 141         "ldr    r1, =0xf000f044  \n" /* invalidate this core's cache */
 142         "ldr    r2, [r1]         \n"
 143         "orr    r2, r2, #6       \n"
 144         "str    r2, [r1]         \n"
 145         "ldr    r1, =0x6000c000  \n"
 146     "1:                          \n"
 147         "ldr    r2, [r1]         \n"
 148         "tst    r2, #0x8000      \n"
 149         "bne    1b               \n"
 150         "mov    pc, r0           \n"
 151         : : : "r1", "r2"
 152     );
 153 #else
 154     asm volatile (
 155         "mov    r2, #0           \n"
 156         "str    r2, [r1, #40]    \n"
 157         "mov    pc, r0           \n"
 158         : : : "r1", "r2"
 159     );
 160 #endif
 161     (void)thread_func;
 162     (void)addr;
 163 }
 164
 165 static inline void load_context(const void* addr)
 166 {
 167     asm volatile(
 168         "ldmia  %0, { r4-r11, sp, lr } \n" /* load regs r4 to r14 from context */
 169         "ldr    r0, [%0, #40]          \n" /* load start pointer */
 170         "cmp    r0, #0                 \n" /* check for NULL */
 171         "movne  r1, %0                 \n" /* if not already running, jump to start */
 172         "ldrne  pc, =start_thread      \n"
 173         : : "r" (addr) : "r0", "r1"
 174     );
 175 }
 176
 177 #if defined (CPU_PP)
 178 static inline void core_sleep(void)
 179 {
 180     unlock_cores();
 181
 182     /* This should sleep the CPU. It appears to wake by itself on
 183        interrupts */
 184     if (CURRENT_CORE == CPU)
 185         CPU_CTL = PROC_SLEEP;
 186     else
 187         COP_CTL = PROC_SLEEP;
 188
 189     lock_cores();
 190 }
 191 #elif CONFIG_CPU == S3C2440
 192 static inline void core_sleep(void)
 193 {
 194     int i;
 195     CLKCON |= (1 << 2); /* set IDLE bit */
 196     for(i=0; i<10; i++); /* wait for IDLE */
 197     CLKCON &= ~(1 << 2); /* reset IDLE bit when wake up */
 198 }
 199 #endif
 200
 201 #elif defined(CPU_COLDFIRE)
 202 /*---------------------------------------------------------------------------
 203  * Store non-volatile context.
 204  *---------------------------------------------------------------------------
 205  */
 206 static inline void store_context(void* addr)
 207 {
 208     asm volatile (
 209         "move.l  %%macsr,%%d0    \n"
 210         "movem.l %%d0/%%d2-%%d7/%%a2-%%a7,(%0)   \n"
 211         : : "a" (addr) : "d0" /* only! */
 212     );
 213 }
 214
 215 /*---------------------------------------------------------------------------
 216  * Load non-volatile context.
 217  *---------------------------------------------------------------------------
 218  */
 219 static inline void load_context(const void* addr)
 220 {
 221     asm volatile (
 222         "movem.l (%0),%%d0/%%d2-%%d7/%%a2-%%a7   \n"  /* Load context */
 223         "move.l  %%d0,%%macsr    \n"
 224         "move.l  (52,%0),%%d0    \n"  /* Get start address */
 225         "beq.b   1f              \n"  /* NULL -> already running */
 226         "clr.l   (52,%0)         \n"  /* Clear start address.. */
 227         "move.l  %%d0,%0         \n"
 228         "jmp     (%0)            \n"  /* ..and start the thread */
 229     "1:                          \n"
 230         : : "a" (addr) : "d0" /* only! */
 231     );
 232 }
 233
 234 static inline void core_sleep(void)
 235 {
 236     asm volatile ("stop #0x2000");
 237 }
 238
 239 /* Set EMAC unit to fractional mode with saturation for each new thread,
 240    since that's what'll be the most useful for most things which the dsp
 241    will do. Codecs should still initialize their preferred modes
 242    explicitly. */
 243 #define THREAD_CPU_INIT(core, thread) \
 244     ({ (thread)->context.macsr = EMAC_FRACTIONAL | EMAC_SATURATE; })
 245
 246 #elif CONFIG_CPU == SH7034
 247 /*---------------------------------------------------------------------------
 248  * Store non-volatile context.
 249  *---------------------------------------------------------------------------
 250  */
 251 static inline void store_context(void* addr)
 252 {
 253     asm volatile (
 254         "add     #36,%0    \n"
 255         "sts.l   pr, @-%0  \n"
 256         "mov.l   r15,@-%0  \n"
 257         "mov.l   r14,@-%0  \n"
 258         "mov.l   r13,@-%0  \n"
 259         "mov.l   r12,@-%0  \n"
 260         "mov.l   r11,@-%0  \n"
 261         "mov.l   r10,@-%0  \n"
 262         "mov.l   r9, @-%0  \n"
 263         "mov.l   r8, @-%0  \n"
 264         : : "r" (addr)
 265     );
 266 }
 267
 268 /*---------------------------------------------------------------------------
 269  * Load non-volatile context.
 270  *---------------------------------------------------------------------------
 271  */
 272 static inline void load_context(const void* addr)
 273 {
 274     asm volatile (
 275         "mov.l   @%0+,r8   \n"
 276         "mov.l   @%0+,r9   \n"
 277         "mov.l   @%0+,r10  \n"
 278         "mov.l   @%0+,r11  \n"
 279         "mov.l   @%0+,r12  \n"
 280         "mov.l   @%0+,r13  \n"
 281         "mov.l   @%0+,r14  \n"
 282         "mov.l   @%0+,r15  \n"
 283         "lds.l   @%0+,pr   \n"
 284         "mov.l   @%0,r0    \n"  /* Get start address */
 285         "tst     r0,r0     \n"
 286         "bt      .running  \n"  /* NULL -> already running */
 287         "lds     r0,pr     \n"
 288         "mov     #0,r0     \n"
 289         "rts               \n"  /* Start the thread */
 290         "mov.l   r0,@%0    \n"  /* Clear start address */
 291     ".running:             \n"
 292         : : "r" (addr) : "r0" /* only! */
 293     );
 294 }
 295
 296 static inline void core_sleep(void)
 297 {
 298     and_b(0x7F, &SBYCR);
 299     asm volatile ("sleep");
 300 }
 301
 302 #endif
 303
 304 #ifndef THREAD_CPU_INIT
 305 /* No cpu specific init - make empty */
 306 #define THREAD_CPU_INIT(core, thread)
 307 #endif
 308
 309 #ifdef THREAD_EXTRA_CHECKS
 310 static void thread_panicf_format_name(char *buffer, struct thread_entry *thread)
 311 {
 312     *buffer = '\0';
 313     if (thread)
 314     {
 315         /* Display thread name if one or ID if none */
 316         const char *fmt = thread->name ? " %s" : " %08lX";
 317         intptr_t name = thread->name ?
 318             (intptr_t)thread->name : (intptr_t)thread;
 319         snprintf(buffer, 16, fmt, name);
 320     }
 321 }
 322
 323 static void thread_panicf(const char *msg,
 324     struct thread_entry *thread1, struct thread_entry *thread2)
 325 {
 326     static char thread1_name[16], thread2_name[16];
 327     thread_panicf_format_name(thread1_name, thread1);
 328     thread_panicf_format_name(thread2_name, thread2);
 329     panicf ("%s%s%s", msg, thread1_name, thread2_name);
 330 }
 331 #else
 332 static void thread_stkov(void)
 333 {
 334     /* Display thread name if one or ID if none */
 335     struct thread_entry *current = cores[CURRENT_CORE].running;
 336     const char *fmt = current->name ? "%s %s" : "%s %08lX";
 337     intptr_t name = current->name ?
 338             (intptr_t)current->name : (intptr_t)current;
 339     panicf(fmt, "Stkov", name);
 340 }
 341 #endif /* THREAD_EXTRA_CHECKS */
 342
 343 static void add_to_list(struct thread_entry **list, struct thread_entry *thread)
 344 {
 345     if (*list == NULL)
 346     {
 347         thread->next = thread;
 348         thread->prev = thread;
 349         *list = thread;
 350     }
 351     else
 352     {
 353         /* Insert last */
 354         thread->next = *list;
 355         thread->prev = (*list)->prev;
 356         thread->prev->next = thread;
 357         (*list)->prev = thread;
 358
 359         /* Insert next
 360          thread->next = (*list)->next;
 361          thread->prev = *list;
 362          thread->next->prev = thread;
 363          (*list)->next = thread;
 364          */
 365     }
 366 }
 367
 368 static void remove_from_list(struct thread_entry **list,
 369                       struct thread_entry *thread)
 370 {
 371     if (list != NULL)
 372     {
 373         if (thread == thread->next)
 374         {
 375             *list = NULL;
 376             return;
 377         }
 378
 379         if (thread == *list)
 380             *list = thread->next;
 381     }
 382
 383     /* Fix links to jump over the removed entry. */
 384     thread->prev->next = thread->next;
 385     thread->next->prev = thread->prev;
 386 }
 387
 388 static void check_sleepers(void) __attribute__ ((noinline));
 389 static void check_sleepers(void)
 390 {
 391     const unsigned int core = CURRENT_CORE;
 392     struct thread_entry *current, *next;
 393
 394     /* Check sleeping threads. */
 395     current = cores[core].sleeping;
 396
 397     for (;;)
 398     {
 399         next = current->next;
 400
 401         if ((unsigned)current_tick >= GET_STATE_ARG(current->statearg))
 402         {
 403             /* Sleep timeout has been reached so bring the thread
 404              * back to life again. */
 405             remove_from_list(&cores[core].sleeping, current);
 406             add_to_list(&cores[core].running, current);
 407             current->statearg = 0;
 408
 409             /* If there is no more processes in the list, break the loop. */
 410             if (cores[core].sleeping == NULL)
 411                 break;
 412
 413             current = next;
 414             continue;
 415         }
 416
 417         current = next;
 418
 419         /* Break the loop once we have walked through the list of all
 420          * sleeping processes. */
 421         if (current == cores[core].sleeping)
 422             break;
 423     }
 424 }
 425
 426 /* Safely finish waking all threads potentialy woken by interrupts -
 427  * statearg already zeroed in wakeup_thread. */
 428 static void wake_list_awaken(void) __attribute__ ((noinline));
 429 static void wake_list_awaken(void)
 430 {
 431     const unsigned int core = CURRENT_CORE;
 432     int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
 433
 434     /* No need for another check in the IRQ lock since IRQs are allowed
 435        only to add threads to the waking list. They won't be adding more
 436        until we're done here though. */
 437
 438     struct thread_entry *waking = cores[core].waking;
 439     struct thread_entry *running = cores[core].running;
 440
 441     if (running != NULL)
 442     {
 443         /* Place waking threads at the end of the running list. */
 444         struct thread_entry *tmp;
 445         waking->prev->next  = running;
 446         running->prev->next = waking;
 447         tmp                 = running->prev;
 448         running->prev       = waking->prev;
 449         waking->prev        = tmp;
 450     }
 451     else
 452     {
 453         /* Just transfer the list as-is - just came out of a core
 454          * sleep. */
 455         cores[core].running = waking;
 456     }
 457
 458     /* Done with waking list */
 459     cores[core].waking = NULL;
 460     set_irq_level(oldlevel);
 461 }
 462
 463 static inline void sleep_core(void)
 464 {
 465     const unsigned int core = CURRENT_CORE;
 466
 467     for (;;)
 468     {
 469         /* We want to do these ASAP as it may change the decision to sleep
 470            the core or the core has woken because an interrupt occurred
 471            and posted a message to a queue. */
 472         if (cores[core].waking != NULL)
 473             wake_list_awaken();
 474
 475         if (cores[core].last_tick != current_tick)
 476         {
 477             if (cores[core].sleeping != NULL)
 478                 check_sleepers();
 479             cores[core].last_tick = current_tick;
 480         }
 481
 482         /* We must sleep until there is at least one process in the list
 483          * of running processes. */
 484         if (cores[core].running != NULL)
 485             break;
 486
 487         /* Enter sleep mode to reduce power usage, woken up on interrupt */
 488         core_sleep();
 489     }
 490 }
 491
 492 #ifdef RB_PROFILE
 493 static int get_threadnum(struct thread_entry *thread)
 494 {
 495     int i;
 496
 497     for (i = 0; i < MAXTHREADS; i++)
 498     {
 499         if (&threads[i] == thread)
 500             return i;
 501     }
 502
 503     return -1;
 504 }
 505
 506 void profile_thread(void) {
 507     profstart(get_threadnum(cores[CURRENT_CORE].running));
 508 }
 509 #endif
 510
 511 static void change_thread_state(struct thread_entry **blocked_list) __attribute__ ((noinline));
 512 static void change_thread_state(struct thread_entry **blocked_list)
 513 {
 514     const unsigned int core = CURRENT_CORE;
 515     struct thread_entry *old;
 516     unsigned long new_state;
 517
 518     /* Remove the thread from the list of running threads. */
 519     old = cores[core].running;
 520     new_state = GET_STATE(old->statearg);
 521
 522     /* Check if a thread state change has been requested. */
 523     if (new_state)
 524     {
 525         /* Change running thread state and switch to next thread. */
 526         remove_from_list(&cores[core].running, old);
 527
 528         /* And put the thread into a new list of inactive threads. */
 529         if (new_state == STATE_BLOCKED)
 530             add_to_list(blocked_list, old);
 531         else
 532             add_to_list(&cores[core].sleeping, old);
 533
 534 #ifdef HAVE_PRIORITY_SCHEDULING
 535         /* Reset priorities */
 536         if (old->priority == cores[core].highest_priority)
 537             cores[core].highest_priority = 100;
 538 #endif
 539     }
 540     else
 541         /* Switch to the next running thread. */
 542         cores[core].running = old->next;
 543 }
 544
 545 /*---------------------------------------------------------------------------
 546  * Switch thread in round robin fashion.
 547  *---------------------------------------------------------------------------
 548  */
 549 void switch_thread(bool save_context, struct thread_entry **blocked_list)
 550 {
 551     const unsigned int core = CURRENT_CORE;
 552
 553 #ifdef RB_PROFILE
 554     profile_thread_stopped(get_threadnum(cores[core].running));
 555 #endif
 556     unsigned int *stackptr;
 557
 558 #ifdef SIMULATOR
 559     /* Do nothing */
 560 #else
 561
 562     lock_cores();
 563
 564     /* Begin task switching by saving our current context so that we can
 565      * restore the state of the current thread later to the point prior
 566      * to this call. */
 567     if (save_context)
 568     {
 569         store_context(&cores[core].running->context);
 570
 571         /* Check if the current thread stack is overflown */
 572         stackptr = cores[core].running->stack;
 573         if(stackptr[0] != DEADBEEF)
 574 #ifdef THREAD_EXTRA_CHECKS
 575             thread_panicf("Stkov", cores[core].running, NULL);
 576 #else
 577             thread_stkov();
 578 #endif
 579
 580         /* Rearrange thread lists as needed */
 581         change_thread_state(blocked_list);
 582
 583         /* This has to be done after the scheduler is finished with the
 584            blocked_list pointer so that an IRQ can't kill us by attempting
 585            a wake but before attempting any core sleep. */
 586         if (cores[core].switch_to_irq_level != STAY_IRQ_LEVEL)
 587         {
 588             int level = cores[core].switch_to_irq_level;
 589             cores[core].switch_to_irq_level = STAY_IRQ_LEVEL;
 590             set_irq_level(level);
 591         }
 592     }
 593
 594     /* Go through the list of sleeping task to check if we need to wake up
 595      * any of them due to timeout. Also puts core into sleep state until
 596      * there is at least one running process again. */
 597     sleep_core();
 598
 599 #ifdef HAVE_PRIORITY_SCHEDULING
 600     /* Select the new task based on priorities and the last time a process
 601      * got CPU time. */
 602     for (;;)
 603     {
 604         int priority = cores[core].running->priority;
 605
 606         if (priority < cores[core].highest_priority)
 607             cores[core].highest_priority = priority;
 608
 609         if (priority == cores[core].highest_priority ||
 610                 (current_tick - cores[core].running->last_run >
 611                  priority * 8) ||
 612             cores[core].running->priority_x != 0)
 613         {
 614             break;
 615         }
 616
 617         cores[core].running = cores[core].running->next;
 618     }
 619
 620     /* Reset the value of thread's last running time to the current time. */
 621     cores[core].running->last_run = current_tick;
 622 #endif
 623
 624 #endif
 625     unlock_cores();
 626
 627     /* And finally give control to the next thread. */
 628     load_context(&cores[core].running->context);
 629
 630 #ifdef RB_PROFILE
 631     profile_thread_started(get_threadnum(cores[core].running));
 632 #endif
 633 }
 634
 635 void sleep_thread(int ticks)
 636 {
 637     struct thread_entry *current;
 638
 639     lock_cores();
 640
 641     current = cores[CURRENT_CORE].running;
 642
 643 #ifdef HAVE_SCHEDULER_BOOSTCTRL
 644     if (STATE_IS_BOOSTED(current->statearg))
 645     {
 646         boosted_threads--;
 647         if (!boosted_threads)
 648         {
 649             cpu_boost(false);
 650         }
 651     }
 652 #endif
 653
 654     /* Set the thread's new state and timeout and finally force a task switch
 655      * so that scheduler removes thread from the list of running processes
 656      * and puts it in list of sleeping tasks. */
 657     SET_STATE(current->statearg, STATE_SLEEPING, current_tick + ticks + 1);
 658
 659     switch_thread(true, NULL);
 660 }
 661
 662 void block_thread(struct thread_entry **list)
 663 {
 664     struct thread_entry *current;
 665
 666     lock_cores();
 667
 668     /* Get the entry for the current running thread. */
 669     current = cores[CURRENT_CORE].running;
 670
 671 #ifdef HAVE_SCHEDULER_BOOSTCTRL
 672     /* Keep the boosted state over indefinite block calls, because
 673      * we are waiting until the earliest time that someone else
 674      * completes an action */
 675     unsigned long boost_flag = STATE_IS_BOOSTED(current->statearg);
 676 #endif
 677
 678 #ifdef THREAD_EXTRA_CHECKS
 679     /* We are not allowed to mix blocking types in one queue. */
 680     if (*list && GET_STATE((*list)->statearg) == STATE_BLOCKED_W_TMO)
 681         thread_panicf("Blocking violation B->*T", current, *list);
 682 #endif
 683
 684     /* Set the state to blocked and ask the scheduler to switch tasks,
 685      * this takes us off of the run queue until we are explicitly woken */
 686     SET_STATE(current->statearg, STATE_BLOCKED, 0);
 687
 688     switch_thread(true, list);
 689
 690 #ifdef HAVE_SCHEDULER_BOOSTCTRL
 691     /* Reset only the boosted flag to indicate we are up and running again. */
 692     current->statearg = boost_flag;
 693 #else
 694     /* Clear all flags to indicate we are up and running again. */
 695     current->statearg = 0;
 696 #endif
 697 }
 698
 699 void block_thread_w_tmo(struct thread_entry **list, int timeout)
 700 {
 701     struct thread_entry *current;
 702     /* Get the entry for the current running thread. */
 703     current = cores[CURRENT_CORE].running;
 704
 705     lock_cores();
 706 #ifdef HAVE_SCHEDULER_BOOSTCTRL
 707     /* A block with a timeout is a sleep situation, whatever we are waiting
 708      * for _may or may not_ happen, regardless of boost state, (user input
 709      * for instance), so this thread no longer needs to boost */
 710     if (STATE_IS_BOOSTED(current->statearg))
 711     {
 712         boosted_threads--;
 713         if (!boosted_threads)
 714         {
 715             cpu_boost(false);
 716         }
 717     }
 718 #endif
 719
 720 #ifdef THREAD_EXTRA_CHECKS
 721     /* We can store only one thread to the "list" if thread is used
 722      * in other list (such as core's list for sleeping tasks). */
 723     if (*list)
 724         thread_panicf("Blocking violation T->*B", current, NULL);
 725 #endif
 726
 727     /* Set the state to blocked with the specified timeout */
 728     SET_STATE(current->statearg, STATE_BLOCKED_W_TMO, current_tick + timeout);
 729
 730     /* Set the "list" for explicit wakeup */
 731     *list = current;
 732
 733     /* Now force a task switch and block until we have been woken up
 734      * by another thread or timeout is reached. */
 735     switch_thread(true, NULL);
 736
 737     /* It is now safe for another thread to block on this "list" */
 738     *list = NULL;
 739 }
 740
 741 #if !defined(SIMULATOR)
 742 void set_irq_level_and_block_thread(struct thread_entry **list, int level)
 743 {
 744     cores[CURRENT_CORE].switch_to_irq_level = level;
 745     block_thread(list);
 746 }
 747
 748 void set_irq_level_and_block_thread_w_tmo(struct thread_entry **list,
 749                                           int timeout, int level)
 750 {
 751     cores[CURRENT_CORE].switch_to_irq_level = level;
 752     block_thread_w_tmo(list, timeout);
 753 }
 754 #endif
 755
 756 void wakeup_thread(struct thread_entry **list)
 757 {
 758     struct thread_entry *thread;
 759
 760     /* Check if there is a blocked thread at all. */
 761     if (*list == NULL)
 762     {
 763         return ;
 764     }
 765
 766     /* Wake up the last thread first. */
 767     thread = *list;
 768
 769     /* Determine thread's current state. */
 770     switch (GET_STATE(thread->statearg))
 771     {
 772         case STATE_BLOCKED:
 773             /* Remove thread from the list of blocked threads and add it
 774              * to the scheduler's list of running processes. List removal
 775              * is safe since each object maintains it's own list of
 776              * sleepers and queues protect against reentrancy. */
 777             remove_from_list(list, thread);
 778             add_to_list(cores[IF_COP2(thread->core)].wakeup_list, thread);
 779
 780         case STATE_BLOCKED_W_TMO:
 781             /* Just remove the timeout to cause scheduler to immediately
 782              * wake up the thread. */
 783             thread->statearg = 0;
 784             break;
 785
 786         default:
 787             /* Nothing to do. Thread has already been woken up
 788              * or it's state is not blocked or blocked with timeout. */
 789             return ;
 790     }
 791 }
 792
 793 inline static int find_empty_thread_slot(void)
 794 {
 795     int n;
 796
 797     for (n = 0; n < MAXTHREADS; n++)
 798     {
 799         if (threads[n].name == NULL)
 800             return n;
 801     }
 802
 803     return -1;
 804 }
 805
 806 /* Like wakeup_thread but safe against IRQ corruption when IRQs are disabled
 807    before calling. */
 808 void wakeup_thread_irq_safe(struct thread_entry **list)
 809 {
 810     struct core_entry *core = &cores[CURRENT_CORE];
 811     /* Switch wakeup lists and call wakeup_thread */
 812     core->wakeup_list = &core->waking;
 813     wakeup_thread(list);
 814     /* Switch back to normal running list */
 815     core->wakeup_list = &core->running;
 816 }
 817
 818 /*---------------------------------------------------------------------------
 819  * Create a thread
 820  * If using a dual core architecture, specify which core to start the thread
 821  * on, and whether to fall back to the other core if it can't be created
 822  * Return ID if context area could be allocated, else NULL.
 823  *---------------------------------------------------------------------------
 824  */
 825 struct thread_entry*
 826     create_thread(void (*function)(void), void* stack, int stack_size,
 827                   const char *name IF_PRIO(, int priority)
 828           IF_COP(, unsigned int core, bool fallback))
 829 {
 830     unsigned int i;
 831     unsigned int stacklen;
 832     unsigned int *stackptr;
 833     int slot;
 834     struct regs *regs;
 835     struct thread_entry *thread;
 836
 837 /*****
 838  * Ugly code alert!
 839  * To prevent ifdef hell while keeping the binary size down, we define
 840  * core here if it hasn't been passed as a parameter
 841  *****/
 842 #if NUM_CORES == 1
 843 #define core CPU
 844 #endif
 845
 846 #if NUM_CORES > 1
 847 /* If the kernel hasn't initialised on the COP (most likely due to an old
 848  * bootloader) then refuse to start threads on the COP
 849  */
 850     if ((core == COP) && !cores[core].kernel_running)
 851     {
 852         if (fallback)
 853             return create_thread(function, stack, stack_size, name
 854                                  IF_PRIO(, priority) IF_COP(, CPU, false));
 855         else
 856             return NULL;
 857     }
 858 #endif
 859
 860     lock_cores();
 861
 862     slot = find_empty_thread_slot();
 863     if (slot < 0)
 864     {
 865         unlock_cores();
 866         return NULL;
 867     }
 868
 869     /* Munge the stack to make it easy to spot stack overflows */
 870     stacklen = stack_size / sizeof(int);
 871     stackptr = stack;
 872     for(i = 0;i < stacklen;i++)
 873     {
 874         stackptr[i] = DEADBEEF;
 875     }
 876
 877     /* Store interesting information */
 878     thread = &threads[slot];
 879     thread->name = name;
 880     thread->stack = stack;
 881     thread->stack_size = stack_size;
 882     thread->statearg = 0;
 883 #ifdef HAVE_PRIORITY_SCHEDULING
 884     thread->priority_x = 0;
 885     thread->priority = priority;
 886     cores[core].highest_priority = 100;
 887 #endif
 888
 889 #if NUM_CORES > 1
 890     thread->core = core;
 891
 892     /* Writeback stack munging or anything else before starting */
 893     if (core != CURRENT_CORE)
 894         flush_icache();
 895 #endif
 896
 897     regs = &thread->context;
 898     /* Align stack to an even 32 bit boundary */
 899     regs->sp = (void*)(((unsigned int)stack + stack_size) & ~3);
 900     regs->start = (void*)function;
 901
 902     /* Do any CPU specific inits after initializing common items
 903        to have access to valid data */
 904     THREAD_CPU_INIT(core, thread);
 905
 906     add_to_list(&cores[core].running, thread);
 907     unlock_cores();
 908
 909     return thread;
 910 #if NUM_CORES == 1
 911 #undef core
 912 #endif
 913 }
 914
 915 #ifdef HAVE_SCHEDULER_BOOSTCTRL
 916 void trigger_cpu_boost(void)
 917 {
 918     lock_cores();
 919
 920     if (!STATE_IS_BOOSTED(cores[CURRENT_CORE].running->statearg))
 921     {
 922         SET_BOOST_STATE(cores[CURRENT_CORE].running->statearg);
 923         if (!boosted_threads)
 924         {
 925             cpu_boost(true);
 926         }
 927         boosted_threads++;
 928     }
 929
 930     unlock_cores();
 931 }
 932 #endif
 933
 934 /*---------------------------------------------------------------------------
 935  * Remove a thread on the current core from the scheduler.
 936  * Parameter is the ID as returned from create_thread().
 937  *---------------------------------------------------------------------------
 938  */
 939 void remove_thread(struct thread_entry *thread)
 940 {
 941     lock_cores();
 942
 943     if (thread == NULL)
 944         thread = cores[CURRENT_CORE].running;
 945
 946     /* Free the entry by removing thread name. */
 947     thread->name = NULL;
 948 #ifdef HAVE_PRIORITY_SCHEDULING
 949     cores[IF_COP2(thread->core)].highest_priority = 100;
 950 #endif
 951
 952     if (thread == cores[IF_COP2(thread->core)].running)
 953     {
 954         remove_from_list(&cores[IF_COP2(thread->core)].running, thread);
 955         switch_thread(false, NULL);
 956         return ;
 957     }
 958
 959     if (thread == cores[IF_COP2(thread->core)].sleeping)
 960         remove_from_list(&cores[IF_COP2(thread->core)].sleeping, thread);
 961     else
 962         remove_from_list(NULL, thread);
 963
 964     unlock_cores();
 965 }
 966
 967 #ifdef HAVE_PRIORITY_SCHEDULING
 968 int thread_set_priority(struct thread_entry *thread, int priority)
 969 {
 970     int old_priority;
 971
 972     lock_cores();
 973     if (thread == NULL)
 974         thread = cores[CURRENT_CORE].running;
 975
 976     old_priority = thread->priority;
 977     thread->priority = priority;
 978     cores[IF_COP2(thread->core)].highest_priority = 100;
 979     unlock_cores();
 980
 981     return old_priority;
 982 }
 983
 984 int thread_get_priority(struct thread_entry *thread)
 985 {
 986     if (thread == NULL)
 987         thread = cores[CURRENT_CORE].running;
 988
 989     return thread->priority;
 990 }
 991
 992 void priority_yield(void)
 993 {
 994     struct thread_entry *thread = cores[CURRENT_CORE].running;
 995     thread->priority_x = 1;
 996     switch_thread(true, NULL);
 997     thread->priority_x = 0;
 998 }
 999 #endif /* HAVE_PRIORITY_SCHEDULING */
1000
1001 struct thread_entry * thread_get_current(void)
1002 {
1003     return cores[CURRENT_CORE].running;
1004 }
1005
1006 void init_threads(void)
1007 {
1008     const unsigned int core = CURRENT_CORE;
1009     int slot;
1010
1011     /* Let main CPU initialize first. */
1012 #if NUM_CORES > 1
1013     if (core != CPU)
1014     {
1015         while (!cores[CPU].kernel_running) ;
1016     }
1017 #endif
1018
1019     lock_cores();
1020     slot = find_empty_thread_slot();
1021
1022     cores[core].sleeping = NULL;
1023     cores[core].running = NULL;
1024     cores[core].waking = NULL;
1025     cores[core].wakeup_list = &cores[core].running;
1026 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
1027     cores[core].switch_to_irq_level = STAY_IRQ_LEVEL;
1028 #endif
1029     threads[slot].name = main_thread_name;
1030     threads[slot].statearg = 0;
1031     threads[slot].context.start = 0; /* core's main thread already running */
1032 #if NUM_CORES > 1
1033     threads[slot].core = core;
1034 #endif
1035 #ifdef HAVE_PRIORITY_SCHEDULING
1036     threads[slot].priority = PRIORITY_USER_INTERFACE;
1037     threads[slot].priority_x = 0;
1038     cores[core].highest_priority = 100;
1039 #endif
1040 #ifdef HAVE_SCHEDULER_BOOSTCTRL
1041     boosted_threads = 0;
1042 #endif
1043     add_to_list(&cores[core].running, &threads[slot]);
1044
1045     /* In multiple core setups, each core has a different stack.  There is
1046      * probably a much better way to do this. */
1047     if (core == CPU)
1048     {
1049         threads[slot].stack = stackbegin;
1050         threads[slot].stack_size = (int)stackend - (int)stackbegin;
1051     }
1052 #if NUM_CORES > 1  /* This code path will not be run on single core targets */
1053     else
1054     {
1055         threads[slot].stack = cop_stackbegin;
1056         threads[slot].stack_size =
1057             (int)cop_stackend - (int)cop_stackbegin;
1058     }
1059
1060     cores[core].kernel_running = true;
1061 #endif
1062
1063     unlock_cores();
1064 }
1065
1066 int thread_stack_usage(const struct thread_entry *thread)
1067 {
1068     unsigned int i;
1069     unsigned int *stackptr = thread->stack;
1070
1071     for (i = 0;i < thread->stack_size/sizeof(int);i++)
1072     {
1073         if (stackptr[i] != DEADBEEF)
1074             break;
1075     }
1076
1077     return ((thread->stack_size - i * sizeof(int)) * 100) /
1078         thread->stack_size;
1079 }
1080
1081 int thread_get_status(const struct thread_entry *thread)
1082 {
1083     return GET_STATE(thread->statearg);
1084 }