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