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  * All files in this archive are subject to the GNU General Public License.
  13  * See the file COPYING in the source tree root for full license agreement.
  14  *
  15  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  16  * KIND, either express or implied.
  17  *
  18  ****************************************************************************/
  19 #include <stdlib.h>
  20 #include <string.h>
  21 #include "config.h"
  22 #include "kernel.h"
  23 #ifdef SIMULATOR
  24 #include "system-sdl.h"
  25 #include "debug.h"
  26 #endif
  27 #include "thread.h"
  28 #include "cpu.h"
  29 #include "system.h"
  30 #include "panic.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 void (*tick_funcs[MAX_NUM_TICK_TASKS])(void);
  56
  57 extern struct core_entry cores[NUM_CORES];
  58
  59 /* This array holds all queues that are initiated. It is used for broadcast. */
  60 static struct
  61 {
  62     int count;
  63     struct event_queue *queues[MAX_NUM_QUEUES];
  64     IF_COP( struct corelock cl; )
  65 } all_queues SHAREDBSS_ATTR;
  66
  67 /****************************************************************************
  68  * Standard kernel stuff
  69  ****************************************************************************/
  70 void kernel_init(void)
  71 {
  72     /* Init the threading API */
  73     init_threads();
  74
  75     /* Other processors will not reach this point in a multicore build.
  76      * In a single-core build with multiple cores they fall-through and
  77      * sleep in cop_main without returning. */
  78     if (CURRENT_CORE == CPU)
  79     {
  80         memset(tick_funcs, 0, sizeof(tick_funcs));
  81         memset(&all_queues, 0, sizeof(all_queues));
  82         corelock_init(&all_queues.cl);
  83         tick_start(1000/HZ);
  84 #ifdef KDEV_INIT
  85         kernel_device_init();
  86 #endif
  87     }
  88 }
  89
  90 /****************************************************************************
  91  * Timer tick
  92  ****************************************************************************/
  93 #if CONFIG_CPU == SH7034
  94 void tick_start(unsigned int interval_in_ms)
  95 {
  96     unsigned long count;
  97
  98     count = CPU_FREQ * interval_in_ms / 1000 / 8;
  99
 100     if(count > 0x10000)
 101     {
 102         panicf("Error! The tick interval is too long (%d ms)\n",
 103                interval_in_ms);
 104         return;
 105     }
 106
 107     /* We are using timer 0 */
 108
 109     TSTR &= ~0x01; /* Stop the timer */
 110     TSNC &= ~0x01; /* No synchronization */
 111     TMDR &= ~0x01; /* Operate normally */
 112
 113     TCNT0 = 0;   /* Start counting at 0 */
 114     GRA0 = (unsigned short)(count - 1);
 115     TCR0 = 0x23; /* Clear at GRA match, sysclock/8 */
 116
 117     /* Enable interrupt on level 1 */
 118     IPRC = (IPRC & ~0x00f0) | 0x0010;
 119
 120     TSR0 &= ~0x01;
 121     TIER0 = 0xf9; /* Enable GRA match interrupt */
 122
 123     TSTR |= 0x01; /* Start timer 1 */
 124 }
 125
 126 void IMIA0(void) __attribute__ ((interrupt_handler));
 127 void IMIA0(void)
 128 {
 129     int i;
 130
 131     /* Run through the list of tick tasks */
 132     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
 133     {
 134         if(tick_funcs[i])
 135         {
 136             tick_funcs[i]();
 137         }
 138     }
 139
 140     current_tick++;
 141
 142     TSR0 &= ~0x01;
 143 }
 144 #elif defined(CPU_COLDFIRE)
 145 void tick_start(unsigned int interval_in_ms)
 146 {
 147     unsigned long count;
 148     int prescale;
 149
 150     count = CPU_FREQ/2 * interval_in_ms / 1000 / 16;
 151
 152     if(count > 0x10000)
 153     {
 154         panicf("Error! The tick interval is too long (%d ms)\n",
 155                interval_in_ms);
 156         return;
 157     }
 158
 159     prescale = cpu_frequency / CPU_FREQ;
 160     /* Note: The prescaler is later adjusted on-the-fly on CPU frequency
 161        changes within timer.c */
 162
 163     /* We are using timer 0 */
 164
 165     TRR0 = (unsigned short)(count - 1); /* The reference count */
 166     TCN0 = 0; /* reset the timer */
 167     TMR0 = 0x001d | ((unsigned short)(prescale - 1) << 8);
 168            /* restart, CLK/16, enabled, prescaler */
 169
 170     TER0 = 0xff; /* Clear all events */
 171
 172     ICR1 = 0x8c; /* Interrupt on level 3.0 */
 173     IMR &= ~0x200;
 174 }
 175
 176 void TIMER0(void) __attribute__ ((interrupt_handler));
 177 void TIMER0(void)
 178 {
 179     int i;
 180
 181     /* Run through the list of tick tasks */
 182     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
 183     {
 184         if(tick_funcs[i])
 185         {
 186             tick_funcs[i]();
 187         }
 188     }
 189
 190     current_tick++;
 191
 192     TER0 = 0xff; /* Clear all events */
 193 }
 194
 195 #elif defined(CPU_PP)
 196
 197 #ifndef BOOTLOADER
 198 void TIMER1(void)
 199 {
 200     int i;
 201
 202     /* Run through the list of tick tasks (using main core) */
 203     TIMER1_VAL; /* Read value to ack IRQ */
 204
 205     /* Run through the list of tick tasks using main CPU core -
 206        wake up the COP through its control interface to provide pulse */
 207     for (i = 0;i < MAX_NUM_TICK_TASKS;i++)
 208     {
 209         if (tick_funcs[i])
 210         {
 211             tick_funcs[i]();
 212         }
 213     }
 214
 215 #if NUM_CORES > 1
 216     /* Pulse the COP */
 217     core_wake(COP);
 218 #endif /* NUM_CORES */
 219
 220     current_tick++;
 221 }
 222 #endif
 223
 224 /* Must be last function called init kernel/thread initialization */
 225 void tick_start(unsigned int interval_in_ms)
 226 {
 227 #ifndef BOOTLOADER
 228     TIMER1_CFG = 0x0;
 229     TIMER1_VAL;
 230     /* enable timer */
 231     TIMER1_CFG = 0xc0000000 | (interval_in_ms*1000 - 1);
 232     /* unmask interrupt source */
 233     CPU_INT_EN = TIMER1_MASK;
 234 #else
 235     /* We don't enable interrupts in the bootloader */
 236     (void)interval_in_ms;
 237 #endif
 238 }
 239
 240 #elif CONFIG_CPU == PNX0101
 241
 242 void timer_handler(void)
 243 {
 244     int i;
 245
 246     /* Run through the list of tick tasks */
 247     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
 248     {
 249         if(tick_funcs[i])
 250             tick_funcs[i]();
 251     }
 252
 253     current_tick++;
 254
 255     TIMER0.clr = 0;
 256 }
 257
 258 void tick_start(unsigned int interval_in_ms)
 259 {
 260     TIMER0.ctrl &= ~0x80; /* Disable the counter */
 261     TIMER0.ctrl |= 0x40;  /* Reload after counting down to zero */
 262     TIMER0.load = 3000000 * interval_in_ms / 1000;
 263     TIMER0.ctrl &= ~0xc;  /* No prescaler */
 264     TIMER0.clr = 1;       /* Clear the interrupt request */
 265
 266     irq_set_int_handler(IRQ_TIMER0, timer_handler);
 267     irq_enable_int(IRQ_TIMER0);
 268
 269     TIMER0.ctrl |= 0x80;  /* Enable the counter */
 270 }
 271 #endif
 272
 273 int tick_add_task(void (*f)(void))
 274 {
 275     int i;
 276     int oldlevel = disable_irq_save();
 277
 278     /* Add a task if there is room */
 279     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
 280     {
 281         if(tick_funcs[i] == NULL)
 282         {
 283             tick_funcs[i] = f;
 284             restore_irq(oldlevel);
 285             return 0;
 286         }
 287     }
 288     restore_irq(oldlevel);
 289     panicf("Error! tick_add_task(): out of tasks");
 290     return -1;
 291 }
 292
 293 int tick_remove_task(void (*f)(void))
 294 {
 295     int i;
 296     int oldlevel = disable_irq_save();
 297
 298     /* Remove a task if it is there */
 299     for(i = 0;i < MAX_NUM_TICK_TASKS;i++)
 300     {
 301         if(tick_funcs[i] == f)
 302         {
 303             tick_funcs[i] = NULL;
 304             restore_irq(oldlevel);
 305             return 0;
 306         }
 307     }
 308
 309     restore_irq(oldlevel);
 310     return -1;
 311 }
 312
 313 /****************************************************************************
 314  * Tick-based interval timers/one-shots - be mindful this is not really
 315  * intended for continuous timers but for events that need to run for a short
 316  * time and be cancelled without further software intervention.
 317  ****************************************************************************/
 318 #ifdef INCLUDE_TIMEOUT_API
 319 static struct timeout *tmo_list = NULL; /* list of active timeout events */
 320
 321 /* timeout tick task - calls event handlers when they expire
 322  * Event handlers may alter ticks, callback and data during operation.
 323  */
 324 static void timeout_tick(void)
 325 {
 326     unsigned long tick = current_tick;
 327     struct timeout *curr, *next;
 328
 329     for (curr = tmo_list; curr != NULL; curr = next)
 330     {
 331         next = (struct timeout *)curr->next;
 332
 333         if (TIME_BEFORE(tick, curr->expires))
 334             continue;
 335
 336         /* this event has expired - call callback */
 337         if (curr->callback(curr))
 338             *(long *)&curr->expires = tick + curr->ticks; /* reload */
 339         else
 340             timeout_cancel(curr); /* cancel */
 341     }
 342 }
 343
 344 /* Cancels a timeout callback - can be called from the ISR */
 345 void timeout_cancel(struct timeout *tmo)
 346 {
 347     int oldlevel = disable_irq_save();
 348
 349     if (tmo_list != NULL)
 350     {
 351         struct timeout *curr = tmo_list;
 352         struct timeout *prev = NULL;
 353
 354         while (curr != tmo && curr != NULL)
 355         {
 356             prev = curr;
 357             curr = (struct timeout *)curr->next;
 358         }
 359
 360         if (curr != NULL)
 361         {
 362             /* in list */
 363             if (prev == NULL)
 364                 tmo_list = (struct timeout *)curr->next;
 365             else
 366                 *(const struct timeout **)&prev->next = curr->next;
 367
 368             if (tmo_list == NULL)
 369                 tick_remove_task(timeout_tick); /* last one - remove task */
 370         }
 371         /* not in list or tmo == NULL */
 372     }
 373
 374     restore_irq(oldlevel);
 375 }
 376
 377 /* Adds a timeout callback - calling with an active timeout resets the
 378    interval - can be called from the ISR */
 379 void timeout_register(struct timeout *tmo, timeout_cb_type callback,
 380                       int ticks, intptr_t data)
 381 {
 382     int oldlevel;
 383     struct timeout *curr;
 384
 385     if (tmo == NULL)
 386         return;
 387
 388     oldlevel = disable_irq_save();
 389
 390     /* see if this one is already registered */
 391     curr = tmo_list;
 392     while (curr != tmo && curr != NULL)
 393         curr = (struct timeout *)curr->next;
 394
 395     if (curr == NULL)
 396     {
 397         /* not found - add it */
 398         if (tmo_list == NULL)
 399             tick_add_task(timeout_tick); /* first one - add task */
 400
 401         *(struct timeout **)&tmo->next = tmo_list;
 402         tmo_list = tmo;
 403     }
 404
 405     tmo->callback = callback;
 406     tmo->ticks = ticks;
 407     tmo->data = data;
 408     *(long *)&tmo->expires = current_tick + ticks;
 409
 410     restore_irq(oldlevel);
 411 }
 412
 413 #endif /* INCLUDE_TIMEOUT_API */
 414
 415 /****************************************************************************
 416  * Thread stuff
 417  ****************************************************************************/
 418 void sleep(int ticks)
 419 {
 420 #if CONFIG_CPU == S3C2440 && defined(BOOTLOADER)
 421     volatile int counter;
 422     TCON &= ~(1 << 20); // stop timer 4
 423     // TODO: this constant depends on dividers settings inherited from
 424     // firmware. Set them explicitly somwhere.
 425     TCNTB4 = 12193 * ticks / HZ;
 426     TCON |= 1 << 21; // set manual bit
 427     TCON &= ~(1 << 21); // reset manual bit
 428     TCON &= ~(1 << 22); //autoreload Off
 429     TCON |= (1 << 20); // start timer 4
 430     do {
 431        counter = TCNTO4;
 432     } while(counter > 0);
 433
 434 #elif defined(CPU_PP) && defined(BOOTLOADER)
 435     unsigned stop = USEC_TIMER + ticks * (1000000/HZ);
 436     while (TIME_BEFORE(USEC_TIMER, stop))
 437         switch_thread();
 438 #else
 439     disable_irq();
 440     sleep_thread(ticks);
 441     switch_thread();
 442 #endif
 443 }
 444
 445 void yield(void)
 446 {
 447 #if ((CONFIG_CPU == S3C2440 || defined(ELIO_TPJ1022)) && defined(BOOTLOADER))
 448     /* Some targets don't like yielding in the bootloader */
 449 #else
 450     switch_thread();
 451 #endif
 452 }
 453
 454 /****************************************************************************
 455  * Queue handling stuff
 456  ****************************************************************************/
 457
 458 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 459 /****************************************************************************
 460  * Sender thread queue structure that aids implementation of priority
 461  * inheritance on queues because the send list structure is the same as
 462  * for all other kernel objects:
 463  *
 464  * Example state:
 465  * E0 added with queue_send and removed by thread via queue_wait(_w_tmo)
 466  * E3 was posted with queue_post
 467  * 4 events remain enqueued (E1-E4)
 468  *
 469  *                                 rd                          wr
 470  * q->events[]:          |  XX  |  E1  |  E2  |  E3  |  E4  |  XX  |
 471  * q->send->senders[]:   | NULL |  T1  |  T2  | NULL |  T3  | NULL |
 472  *                                 \/     \/            \/
 473  * q->send->list:       >->|T0|<->|T1|<->|T2|<-------->|T3|<-<
 474  * q->send->curr_sender:    /\
 475  *
 476  * Thread has E0 in its own struct queue_event.
 477  *
 478  ****************************************************************************/
 479
 480 /* Puts the specified return value in the waiting thread's return value
 481  * and wakes the thread.
 482  *
 483  * A sender should be confirmed to exist before calling which makes it
 484  * more efficent to reject the majority of cases that don't need this
 485  * called.
 486  */
 487 static void queue_release_sender(struct thread_entry **sender,
 488                                  intptr_t retval)
 489 {
 490     struct thread_entry *thread = *sender;
 491
 492     *sender = NULL;               /* Clear slot. */
 493     thread->wakeup_ext_cb = NULL; /* Clear callback. */
 494     thread->retval = retval;      /* Assign thread-local return value. */
 495     *thread->bqp = thread;        /* Move blocking queue head to thread since
 496                                      wakeup_thread wakes the first thread in
 497                                      the list. */
 498     wakeup_thread(thread->bqp);
 499 }
 500
 501 /* Releases any waiting threads that are queued with queue_send -
 502  * reply with 0.
 503  */
 504 static void queue_release_all_senders(struct event_queue *q)
 505 {
 506     if(q->send)
 507     {
 508         unsigned int i;
 509         for(i = q->read; i != q->write; i++)
 510         {
 511             struct thread_entry **spp =
 512                 &q->send->senders[i & QUEUE_LENGTH_MASK];
 513
 514             if(*spp)
 515             {
 516                 queue_release_sender(spp, 0);
 517             }
 518         }
 519     }
 520 }
 521
 522 /* Callback to do extra forced removal steps from sender list in addition
 523  * to the normal blocking queue removal and priority dis-inherit */
 524 static void queue_remove_sender_thread_cb(struct thread_entry *thread)
 525 {
 526     *((struct thread_entry **)thread->retval) = NULL;
 527     thread->wakeup_ext_cb = NULL;
 528     thread->retval = 0;
 529 }
 530
 531 /* Enables queue_send on the specified queue - caller allocates the extra
 532  * data structure. Only queues which are taken to be owned by a thread should
 533  * enable this however an official owner is not compulsory but must be
 534  * specified for priority inheritance to operate.
 535  *
 536  * Use of queue_wait(_w_tmo) by multiple threads on a queue using synchronous
 537  * messages results in an undefined order of message replies.
 538  */
 539 void queue_enable_queue_send(struct event_queue *q,
 540                              struct queue_sender_list *send,
 541                              struct thread_entry *owner)
 542 {
 543     int oldlevel = disable_irq_save();
 544     corelock_lock(&q->cl);
 545
 546     if(send != NULL && q->send == NULL)
 547     {
 548         memset(send, 0, sizeof(*send));
 549 #ifdef HAVE_PRIORITY_SCHEDULING
 550         send->blocker.wakeup_protocol = wakeup_priority_protocol_release;
 551         send->blocker.priority = PRIORITY_IDLE;
 552         send->blocker.thread = owner;
 553         if(owner != NULL)
 554             q->blocker_p = &send->blocker;
 555 #endif
 556         q->send = send;
 557     }
 558
 559     corelock_unlock(&q->cl);
 560     restore_irq(oldlevel);
 561
 562     (void)owner;
 563 }
 564
 565 /* Unblock a blocked thread at a given event index */
 566 static inline void queue_do_unblock_sender(struct queue_sender_list *send,
 567                                            unsigned int i)
 568 {
 569     if(send)
 570     {
 571         struct thread_entry **spp = &send->senders[i];
 572
 573         if(*spp)
 574         {
 575             queue_release_sender(spp, 0);
 576         }
 577     }
 578 }
 579
 580 /* Perform the auto-reply sequence */
 581 static inline void queue_do_auto_reply(struct queue_sender_list *send)
 582 {
 583     if(send && send->curr_sender)
 584     {
 585         /* auto-reply */
 586         queue_release_sender(&send->curr_sender, 0);
 587     }
 588 }
 589
 590 /* Moves waiting thread's refrence from the senders array to the
 591  * current_sender which represents the thread waiting for a reponse to the
 592  * last message removed from the queue. This also protects the thread from
 593  * being bumped due to overflow which would not be a valid action since its
 594  * message _is_ being processed at this point. */
 595 static inline void queue_do_fetch_sender(struct queue_sender_list *send,
 596                                          unsigned int rd)
 597 {
 598     if(send)
 599     {
 600         struct thread_entry **spp = &send->senders[rd];
 601
 602         if(*spp)
 603         {
 604             /* Move thread reference from array to the next thread
 605                that queue_reply will release */
 606             send->curr_sender = *spp;
 607             (*spp)->retval = (intptr_t)spp;
 608             *spp = NULL;
 609         }
 610         /* else message was posted asynchronously with queue_post */
 611     }
 612 }
 613 #else
 614 /* Empty macros for when synchoronous sending is not made */
 615 #define queue_release_all_senders(q)
 616 #define queue_do_unblock_sender(send, i)
 617 #define queue_do_auto_reply(send)
 618 #define queue_do_fetch_sender(send, rd)
 619 #endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */
 620
 621 /* Queue must not be available for use during this call */
 622 void queue_init(struct event_queue *q, bool register_queue)
 623 {
 624     int oldlevel = disable_irq_save();
 625
 626     if(register_queue)
 627     {
 628         corelock_lock(&all_queues.cl);
 629     }
 630
 631     corelock_init(&q->cl);
 632     q->queue = NULL;
 633     q->read = 0;
 634     q->write = 0;
 635 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 636     q->send = NULL; /* No message sending by default */
 637     IF_PRIO( q->blocker_p = NULL; )
 638 #endif
 639
 640     if(register_queue)
 641     {
 642         if(all_queues.count >= MAX_NUM_QUEUES)
 643         {
 644             panicf("queue_init->out of queues");
 645         }
 646         /* Add it to the all_queues array */
 647         all_queues.queues[all_queues.count++] = q;
 648         corelock_unlock(&all_queues.cl);
 649     }
 650
 651     restore_irq(oldlevel);
 652 }
 653
 654 /* Queue must not be available for use during this call */
 655 void queue_delete(struct event_queue *q)
 656 {
 657     int oldlevel;
 658     int i;
 659
 660     oldlevel = disable_irq_save();
 661     corelock_lock(&all_queues.cl);
 662     corelock_lock(&q->cl);
 663
 664     /* Find the queue to be deleted */
 665     for(i = 0;i < all_queues.count;i++)
 666     {
 667         if(all_queues.queues[i] == q)
 668         {
 669             /* Move the following queues up in the list */
 670             all_queues.count--;
 671
 672             for(;i < all_queues.count;i++)
 673             {
 674                 all_queues.queues[i] = all_queues.queues[i+1];
 675             }
 676
 677             break;
 678         }
 679     }
 680
 681     corelock_unlock(&all_queues.cl);
 682
 683     /* Release thread(s) waiting on queue head */
 684     thread_queue_wake(&q->queue);
 685
 686 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 687     if(q->send)
 688     {
 689         /* Release threads waiting for replies */
 690         queue_release_all_senders(q);
 691
 692         /* Reply to any dequeued message waiting for one */
 693         queue_do_auto_reply(q->send);
 694
 695         q->send = NULL;
 696         IF_PRIO( q->blocker_p = NULL; )
 697     }
 698 #endif
 699
 700     q->read = 0;
 701     q->write = 0;
 702
 703     corelock_unlock(&q->cl);
 704     restore_irq(oldlevel);
 705 }
 706
 707 /* NOTE: multiple threads waiting on a queue head cannot have a well-
 708    defined release order if timeouts are used. If multiple threads must
 709    access the queue head, use a dispatcher or queue_wait only. */
 710 void queue_wait(struct event_queue *q, struct queue_event *ev)
 711 {
 712     int oldlevel;
 713     unsigned int rd;
 714
 715 #ifdef HAVE_PRIORITY_SCHEDULING
 716     KERNEL_ASSERT(QUEUE_GET_THREAD(q) == NULL ||
 717                   QUEUE_GET_THREAD(q) == thread_get_current(),
 718                   "queue_wait->wrong thread\n");
 719 #endif
 720
 721     oldlevel = disable_irq_save();
 722     corelock_lock(&q->cl);
 723
 724     /* auto-reply */
 725     queue_do_auto_reply(q->send);
 726
 727     if (q->read == q->write)
 728     {
 729         struct thread_entry *current = cores[CURRENT_CORE].running;
 730
 731         do
 732         {
 733             IF_COP( current->obj_cl = &q->cl; )
 734             current->bqp = &q->queue;
 735
 736             block_thread(current);
 737
 738             corelock_unlock(&q->cl);
 739             switch_thread();
 740
 741             oldlevel = disable_irq_save();
 742             corelock_lock(&q->cl);
 743         }
 744         /* A message that woke us could now be gone */
 745         while (q->read == q->write);
 746     }
 747
 748     rd = q->read++ & QUEUE_LENGTH_MASK;
 749     *ev = q->events[rd];
 750
 751     /* Get data for a waiting thread if one */
 752     queue_do_fetch_sender(q->send, rd);
 753
 754     corelock_unlock(&q->cl);
 755     restore_irq(oldlevel);
 756 }
 757
 758 void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
 759 {
 760     int oldlevel;
 761
 762 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 763     KERNEL_ASSERT(QUEUE_GET_THREAD(q) == NULL ||
 764                   QUEUE_GET_THREAD(q) == thread_get_current(),
 765                   "queue_wait_w_tmo->wrong thread\n");
 766 #endif
 767
 768     oldlevel = disable_irq_save();
 769     corelock_lock(&q->cl);
 770
 771     /* Auto-reply */
 772     queue_do_auto_reply(q->send);
 773
 774     if (q->read == q->write && ticks > 0)
 775     {
 776         struct thread_entry *current = cores[CURRENT_CORE].running;
 777
 778         IF_COP( current->obj_cl = &q->cl; )
 779         current->bqp = &q->queue;
 780
 781         block_thread_w_tmo(current, ticks);
 782         corelock_unlock(&q->cl);
 783
 784         switch_thread();
 785
 786         oldlevel = disable_irq_save();
 787         corelock_lock(&q->cl);
 788     }
 789
 790     /* no worry about a removed message here - status is checked inside
 791        locks - perhaps verify if timeout or false alarm */
 792     if (q->read != q->write)
 793     {
 794         unsigned int rd = q->read++ & QUEUE_LENGTH_MASK;
 795         *ev = q->events[rd];
 796         /* Get data for a waiting thread if one */
 797         queue_do_fetch_sender(q->send, rd);
 798     }
 799     else
 800     {
 801         ev->id = SYS_TIMEOUT;
 802     }
 803
 804     corelock_unlock(&q->cl);
 805     restore_irq(oldlevel);
 806 }
 807
 808 void queue_post(struct event_queue *q, long id, intptr_t data)
 809 {
 810     int oldlevel;
 811     unsigned int wr;
 812
 813     oldlevel = disable_irq_save();
 814     corelock_lock(&q->cl);
 815
 816     wr = q->write++ & QUEUE_LENGTH_MASK;
 817
 818     q->events[wr].id   = id;
 819     q->events[wr].data = data;
 820
 821     /* overflow protect - unblock any thread waiting at this index */
 822     queue_do_unblock_sender(q->send, wr);
 823
 824     /* Wakeup a waiting thread if any */
 825     wakeup_thread(&q->queue);
 826
 827     corelock_unlock(&q->cl);
 828     restore_irq(oldlevel);
 829 }
 830
 831 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 832 /* IRQ handlers are not allowed use of this function - we only aim to
 833    protect the queue integrity by turning them off. */
 834 intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
 835 {
 836     int oldlevel;
 837     unsigned int wr;
 838
 839     oldlevel = disable_irq_save();
 840     corelock_lock(&q->cl);
 841
 842     wr = q->write++ & QUEUE_LENGTH_MASK;
 843
 844     q->events[wr].id   = id;
 845     q->events[wr].data = data;
 846
 847     if(q->send)
 848     {
 849         struct queue_sender_list *send = q->send;
 850         struct thread_entry **spp = &send->senders[wr];
 851         struct thread_entry *current = cores[CURRENT_CORE].running;
 852
 853         if(*spp)
 854         {
 855             /* overflow protect - unblock any thread waiting at this index */
 856             queue_release_sender(spp, 0);
 857         }
 858
 859         /* Wakeup a waiting thread if any */
 860         wakeup_thread(&q->queue);
 861
 862         /* Save thread in slot, add to list and wait for reply */
 863         *spp = current;
 864         IF_COP( current->obj_cl = &q->cl; )
 865         IF_PRIO( current->blocker = q->blocker_p; )
 866         current->wakeup_ext_cb = queue_remove_sender_thread_cb;
 867         current->retval = (intptr_t)spp;
 868         current->bqp = &send->list;
 869
 870         block_thread(current);
 871
 872         corelock_unlock(&q->cl);
 873         switch_thread();
 874
 875         return current->retval;
 876     }
 877
 878     /* Function as queue_post if sending is not enabled */
 879     wakeup_thread(&q->queue);
 880
 881     corelock_unlock(&q->cl);
 882     restore_irq(oldlevel);
 883
 884     return 0;
 885 }
 886
 887 #if 0 /* not used now but probably will be later */
 888 /* Query if the last message dequeued was added by queue_send or not */
 889 bool queue_in_queue_send(struct event_queue *q)
 890 {
 891     bool in_send;
 892
 893 #if NUM_CORES > 1
 894     int oldlevel = disable_irq_save();
 895     corelock_lock(&q->cl);
 896 #endif
 897
 898     in_send = q->send && q->send->curr_sender;
 899
 900 #if NUM_CORES > 1
 901     corelock_unlock(&q->cl);
 902     restore_irq(oldlevel);
 903 #endif
 904
 905     return in_send;
 906 }
 907 #endif
 908
 909 /* Replies with retval to the last dequeued message sent with queue_send */
 910 void queue_reply(struct event_queue *q, intptr_t retval)
 911 {
 912     if(q->send && q->send->curr_sender)
 913     {
 914         int oldlevel = disable_irq_save();
 915         corelock_lock(&q->cl);
 916         /* Double-check locking */
 917         IF_COP( if(q->send && q->send->curr_sender) )
 918         {
 919             queue_release_sender(&q->send->curr_sender, retval);
 920         }
 921
 922         corelock_unlock(&q->cl);
 923         restore_irq(oldlevel);
 924     }
 925 }
 926
 927 bool queue_peek(struct event_queue *q, struct queue_event *ev)
 928 {
 929     if(q->read == q->write)
 930          return false;
 931
 932     bool have_msg = false;
 933
 934     int oldlevel = disable_irq_save();
 935     corelock_lock(&q->cl);
 936
 937     if(q->read != q->write)
 938     {
 939         *ev = q->events[q->read & QUEUE_LENGTH_MASK];
 940         have_msg = true;
 941     }
 942
 943     corelock_unlock(&q->cl);
 944     restore_irq(oldlevel);
 945
 946     return have_msg;
 947 }
 948 #endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */
 949
 950 /* Poll queue to see if a message exists - careful in using the result if
 951  * queue_remove_from_head is called when messages are posted - possibly use
 952  * queue_wait_w_tmo(&q, 0) in that case or else a removed message that
 953  * unsignals the queue may cause an unwanted block */
 954 bool queue_empty(const struct event_queue* q)
 955 {
 956     return ( q->read == q->write );
 957 }
 958
 959 void queue_clear(struct event_queue* q)
 960 {
 961     int oldlevel;
 962
 963     oldlevel = disable_irq_save();
 964     corelock_lock(&q->cl);
 965
 966     /* Release all threads waiting in the queue for a reply -
 967        dequeued sent message will be handled by owning thread */
 968     queue_release_all_senders(q);
 969
 970     q->read = 0;
 971     q->write = 0;
 972
 973     corelock_unlock(&q->cl);
 974     restore_irq(oldlevel);
 975 }
 976
 977 void queue_remove_from_head(struct event_queue *q, long id)
 978 {
 979     int oldlevel;
 980
 981     oldlevel = disable_irq_save();
 982     corelock_lock(&q->cl);
 983
 984     while(q->read != q->write)
 985     {
 986         unsigned int rd = q->read & QUEUE_LENGTH_MASK;
 987
 988         if(q->events[rd].id != id)
 989         {
 990             break;
 991         }
 992
 993         /* Release any thread waiting on this message */
 994         queue_do_unblock_sender(q->send, rd);
 995
 996         q->read++;
 997     }
 998
 999     corelock_unlock(&q->cl);
1000     restore_irq(oldlevel);
1001 }
1002
1003 /**
1004  * The number of events waiting in the queue.
1005  *
1006  * @param struct of event_queue
1007  * @return number of events in the queue
1008  */
1009 int queue_count(const struct event_queue *q)
1010 {
1011     return q->write - q->read;
1012 }
1013
1014 int queue_broadcast(long id, intptr_t data)
1015 {
1016     int i;
1017
1018 #if NUM_CORES > 1
1019     int oldlevel = disable_irq_save();
1020     corelock_lock(&all_queues.cl);
1021 #endif
1022
1023     for(i = 0;i < all_queues.count;i++)
1024     {
1025         queue_post(all_queues.queues[i], id, data);
1026     }
1027
1028 #if NUM_CORES > 1
1029     corelock_unlock(&all_queues.cl);
1030     restore_irq(oldlevel);
1031 #endif
1032
1033     return i;
1034 }
1035
1036 /****************************************************************************
1037  * Simple mutex functions ;)
1038  ****************************************************************************/
1039
1040 /* Initialize a mutex object - call before any use and do not call again once
1041  * the object is available to other threads */
1042 void mutex_init(struct mutex *m)
1043 {
1044     corelock_init(&m->cl);
1045     m->queue = NULL;
1046     m->count = 0;
1047     m->locked = 0;
1048     MUTEX_SET_THREAD(m, NULL);
1049 #ifdef HAVE_PRIORITY_SCHEDULING
1050     m->blocker.priority = PRIORITY_IDLE;
1051     m->blocker.wakeup_protocol = wakeup_priority_protocol_transfer;
1052     m->no_preempt = false;
1053 #endif
1054 }
1055
1056 /* Gain ownership of a mutex object or block until it becomes free */
1057 void mutex_lock(struct mutex *m)
1058 {
1059     const unsigned int core = CURRENT_CORE;
1060     struct thread_entry *current = cores[core].running;
1061
1062     if(current == MUTEX_GET_THREAD(m))
1063     {
1064         /* current thread already owns this mutex */
1065         m->count++;
1066         return;
1067     }
1068
1069     /* lock out other cores */
1070     corelock_lock(&m->cl);
1071
1072     if(m->locked == 0)
1073     {
1074         /* lock is open */
1075         MUTEX_SET_THREAD(m, current);
1076         m->locked = 1;
1077         corelock_unlock(&m->cl);
1078         return;
1079     }
1080
1081     /* block until the lock is open... */
1082     IF_COP( current->obj_cl = &m->cl; )
1083     IF_PRIO( current->blocker = &m->blocker; )
1084     current->bqp = &m->queue;
1085
1086     disable_irq();
1087     block_thread(current);
1088
1089     corelock_unlock(&m->cl);
1090
1091     /* ...and turn control over to next thread */
1092     switch_thread();
1093 }
1094
1095 /* Release ownership of a mutex object - only owning thread must call this */
1096 void mutex_unlock(struct mutex *m)
1097 {
1098     /* unlocker not being the owner is an unlocking violation */
1099     KERNEL_ASSERT(MUTEX_GET_THREAD(m) == thread_get_current(),
1100                   "mutex_unlock->wrong thread (%s != %s)\n",
1101                   MUTEX_GET_THREAD(m)->name,
1102                   thread_get_current()->name);
1103
1104     if(m->count > 0)
1105     {
1106         /* this thread still owns lock */
1107         m->count--;
1108         return;
1109     }
1110
1111     /* lock out other cores */
1112     corelock_lock(&m->cl);
1113
1114     /* transfer to next queued thread if any */
1115     if(m->queue == NULL)
1116     {
1117         /* no threads waiting - open the lock */
1118         MUTEX_SET_THREAD(m, NULL);
1119         m->locked = 0;
1120         corelock_unlock(&m->cl);
1121         return;
1122     }
1123     else
1124     {
1125         const int oldlevel = disable_irq_save();
1126         /* Tranfer of owning thread is handled in the wakeup protocol
1127          * if priorities are enabled otherwise just set it from the
1128          * queue head. */
1129         IFN_PRIO( MUTEX_SET_THREAD(m, m->queue); )
1130         IF_PRIO( unsigned int result = ) wakeup_thread(&m->queue);
1131         restore_irq(oldlevel);
1132
1133         corelock_unlock(&m->cl);
1134
1135 #ifdef HAVE_PRIORITY_SCHEDULING
1136         if((result & THREAD_SWITCH) && !m->no_preempt)
1137             switch_thread();
1138 #endif
1139     }
1140 }
1141
1142 /****************************************************************************
1143  * Simpl-er mutex functions ;)
1144  ****************************************************************************/
1145 #if NUM_CORES > 1
1146 void spinlock_init(struct spinlock *l)
1147 {
1148     corelock_init(&l->cl);
1149     l->thread = NULL;
1150     l->count = 0;
1151 }
1152
1153 void spinlock_lock(struct spinlock *l)
1154 {
1155     const unsigned int core = CURRENT_CORE;
1156     struct thread_entry *current = cores[core].running;
1157
1158     if(l->thread == current)
1159     {
1160         /* current core already owns it */
1161         l->count++;
1162         return;
1163     }
1164
1165     /* lock against other processor cores */
1166     corelock_lock(&l->cl);
1167
1168     /* take ownership */
1169     l->thread = current;
1170 }
1171
1172 void spinlock_unlock(struct spinlock *l)
1173 {
1174     /* unlocker not being the owner is an unlocking violation */
1175     KERNEL_ASSERT(l->thread == thread_get_current(),
1176                   "spinlock_unlock->wrong thread\n");
1177
1178     if(l->count > 0)
1179     {
1180         /* this core still owns lock */
1181         l->count--;
1182         return;
1183     }
1184
1185     /* clear owner */
1186     l->thread = NULL;
1187
1188     /* release lock */
1189     corelock_unlock(&l->cl);
1190 }
1191 #endif /* NUM_CORES > 1 */
1192
1193 /****************************************************************************
1194  * Simple semaphore functions ;)
1195  ****************************************************************************/
1196 #ifdef HAVE_SEMAPHORE_OBJECTS
1197 void semaphore_init(struct semaphore *s, int max, int start)
1198 {
1199     KERNEL_ASSERT(max > 0 && start >= 0 && start <= max,
1200                   "semaphore_init->inv arg\n");
1201     s->queue = NULL;
1202     s->max = max;
1203     s->count = start;
1204     corelock_init(&s->cl);
1205 }
1206
1207 void semaphore_wait(struct semaphore *s)
1208 {
1209     struct thread_entry *current;
1210
1211     corelock_lock(&s->cl);
1212
1213     if(--s->count >= 0)
1214     {
1215         /* wait satisfied */
1216         corelock_unlock(&s->cl);
1217         return;
1218     }
1219
1220     /* too many waits - block until dequeued... */
1221     current = cores[CURRENT_CORE].running;
1222
1223     IF_COP( current->obj_cl = &s->cl; )
1224     current->bqp = &s->queue;
1225
1226     disable_irq();
1227     block_thread(current);
1228
1229     corelock_unlock(&s->cl);
1230
1231     /* ...and turn control over to next thread */
1232     switch_thread();
1233 }
1234
1235 void semaphore_release(struct semaphore *s)
1236 {
1237     IF_PRIO( unsigned int result = THREAD_NONE; )
1238
1239     corelock_lock(&s->cl);
1240
1241     if(s->count < s->max && ++s->count <= 0)
1242     {
1243         /* there should be threads in this queue */
1244         KERNEL_ASSERT(s->queue != NULL, "semaphore->wakeup\n");
1245         /* a thread was queued - wake it up */
1246         int oldlevel = disable_irq_save();
1247         IF_PRIO( result = ) wakeup_thread(&s->queue);
1248         restore_irq(oldlevel);
1249     }
1250
1251     corelock_unlock(&s->cl);
1252
1253 #ifdef HAVE_PRIORITY_SCHEDULING
1254     if(result & THREAD_SWITCH)
1255         switch_thread();
1256 #endif
1257 }
1258 #endif /* HAVE_SEMAPHORE_OBJECTS */
1259
1260 /****************************************************************************
1261  * Simple event functions ;)
1262  ****************************************************************************/
1263 #ifdef HAVE_EVENT_OBJECTS
1264 void event_init(struct event *e, unsigned int flags)
1265 {
1266     e->queues[STATE_NONSIGNALED] = NULL;
1267     e->queues[STATE_SIGNALED] = NULL;
1268     e->state = flags & STATE_SIGNALED;
1269     e->automatic = (flags & EVENT_AUTOMATIC) ? 1 : 0;
1270     corelock_init(&e->cl);
1271 }
1272
1273 void event_wait(struct event *e, unsigned int for_state)
1274 {
1275     struct thread_entry *current;
1276
1277     corelock_lock(&e->cl);
1278
1279     if(e->automatic != 0)
1280     {
1281         /* wait for false always satisfied by definition
1282            or if it just changed to false */
1283         if(e->state == STATE_SIGNALED || for_state == STATE_NONSIGNALED)
1284         {
1285             /* automatic - unsignal */
1286             e->state = STATE_NONSIGNALED;
1287             corelock_unlock(&e->cl);
1288             return;
1289         }
1290         /* block until state matches */
1291     }
1292     else if(for_state == e->state)
1293     {
1294         /* the state being waited for is the current state */
1295         corelock_unlock(&e->cl);
1296         return;
1297     }
1298
1299     /* block until state matches what callers requests */
1300     current = cores[CURRENT_CORE].running;
1301
1302     IF_COP( current->obj_cl = &e->cl; )
1303     current->bqp = &e->queues[for_state];
1304
1305     disable_irq();
1306     block_thread(current);
1307
1308     corelock_unlock(&e->cl);
1309
1310     /* turn control over to next thread */
1311     switch_thread();
1312 }
1313
1314 void event_set_state(struct event *e, unsigned int state)
1315 {
1316     unsigned int result;
1317     int oldlevel;
1318
1319     corelock_lock(&e->cl);
1320
1321     if(e->state == state)
1322     {
1323         /* no change */
1324         corelock_unlock(&e->cl);
1325         return;
1326     }
1327
1328     IF_PRIO( result = THREAD_OK; )
1329
1330     oldlevel = disable_irq_save();
1331
1332     if(state == STATE_SIGNALED)
1333     {
1334         if(e->automatic != 0)
1335         {
1336             /* no thread should have ever blocked for nonsignaled */
1337             KERNEL_ASSERT(e->queues[STATE_NONSIGNALED] == NULL,
1338                           "set_event_state->queue[NS]:S\n");
1339             /* pass to next thread and keep unsignaled - "pulse" */
1340             result = wakeup_thread(&e->queues[STATE_SIGNALED]);
1341             e->state = (result & THREAD_OK) ? STATE_NONSIGNALED : STATE_SIGNALED;
1342         }
1343         else
1344         {
1345             /* release all threads waiting for signaled */
1346             e->state = STATE_SIGNALED;
1347             IF_PRIO( result = )
1348                 thread_queue_wake(&e->queues[STATE_SIGNALED]);
1349         }
1350     }
1351     else
1352     {
1353         /* release all threads waiting for nonsignaled */
1354
1355         /* no thread should have ever blocked if automatic */
1356         KERNEL_ASSERT(e->queues[STATE_NONSIGNALED] == NULL ||
1357                       e->automatic == 0, "set_event_state->queue[NS]:NS\n");
1358
1359         e->state = STATE_NONSIGNALED;
1360         IF_PRIO( result = )
1361             thread_queue_wake(&e->queues[STATE_NONSIGNALED]);
1362     }
1363
1364     restore_irq(oldlevel);
1365
1366     corelock_unlock(&e->cl);
1367
1368 #ifdef HAVE_PRIORITY_SCHEDULING
1369     if(result & THREAD_SWITCH)
1370         switch_thread();
1371 #endif
1372 }
1373 #endif /* HAVE_EVENT_OBJECTS */
1374
1375
1376 #ifdef HAVE_WAKEUP_OBJECTS
1377 /****************************************************************************
1378  * Lightweight IRQ-compatible wakeup object
1379  */
1380
1381 /* Initialize the wakeup object */
1382 void wakeup_init(struct wakeup *w)
1383 {
1384     w->queue = NULL;
1385     w->signalled = 0;
1386     IF_COP( corelock_init(&w->cl); )
1387 }
1388
1389 /* Wait for a signal blocking indefinitely or for a specified period */
1390 int wakeup_wait(struct wakeup *w, int timeout)
1391 {
1392     int ret = OBJ_WAIT_SUCCEEDED; /* Presume success */
1393     int oldlevel = disable_irq_save();
1394
1395     corelock_lock(&w->cl);
1396
1397     if(w->signalled == 0 && timeout != TIMEOUT_NOBLOCK)
1398     {
1399         struct thread_entry * current = cores[CURRENT_CORE].running;
1400
1401         IF_COP( current->obj_cl = &w->cl; )
1402         current->bqp = &w->queue;
1403
1404         if (timeout != TIMEOUT_BLOCK)
1405             block_thread_w_tmo(current, timeout);
1406         else
1407             block_thread(current);
1408
1409         corelock_unlock(&w->cl);
1410         switch_thread();
1411
1412         oldlevel = disable_irq_save();
1413         corelock_lock(&w->cl);
1414     }
1415
1416     if(w->signalled == 0)
1417     {
1418         /* Timed-out or failed */
1419         ret = (timeout != TIMEOUT_BLOCK) ?
1420             OBJ_WAIT_TIMEDOUT : OBJ_WAIT_FAILED;
1421     }
1422
1423     w->signalled = 0;  /* Reset */
1424
1425     corelock_unlock(&w->cl);
1426     restore_irq(oldlevel);
1427
1428     return ret;
1429 }
1430
1431 /* Signal the thread waiting or leave the signal if the thread hasn't
1432  * waited yet.
1433  *
1434  * returns THREAD_NONE or THREAD_OK
1435  */
1436 int wakeup_signal(struct wakeup *w)
1437 {
1438     int oldlevel = disable_irq_save();
1439     int ret;
1440
1441     corelock_lock(&w->cl);
1442
1443     w->signalled = 1;
1444     ret = wakeup_thread(&w->queue);
1445
1446     corelock_unlock(&w->cl);
1447     restore_irq(oldlevel);
1448
1449     return ret;
1450 }
1451 #endif /* HAVE_WAKEUP_OBJECTS */
1452