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