firmware/kernel.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2002 by Björn Stenberg
  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 <stdlib.h>
  20 #include <string.h>
  21 #include "kernel.h"
  22 #include "thread.h"
  23 #include "sh7034.h"
  24 #include "system.h"
  25 #include "panic.h"
  26
  27 long current_tick = 0;
  28
  29 void (*tick_funcs[MAX_NUM_TICK_TASKS])(void);
  30
  31 static void tick_start(unsigned int interval_in_ms);
  32
  33 /* This array holds all queues that are initiated. It is used for broadcast. */
  34 static struct event_queue *all_queues[32];
  35 static int num_queues;
  36
  37 void sleep(int ticks) __attribute__ ((section(".icode")));
  38 void queue_wait(struct event_queue *q, struct event *ev) __attribute__ ((section(".icode")));;
  39
  40 /****************************************************************************
  41  * Standard kernel stuff
  42  ****************************************************************************/
  43 void kernel_init(void)
  44 {
  45     /* Init the threading API */
  46     init_threads();
  47
  48     memset(tick_funcs, 0, sizeof(tick_funcs));
  49
  50     num_queues = 0;
  51     memset(all_queues, 0, sizeof(all_queues));
  52
  53     tick_start(1000/HZ);
  54 }
  55
  56 void sleep(int ticks)
  57 {
  58     /* Always sleep at least 1 tick */
  59     int timeout = current_tick + ticks + 1;
  60
  61     while (TIME_BEFORE( current_tick, timeout )) {
  62         yield();
  63     }
  64 }
  65
  66 void yield(void)
  67 {
  68     switch_thread();
  69 }
  70
  71 /****************************************************************************
  72  * Interrupt level setting
  73  ****************************************************************************/
  74 int set_irq_level(int level)
  75 {
  76     int i;
  77     /* Read the old level and set the new one */
  78     asm volatile ("stc sr, %0" : "=r" (i));
  79     asm volatile ("ldc %0, sr" : : "r" (level << 4));
  80     return (i >> 4) & 0x0f;
  81 }
  82
  83 /****************************************************************************
  84  * Queue handling stuff
  85  ****************************************************************************/
  86 void queue_init(struct event_queue *q)
  87 {
  88     q->read = 0;
  89     q->write = 0;
  90
  91     /* Add it to the all_queues array */
  92     all_queues[num_queues++] = q;
  93 }
  94
  95 void queue_wait(struct event_queue *q, struct event *ev)
  96 {
  97     while(q->read == q->write)
  98     {
  99         switch_thread();
 100     }
 101
 102     *ev = q->events[(q->read++) & QUEUE_LENGTH_MASK];
 103 }
 104
 105 void queue_wait_w_tmo(struct event_queue *q, struct event *ev, int ticks)
 106 {
 107     unsigned int timeout = current_tick + ticks;
 108
 109     while(q->read == q->write && TIME_BEFORE( current_tick, timeout ))
 110     {
 111         switch_thread();
 112     }
 113
 114     if(q->read != q->write)
 115     {
 116         *ev = q->events[(q->read++) & QUEUE_LENGTH_MASK];
 117     }
 118     else
 119     {
 120         ev->id = SYS_TIMEOUT;
 121     }
 122 }
 123
 124 void queue_post(struct event_queue *q, int id, void *data)
 125 {
 126     int wr;
 127     int oldlevel;
 128
 129     oldlevel = set_irq_level(15);
 130     wr = (q->write++) & QUEUE_LENGTH_MASK;
 131
 132     q->events[wr].id = id;
 133     q->events[wr].data = data;
 134     set_irq_level(oldlevel);
 135 }
 136
 137 bool queue_empty(struct event_queue* q)
 138 {
 139     return ( q->read == q->write );
 140 }
 141
 142 int queue_broadcast(int id, void *data)
 143 {
 144    int i;
 145
 146    for(i = 0;i < num_queues;i++)
 147    {
 148       queue_post(all_queues[i], id, data);
 149    }
 150
 151    return num_queues;
 152 }
 153
 154 /****************************************************************************
 155  * Timer tick
 156  ****************************************************************************/
 157 static void tick_start(unsigned int interval_in_ms)
 158 {
 159     unsigned int count;
 160
 161     count = FREQ * interval_in_ms / 1000 / 8;
 162
 163     if(count > 0xffff)
 164     {
 165         panicf("Error! The tick interval is too long (%d ms)\n",
 166                interval_in_ms);
 167         return;
 168     }
 169
 170     /* We are using timer 0 */
 171
 172     TSTR &= ~0x01; /* Stop the timer */
 173     TSNC &= ~0x01; /* No synchronization */
 174     TMDR &= ~0x01; /* Operate normally */
 175
 176     TCNT0 = 0;   /* Start counting at 0 */
 177     GRA0 = count;
 178     TCR0 = 0x23; /* Clear at GRA match, sysclock/8 */
 179
 180     /* Enable interrupt on level 1 */
 181     IPRC = (IPRC & ~0x00f0) | 0x0010;
 182
 183     TSR0 &= ~0x01;
 184     TIER0 = 0xf9; /* Enable GRA match interrupt */
 185
 186     TSTR |= 0x01; /* Start timer 1 */
 187 }
 188
 189 #pragma interrupt
 190 void IMIA0(void)
 191 {
 192     int i;
 193
 194     /* Run through the list of tick tasks */
 195     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
 196     {
 197         if(tick_funcs[i])
 198         {
 199             tick_funcs[i]();
 200         }
 201     }
 202
 203     current_tick++;
 204
 205     TSR0 &= ~0x01;
 206 }
 207
 208 int tick_add_task(void (*f)(void))
 209 {
 210     int i;
 211     int oldlevel = set_irq_level(15);
 212
 213     /* Add a task if there is room */
 214     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
 215     {
 216         if(tick_funcs[i] == NULL)
 217         {
 218             tick_funcs[i] = f;
 219             set_irq_level(oldlevel);
 220             return 0;
 221         }
 222     }
 223     set_irq_level(oldlevel);
 224     panicf("Error! tick_add_task(): out of tasks");
 225     return -1;
 226 }
 227
 228 int tick_remove_task(void (*f)(void))
 229 {
 230     int i;
 231     int oldlevel = set_irq_level(15);
 232
 233     /* Remove a task if it is there */
 234     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
 235     {
 236         if(tick_funcs[i] == f)
 237         {
 238             tick_funcs[i] = NULL;
 239             set_irq_level(oldlevel);
 240             return 0;
 241         }
 242     }
 243
 244     set_irq_level(oldlevel);
 245     return -1;
 246 }
 247
 248 /****************************************************************************
 249  * Simple mutex functions
 250  ****************************************************************************/
 251 void mutex_init(struct mutex *m)
 252 {
 253     m->locked = false;
 254 }
 255
 256 void mutex_lock(struct mutex *m)
 257 {
 258     /* Wait until the lock is open... */
 259     while(m->locked)
 260         yield();
 261
 262     /* ...and lock it */
 263     m->locked = true;
 264 }
 265
 266 void mutex_unlock(struct mutex *m)
 267 {
 268     m->locked = false;
 269 }