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 "thread.h"
  20 #include "panic.h"
  21
  22 struct regs
  23 {
  24     unsigned int  r[7]; /* Registers r8 thru r14 */
  25     void          *sp;  /* Stack pointer (r15) */
  26     unsigned int  mach;
  27     unsigned int  macl;
  28     unsigned int  sr;   /* Status register */
  29     void*         pr;   /* Procedure register */
  30 };
  31
  32 int num_threads;
  33 static int current_thread;
  34 static struct regs thread_contexts[MAXTHREADS] __attribute__ ((section(".idata")));
  35 char *thread_name[MAXTHREADS];
  36 void *thread_stack[MAXTHREADS];
  37 int thread_stack_size[MAXTHREADS];
  38 static char main_thread_name[] = "main";
  39
  40 extern int stackbegin[];
  41 extern int stackend[];
  42
  43 void switch_thread(void) __attribute__ ((section(".icode")));
  44
  45 /*---------------------------------------------------------------------------
  46  * Store non-volatile context.
  47  *---------------------------------------------------------------------------
  48  */
  49 static inline void store_context(void* addr)
  50 {
  51     asm volatile ("add #48, %0\n\t"
  52                   "sts.l pr,  @-%0\n\t"
  53                   "stc.l sr,  @-%0\n\t"
  54                   "sts.l macl,@-%0\n\t"
  55                   "sts.l mach,@-%0\n\t"
  56                   "mov.l r15, @-%0\n\t"
  57                   "mov.l r14, @-%0\n\t"
  58                   "mov.l r13, @-%0\n\t"
  59                   "mov.l r12, @-%0\n\t"
  60                   "mov.l r11, @-%0\n\t"
  61                   "mov.l r10, @-%0\n\t"
  62                   "mov.l r9,  @-%0\n\t"
  63                   "mov.l r8,  @-%0" : : "r" (addr));
  64 }
  65
  66 /*---------------------------------------------------------------------------
  67  * Load non-volatile context.
  68  *---------------------------------------------------------------------------
  69  */
  70 static inline void load_context(void* addr)
  71 {
  72     asm volatile ("mov.l @%0+,r8\n\t"
  73                   "mov.l @%0+,r9\n\t"
  74                   "mov.l @%0+,r10\n\t"
  75                   "mov.l @%0+,r11\n\t"
  76                   "mov.l @%0+,r12\n\t"
  77                   "mov.l @%0+,r13\n\t"
  78                   "mov.l @%0+,r14\n\t"
  79                   "mov.l @%0+,r15\n\t"
  80                   "lds.l @%0+,mach\n\t"
  81                   "lds.l @%0+,macl\n\t"
  82                   "ldc.l @%0+,sr\n\t"
  83                   "mov.l @%0,%0\n\t"
  84                   "lds %0,pr\n\t"
  85                   "mov.l %0, @(0, r15)" : "+r" (addr));
  86 }
  87
  88 /*---------------------------------------------------------------------------
  89  * Switch thread in round robin fashion.
  90  *---------------------------------------------------------------------------
  91  */
  92 void switch_thread(void)
  93 {
  94     int current;
  95     int next;
  96     unsigned int *stackptr;
  97
  98     next = current = current_thread;
  99     if (++next >= num_threads)
 100         next = 0;
 101     current_thread = next;
 102     store_context(&thread_contexts[current]);
 103     load_context(&thread_contexts[next]);
 104
 105     stackptr = thread_stack[next];
 106
 107     if(stackptr[0] != 0xdeadbeef)
 108        panicf("Stkov %s", thread_name[next]);
 109 }
 110
 111 /*---------------------------------------------------------------------------
 112  * Create thread.
 113  * Return 0 if context area could be allocated, else -1.
 114  *---------------------------------------------------------------------------
 115  */
 116 int create_thread(void* function, void* stack, int stack_size, char *name)
 117 {
 118     unsigned int i;
 119     unsigned int stacklen;
 120     unsigned int *stackptr;
 121     struct regs *regs;
 122
 123     if (num_threads >= MAXTHREADS)
 124         return -1;
 125     else
 126     {
 127         /* Munge the stack to make it easy to spot stack overflows */
 128         stacklen = stack_size / 4;
 129         stackptr = stack;
 130         for(i = 0;i < stacklen;i++)
 131         {
 132             stackptr[i] = 0xdeadbeef;
 133         }
 134
 135         /* Store interesting information */
 136         thread_name[num_threads] = name;
 137         thread_stack[num_threads] = stack;
 138         thread_stack_size[num_threads] = stack_size;
 139         regs = &thread_contexts[num_threads++];
 140         store_context(regs);
 141         /* Subtract 4 to leave room for the PR push in load_context()
 142            Align it on an even 32 bit boundary */
 143         regs->sp = (void*)(((unsigned int)stack + stack_size - 4) & ~3);
 144         regs->sr = 0;
 145         regs->pr = function;
 146     }
 147     return 0;
 148 }
 149
 150 void init_threads(void)
 151 {
 152     num_threads = 1; /* We have 1 thread to begin with */
 153     current_thread = 0; /* The current thread is number 0 */
 154     thread_name[0] = main_thread_name;
 155     thread_stack[0] = stackbegin;
 156     thread_stack_size[0] = (int)stackend - (int)stackbegin;
 157 }
 158
 159 int thread_stack_usage(int threadnum)
 160 {
 161    unsigned int i;
 162    unsigned int *stackptr = thread_stack[threadnum];
 163
 164    if(threadnum >= num_threads)
 165       return -1;
 166
 167    for(i = 0;i < thread_stack_size[threadnum]/sizeof(int);i++)
 168    {
 169       if(stackptr[i] != 0xdeadbeef)
 170          break;
 171    }
 172
 173    return ((thread_stack_size[threadnum] - i * 4) * 100) /
 174       thread_stack_size[threadnum];
 175 }