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