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