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 "kernel.h"
  24 #include "cpu.h"
  25
  26 #if CONFIG_CPU == MCF5249
  27 struct regs
  28 {
  29     unsigned int d[6]; /* d2-d7 */
  30     unsigned int a[5]; /* a2-a6 */
  31     void         *sp;  /* Stack pointer (a7) */
  32 };
  33 #elif CONFIG_CPU == SH7034
  34 struct regs
  35 {
  36     unsigned int  r[7]; /* Registers r8 thru r14 */
  37     void          *sp;  /* Stack pointer (r15) */
  38     void*         pr;   /* Procedure register */
  39 };
  40 #endif
  41
  42 int num_threads;
  43 static volatile int num_sleepers;
  44 static int current_thread;
  45 static struct regs thread_contexts[MAXTHREADS] __attribute__ ((section(".idata")));
  46 const char *thread_name[MAXTHREADS];
  47 void *thread_stack[MAXTHREADS];
  48 int thread_stack_size[MAXTHREADS];
  49 static const char main_thread_name[] = "main";
  50
  51 extern int stackbegin[];
  52 extern int stackend[];
  53
  54 void switch_thread(void) __attribute__ ((section(".icode")));
  55
  56 #if CONFIG_CPU == MCF5249
  57 /*---------------------------------------------------------------------------
  58  * Store non-volatile context.
  59  *---------------------------------------------------------------------------
  60  */
  61 static inline void store_context(void* addr)
  62 {
  63     asm volatile ("movem.l %%d2-%%d7/%%a2-%%a7,(%0)\n\t" : : "a" (addr));
  64 }
  65
  66 /*---------------------------------------------------------------------------
  67  * Load non-volatile context.
  68  *---------------------------------------------------------------------------
  69  */
  70 static inline void load_context(const void* addr)
  71 {
  72     asm volatile ("movem.l (%0),%%d2-%%d7/%%a2-%%a7\n\t" : : "a" (addr));
  73 }
  74
  75 #elif CONFIG_CPU == SH7034
  76 /*---------------------------------------------------------------------------
  77  * Store non-volatile context.
  78  *---------------------------------------------------------------------------
  79  */
  80 static inline void store_context(void* addr)
  81 {
  82     asm volatile ("add #36, %0\n\t"
  83                   "sts.l pr,  @-%0\n\t"
  84                   "mov.l r15, @-%0\n\t"
  85                   "mov.l r14, @-%0\n\t"
  86                   "mov.l r13, @-%0\n\t"
  87                   "mov.l r12, @-%0\n\t"
  88                   "mov.l r11, @-%0\n\t"
  89                   "mov.l r10, @-%0\n\t"
  90                   "mov.l r9,  @-%0\n\t"
  91                   "mov.l r8,  @-%0" : : "r" (addr));
  92 }
  93
  94 /*---------------------------------------------------------------------------
  95  * Load non-volatile context.
  96  *---------------------------------------------------------------------------
  97  */
  98 static inline void load_context(const void* addr)
  99 {
 100     asm volatile ("mov.l @%0+,r8\n\t"
 101                   "mov.l @%0+,r9\n\t"
 102                   "mov.l @%0+,r10\n\t"
 103                   "mov.l @%0+,r11\n\t"
 104                   "mov.l @%0+,r12\n\t"
 105                   "mov.l @%0+,r13\n\t"
 106                   "mov.l @%0+,r14\n\t"
 107                   "mov.l @%0+,r15\n\t"
 108                   "mov.l @%0,%0\n\t"
 109                   "lds %0,pr\n\t"
 110                   "mov.l %0, @(0, r15)" : "+r" (addr));
 111 }
 112 #endif
 113
 114 /*---------------------------------------------------------------------------
 115  * Switch thread in round robin fashion.
 116  *---------------------------------------------------------------------------
 117  */
 118 void switch_thread(void)
 119 {
 120     int current;
 121     int next;
 122     unsigned int *stackptr;
 123
 124 #ifdef SIMULATOR
 125     /* Do nothing */
 126 #else
 127
 128     while (num_sleepers == num_threads)
 129     {
 130         /* Enter sleep mode, woken up on interrupt */
 131 #if CONFIG_CPU == MCF5249
 132         asm volatile ("stop #0x2000");
 133 #else
 134         SBYCR &= 0x7F;
 135         asm volatile ("sleep");
 136 #endif
 137     }
 138
 139 #endif
 140     next = current = current_thread;
 141
 142     if (++next >= num_threads)
 143         next = 0;
 144     current_thread = next;
 145     store_context(&thread_contexts[current]);
 146
 147     /* Check if the current thread stack is overflown */
 148     stackptr = thread_stack[current];
 149 #ifndef IRIVER_H100
 150     if(stackptr[0] != 0xdeadbeef)
 151        panicf("Stkov %s", thread_name[current]);
 152 #endif
 153
 154     load_context(&thread_contexts[next]);
 155 }
 156
 157 void sleep_thread(void)
 158 {
 159     ++num_sleepers;
 160     switch_thread();
 161 }
 162
 163 void wake_up_thread(void)
 164 {
 165     num_sleepers = 0;
 166 }
 167
 168 /*---------------------------------------------------------------------------
 169  * Create thread.
 170  * Return ID if context area could be allocated, else -1.
 171  *---------------------------------------------------------------------------
 172  */
 173 int create_thread(void* function, void* stack, int stack_size,
 174                   const char *name)
 175 {
 176    unsigned int i;
 177    unsigned int stacklen;
 178    unsigned int *stackptr;
 179    struct regs *regs;
 180
 181    if (num_threads >= MAXTHREADS)
 182       return -1;
 183
 184    /* Munge the stack to make it easy to spot stack overflows */
 185    stacklen = stack_size / 4;
 186    stackptr = stack;
 187    for(i = 0;i < stacklen;i++)
 188    {
 189       stackptr[i] = 0xdeadbeef;
 190    }
 191
 192    /* Store interesting information */
 193    thread_name[num_threads] = name;
 194    thread_stack[num_threads] = stack;
 195    thread_stack_size[num_threads] = stack_size;
 196    regs = &thread_contexts[num_threads];
 197    store_context(regs);
 198 #if CONFIG_CPU == MCF5249
 199    regs->sp = (void*)(((unsigned int)stack + stack_size - 4) & ~3);
 200    /* Put the return address on the stack */
 201    *(unsigned long *)(regs->sp) = (int)function;
 202 #elif CONFIG_CPU == SH7034
 203    /* Subtract 4 to leave room for the PR push in load_context()
 204       Align it on an even 32 bit boundary */
 205    regs->sp = (void*)(((unsigned int)stack + stack_size - 4) & ~3);
 206    regs->pr = function;
 207 #endif
 208
 209    wake_up_thread();
 210    return num_threads++; /* return the current ID, e.g for remove_thread() */
 211 }
 212
 213 /*---------------------------------------------------------------------------
 214  * Remove a thread from the scheduler.
 215  * Parameter is the ID as returned from create_thread().
 216  *---------------------------------------------------------------------------
 217  */
 218 void remove_thread(int threadnum)
 219 {
 220     int i;
 221
 222     if(threadnum >= num_threads)
 223        return;
 224
 225     num_threads--;
 226     for (i=threadnum; i<num_threads-1; i++)
 227     {   /* move all entries which are behind */
 228         thread_name[i]       = thread_name[i+1];
 229         thread_stack[i]      = thread_stack[i+1];
 230         thread_stack_size[i] = thread_stack_size[i+1];
 231         thread_contexts[i]   = thread_contexts[i+1];
 232     }
 233
 234     if (current_thread == threadnum) /* deleting the current one? */
 235         current_thread = num_threads; /* set beyond last, avoid store harm */
 236     else if (current_thread > threadnum) /* within the moved positions? */
 237         current_thread--; /* adjust it, point to same context again */
 238 }
 239
 240 void init_threads(void)
 241 {
 242     num_threads = 1; /* We have 1 thread to begin with */
 243     current_thread = 0; /* The current thread is number 0 */
 244     thread_name[0] = main_thread_name;
 245     thread_stack[0] = stackbegin;
 246     thread_stack_size[0] = (int)stackend - (int)stackbegin;
 247     num_sleepers = 0;
 248 }
 249
 250 int thread_stack_usage(int threadnum)
 251 {
 252    unsigned int i;
 253    unsigned int *stackptr = thread_stack[threadnum];
 254
 255    if(threadnum >= num_threads)
 256       return -1;
 257
 258    for(i = 0;i < thread_stack_size[threadnum]/sizeof(int);i++)
 259    {
 260       if(stackptr[i] != 0xdeadbeef)
 261          break;
 262    }
 263
 264    return ((thread_stack_size[threadnum] - i * 4) * 100) /
 265       thread_stack_size[threadnum];
 266 }