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