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