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 "system.h"
  24 #include "kernel.h"
  25 #include "cpu.h"
  26
  27 #ifdef CPU_COLDFIRE
  28 struct regs
  29 {
  30     unsigned int macsr;  /* EMAC status register */
  31     unsigned int d[6];   /* d2-d7 */
  32     unsigned int a[5];   /* a2-a6 */
  33     void         *sp;    /* Stack pointer (a7) */
  34     void         *start; /* Thread start address, or NULL when started */
  35 };
  36 #elif CONFIG_CPU == SH7034
  37 struct regs
  38 {
  39     unsigned int r[7];   /* Registers r8 thru r14 */
  40     void         *sp;    /* Stack pointer (r15) */
  41     void         *pr;    /* Procedure register */
  42     void         *start; /* Thread start address, or NULL when started */
  43 };
  44 #elif defined(CPU_ARM)
  45 struct regs
  46 {
  47     unsigned int r[8];   /* Registers r4-r11 */
  48     void         *sp;    /* Stack pointer (r13) */
  49     unsigned int lr;     /* r14 (lr) */
  50     void         *start; /* Thread start address, or NULL when started */
  51 };
  52 #elif CONFIG_CPU == TCC730
  53 struct regs
  54 {
  55     void *sp;    /* Stack pointer (a15) */
  56     void *start; /* Thread start address */
  57     int started; /* 0 when not started */
  58 };
  59 #endif
  60
  61 #define DEADBEEF ((unsigned int)0xdeadbeef)
  62 /* Cast to the the machine int type, whose size could be < 4. */
  63
  64
  65 int num_threads;
  66 static volatile int num_sleepers;
  67 static int current_thread;
  68 static struct regs thread_contexts[MAXTHREADS] IBSS_ATTR;
  69 const char *thread_name[MAXTHREADS];
  70 void *thread_stack[MAXTHREADS];
  71 int thread_stack_size[MAXTHREADS];
  72 static const char main_thread_name[] = "main";
  73
  74 extern int stackbegin[];
  75 extern int stackend[];
  76
  77 void switch_thread(void) ICODE_ATTR;
  78 static inline void store_context(void* addr) __attribute__ ((always_inline));
  79 static inline void load_context(const void* addr) __attribute__ ((always_inline));
  80
  81 #ifdef RB_PROFILE
  82 #include <profile.h>
  83 void profile_thread(void) {
  84     profstart(current_thread);
  85 }
  86 #endif
  87
  88 #if defined(CPU_ARM)
  89 /*---------------------------------------------------------------------------
  90  * Store non-volatile context.
  91  *---------------------------------------------------------------------------
  92  */
  93 static inline void store_context(void* addr)
  94 {
  95     asm volatile(
  96         "stmia  %0, { r4-r11, sp, lr }\n"
  97         : : "r" (addr)
  98     );
  99 }
 100
 101 /*---------------------------------------------------------------------------
 102  * Load non-volatile context.
 103  *---------------------------------------------------------------------------
 104  */
 105 static inline void load_context(const void* addr)
 106 {
 107     asm volatile(
 108         "ldmia  %0, { r4-r11, sp, lr }\n" /* load regs r4 to r14 from context */
 109         "ldr    r0, [%0, #40]    \n" /* load start pointer */
 110         "mov    r1, #0           \n"
 111         "cmp    r0, r1           \n" /* check for NULL */
 112         "strne  r1, [%0, #40]    \n" /* if it's NULL, we're already running */
 113         "movne  pc, r0           \n" /* not already running, so jump to start */
 114         : : "r" (addr) : "r0", "r1"
 115     );
 116 }
 117
 118 #elif defined(CPU_COLDFIRE)
 119 /*---------------------------------------------------------------------------
 120  * Store non-volatile context.
 121  *---------------------------------------------------------------------------
 122  */
 123 static inline void store_context(void* addr)
 124 {
 125     asm volatile (
 126         "move.l  %%macsr,%%d0    \n"
 127         "movem.l %%d0/%%d2-%%d7/%%a2-%%a7,(%0)   \n"
 128         : : "a" (addr) : "d0" /* only! */
 129     );
 130 }
 131
 132 /*---------------------------------------------------------------------------
 133  * Load non-volatile context.
 134  *---------------------------------------------------------------------------
 135  */
 136 static inline void load_context(const void* addr)
 137 {
 138     asm volatile (
 139         "movem.l (%0),%%d0/%%d2-%%d7/%%a2-%%a7   \n"  /* Load context */
 140         "move.l  %%d0,%%macsr    \n"
 141         "move.l  (52,%0),%%d0    \n"  /* Get start address */
 142         "beq.b   .running        \n"  /* NULL -> already running */
 143         "clr.l   (52,%0)         \n"  /* Clear start address.. */
 144         "move.l  %%d0,%0         \n"
 145         "jmp     (%0)            \n"  /* ..and start the thread */
 146     ".running:                   \n"
 147         : : "a" (addr) : "d0" /* only! */
 148     );
 149 }
 150
 151 #elif CONFIG_CPU == SH7034
 152 /*---------------------------------------------------------------------------
 153  * Store non-volatile context.
 154  *---------------------------------------------------------------------------
 155  */
 156 static inline void store_context(void* addr)
 157 {
 158     asm volatile (
 159         "add     #36,%0    \n"
 160         "sts.l   pr, @-%0  \n"
 161         "mov.l   r15,@-%0  \n"
 162         "mov.l   r14,@-%0  \n"
 163         "mov.l   r13,@-%0  \n"
 164         "mov.l   r12,@-%0  \n"
 165         "mov.l   r11,@-%0  \n"
 166         "mov.l   r10,@-%0  \n"
 167         "mov.l   r9, @-%0  \n"
 168         "mov.l   r8, @-%0  \n"
 169         : : "r" (addr)
 170     );
 171 }
 172
 173 /*---------------------------------------------------------------------------
 174  * Load non-volatile context.
 175  *---------------------------------------------------------------------------
 176  */
 177 static inline void load_context(const void* addr)
 178 {
 179     asm volatile (
 180         "mov.l   @%0+,r8   \n"
 181         "mov.l   @%0+,r9   \n"
 182         "mov.l   @%0+,r10  \n"
 183         "mov.l   @%0+,r11  \n"
 184         "mov.l   @%0+,r12  \n"
 185         "mov.l   @%0+,r13  \n"
 186         "mov.l   @%0+,r14  \n"
 187         "mov.l   @%0+,r15  \n"
 188         "lds.l   @%0+,pr   \n"
 189         "mov.l   @%0,r0    \n"  /* Get start address */
 190         "tst     r0,r0     \n"
 191         "bt      .running  \n"  /* NULL -> already running */
 192         "lds     r0,pr     \n"
 193         "mov     #0,r0     \n"
 194         "rts               \n"  /* Start the thread */
 195         "mov.l   r0,@%0    \n"  /* Clear start address */
 196     ".running:             \n"
 197         : : "r" (addr) : "r0" /* only! */
 198     );
 199 }
 200
 201 #elif CONFIG_CPU == TCC730
 202 /*---------------------------------------------------------------------------
 203  * Store non-volatile context.
 204  *---------------------------------------------------------------------------
 205  */
 206 #define store_context(addr)                             \
 207     __asm__ volatile (                                  \
 208         "push r0,r1\n\t"                                \
 209         "push r2,r3\n\t"                                \
 210         "push r4,r5\n\t"                                \
 211         "push r6,r7\n\t"                                \
 212         "push a8,a9\n\t"                                \
 213         "push a10,a11\n\t"                              \
 214         "push a12,a13\n\t"                              \
 215         "push a14\n\t"                                  \
 216         "ldw @[%0+0], a15\n\t" : : "a" (addr) );
 217
 218 /*---------------------------------------------------------------------------
 219  * Load non-volatile context.
 220  *---------------------------------------------------------------------------
 221  */
 222 #define load_context(addr)                      \
 223     {                                           \
 224         if (!(addr)->started) {                 \
 225             (addr)->started = 1;                \
 226             __asm__ volatile (                  \
 227                 "ldw a15, @[%0+0]\n\t"          \
 228                 "ldw a14, @[%0+4]\n\t"          \
 229                 "jmp a14\n\t" : : "a" (addr)    \
 230                 );                              \
 231         } else                                  \
 232             __asm__ volatile (                  \
 233                 "ldw a15, @[%0+0]\n\t"          \
 234                 "pop a14\n\t"                   \
 235                 "pop a13,a12\n\t"               \
 236                 "pop a11,a10\n\t"               \
 237                 "pop a9,a8\n\t"                 \
 238                 "pop r7,r6\n\t"                 \
 239                 "pop r5,r4\n\t"                 \
 240                 "pop r3,r2\n\t"                 \
 241                 "pop r1,r0\n\t" : : "a" (addr)  \
 242                 );                              \
 243                                                 \
 244     }
 245
 246 #endif
 247
 248 /*---------------------------------------------------------------------------
 249  * Switch thread in round robin fashion.
 250  *---------------------------------------------------------------------------
 251  */
 252 void switch_thread(void)
 253 {
 254 #ifdef RB_PROFILE
 255     profile_thread_stopped(current_thread);
 256 #endif
 257     int current;
 258     unsigned int *stackptr;
 259
 260 #ifdef SIMULATOR
 261     /* Do nothing */
 262 #else
 263     while (num_sleepers == num_threads)
 264     {
 265         /* Enter sleep mode, woken up on interrupt */
 266 #ifdef CPU_COLDFIRE
 267         asm volatile ("stop #0x2000");
 268 #elif CONFIG_CPU == SH7034
 269         and_b(0x7F, &SBYCR);
 270         asm volatile ("sleep");
 271 #elif CONFIG_CPU == PP5020
 272         /* This should sleep the CPU. It appears to wake by itself on
 273            interrupts */
 274         CPU_CTL = 0x80000000;
 275 #elif CONFIG_CPU == TCC730
 276             /* Sleep mode is triggered by the SYS instr on CalmRisc16.
 277          * Unfortunately, the manual doesn't specify which arg to use.
 278          __asm__ volatile ("sys #0x0f");
 279          0x1f seems to trigger a reset;
 280          0x0f is the only one other argument used by Archos.
 281          */
 282 #endif
 283     }
 284 #endif
 285     current = current_thread;
 286     store_context(&thread_contexts[current]);
 287
 288 #if CONFIG_CPU != TCC730
 289     /* Check if the current thread stack is overflown */
 290     stackptr = thread_stack[current];
 291     if(stackptr[0] != DEADBEEF)
 292        panicf("Stkov %s", thread_name[current]);
 293 #endif
 294
 295     if (++current >= num_threads)
 296         current = 0;
 297
 298     current_thread = current;
 299     load_context(&thread_contexts[current]);
 300 #ifdef RB_PROFILE
 301     profile_thread_started(current_thread);
 302 #endif
 303 }
 304
 305 void sleep_thread(void)
 306 {
 307     ++num_sleepers;
 308     switch_thread();
 309 }
 310
 311 void wake_up_thread(void)
 312 {
 313     num_sleepers = 0;
 314 }
 315
 316 /*---------------------------------------------------------------------------
 317  * Create thread.
 318  * Return ID if context area could be allocated, else -1.
 319  *---------------------------------------------------------------------------
 320  */
 321 int create_thread(void (*function)(void), void* stack, int stack_size,
 322                   const char *name)
 323 {
 324     unsigned int i;
 325     unsigned int stacklen;
 326     unsigned int *stackptr;
 327     struct regs *regs;
 328
 329     if (num_threads >= MAXTHREADS)
 330         return -1;
 331
 332     /* Munge the stack to make it easy to spot stack overflows */
 333     stacklen = stack_size / sizeof(int);
 334     stackptr = stack;
 335     for(i = 0;i < stacklen;i++)
 336     {
 337         stackptr[i] = DEADBEEF;
 338     }
 339
 340     /* Store interesting information */
 341     thread_name[num_threads] = name;
 342     thread_stack[num_threads] = stack;
 343     thread_stack_size[num_threads] = stack_size;
 344     regs = &thread_contexts[num_threads];
 345 #if defined(CPU_COLDFIRE) || (CONFIG_CPU == SH7034) || defined(CPU_ARM)
 346     /* Align stack to an even 32 bit boundary */
 347     regs->sp = (void*)(((unsigned int)stack + stack_size) & ~3);
 348 #elif CONFIG_CPU == TCC730
 349    /* Align stack on word boundary */
 350     regs->sp = (void*)(((unsigned long)stack + stack_size - 2) & ~1);
 351     regs->started = 0;
 352 #endif
 353     regs->start = (void*)function;
 354
 355     wake_up_thread();
 356     return num_threads++; /* return the current ID, e.g for remove_thread() */
 357 }
 358
 359 /*---------------------------------------------------------------------------
 360  * Remove a thread from the scheduler.
 361  * Parameter is the ID as returned from create_thread().
 362  *---------------------------------------------------------------------------
 363  */
 364 void remove_thread(int threadnum)
 365 {
 366     int i;
 367
 368     if(threadnum >= num_threads)
 369        return;
 370
 371     num_threads--;
 372     for (i=threadnum; i<num_threads-1; i++)
 373     {   /* move all entries which are behind */
 374         thread_name[i]       = thread_name[i+1];
 375         thread_stack[i]      = thread_stack[i+1];
 376         thread_stack_size[i] = thread_stack_size[i+1];
 377         thread_contexts[i]   = thread_contexts[i+1];
 378     }
 379
 380     if (current_thread == threadnum) /* deleting the current one? */
 381         current_thread = num_threads; /* set beyond last, avoid store harm */
 382     else if (current_thread > threadnum) /* within the moved positions? */
 383         current_thread--; /* adjust it, point to same context again */
 384 }
 385
 386 void init_threads(void)
 387 {
 388     num_threads = 1; /* We have 1 thread to begin with */
 389     current_thread = 0; /* The current thread is number 0 */
 390     thread_name[0] = main_thread_name;
 391     thread_stack[0] = stackbegin;
 392     thread_stack_size[0] = (int)stackend - (int)stackbegin;
 393 #if CONFIG_CPU == TCC730
 394     thread_contexts[0].started = 1;
 395 #else
 396     thread_contexts[0].start = 0; /* thread 0 already running */
 397 #endif
 398     num_sleepers = 0;
 399 }
 400
 401 int thread_stack_usage(int threadnum)
 402 {
 403     unsigned int i;
 404     unsigned int *stackptr = thread_stack[threadnum];
 405
 406     if(threadnum >= num_threads)
 407         return -1;
 408
 409     for(i = 0;i < thread_stack_size[threadnum]/sizeof(int);i++)
 410     {
 411         if(stackptr[i] != DEADBEEF)
 412             break;
 413     }
 414
 415     return ((thread_stack_size[threadnum] - i * sizeof(int)) * 100) /
 416         thread_stack_size[threadnum];
 417 }