firmware/kernel.c

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