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