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  * This program is free software; you can redistribute it and/or
  13  * modify it under the terms of the GNU General Public License
  14  * as published by the Free Software Foundation; either version 2
  15  * of the License, or (at your option) any later version.
  16  *
  17  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  18  * KIND, either express or implied.
  19  *
  20  ****************************************************************************/
  21 #include <stdlib.h>
  22 #include <string.h>
  23 #include "config.h"
  24 #include "kernel.h"
  25 #include "thread.h"
  26 #include "cpu.h"
  27 #include "system.h"
  28 #include "panic.h"
  29 #include "debug.h"
  30 #include "general.h"
  31
  32 /* Make this nonzero to enable more elaborate checks on objects */
  33 #if defined(DEBUG) || defined(SIMULATOR)
  34 #define KERNEL_OBJECT_CHECKS 1 /* Always 1 for DEBUG and sim*/
  35 #else
  36 #define KERNEL_OBJECT_CHECKS 0
  37 #endif
  38
  39 #if KERNEL_OBJECT_CHECKS
  40 #ifdef SIMULATOR
  41 #define KERNEL_ASSERT(exp, msg...) \
  42     ({ if (!({ exp; })) { DEBUGF(msg); exit(-1); } })
  43 #else
  44 #define KERNEL_ASSERT(exp, msg...) \
  45     ({ if (!({ exp; })) panicf(msg); })
  46 #endif
  47 #else
  48 #define KERNEL_ASSERT(exp, msg...) ({})
  49 #endif
  50
  51 #if !defined(CPU_PP) || !defined(BOOTLOADER)
  52 volatile long current_tick SHAREDDATA_ATTR = 0;
  53 #endif
  54
  55 /* List of tick tasks - final element always NULL for termination */
  56 void (*tick_funcs[MAX_NUM_TICK_TASKS+1])(void);
  57
  58 /* This array holds all queues that are initiated. It is used for broadcast. */
  59 static struct
  60 {
  61     struct event_queue *queues[MAX_NUM_QUEUES+1];
  62     IF_COP( struct corelock cl; )
  63 } all_queues SHAREDBSS_ATTR;
  64
  65 /****************************************************************************
  66  * Standard kernel stuff
  67  ****************************************************************************/
  68 void kernel_init(void)
  69 {
  70     /* Init the threading API */
  71     init_threads();
  72
  73     /* Other processors will not reach this point in a multicore build.
  74      * In a single-core build with multiple cores they fall-through and
  75      * sleep in cop_main without returning. */
  76     if (CURRENT_CORE == CPU)
  77     {
  78         memset(tick_funcs, 0, sizeof(tick_funcs));
  79         memset(&all_queues, 0, sizeof(all_queues));
  80         corelock_init(&all_queues.cl);
  81         tick_start(1000/HZ);
  82 #ifdef KDEV_INIT
  83         kernel_device_init();
  84 #endif
  85     }
  86 }
  87
  88 /****************************************************************************
  89  * Timer tick - Timer initialization and interrupt handler is defined at
  90  * the target level.
  91  ****************************************************************************/
  92 int tick_add_task(void (*f)(void))
  93 {
  94     int oldlevel = disable_irq_save();
  95     void **arr = (void **)tick_funcs;
  96     void **p = find_array_ptr(arr, f);
  97
  98     /* Add a task if there is room */
  99     if(p - arr < MAX_NUM_TICK_TASKS)
 100     {
 101         *p = f; /* If already in list, no problem. */
 102     }
 103     else
 104     {
 105         panicf("Error! tick_add_task(): out of tasks");
 106     }
 107
 108     restore_irq(oldlevel);
 109     return 0;
 110 }
 111
 112 int tick_remove_task(void (*f)(void))
 113 {
 114     int oldlevel = disable_irq_save();
 115     int rc = remove_array_ptr((void **)tick_funcs, f);
 116     restore_irq(oldlevel);
 117     return rc;
 118 }
 119
 120 /****************************************************************************
 121  * Tick-based interval timers/one-shots - be mindful this is not really
 122  * intended for continuous timers but for events that need to run for a short
 123  * time and be cancelled without further software intervention.
 124  ****************************************************************************/
 125 #ifdef INCLUDE_TIMEOUT_API
 126 /* list of active timeout events */
 127 static struct timeout *tmo_list[MAX_NUM_TIMEOUTS+1];
 128
 129 /* timeout tick task - calls event handlers when they expire
 130  * Event handlers may alter expiration, callback and data during operation.
 131  */
 132 static void timeout_tick(void)
 133 {
 134     unsigned long tick = current_tick;
 135     struct timeout **p = tmo_list;
 136     struct timeout *curr;
 137
 138     for(curr = *p; curr != NULL; curr = *(++p))
 139     {
 140         int ticks;
 141
 142         if(TIME_BEFORE(tick, curr->expires))
 143             continue;
 144
 145         /* this event has expired - call callback */
 146         ticks = curr->callback(curr);
 147         if(ticks > 0)
 148         {
 149             curr->expires = tick + ticks; /* reload */
 150         }
 151         else
 152         {
 153             timeout_cancel(curr); /* cancel */
 154         }
 155     }
 156 }
 157
 158 /* Cancels a timeout callback - can be called from the ISR */
 159 void timeout_cancel(struct timeout *tmo)
 160 {
 161     int oldlevel = disable_irq_save();
 162     void **arr = (void **)tmo_list;
 163     int rc = remove_array_ptr(arr, tmo);
 164
 165     if(rc >= 0 && *arr == NULL)
 166     {
 167         tick_remove_task(timeout_tick); /* Last one - remove task */
 168     }
 169
 170     restore_irq(oldlevel);
 171 }
 172
 173 /* Adds a timeout callback - calling with an active timeout resets the
 174    interval - can be called from the ISR */
 175 void timeout_register(struct timeout *tmo, timeout_cb_type callback,
 176                       int ticks, intptr_t data)
 177 {
 178     int oldlevel;
 179     void **arr, **p;
 180
 181     if(tmo == NULL)
 182         return;
 183
 184     oldlevel = disable_irq_save();
 185
 186     /* See if this one is already registered */
 187     arr = (void **)tmo_list;
 188     p = find_array_ptr(arr, tmo);
 189
 190     if(p - arr < MAX_NUM_TIMEOUTS)
 191     {
 192         /* Vacancy */
 193         if(*p == NULL)
 194         {
 195             /* Not present */
 196             if(*arr == NULL)
 197             {
 198                 tick_add_task(timeout_tick); /* First one - add task */
 199             }
 200
 201             *p = tmo;
 202         }
 203
 204         tmo->callback = callback;
 205         tmo->data = data;
 206         tmo->expires = current_tick + ticks;
 207     }
 208
 209     restore_irq(oldlevel);
 210 }
 211
 212 #endif /* INCLUDE_TIMEOUT_API */
 213
 214 /****************************************************************************
 215  * Thread stuff
 216  ****************************************************************************/
 217 void sleep(int ticks)
 218 {
 219 #if defined(CPU_PP) && defined(BOOTLOADER)
 220     unsigned stop = USEC_TIMER + ticks * (1000000/HZ);
 221     while (TIME_BEFORE(USEC_TIMER, stop))
 222         switch_thread();
 223 #elif defined(CREATIVE_ZVx) && defined(BOOTLOADER)
 224     /* hacky.. */
 225     long sleep_ticks = current_tick + ticks + 1;
 226     while (TIME_BEFORE(current_tick, sleep_ticks))
 227         switch_thread();
 228 #else
 229     disable_irq();
 230     sleep_thread(ticks);
 231     switch_thread();
 232 #endif
 233 }
 234
 235 void yield(void)
 236 {
 237 #if ((defined(TATUNG_TPJ1022)) && defined(BOOTLOADER))
 238     /* Some targets don't like yielding in the bootloader */
 239 #else
 240     switch_thread();
 241 #endif
 242 }
 243
 244 /****************************************************************************
 245  * Queue handling stuff
 246  ****************************************************************************/
 247
 248 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 249 /****************************************************************************
 250  * Sender thread queue structure that aids implementation of priority
 251  * inheritance on queues because the send list structure is the same as
 252  * for all other kernel objects:
 253  *
 254  * Example state:
 255  * E0 added with queue_send and removed by thread via queue_wait(_w_tmo)
 256  * E3 was posted with queue_post
 257  * 4 events remain enqueued (E1-E4)
 258  *
 259  *                                 rd                          wr
 260  * q->events[]:          |  XX  |  E1  |  E2  |  E3  |  E4  |  XX  |
 261  * q->send->senders[]:   | NULL |  T1  |  T2  | NULL |  T3  | NULL |
 262  *                                 \/     \/            \/
 263  * q->send->list:       >->|T0|<->|T1|<->|T2|<-------->|T3|<-<
 264  * q->send->curr_sender:    /\
 265  *
 266  * Thread has E0 in its own struct queue_event.
 267  *
 268  ****************************************************************************/
 269
 270 /* Puts the specified return value in the waiting thread's return value
 271  * and wakes the thread.
 272  *
 273  * A sender should be confirmed to exist before calling which makes it
 274  * more efficent to reject the majority of cases that don't need this
 275  * called.
 276  */
 277 static void queue_release_sender(struct thread_entry **sender,
 278                                  intptr_t retval)
 279 {
 280     struct thread_entry *thread = *sender;
 281
 282     *sender = NULL;               /* Clear slot. */
 283 #ifdef HAVE_WAKEUP_EXT_CB
 284     thread->wakeup_ext_cb = NULL; /* Clear callback. */
 285 #endif
 286     thread->retval = retval;      /* Assign thread-local return value. */
 287     *thread->bqp = thread;        /* Move blocking queue head to thread since
 288                                      wakeup_thread wakes the first thread in
 289                                      the list. */
 290     wakeup_thread(thread->bqp);
 291 }
 292
 293 /* Releases any waiting threads that are queued with queue_send -
 294  * reply with 0.
 295  */
 296 static void queue_release_all_senders(struct event_queue *q)
 297 {
 298     if(q->send)
 299     {
 300         unsigned int i;
 301         for(i = q->read; i != q->write; i++)
 302         {
 303             struct thread_entry **spp =
 304                 &q->send->senders[i & QUEUE_LENGTH_MASK];
 305
 306             if(*spp)
 307             {
 308                 queue_release_sender(spp, 0);
 309             }
 310         }
 311     }
 312 }
 313
 314 /* Callback to do extra forced removal steps from sender list in addition
 315  * to the normal blocking queue removal and priority dis-inherit */
 316 static void queue_remove_sender_thread_cb(struct thread_entry *thread)
 317 {
 318     *((struct thread_entry **)thread->retval) = NULL;
 319 #ifdef HAVE_WAKEUP_EXT_CB
 320     thread->wakeup_ext_cb = NULL;
 321 #endif
 322     thread->retval = 0;
 323 }
 324
 325 /* Enables queue_send on the specified queue - caller allocates the extra
 326  * data structure. Only queues which are taken to be owned by a thread should
 327  * enable this however an official owner is not compulsory but must be
 328  * specified for priority inheritance to operate.
 329  *
 330  * Use of queue_wait(_w_tmo) by multiple threads on a queue using synchronous
 331  * messages results in an undefined order of message replies or possible default
 332  * replies if two or more waits happen before a reply is done.
 333  */
 334 void queue_enable_queue_send(struct event_queue *q,
 335                              struct queue_sender_list *send,
 336                              unsigned int owner_id)
 337 {
 338     int oldlevel = disable_irq_save();
 339     corelock_lock(&q->cl);
 340
 341     if(send != NULL && q->send == NULL)
 342     {
 343         memset(send, 0, sizeof(*send));
 344 #ifdef HAVE_PRIORITY_SCHEDULING
 345         send->blocker.wakeup_protocol = wakeup_priority_protocol_release;
 346         send->blocker.priority = PRIORITY_IDLE;
 347         if(owner_id != 0)
 348         {
 349             send->blocker.thread = thread_id_entry(owner_id);
 350             q->blocker_p = &send->blocker;
 351         }
 352 #endif
 353         q->send = send;
 354     }
 355
 356     corelock_unlock(&q->cl);
 357     restore_irq(oldlevel);
 358
 359     (void)owner_id;
 360 }
 361
 362 /* Unblock a blocked thread at a given event index */
 363 static inline void queue_do_unblock_sender(struct queue_sender_list *send,
 364                                            unsigned int i)
 365 {
 366     if(send)
 367     {
 368         struct thread_entry **spp = &send->senders[i];
 369
 370         if(UNLIKELY(*spp))
 371         {
 372             queue_release_sender(spp, 0);
 373         }
 374     }
 375 }
 376
 377 /* Perform the auto-reply sequence */
 378 static inline void queue_do_auto_reply(struct queue_sender_list *send)
 379 {
 380     if(send && send->curr_sender)
 381     {
 382         /* auto-reply */
 383         queue_release_sender(&send->curr_sender, 0);
 384     }
 385 }
 386
 387 /* Moves waiting thread's refrence from the senders array to the
 388  * current_sender which represents the thread waiting for a reponse to the
 389  * last message removed from the queue. This also protects the thread from
 390  * being bumped due to overflow which would not be a valid action since its
 391  * message _is_ being processed at this point. */
 392 static inline void queue_do_fetch_sender(struct queue_sender_list *send,
 393                                          unsigned int rd)
 394 {
 395     if(send)
 396     {
 397         struct thread_entry **spp = &send->senders[rd];
 398
 399         if(*spp)
 400         {
 401             /* Move thread reference from array to the next thread
 402                that queue_reply will release */
 403             send->curr_sender = *spp;
 404             (*spp)->retval = (intptr_t)spp;
 405             *spp = NULL;
 406         }
 407         /* else message was posted asynchronously with queue_post */
 408     }
 409 }
 410 #else
 411 /* Empty macros for when synchoronous sending is not made */
 412 #define queue_release_all_senders(q)
 413 #define queue_do_unblock_sender(send, i)
 414 #define queue_do_auto_reply(send)
 415 #define queue_do_fetch_sender(send, rd)
 416 #endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */
 417
 418 /* Queue must not be available for use during this call */
 419 void queue_init(struct event_queue *q, bool register_queue)
 420 {
 421     int oldlevel = disable_irq_save();
 422
 423     if(register_queue)
 424     {
 425         corelock_lock(&all_queues.cl);
 426     }
 427
 428     corelock_init(&q->cl);
 429     q->queue = NULL;
 430     q->read = 0;
 431     q->write = 0;
 432 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 433     q->send = NULL; /* No message sending by default */
 434     IF_PRIO( q->blocker_p = NULL; )
 435 #endif
 436
 437     if(register_queue)
 438     {
 439         void **queues = (void **)all_queues.queues;
 440         void **p = find_array_ptr(queues, q);
 441
 442         if(p - queues >= MAX_NUM_QUEUES)
 443         {
 444             panicf("queue_init->out of queues");
 445         }
 446
 447         if(*p == NULL)
 448         {
 449             /* Add it to the all_queues array */
 450             *p = q;
 451             corelock_unlock(&all_queues.cl);
 452         }
 453     }
 454
 455     restore_irq(oldlevel);
 456 }
 457
 458 /* Queue must not be available for use during this call */
 459 void queue_delete(struct event_queue *q)
 460 {
 461     int oldlevel = disable_irq_save();
 462     corelock_lock(&all_queues.cl);
 463     corelock_lock(&q->cl);
 464
 465     /* Remove the queue if registered */
 466     remove_array_ptr((void **)all_queues.queues, q);
 467
 468     corelock_unlock(&all_queues.cl);
 469
 470     /* Release thread(s) waiting on queue head */
 471     thread_queue_wake(&q->queue);
 472
 473 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 474     if(q->send)
 475     {
 476         /* Release threads waiting for replies */
 477         queue_release_all_senders(q);
 478
 479         /* Reply to any dequeued message waiting for one */
 480         queue_do_auto_reply(q->send);
 481
 482         q->send = NULL;
 483         IF_PRIO( q->blocker_p = NULL; )
 484     }
 485 #endif
 486
 487     q->read = 0;
 488     q->write = 0;
 489
 490     corelock_unlock(&q->cl);
 491     restore_irq(oldlevel);
 492 }
 493
 494 /* NOTE: multiple threads waiting on a queue head cannot have a well-
 495    defined release order if timeouts are used. If multiple threads must
 496    access the queue head, use a dispatcher or queue_wait only. */
 497 void queue_wait(struct event_queue *q, struct queue_event *ev)
 498 {
 499     int oldlevel;
 500     unsigned int rd;
 501
 502 #ifdef HAVE_PRIORITY_SCHEDULING
 503     KERNEL_ASSERT(QUEUE_GET_THREAD(q) == NULL ||
 504                   QUEUE_GET_THREAD(q) == thread_id_entry(THREAD_ID_CURRENT),
 505                   "queue_wait->wrong thread\n");
 506 #endif
 507
 508     oldlevel = disable_irq_save();
 509     corelock_lock(&q->cl);
 510
 511     /* auto-reply */
 512     queue_do_auto_reply(q->send);
 513
 514     if (q->read == q->write)
 515     {
 516         struct thread_entry *current = thread_id_entry(THREAD_ID_CURRENT);
 517
 518         do
 519         {
 520             IF_COP( current->obj_cl = &q->cl; )
 521             current->bqp = &q->queue;
 522
 523             block_thread(current);
 524
 525             corelock_unlock(&q->cl);
 526             switch_thread();
 527
 528             oldlevel = disable_irq_save();
 529             corelock_lock(&q->cl);
 530         }
 531         /* A message that woke us could now be gone */
 532         while (q->read == q->write);
 533     }
 534
 535     rd = q->read++ & QUEUE_LENGTH_MASK;
 536     *ev = q->events[rd];
 537
 538     /* Get data for a waiting thread if one */
 539     queue_do_fetch_sender(q->send, rd);
 540
 541     corelock_unlock(&q->cl);
 542     restore_irq(oldlevel);
 543 }
 544
 545 void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
 546 {
 547     int oldlevel;
 548
 549 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 550     KERNEL_ASSERT(QUEUE_GET_THREAD(q) == NULL ||
 551                   QUEUE_GET_THREAD(q) == thread_id_entry(THREAD_ID_CURRENT),
 552                   "queue_wait_w_tmo->wrong thread\n");
 553 #endif
 554
 555     oldlevel = disable_irq_save();
 556     corelock_lock(&q->cl);
 557
 558     /* Auto-reply */
 559     queue_do_auto_reply(q->send);
 560
 561     if (q->read == q->write && ticks > 0)
 562     {
 563         struct thread_entry *current = thread_id_entry(THREAD_ID_CURRENT);
 564
 565         IF_COP( current->obj_cl = &q->cl; )
 566         current->bqp = &q->queue;
 567
 568         block_thread_w_tmo(current, ticks);
 569         corelock_unlock(&q->cl);
 570
 571         switch_thread();
 572
 573         oldlevel = disable_irq_save();
 574         corelock_lock(&q->cl);
 575     }
 576
 577     /* no worry about a removed message here - status is checked inside
 578        locks - perhaps verify if timeout or false alarm */
 579     if (q->read != q->write)
 580     {
 581         unsigned int rd = q->read++ & QUEUE_LENGTH_MASK;
 582         *ev = q->events[rd];
 583         /* Get data for a waiting thread if one */
 584         queue_do_fetch_sender(q->send, rd);
 585     }
 586     else
 587     {
 588         ev->id = SYS_TIMEOUT;
 589     }
 590
 591     corelock_unlock(&q->cl);
 592     restore_irq(oldlevel);
 593 }
 594
 595 void queue_post(struct event_queue *q, long id, intptr_t data)
 596 {
 597     int oldlevel;
 598     unsigned int wr;
 599
 600     oldlevel = disable_irq_save();
 601     corelock_lock(&q->cl);
 602
 603     wr = q->write++ & QUEUE_LENGTH_MASK;
 604
 605     q->events[wr].id   = id;
 606     q->events[wr].data = data;
 607
 608     /* overflow protect - unblock any thread waiting at this index */
 609     queue_do_unblock_sender(q->send, wr);
 610
 611     /* Wakeup a waiting thread if any */
 612     wakeup_thread(&q->queue);
 613
 614     corelock_unlock(&q->cl);
 615     restore_irq(oldlevel);
 616 }
 617
 618 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 619 /* IRQ handlers are not allowed use of this function - we only aim to
 620    protect the queue integrity by turning them off. */
 621 intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
 622 {
 623     int oldlevel;
 624     unsigned int wr;
 625
 626     oldlevel = disable_irq_save();
 627     corelock_lock(&q->cl);
 628
 629     wr = q->write++ & QUEUE_LENGTH_MASK;
 630
 631     q->events[wr].id   = id;
 632     q->events[wr].data = data;
 633
 634     if(LIKELY(q->send))
 635     {
 636         struct queue_sender_list *send = q->send;
 637         struct thread_entry **spp = &send->senders[wr];
 638         struct thread_entry *current = thread_id_entry(THREAD_ID_CURRENT);
 639
 640         if(UNLIKELY(*spp))
 641         {
 642             /* overflow protect - unblock any thread waiting at this index */
 643             queue_release_sender(spp, 0);
 644         }
 645
 646         /* Wakeup a waiting thread if any */
 647         wakeup_thread(&q->queue);
 648
 649         /* Save thread in slot, add to list and wait for reply */
 650         *spp = current;
 651         IF_COP( current->obj_cl = &q->cl; )
 652         IF_PRIO( current->blocker = q->blocker_p; )
 653 #ifdef HAVE_WAKEUP_EXT_CB
 654         current->wakeup_ext_cb = queue_remove_sender_thread_cb;
 655 #endif
 656         current->retval = (intptr_t)spp;
 657         current->bqp = &send->list;
 658
 659         block_thread(current);
 660
 661         corelock_unlock(&q->cl);
 662         switch_thread();
 663
 664         return current->retval;
 665     }
 666
 667     /* Function as queue_post if sending is not enabled */
 668     wakeup_thread(&q->queue);
 669
 670     corelock_unlock(&q->cl);
 671     restore_irq(oldlevel);
 672
 673     return 0;
 674 }
 675
 676 #if 0 /* not used now but probably will be later */
 677 /* Query if the last message dequeued was added by queue_send or not */
 678 bool queue_in_queue_send(struct event_queue *q)
 679 {
 680     bool in_send;
 681
 682 #if NUM_CORES > 1
 683     int oldlevel = disable_irq_save();
 684     corelock_lock(&q->cl);
 685 #endif
 686
 687     in_send = q->send && q->send->curr_sender;
 688
 689 #if NUM_CORES > 1
 690     corelock_unlock(&q->cl);
 691     restore_irq(oldlevel);
 692 #endif
 693
 694     return in_send;
 695 }
 696 #endif
 697
 698 /* Replies with retval to the last dequeued message sent with queue_send */
 699 void queue_reply(struct event_queue *q, intptr_t retval)
 700 {
 701     if(q->send && q->send->curr_sender)
 702     {
 703         int oldlevel = disable_irq_save();
 704         corelock_lock(&q->cl);
 705         /* Double-check locking */
 706         IF_COP( if(LIKELY(q->send && q->send->curr_sender)) )
 707         {
 708             queue_release_sender(&q->send->curr_sender, retval);
 709         }
 710
 711         corelock_unlock(&q->cl);
 712         restore_irq(oldlevel);
 713     }
 714 }
 715
 716 bool queue_peek(struct event_queue *q, struct queue_event *ev)
 717 {
 718     if(q->read == q->write)
 719          return false;
 720
 721     bool have_msg = false;
 722
 723     int oldlevel = disable_irq_save();
 724     corelock_lock(&q->cl);
 725
 726     if(q->read != q->write)
 727     {
 728         *ev = q->events[q->read & QUEUE_LENGTH_MASK];
 729         have_msg = true;
 730     }
 731
 732     corelock_unlock(&q->cl);
 733     restore_irq(oldlevel);
 734
 735     return have_msg;
 736 }
 737 #endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */
 738
 739 /* Poll queue to see if a message exists - careful in using the result if
 740  * queue_remove_from_head is called when messages are posted - possibly use
 741  * queue_wait_w_tmo(&q, 0) in that case or else a removed message that
 742  * unsignals the queue may cause an unwanted block */
 743 bool queue_empty(const struct event_queue* q)
 744 {
 745     return ( q->read == q->write );
 746 }
 747
 748 void queue_clear(struct event_queue* q)
 749 {
 750     int oldlevel;
 751
 752     oldlevel = disable_irq_save();
 753     corelock_lock(&q->cl);
 754
 755     /* Release all threads waiting in the queue for a reply -
 756        dequeued sent message will be handled by owning thread */
 757     queue_release_all_senders(q);
 758
 759     q->read = 0;
 760     q->write = 0;
 761
 762     corelock_unlock(&q->cl);
 763     restore_irq(oldlevel);
 764 }
 765
 766 void queue_remove_from_head(struct event_queue *q, long id)
 767 {
 768     int oldlevel;
 769
 770     oldlevel = disable_irq_save();
 771     corelock_lock(&q->cl);
 772
 773     while(q->read != q->write)
 774     {
 775         unsigned int rd = q->read & QUEUE_LENGTH_MASK;
 776
 777         if(q->events[rd].id != id)
 778         {
 779             break;
 780         }
 781
 782         /* Release any thread waiting on this message */
 783         queue_do_unblock_sender(q->send, rd);
 784
 785         q->read++;
 786     }
 787
 788     corelock_unlock(&q->cl);
 789     restore_irq(oldlevel);
 790 }
 791
 792 /**
 793  * The number of events waiting in the queue.
 794  *
 795  * @param struct of event_queue
 796  * @return number of events in the queue
 797  */
 798 int queue_count(const struct event_queue *q)
 799 {
 800     return q->write - q->read;
 801 }
 802
 803 int queue_broadcast(long id, intptr_t data)
 804 {
 805     struct event_queue **p = all_queues.queues;
 806     struct event_queue *q;
 807
 808 #if NUM_CORES > 1
 809     int oldlevel = disable_irq_save();
 810     corelock_lock(&all_queues.cl);
 811 #endif
 812
 813     for(q = *p; q != NULL; q = *(++p))
 814     {
 815         queue_post(q, id, data);
 816     }
 817
 818 #if NUM_CORES > 1
 819     corelock_unlock(&all_queues.cl);
 820     restore_irq(oldlevel);
 821 #endif
 822
 823     return p - all_queues.queues;
 824 }
 825
 826 /****************************************************************************
 827  * Simple mutex functions ;)
 828  ****************************************************************************/
 829
 830 static inline void mutex_set_thread(struct mutex *mtx, struct thread_entry *td)
 831         __attribute__((always_inline));
 832 static inline void mutex_set_thread(struct mutex *mtx, struct thread_entry *td)
 833 {
 834 #ifdef HAVE_PRIORITY_SCHEDULING
 835     mtx->blocker.thread = td;
 836 #else
 837     mtx->thread = td;
 838 #endif
 839 }
 840
 841 static inline struct thread_entry* mutex_get_thread(struct mutex *mtx)
 842         __attribute__((always_inline));
 843 static inline struct thread_entry* mutex_get_thread(struct mutex *mtx)
 844 {
 845 #ifdef HAVE_PRIORITY_SCHEDULING
 846     return mtx->blocker.thread;
 847 #else
 848     return mtx->thread;
 849 #endif
 850 }
 851
 852 /* Initialize a mutex object - call before any use and do not call again once
 853  * the object is available to other threads */
 854 void mutex_init(struct mutex *m)
 855 {
 856     corelock_init(&m->cl);
 857     m->queue = NULL;
 858     m->count = 0;
 859     m->locked = false;
 860     mutex_set_thread(m, NULL);
 861 #ifdef HAVE_PRIORITY_SCHEDULING
 862     m->blocker.priority = PRIORITY_IDLE;
 863     m->blocker.wakeup_protocol = wakeup_priority_protocol_transfer;
 864     m->no_preempt = false;
 865 #endif
 866 }
 867
 868 /* Gain ownership of a mutex object or block until it becomes free */
 869 void mutex_lock(struct mutex *m)
 870 {
 871     struct thread_entry *current = thread_id_entry(THREAD_ID_CURRENT);
 872
 873     if(current == mutex_get_thread(m))
 874     {
 875         /* current thread already owns this mutex */
 876         m->count++;
 877         return;
 878     }
 879
 880     /* lock out other cores */
 881     corelock_lock(&m->cl);
 882
 883     if(LIKELY(!m->locked))
 884     {
 885         /* lock is open */
 886         mutex_set_thread(m, current);
 887         m->locked = true;
 888         corelock_unlock(&m->cl);
 889         return;
 890     }
 891
 892     /* block until the lock is open... */
 893     IF_COP( current->obj_cl = &m->cl; )
 894     IF_PRIO( current->blocker = &m->blocker; )
 895     current->bqp = &m->queue;
 896
 897     disable_irq();
 898     block_thread(current);
 899
 900     corelock_unlock(&m->cl);
 901
 902     /* ...and turn control over to next thread */
 903     switch_thread();
 904 }
 905
 906 /* Release ownership of a mutex object - only owning thread must call this */
 907 void mutex_unlock(struct mutex *m)
 908 {
 909     /* unlocker not being the owner is an unlocking violation */
 910     KERNEL_ASSERT(mutex_get_thread(m) == thread_id_entry(THREAD_ID_CURRENT),
 911                   "mutex_unlock->wrong thread (%s != %s)\n",
 912                   mutex_get_thread(m)->name,
 913                   thread_id_entry(THREAD_ID_CURRENT)->name);
 914
 915     if(m->count > 0)
 916     {
 917         /* this thread still owns lock */
 918         m->count--;
 919         return;
 920     }
 921
 922     /* lock out other cores */
 923     corelock_lock(&m->cl);
 924
 925     /* transfer to next queued thread if any */
 926     if(LIKELY(m->queue == NULL))
 927     {
 928         /* no threads waiting - open the lock */
 929         mutex_set_thread(m, NULL);
 930         m->locked = false;
 931         corelock_unlock(&m->cl);
 932         return;
 933     }
 934     else
 935     {
 936         const int oldlevel = disable_irq_save();
 937         /* Tranfer of owning thread is handled in the wakeup protocol
 938          * if priorities are enabled otherwise just set it from the
 939          * queue head. */
 940         IFN_PRIO( mutex_set_thread(m, m->queue); )
 941         IF_PRIO( unsigned int result = ) wakeup_thread(&m->queue);
 942         restore_irq(oldlevel);
 943
 944         corelock_unlock(&m->cl);
 945
 946 #ifdef HAVE_PRIORITY_SCHEDULING
 947         if((result & THREAD_SWITCH) && !m->no_preempt)
 948             switch_thread();
 949 #endif
 950     }
 951 }
 952
 953 /****************************************************************************
 954  * Simple semaphore functions ;)
 955  ****************************************************************************/
 956 #ifdef HAVE_SEMAPHORE_OBJECTS
 957 void semaphore_init(struct semaphore *s, int max, int start)
 958 {
 959     KERNEL_ASSERT(max > 0 && start >= 0 && start <= max,
 960                   "semaphore_init->inv arg\n");
 961     s->queue = NULL;
 962     s->max = max;
 963     s->count = start;
 964     corelock_init(&s->cl);
 965 }
 966
 967 void semaphore_wait(struct semaphore *s)
 968 {
 969     struct thread_entry *current;
 970
 971     corelock_lock(&s->cl);
 972
 973     if(LIKELY(--s->count >= 0))
 974     {
 975         /* wait satisfied */
 976         corelock_unlock(&s->cl);
 977         return;
 978     }
 979
 980     /* too many waits - block until dequeued... */
 981     current = thread_id_entry(THREAD_ID_CURRENT);
 982
 983     IF_COP( current->obj_cl = &s->cl; )
 984     current->bqp = &s->queue;
 985
 986     disable_irq();
 987     block_thread(current);
 988
 989     corelock_unlock(&s->cl);
 990
 991     /* ...and turn control over to next thread */
 992     switch_thread();
 993 }
 994
 995 void semaphore_release(struct semaphore *s)
 996 {
 997     IF_PRIO( unsigned int result = THREAD_NONE; )
 998
 999     corelock_lock(&s->cl);
1000
1001     if(s->count < s->max && ++s->count <= 0)
1002     {
1003         /* there should be threads in this queue */
1004         KERNEL_ASSERT(s->queue != NULL, "semaphore->wakeup\n");
1005         /* a thread was queued - wake it up */
1006         int oldlevel = disable_irq_save();
1007         IF_PRIO( result = ) wakeup_thread(&s->queue);
1008         restore_irq(oldlevel);
1009     }
1010
1011     corelock_unlock(&s->cl);
1012
1013 #ifdef HAVE_PRIORITY_SCHEDULING
1014     if(result & THREAD_SWITCH)
1015         switch_thread();
1016 #endif
1017 }
1018 #endif /* HAVE_SEMAPHORE_OBJECTS */
1019
1020 #ifdef HAVE_WAKEUP_OBJECTS
1021 /****************************************************************************
1022  * Lightweight IRQ-compatible wakeup object
1023  */
1024
1025 /* Initialize the wakeup object */
1026 void wakeup_init(struct wakeup *w)
1027 {
1028     w->queue = NULL;
1029     w->signalled = false;
1030     IF_COP( corelock_init(&w->cl); )
1031 }
1032
1033 /* Wait for a signal blocking indefinitely or for a specified period */
1034 int wakeup_wait(struct wakeup *w, int timeout)
1035 {
1036     int ret = OBJ_WAIT_SUCCEEDED; /* Presume success */
1037     int oldlevel = disable_irq_save();
1038
1039     corelock_lock(&w->cl);
1040
1041     if(LIKELY(!w->signalled && timeout != TIMEOUT_NOBLOCK))
1042     {
1043         struct thread_entry * current = thread_id_entry(THREAD_ID_CURRENT);
1044
1045         IF_COP( current->obj_cl = &w->cl; )
1046         current->bqp = &w->queue;
1047
1048         if (timeout != TIMEOUT_BLOCK)
1049             block_thread_w_tmo(current, timeout);
1050         else
1051             block_thread(current);
1052
1053         corelock_unlock(&w->cl);
1054         switch_thread();
1055
1056         oldlevel = disable_irq_save();
1057         corelock_lock(&w->cl);
1058     }
1059
1060     if(UNLIKELY(!w->signalled))
1061     {
1062         /* Timed-out or failed */
1063         ret = (timeout != TIMEOUT_BLOCK) ?
1064             OBJ_WAIT_TIMEDOUT : OBJ_WAIT_FAILED;
1065     }
1066
1067     w->signalled = false;  /* Reset */
1068
1069     corelock_unlock(&w->cl);
1070     restore_irq(oldlevel);
1071
1072     return ret;
1073 }
1074
1075 /* Signal the thread waiting or leave the signal if the thread hasn't
1076  * waited yet.
1077  *
1078  * returns THREAD_NONE or THREAD_OK
1079  */
1080 int wakeup_signal(struct wakeup *w)
1081 {
1082     int oldlevel = disable_irq_save();
1083     int ret;
1084
1085     corelock_lock(&w->cl);
1086
1087     w->signalled = true;
1088     ret = wakeup_thread(&w->queue);
1089
1090     corelock_unlock(&w->cl);
1091     restore_irq(oldlevel);
1092
1093     return ret;
1094 }
1095 #endif /* HAVE_WAKEUP_OBJECTS */