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 "config.h"
  22 #include "kernel.h"
  23 #include "thread.h"
  24 #include "cpu.h"
  25 #include "system.h"
  26 #include "panic.h"
  27
  28 #if !defined(CPU_PP) || !defined(BOOTLOADER)
  29 long current_tick NOCACHEDATA_ATTR = 0;
  30 #endif
  31
  32 static void (*tick_funcs[MAX_NUM_TICK_TASKS])(void);
  33
  34 /* This array holds all queues that are initiated. It is used for broadcast. */
  35 static struct event_queue *all_queues[32] NOCACHEBSS_ATTR;
  36 static int num_queues NOCACHEBSS_ATTR;
  37
  38 void queue_wait(struct event_queue *q, struct event *ev) ICODE_ATTR;
  39
  40 /****************************************************************************
  41  * Standard kernel stuff
  42  ****************************************************************************/
  43 void kernel_init(void)
  44 {
  45     /* Init the threading API */
  46     init_threads();
  47
  48     if(CURRENT_CORE == CPU)
  49     {
  50         memset(tick_funcs, 0, sizeof(tick_funcs));
  51
  52         num_queues = 0;
  53         memset(all_queues, 0, sizeof(all_queues));
  54     }
  55
  56     tick_start(1000/HZ);
  57 }
  58
  59 void sleep(int ticks)
  60 {
  61 #if CONFIG_CPU == S3C2440 && defined(BOOTLOADER)
  62     volatile int counter;
  63     TCON &= ~(1 << 20); // stop timer 4
  64     // TODO: this constant depends on dividers settings inherited from
  65     // firmware. Set them explicitly somwhere.
  66     TCNTB4 = 12193 * ticks / HZ;
  67     TCON |= 1 << 21; // set manual bit
  68     TCON &= ~(1 << 21); // reset manual bit
  69     TCON &= ~(1 << 22); //autoreload Off
  70     TCON |= (1 << 20); // start timer 4
  71     do {
  72        counter = TCNTO4;
  73     } while(counter > 0);
  74
  75 #elif defined(CPU_PP) && defined(BOOTLOADER)
  76     unsigned stop = USEC_TIMER + ticks * (1000000/HZ);
  77     while (TIME_BEFORE(USEC_TIMER, stop))
  78         switch_thread(true,NULL);
  79 #else
  80     sleep_thread(ticks);
  81 #endif
  82 }
  83
  84 void yield(void)
  85 {
  86 #if ((CONFIG_CPU == S3C2440 || defined(ELIO_TPJ1022)) && defined(BOOTLOADER))
  87     /* Some targets don't like yielding in the bootloader */
  88 #else
  89     switch_thread(true, NULL);
  90 #endif
  91 }
  92
  93 /****************************************************************************
  94  * Queue handling stuff
  95  ****************************************************************************/
  96
  97 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
  98 /* Moves waiting thread's descriptor to the current sender when a
  99    message is dequeued */
 100 static void queue_fetch_sender(struct queue_sender_list *send,
 101                                unsigned int i)
 102 {
 103     /* Disable interrupts to protect against collision in this slot */
 104     int old_level = set_irq_level(HIGHEST_IRQ_LEVEL);
 105     struct thread_entry **spp = &send->senders[i];
 106
 107     if (*spp)
 108     {
 109         send->curr_sender = *spp;
 110         *spp = NULL;
 111     }
 112
 113     set_irq_level(old_level);
 114 }
 115
 116 /* Puts the specified return value in the waiting thread's return value
 117  * and wakes the thread.
 118  * 1) A sender should be confirmed to exist before calling which makes it
 119  *    more efficent to reject the majority of cases that don't need this
 120       called.
 121  * 2) Requires interrupts disabled since queue overflows can cause posts
 122  *    from interrupt handlers to wake threads. Not doing so could cause
 123  *    an attempt at multiple wakes or other problems.
 124  */
 125 static void queue_release_sender(struct thread_entry **sender,
 126                                  intptr_t retval)
 127 {
 128     (*sender)->retval = retval;
 129     wakeup_thread_irq_safe(sender);
 130 #if 0
 131     /* This should _never_ happen - there must never be multiple
 132        threads in this list and it is a corrupt state */
 133     if (*sender != NULL)
 134         panicf("Queue: send slot ovf");
 135 #endif
 136 }
 137
 138 /* Releases any waiting threads that are queued with queue_send -
 139  * reply with 0.
 140  * Disable IRQs before calling since it uses queue_release_sender.
 141  */
 142 static void queue_release_all_senders(struct event_queue *q)
 143 {
 144     if(q->send)
 145     {
 146         unsigned int i;
 147         for(i = q->read; i != q->write; i++)
 148         {
 149             struct thread_entry **spp =
 150                 &q->send->senders[i & QUEUE_LENGTH_MASK];
 151
 152             if(*spp)
 153             {
 154                 queue_release_sender(spp, 0);
 155             }
 156         }
 157     }
 158 }
 159
 160 /* Enables queue_send on the specified queue - caller allocates the extra
 161    data structure */
 162 void queue_enable_queue_send(struct event_queue *q,
 163                              struct queue_sender_list *send)
 164 {
 165     q->send = send;
 166     memset(send, 0, sizeof(struct queue_sender_list));
 167 }
 168 #endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */
 169
 170
 171 void queue_init(struct event_queue *q, bool register_queue)
 172 {
 173     q->read   = 0;
 174     q->write  = 0;
 175     q->thread = NULL;
 176 #if NUM_CORES > 1
 177     q->irq_safe = false;
 178 #endif
 179 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 180     q->send   = NULL; /* No message sending by default */
 181 #endif
 182
 183     if(register_queue)
 184     {
 185         /* Add it to the all_queues array */
 186         all_queues[num_queues++] = q;
 187     }
 188 }
 189
 190 #if NUM_CORES > 1
 191 /**
 192  * If IRQ mode is enabled, some core-wise locking mechanisms are disabled
 193  * causing accessing queue to be no longer thread safe from the other core.
 194  * However, that locking mechanism would also kill IRQ handlers.
 195  *
 196  * @param q struct of an event_queue
 197  * @param state enable/disable IRQ mode
 198  * @default state disabled
 199  */
 200 void queue_set_irq_safe(struct event_queue *q, bool state)
 201 {
 202     q->irq_safe = state;
 203 }
 204 #endif
 205
 206 void queue_delete(struct event_queue *q)
 207 {
 208     int i;
 209     bool found = false;
 210
 211     lock_cores();
 212     wakeup_thread(&q->thread);
 213
 214     /* Find the queue to be deleted */
 215     for(i = 0;i < num_queues;i++)
 216     {
 217         if(all_queues[i] == q)
 218         {
 219             found = true;
 220             break;
 221         }
 222     }
 223
 224     if(found)
 225     {
 226 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 227         /* Release waiting threads and reply to any dequeued message
 228            waiting for one. */
 229         int level = set_irq_level(HIGHEST_IRQ_LEVEL);
 230         queue_release_all_senders(q);
 231         queue_reply(q, NULL);
 232         set_irq_level(level);
 233 #endif
 234         /* Move the following queues up in the list */
 235         for(;i < num_queues-1;i++)
 236         {
 237             all_queues[i] = all_queues[i+1];
 238         }
 239
 240         num_queues--;
 241     }
 242
 243     unlock_cores();
 244 }
 245
 246 void queue_wait(struct event_queue *q, struct event *ev)
 247 {
 248     unsigned int rd;
 249
 250     lock_cores();
 251
 252     if (q->read == q->write)
 253     {
 254         block_thread(&q->thread);
 255         lock_cores();
 256     }
 257
 258     rd = q->read++ & QUEUE_LENGTH_MASK;
 259     *ev = q->events[rd];
 260
 261 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 262     if(q->send && q->send->senders[rd])
 263     {
 264         /* Get data for a waiting thread if one */
 265         queue_fetch_sender(q->send, rd);
 266     }
 267 #endif
 268
 269     unlock_cores();
 270 }
 271
 272 void queue_wait_w_tmo(struct event_queue *q, struct event *ev, int ticks)
 273 {
 274     lock_cores();
 275
 276     if (q->read == q->write && ticks > 0)
 277     {
 278         block_thread_w_tmo(&q->thread, ticks);
 279         lock_cores();
 280     }
 281
 282     if (q->read != q->write)
 283     {
 284         unsigned int rd = q->read++ & QUEUE_LENGTH_MASK;
 285         *ev = q->events[rd];
 286
 287 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 288         if(q->send && q->send->senders[rd])
 289         {
 290             /* Get data for a waiting thread if one */
 291             queue_fetch_sender(q->send, rd);
 292         }
 293 #endif
 294     }
 295     else
 296     {
 297         ev->id = SYS_TIMEOUT;
 298     }
 299
 300     unlock_cores();
 301 }
 302
 303 void queue_post(struct event_queue *q, long id, intptr_t data)
 304 {
 305     int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
 306     unsigned int wr;
 307
 308 #if NUM_CORES > 1
 309     if (!q->irq_safe)
 310         lock_cores();
 311 #endif
 312
 313     wr = q->write++ & QUEUE_LENGTH_MASK;
 314
 315     q->events[wr].id   = id;
 316     q->events[wr].data = data;
 317
 318 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 319     if(q->send)
 320     {
 321         struct thread_entry **spp = &q->send->senders[wr];
 322
 323         if (*spp)
 324         {
 325             /* overflow protect - unblock any thread waiting at this index */
 326             queue_release_sender(spp, 0);
 327         }
 328     }
 329 #endif
 330
 331     wakeup_thread_irq_safe(&q->thread);
 332 #if NUM_CORES > 1
 333     if (!q->irq_safe)
 334         unlock_cores();
 335 #endif
 336     set_irq_level(oldlevel);
 337
 338 }
 339
 340 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 341 /* No wakeup_thread_irq_safe here because IRQ handlers are not allowed
 342    use of this function - we only aim to protect the queue integrity by
 343    turning them off. */
 344 intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
 345 {
 346     int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
 347     unsigned int wr;
 348
 349     lock_cores();
 350
 351     wr = q->write++ & QUEUE_LENGTH_MASK;
 352
 353     q->events[wr].id   = id;
 354     q->events[wr].data = data;
 355
 356     if(q->send)
 357     {
 358         struct thread_entry **spp = &q->send->senders[wr];
 359
 360         if (*spp)
 361         {
 362             /* overflow protect - unblock any thread waiting at this index */
 363             queue_release_sender(spp, 0);
 364         }
 365
 366         wakeup_thread(&q->thread);
 367         set_irq_level_and_block_thread(spp, oldlevel);
 368         return thread_get_current()->retval;
 369     }
 370
 371     /* Function as queue_post if sending is not enabled */
 372     wakeup_thread(&q->thread);
 373     unlock_cores();
 374     set_irq_level(oldlevel);
 375
 376     return 0;
 377 }
 378
 379 #if 0 /* not used now but probably will be later */
 380 /* Query if the last message dequeued was added by queue_send or not */
 381 bool queue_in_queue_send(struct event_queue *q)
 382 {
 383     return q->send && q->send->curr_sender;
 384 }
 385 #endif
 386
 387 /* Replies with retval to any dequeued message sent with queue_send */
 388 void queue_reply(struct event_queue *q, intptr_t retval)
 389 {
 390     lock_cores();
 391     if(q->send && q->send->curr_sender)
 392     {
 393         int level = set_irq_level(HIGHEST_IRQ_LEVEL);
 394         if(q->send->curr_sender)
 395         {
 396             queue_release_sender(&q->send->curr_sender, retval);
 397         }
 398         set_irq_level(level);
 399     }
 400     unlock_cores();
 401 }
 402 #endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */
 403
 404 bool queue_empty(const struct event_queue* q)
 405 {
 406     bool is_empty;
 407
 408 #if NUM_CORES > 1
 409     if (!q->irq_safe)
 410         lock_cores();
 411 #endif
 412
 413     is_empty = ( q->read == q->write );
 414 #if NUM_CORES > 1
 415     if (!q->irq_safe)
 416         unlock_cores();
 417 #endif
 418
 419     return is_empty;
 420 }
 421
 422 void queue_clear(struct event_queue* q)
 423 {
 424     int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
 425
 426 #if NUM_CORES > 1
 427     if (!q->irq_safe)
 428         lock_cores();
 429 #endif
 430
 431 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 432     /* Release all thread waiting in the queue for a reply -
 433        dequeued sent message will be handled by owning thread */
 434     queue_release_all_senders(q);
 435 #endif
 436
 437     q->read = 0;
 438     q->write = 0;
 439
 440 #if NUM_CORES > 1
 441     if (!q->irq_safe)
 442         unlock_cores();
 443 #endif
 444
 445     set_irq_level(oldlevel);
 446 }
 447
 448 void queue_remove_from_head(struct event_queue *q, long id)
 449 {
 450     int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
 451
 452 #if NUM_CORES > 1
 453     if (!q->irq_safe)
 454         lock_cores();
 455 #endif
 456
 457     while(q->read != q->write)
 458     {
 459         unsigned int rd = q->read & QUEUE_LENGTH_MASK;
 460
 461         if(q->events[rd].id != id)
 462         {
 463             break;
 464         }
 465
 466 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 467         if(q->send)
 468         {
 469             struct thread_entry **spp = &q->send->senders[rd];
 470
 471             if (*spp)
 472             {
 473                 /* Release any thread waiting on this message */
 474                 queue_release_sender(spp, 0);
 475             }
 476         }
 477 #endif
 478         q->read++;
 479     }
 480
 481 #if NUM_CORES > 1
 482     if (!q->irq_safe)
 483         unlock_cores();
 484 #endif
 485
 486     set_irq_level(oldlevel);
 487 }
 488
 489 /**
 490  * The number of events waiting in the queue.
 491  *
 492  * @param struct of event_queue
 493  * @return number of events in the queue
 494  */
 495 int queue_count(const struct event_queue *q)
 496 {
 497     int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
 498     int result = 0;
 499
 500 #if NUM_CORES > 1
 501     if (!q->irq_safe)
 502         lock_cores();
 503 #endif
 504
 505     if (q->read <= q->write)
 506         result = q->write - q->read;
 507     else
 508         result = QUEUE_LENGTH - (q->read - q->write);
 509
 510 #if NUM_CORES > 1
 511     if (!q->irq_safe)
 512         unlock_cores();
 513 #endif
 514
 515     set_irq_level(oldlevel);
 516
 517     return result;
 518 }
 519
 520 int queue_broadcast(long id, intptr_t data)
 521 {
 522     int i;
 523
 524     for(i = 0;i < num_queues;i++)
 525     {
 526         queue_post(all_queues[i], id, data);
 527     }
 528
 529     return num_queues;
 530 }
 531
 532 /****************************************************************************
 533  * Timer tick
 534  ****************************************************************************/
 535 #if CONFIG_CPU == SH7034
 536 void tick_start(unsigned int interval_in_ms)
 537 {
 538     unsigned long count;
 539
 540     count = CPU_FREQ * interval_in_ms / 1000 / 8;
 541
 542     if(count > 0x10000)
 543     {
 544         panicf("Error! The tick interval is too long (%d ms)\n",
 545                interval_in_ms);
 546         return;
 547     }
 548
 549     /* We are using timer 0 */
 550
 551     TSTR &= ~0x01; /* Stop the timer */
 552     TSNC &= ~0x01; /* No synchronization */
 553     TMDR &= ~0x01; /* Operate normally */
 554
 555     TCNT0 = 0;   /* Start counting at 0 */
 556     GRA0 = (unsigned short)(count - 1);
 557     TCR0 = 0x23; /* Clear at GRA match, sysclock/8 */
 558
 559     /* Enable interrupt on level 1 */
 560     IPRC = (IPRC & ~0x00f0) | 0x0010;
 561
 562     TSR0 &= ~0x01;
 563     TIER0 = 0xf9; /* Enable GRA match interrupt */
 564
 565     TSTR |= 0x01; /* Start timer 1 */
 566 }
 567
 568 void IMIA0(void) __attribute__ ((interrupt_handler));
 569 void IMIA0(void)
 570 {
 571     int i;
 572
 573     /* Run through the list of tick tasks */
 574     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
 575     {
 576         if(tick_funcs[i])
 577         {
 578             tick_funcs[i]();
 579         }
 580     }
 581
 582     current_tick++;
 583
 584     TSR0 &= ~0x01;
 585 }
 586 #elif defined(CPU_COLDFIRE)
 587 void tick_start(unsigned int interval_in_ms)
 588 {
 589     unsigned long count;
 590     int prescale;
 591
 592     count = CPU_FREQ/2 * interval_in_ms / 1000 / 16;
 593
 594     if(count > 0x10000)
 595     {
 596         panicf("Error! The tick interval is too long (%d ms)\n",
 597                interval_in_ms);
 598         return;
 599     }
 600
 601     prescale = cpu_frequency / CPU_FREQ;
 602     /* Note: The prescaler is later adjusted on-the-fly on CPU frequency
 603        changes within timer.c */
 604
 605     /* We are using timer 0 */
 606
 607     TRR0 = (unsigned short)(count - 1); /* The reference count */
 608     TCN0 = 0; /* reset the timer */
 609     TMR0 = 0x001d | ((unsigned short)(prescale - 1) << 8);
 610            /* restart, CLK/16, enabled, prescaler */
 611
 612     TER0 = 0xff; /* Clear all events */
 613
 614     ICR1 = 0x8c; /* Interrupt on level 3.0 */
 615     IMR &= ~0x200;
 616 }
 617
 618 void TIMER0(void) __attribute__ ((interrupt_handler));
 619 void TIMER0(void)
 620 {
 621     int i;
 622
 623     /* Run through the list of tick tasks */
 624     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
 625     {
 626         if(tick_funcs[i])
 627         {
 628             tick_funcs[i]();
 629         }
 630     }
 631
 632     current_tick++;
 633
 634     TER0 = 0xff; /* Clear all events */
 635 }
 636
 637 #elif defined(CPU_PP)
 638
 639 #ifndef BOOTLOADER
 640 void TIMER1(void)
 641 {
 642     int i;
 643
 644     TIMER1_VAL; /* Read value to ack IRQ */
 645     /* Run through the list of tick tasks (using main core) */
 646     if (CURRENT_CORE == CPU)
 647     {
 648         for (i = 0;i < MAX_NUM_TICK_TASKS;i++)
 649         {
 650             if (tick_funcs[i])
 651             {
 652                 tick_funcs[i]();
 653             }
 654         }
 655
 656         current_tick++;
 657     }
 658 }
 659 #endif
 660
 661 void tick_start(unsigned int interval_in_ms)
 662 {
 663 #ifndef BOOTLOADER
 664     if(CURRENT_CORE == CPU)
 665     {
 666         TIMER1_CFG = 0x0;
 667         TIMER1_VAL;
 668         /* enable timer */
 669         TIMER1_CFG = 0xc0000000 | (interval_in_ms*1000 - 1);
 670         /* unmask interrupt source */
 671         CPU_INT_EN = TIMER1_MASK;
 672     } else {
 673         COP_INT_EN = TIMER1_MASK;
 674     }
 675 #else
 676     /* We don't enable interrupts in the bootloader */
 677     (void)interval_in_ms;
 678 #endif
 679 }
 680
 681 #elif CONFIG_CPU == PNX0101
 682
 683 void timer_handler(void)
 684 {
 685     int i;
 686
 687     /* Run through the list of tick tasks */
 688     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
 689     {
 690         if(tick_funcs[i])
 691             tick_funcs[i]();
 692     }
 693
 694     current_tick++;
 695
 696     TIMER0.clr = 0;
 697 }
 698
 699 void tick_start(unsigned int interval_in_ms)
 700 {
 701     TIMER0.ctrl &= ~0x80; /* Disable the counter */
 702     TIMER0.ctrl |= 0x40;  /* Reload after counting down to zero */
 703     TIMER0.load = 3000000 * interval_in_ms / 1000;
 704     TIMER0.ctrl &= ~0xc;  /* No prescaler */
 705     TIMER0.clr = 1;       /* Clear the interrupt request */
 706
 707     irq_set_int_handler(IRQ_TIMER0, timer_handler);
 708     irq_enable_int(IRQ_TIMER0);
 709
 710     TIMER0.ctrl |= 0x80;  /* Enable the counter */
 711 }
 712 #elif CONFIG_CPU == S3C2440
 713 void tick_start(unsigned int interval_in_ms)
 714 {
 715     TCON &= ~(1 << 20); // stop timer 4
 716     // TODO: this constant depends on dividers settings inherited from
 717     // firmware. Set them explicitly somwhere.
 718     TCNTB4 = 12193 * interval_in_ms / 1000;
 719     TCON |= 1 << 21; // set manual bit
 720     TCON &= ~(1 << 21); // reset manual bit
 721     TCON |= 1 << 22; //interval mode
 722     TCON |= (1 << 20); // start timer 4
 723
 724     INTMOD &= ~(1 << 14); // timer 4 to IRQ mode
 725     INTMSK &= ~(1 << 14); // timer 4 unmask interrupts
 726 }
 727
 728 void timer4(void) {
 729     int i;
 730     /* Run through the list of tick tasks */
 731     for(i = 0; i < MAX_NUM_TICK_TASKS; i++)
 732     {
 733         if(tick_funcs[i])
 734         {
 735             tick_funcs[i]();
 736         }
 737     }
 738
 739     current_tick++;
 740
 741     /* following needs to be fixed.  */
 742     /*wake_up_thread();*/
 743 }
 744 #endif
 745
 746 int tick_add_task(void (*f)(void))
 747 {
 748     int i;
 749     int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
 750
 751     /* Add a task if there is room */
 752     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
 753     {
 754         if(tick_funcs[i] == NULL)
 755         {
 756             tick_funcs[i] = f;
 757             set_irq_level(oldlevel);
 758             return 0;
 759         }
 760     }
 761     set_irq_level(oldlevel);
 762     panicf("Error! tick_add_task(): out of tasks");
 763     return -1;
 764 }
 765
 766 int tick_remove_task(void (*f)(void))
 767 {
 768     int i;
 769     int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
 770
 771     /* Remove a task if it is there */
 772     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
 773     {
 774         if(tick_funcs[i] == f)
 775         {
 776             tick_funcs[i] = NULL;
 777             set_irq_level(oldlevel);
 778             return 0;
 779         }
 780     }
 781
 782     set_irq_level(oldlevel);
 783     return -1;
 784 }
 785
 786 #ifndef SIMULATOR
 787 /*
 788  * Simulator versions in uisimulator/SIMVER/
 789  */
 790
 791 /****************************************************************************
 792  * Simple mutex functions
 793  ****************************************************************************/
 794 void mutex_init(struct mutex *m)
 795 {
 796     m->locked = false;
 797     m->thread = NULL;
 798 }
 799
 800 void mutex_lock(struct mutex *m)
 801 {
 802     if (test_and_set(&m->locked, 1))
 803     {
 804         /* Wait until the lock is open... */
 805         block_thread(&m->thread);
 806     }
 807 }
 808
 809 void mutex_unlock(struct mutex *m)
 810 {
 811     lock_cores();
 812
 813     if (m->thread == NULL)
 814         m->locked = 0;
 815     else
 816         wakeup_thread(&m->thread);
 817
 818     unlock_cores();
 819 }
 820
 821 void spinlock_lock(struct mutex *m)
 822 {
 823     while (test_and_set(&m->locked, 1))
 824     {
 825         /* wait until the lock is open... */
 826         switch_thread(true, NULL);
 827     }
 828 }
 829
 830 void spinlock_unlock(struct mutex *m)
 831 {
 832     m->locked = 0;
 833 }
 834
 835 #endif /* ndef SIMULATOR */