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