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 #elif defined(CREATIVE_ZVx) && defined(BOOTLOADER)
 439     /* hacky.. */
 440         long sleep_ticks = current_tick + ticks + 1;
 441     while (sleep_ticks > current_tick)
 442         switch_thread();
 443 #else
 444     disable_irq();
 445     sleep_thread(ticks);
 446     switch_thread();
 447 #endif
 448 }
 449
 450 void yield(void)
 451 {
 452 #if ((CONFIG_CPU == S3C2440 || defined(ELIO_TPJ1022)) && defined(BOOTLOADER))
 453     /* Some targets don't like yielding in the bootloader */
 454 #else
 455     switch_thread();
 456 #endif
 457 }
 458
 459 /****************************************************************************
 460  * Queue handling stuff
 461  ****************************************************************************/
 462
 463 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 464 /****************************************************************************
 465  * Sender thread queue structure that aids implementation of priority
 466  * inheritance on queues because the send list structure is the same as
 467  * for all other kernel objects:
 468  *
 469  * Example state:
 470  * E0 added with queue_send and removed by thread via queue_wait(_w_tmo)
 471  * E3 was posted with queue_post
 472  * 4 events remain enqueued (E1-E4)
 473  *
 474  *                                 rd                          wr
 475  * q->events[]:          |  XX  |  E1  |  E2  |  E3  |  E4  |  XX  |
 476  * q->send->senders[]:   | NULL |  T1  |  T2  | NULL |  T3  | NULL |
 477  *                                 \/     \/            \/
 478  * q->send->list:       >->|T0|<->|T1|<->|T2|<-------->|T3|<-<
 479  * q->send->curr_sender:    /\
 480  *
 481  * Thread has E0 in its own struct queue_event.
 482  *
 483  ****************************************************************************/
 484
 485 /* Puts the specified return value in the waiting thread's return value
 486  * and wakes the thread.
 487  *
 488  * A sender should be confirmed to exist before calling which makes it
 489  * more efficent to reject the majority of cases that don't need this
 490  * called.
 491  */
 492 static void queue_release_sender(struct thread_entry **sender,
 493                                  intptr_t retval)
 494 {
 495     struct thread_entry *thread = *sender;
 496
 497     *sender = NULL;               /* Clear slot. */
 498     thread->wakeup_ext_cb = NULL; /* Clear callback. */
 499     thread->retval = retval;      /* Assign thread-local return value. */
 500     *thread->bqp = thread;        /* Move blocking queue head to thread since
 501                                      wakeup_thread wakes the first thread in
 502                                      the list. */
 503     wakeup_thread(thread->bqp);
 504 }
 505
 506 /* Releases any waiting threads that are queued with queue_send -
 507  * reply with 0.
 508  */
 509 static void queue_release_all_senders(struct event_queue *q)
 510 {
 511     if(q->send)
 512     {
 513         unsigned int i;
 514         for(i = q->read; i != q->write; i++)
 515         {
 516             struct thread_entry **spp =
 517                 &q->send->senders[i & QUEUE_LENGTH_MASK];
 518
 519             if(*spp)
 520             {
 521                 queue_release_sender(spp, 0);
 522             }
 523         }
 524     }
 525 }
 526
 527 /* Callback to do extra forced removal steps from sender list in addition
 528  * to the normal blocking queue removal and priority dis-inherit */
 529 static void queue_remove_sender_thread_cb(struct thread_entry *thread)
 530 {
 531     *((struct thread_entry **)thread->retval) = NULL;
 532     thread->wakeup_ext_cb = NULL;
 533     thread->retval = 0;
 534 }
 535
 536 /* Enables queue_send on the specified queue - caller allocates the extra
 537  * data structure. Only queues which are taken to be owned by a thread should
 538  * enable this however an official owner is not compulsory but must be
 539  * specified for priority inheritance to operate.
 540  *
 541  * Use of queue_wait(_w_tmo) by multiple threads on a queue using synchronous
 542  * messages results in an undefined order of message replies.
 543  */
 544 void queue_enable_queue_send(struct event_queue *q,
 545                              struct queue_sender_list *send,
 546                              struct thread_entry *owner)
 547 {
 548     int oldlevel = disable_irq_save();
 549     corelock_lock(&q->cl);
 550
 551     if(send != NULL && q->send == NULL)
 552     {
 553         memset(send, 0, sizeof(*send));
 554 #ifdef HAVE_PRIORITY_SCHEDULING
 555         send->blocker.wakeup_protocol = wakeup_priority_protocol_release;
 556         send->blocker.priority = PRIORITY_IDLE;
 557         send->blocker.thread = owner;
 558         if(owner != NULL)
 559             q->blocker_p = &send->blocker;
 560 #endif
 561         q->send = send;
 562     }
 563
 564     corelock_unlock(&q->cl);
 565     restore_irq(oldlevel);
 566
 567     (void)owner;
 568 }
 569
 570 /* Unblock a blocked thread at a given event index */
 571 static inline void queue_do_unblock_sender(struct queue_sender_list *send,
 572                                            unsigned int i)
 573 {
 574     if(send)
 575     {
 576         struct thread_entry **spp = &send->senders[i];
 577
 578         if(*spp)
 579         {
 580             queue_release_sender(spp, 0);
 581         }
 582     }
 583 }
 584
 585 /* Perform the auto-reply sequence */
 586 static inline void queue_do_auto_reply(struct queue_sender_list *send)
 587 {
 588     if(send && send->curr_sender)
 589     {
 590         /* auto-reply */
 591         queue_release_sender(&send->curr_sender, 0);
 592     }
 593 }
 594
 595 /* Moves waiting thread's refrence from the senders array to the
 596  * current_sender which represents the thread waiting for a reponse to the
 597  * last message removed from the queue. This also protects the thread from
 598  * being bumped due to overflow which would not be a valid action since its
 599  * message _is_ being processed at this point. */
 600 static inline void queue_do_fetch_sender(struct queue_sender_list *send,
 601                                          unsigned int rd)
 602 {
 603     if(send)
 604     {
 605         struct thread_entry **spp = &send->senders[rd];
 606
 607         if(*spp)
 608         {
 609             /* Move thread reference from array to the next thread
 610                that queue_reply will release */
 611             send->curr_sender = *spp;
 612             (*spp)->retval = (intptr_t)spp;
 613             *spp = NULL;
 614         }
 615         /* else message was posted asynchronously with queue_post */
 616     }
 617 }
 618 #else
 619 /* Empty macros for when synchoronous sending is not made */
 620 #define queue_release_all_senders(q)
 621 #define queue_do_unblock_sender(send, i)
 622 #define queue_do_auto_reply(send)
 623 #define queue_do_fetch_sender(send, rd)
 624 #endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */
 625
 626 /* Queue must not be available for use during this call */
 627 void queue_init(struct event_queue *q, bool register_queue)
 628 {
 629     int oldlevel = disable_irq_save();
 630
 631     if(register_queue)
 632     {
 633         corelock_lock(&all_queues.cl);
 634     }
 635
 636     corelock_init(&q->cl);
 637     q->queue = NULL;
 638     q->read = 0;
 639     q->write = 0;
 640 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 641     q->send = NULL; /* No message sending by default */
 642     IF_PRIO( q->blocker_p = NULL; )
 643 #endif
 644
 645     if(register_queue)
 646     {
 647         if(all_queues.count >= MAX_NUM_QUEUES)
 648         {
 649             panicf("queue_init->out of queues");
 650         }
 651         /* Add it to the all_queues array */
 652         all_queues.queues[all_queues.count++] = q;
 653         corelock_unlock(&all_queues.cl);
 654     }
 655
 656     restore_irq(oldlevel);
 657 }
 658
 659 /* Queue must not be available for use during this call */
 660 void queue_delete(struct event_queue *q)
 661 {
 662     int oldlevel;
 663     int i;
 664
 665     oldlevel = disable_irq_save();
 666     corelock_lock(&all_queues.cl);
 667     corelock_lock(&q->cl);
 668
 669     /* Find the queue to be deleted */
 670     for(i = 0;i < all_queues.count;i++)
 671     {
 672         if(all_queues.queues[i] == q)
 673         {
 674             /* Move the following queues up in the list */
 675             all_queues.count--;
 676
 677             for(;i < all_queues.count;i++)
 678             {
 679                 all_queues.queues[i] = all_queues.queues[i+1];
 680             }
 681
 682             break;
 683         }
 684     }
 685
 686     corelock_unlock(&all_queues.cl);
 687
 688     /* Release thread(s) waiting on queue head */
 689     thread_queue_wake(&q->queue);
 690
 691 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 692     if(q->send)
 693     {
 694         /* Release threads waiting for replies */
 695         queue_release_all_senders(q);
 696
 697         /* Reply to any dequeued message waiting for one */
 698         queue_do_auto_reply(q->send);
 699
 700         q->send = NULL;
 701         IF_PRIO( q->blocker_p = NULL; )
 702     }
 703 #endif
 704
 705     q->read = 0;
 706     q->write = 0;
 707
 708     corelock_unlock(&q->cl);
 709     restore_irq(oldlevel);
 710 }
 711
 712 /* NOTE: multiple threads waiting on a queue head cannot have a well-
 713    defined release order if timeouts are used. If multiple threads must
 714    access the queue head, use a dispatcher or queue_wait only. */
 715 void queue_wait(struct event_queue *q, struct queue_event *ev)
 716 {
 717     int oldlevel;
 718     unsigned int rd;
 719
 720 #ifdef HAVE_PRIORITY_SCHEDULING
 721     KERNEL_ASSERT(QUEUE_GET_THREAD(q) == NULL ||
 722                   QUEUE_GET_THREAD(q) == thread_get_current(),
 723                   "queue_wait->wrong thread\n");
 724 #endif
 725
 726     oldlevel = disable_irq_save();
 727     corelock_lock(&q->cl);
 728
 729     /* auto-reply */
 730     queue_do_auto_reply(q->send);
 731
 732     if (q->read == q->write)
 733     {
 734         struct thread_entry *current = cores[CURRENT_CORE].running;
 735
 736         do
 737         {
 738             IF_COP( current->obj_cl = &q->cl; )
 739             current->bqp = &q->queue;
 740
 741             block_thread(current);
 742
 743             corelock_unlock(&q->cl);
 744             switch_thread();
 745
 746             oldlevel = disable_irq_save();
 747             corelock_lock(&q->cl);
 748         }
 749         /* A message that woke us could now be gone */
 750         while (q->read == q->write);
 751     }
 752
 753     rd = q->read++ & QUEUE_LENGTH_MASK;
 754     *ev = q->events[rd];
 755
 756     /* Get data for a waiting thread if one */
 757     queue_do_fetch_sender(q->send, rd);
 758
 759     corelock_unlock(&q->cl);
 760     restore_irq(oldlevel);
 761 }
 762
 763 void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
 764 {
 765     int oldlevel;
 766
 767 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 768     KERNEL_ASSERT(QUEUE_GET_THREAD(q) == NULL ||
 769                   QUEUE_GET_THREAD(q) == thread_get_current(),
 770                   "queue_wait_w_tmo->wrong thread\n");
 771 #endif
 772
 773     oldlevel = disable_irq_save();
 774     corelock_lock(&q->cl);
 775
 776     /* Auto-reply */
 777     queue_do_auto_reply(q->send);
 778
 779     if (q->read == q->write && ticks > 0)
 780     {
 781         struct thread_entry *current = cores[CURRENT_CORE].running;
 782
 783         IF_COP( current->obj_cl = &q->cl; )
 784         current->bqp = &q->queue;
 785
 786         block_thread_w_tmo(current, ticks);
 787         corelock_unlock(&q->cl);
 788
 789         switch_thread();
 790
 791         oldlevel = disable_irq_save();
 792         corelock_lock(&q->cl);
 793     }
 794
 795     /* no worry about a removed message here - status is checked inside
 796        locks - perhaps verify if timeout or false alarm */
 797     if (q->read != q->write)
 798     {
 799         unsigned int rd = q->read++ & QUEUE_LENGTH_MASK;
 800         *ev = q->events[rd];
 801         /* Get data for a waiting thread if one */
 802         queue_do_fetch_sender(q->send, rd);
 803     }
 804     else
 805     {
 806         ev->id = SYS_TIMEOUT;
 807     }
 808
 809     corelock_unlock(&q->cl);
 810     restore_irq(oldlevel);
 811 }
 812
 813 void queue_post(struct event_queue *q, long id, intptr_t data)
 814 {
 815     int oldlevel;
 816     unsigned int wr;
 817
 818     oldlevel = disable_irq_save();
 819     corelock_lock(&q->cl);
 820
 821     wr = q->write++ & QUEUE_LENGTH_MASK;
 822
 823     q->events[wr].id   = id;
 824     q->events[wr].data = data;
 825
 826     /* overflow protect - unblock any thread waiting at this index */
 827     queue_do_unblock_sender(q->send, wr);
 828
 829     /* Wakeup a waiting thread if any */
 830     wakeup_thread(&q->queue);
 831
 832     corelock_unlock(&q->cl);
 833     restore_irq(oldlevel);
 834 }
 835
 836 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 837 /* IRQ handlers are not allowed use of this function - we only aim to
 838    protect the queue integrity by turning them off. */
 839 intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
 840 {
 841     int oldlevel;
 842     unsigned int wr;
 843
 844     oldlevel = disable_irq_save();
 845     corelock_lock(&q->cl);
 846
 847     wr = q->write++ & QUEUE_LENGTH_MASK;
 848
 849     q->events[wr].id   = id;
 850     q->events[wr].data = data;
 851
 852     if(q->send)
 853     {
 854         struct queue_sender_list *send = q->send;
 855         struct thread_entry **spp = &send->senders[wr];
 856         struct thread_entry *current = cores[CURRENT_CORE].running;
 857
 858         if(*spp)
 859         {
 860             /* overflow protect - unblock any thread waiting at this index */
 861             queue_release_sender(spp, 0);
 862         }
 863
 864         /* Wakeup a waiting thread if any */
 865         wakeup_thread(&q->queue);
 866
 867         /* Save thread in slot, add to list and wait for reply */
 868         *spp = current;
 869         IF_COP( current->obj_cl = &q->cl; )
 870         IF_PRIO( current->blocker = q->blocker_p; )
 871         current->wakeup_ext_cb = queue_remove_sender_thread_cb;
 872         current->retval = (intptr_t)spp;
 873         current->bqp = &send->list;
 874
 875         block_thread(current);
 876
 877         corelock_unlock(&q->cl);
 878         switch_thread();
 879
 880         return current->retval;
 881     }
 882
 883     /* Function as queue_post if sending is not enabled */
 884     wakeup_thread(&q->queue);
 885
 886     corelock_unlock(&q->cl);
 887     restore_irq(oldlevel);
 888
 889     return 0;
 890 }
 891
 892 #if 0 /* not used now but probably will be later */
 893 /* Query if the last message dequeued was added by queue_send or not */
 894 bool queue_in_queue_send(struct event_queue *q)
 895 {
 896     bool in_send;
 897
 898 #if NUM_CORES > 1
 899     int oldlevel = disable_irq_save();
 900     corelock_lock(&q->cl);
 901 #endif
 902
 903     in_send = q->send && q->send->curr_sender;
 904
 905 #if NUM_CORES > 1
 906     corelock_unlock(&q->cl);
 907     restore_irq(oldlevel);
 908 #endif
 909
 910     return in_send;
 911 }
 912 #endif
 913
 914 /* Replies with retval to the last dequeued message sent with queue_send */
 915 void queue_reply(struct event_queue *q, intptr_t retval)
 916 {
 917     if(q->send && q->send->curr_sender)
 918     {
 919         int oldlevel = disable_irq_save();
 920         corelock_lock(&q->cl);
 921         /* Double-check locking */
 922         IF_COP( if(q->send && q->send->curr_sender) )
 923         {
 924             queue_release_sender(&q->send->curr_sender, retval);
 925         }
 926
 927         corelock_unlock(&q->cl);
 928         restore_irq(oldlevel);
 929     }
 930 }
 931
 932 bool queue_peek(struct event_queue *q, struct queue_event *ev)
 933 {
 934     if(q->read == q->write)
 935          return false;
 936
 937     bool have_msg = false;
 938
 939     int oldlevel = disable_irq_save();
 940     corelock_lock(&q->cl);
 941
 942     if(q->read != q->write)
 943     {
 944         *ev = q->events[q->read & QUEUE_LENGTH_MASK];
 945         have_msg = true;
 946     }
 947
 948     corelock_unlock(&q->cl);
 949     restore_irq(oldlevel);
 950
 951     return have_msg;
 952 }
 953 #endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */
 954
 955 /* Poll queue to see if a message exists - careful in using the result if
 956  * queue_remove_from_head is called when messages are posted - possibly use
 957  * queue_wait_w_tmo(&q, 0) in that case or else a removed message that
 958  * unsignals the queue may cause an unwanted block */
 959 bool queue_empty(const struct event_queue* q)
 960 {
 961     return ( q->read == q->write );
 962 }
 963
 964 void queue_clear(struct event_queue* q)
 965 {
 966     int oldlevel;
 967
 968     oldlevel = disable_irq_save();
 969     corelock_lock(&q->cl);
 970
 971     /* Release all threads waiting in the queue for a reply -
 972        dequeued sent message will be handled by owning thread */
 973     queue_release_all_senders(q);
 974
 975     q->read = 0;
 976     q->write = 0;
 977
 978     corelock_unlock(&q->cl);
 979     restore_irq(oldlevel);
 980 }
 981
 982 void queue_remove_from_head(struct event_queue *q, long id)
 983 {
 984     int oldlevel;
 985
 986     oldlevel = disable_irq_save();
 987     corelock_lock(&q->cl);
 988
 989     while(q->read != q->write)
 990     {
 991         unsigned int rd = q->read & QUEUE_LENGTH_MASK;
 992
 993         if(q->events[rd].id != id)
 994         {
 995             break;
 996         }
 997
 998         /* Release any thread waiting on this message */
 999         queue_do_unblock_sender(q->send, rd);
1000
1001         q->read++;
1002     }
1003
1004     corelock_unlock(&q->cl);
1005     restore_irq(oldlevel);
1006 }
1007
1008 /**
1009  * The number of events waiting in the queue.
1010  *
1011  * @param struct of event_queue
1012  * @return number of events in the queue
1013  */
1014 int queue_count(const struct event_queue *q)
1015 {
1016     return q->write - q->read;
1017 }
1018
1019 int queue_broadcast(long id, intptr_t data)
1020 {
1021     int i;
1022
1023 #if NUM_CORES > 1
1024     int oldlevel = disable_irq_save();
1025     corelock_lock(&all_queues.cl);
1026 #endif
1027
1028     for(i = 0;i < all_queues.count;i++)
1029     {
1030         queue_post(all_queues.queues[i], id, data);
1031     }
1032
1033 #if NUM_CORES > 1
1034     corelock_unlock(&all_queues.cl);
1035     restore_irq(oldlevel);
1036 #endif
1037
1038     return i;
1039 }
1040
1041 /****************************************************************************
1042  * Simple mutex functions ;)
1043  ****************************************************************************/
1044
1045 /* Initialize a mutex object - call before any use and do not call again once
1046  * the object is available to other threads */
1047 void mutex_init(struct mutex *m)
1048 {
1049     corelock_init(&m->cl);
1050     m->queue = NULL;
1051     m->count = 0;
1052     m->locked = 0;
1053     MUTEX_SET_THREAD(m, NULL);
1054 #ifdef HAVE_PRIORITY_SCHEDULING
1055     m->blocker.priority = PRIORITY_IDLE;
1056     m->blocker.wakeup_protocol = wakeup_priority_protocol_transfer;
1057     m->no_preempt = false;
1058 #endif
1059 }
1060
1061 /* Gain ownership of a mutex object or block until it becomes free */
1062 void mutex_lock(struct mutex *m)
1063 {
1064     const unsigned int core = CURRENT_CORE;
1065     struct thread_entry *current = cores[core].running;
1066
1067     if(current == MUTEX_GET_THREAD(m))
1068     {
1069         /* current thread already owns this mutex */
1070         m->count++;
1071         return;
1072     }
1073
1074     /* lock out other cores */
1075     corelock_lock(&m->cl);
1076
1077     if(m->locked == 0)
1078     {
1079         /* lock is open */
1080         MUTEX_SET_THREAD(m, current);
1081         m->locked = 1;
1082         corelock_unlock(&m->cl);
1083         return;
1084     }
1085
1086     /* block until the lock is open... */
1087     IF_COP( current->obj_cl = &m->cl; )
1088     IF_PRIO( current->blocker = &m->blocker; )
1089     current->bqp = &m->queue;
1090
1091     disable_irq();
1092     block_thread(current);
1093
1094     corelock_unlock(&m->cl);
1095
1096     /* ...and turn control over to next thread */
1097     switch_thread();
1098 }
1099
1100 /* Release ownership of a mutex object - only owning thread must call this */
1101 void mutex_unlock(struct mutex *m)
1102 {
1103     /* unlocker not being the owner is an unlocking violation */
1104     KERNEL_ASSERT(MUTEX_GET_THREAD(m) == thread_get_current(),
1105                   "mutex_unlock->wrong thread (%s != %s)\n",
1106                   MUTEX_GET_THREAD(m)->name,
1107                   thread_get_current()->name);
1108
1109     if(m->count > 0)
1110     {
1111         /* this thread still owns lock */
1112         m->count--;
1113         return;
1114     }
1115
1116     /* lock out other cores */
1117     corelock_lock(&m->cl);
1118
1119     /* transfer to next queued thread if any */
1120     if(m->queue == NULL)
1121     {
1122         /* no threads waiting - open the lock */
1123         MUTEX_SET_THREAD(m, NULL);
1124         m->locked = 0;
1125         corelock_unlock(&m->cl);
1126         return;
1127     }
1128     else
1129     {
1130         const int oldlevel = disable_irq_save();
1131         /* Tranfer of owning thread is handled in the wakeup protocol
1132          * if priorities are enabled otherwise just set it from the
1133          * queue head. */
1134         IFN_PRIO( MUTEX_SET_THREAD(m, m->queue); )
1135         IF_PRIO( unsigned int result = ) wakeup_thread(&m->queue);
1136         restore_irq(oldlevel);
1137
1138         corelock_unlock(&m->cl);
1139
1140 #ifdef HAVE_PRIORITY_SCHEDULING
1141         if((result & THREAD_SWITCH) && !m->no_preempt)
1142             switch_thread();
1143 #endif
1144     }
1145 }
1146
1147 /****************************************************************************
1148  * Simpl-er mutex functions ;)
1149  ****************************************************************************/
1150 #if NUM_CORES > 1
1151 void spinlock_init(struct spinlock *l)
1152 {
1153     corelock_init(&l->cl);
1154     l->thread = NULL;
1155     l->count = 0;
1156 }
1157
1158 void spinlock_lock(struct spinlock *l)
1159 {
1160     const unsigned int core = CURRENT_CORE;
1161     struct thread_entry *current = cores[core].running;
1162
1163     if(l->thread == current)
1164     {
1165         /* current core already owns it */
1166         l->count++;
1167         return;
1168     }
1169
1170     /* lock against other processor cores */
1171     corelock_lock(&l->cl);
1172
1173     /* take ownership */
1174     l->thread = current;
1175 }
1176
1177 void spinlock_unlock(struct spinlock *l)
1178 {
1179     /* unlocker not being the owner is an unlocking violation */
1180     KERNEL_ASSERT(l->thread == thread_get_current(),
1181                   "spinlock_unlock->wrong thread\n");
1182
1183     if(l->count > 0)
1184     {
1185         /* this core still owns lock */
1186         l->count--;
1187         return;
1188     }
1189
1190     /* clear owner */
1191     l->thread = NULL;
1192
1193     /* release lock */
1194     corelock_unlock(&l->cl);
1195 }
1196 #endif /* NUM_CORES > 1 */
1197
1198 /****************************************************************************
1199  * Simple semaphore functions ;)
1200  ****************************************************************************/
1201 #ifdef HAVE_SEMAPHORE_OBJECTS
1202 void semaphore_init(struct semaphore *s, int max, int start)
1203 {
1204     KERNEL_ASSERT(max > 0 && start >= 0 && start <= max,
1205                   "semaphore_init->inv arg\n");
1206     s->queue = NULL;
1207     s->max = max;
1208     s->count = start;
1209     corelock_init(&s->cl);
1210 }
1211
1212 void semaphore_wait(struct semaphore *s)
1213 {
1214     struct thread_entry *current;
1215
1216     corelock_lock(&s->cl);
1217
1218     if(--s->count >= 0)
1219     {
1220         /* wait satisfied */
1221         corelock_unlock(&s->cl);
1222         return;
1223     }
1224
1225     /* too many waits - block until dequeued... */
1226     current = cores[CURRENT_CORE].running;
1227
1228     IF_COP( current->obj_cl = &s->cl; )
1229     current->bqp = &s->queue;
1230
1231     disable_irq();
1232     block_thread(current);
1233
1234     corelock_unlock(&s->cl);
1235
1236     /* ...and turn control over to next thread */
1237     switch_thread();
1238 }
1239
1240 void semaphore_release(struct semaphore *s)
1241 {
1242     IF_PRIO( unsigned int result = THREAD_NONE; )
1243
1244     corelock_lock(&s->cl);
1245
1246     if(s->count < s->max && ++s->count <= 0)
1247     {
1248         /* there should be threads in this queue */
1249         KERNEL_ASSERT(s->queue != NULL, "semaphore->wakeup\n");
1250         /* a thread was queued - wake it up */
1251         int oldlevel = disable_irq_save();
1252         IF_PRIO( result = ) wakeup_thread(&s->queue);
1253         restore_irq(oldlevel);
1254     }
1255
1256     corelock_unlock(&s->cl);
1257
1258 #ifdef HAVE_PRIORITY_SCHEDULING
1259     if(result & THREAD_SWITCH)
1260         switch_thread();
1261 #endif
1262 }
1263 #endif /* HAVE_SEMAPHORE_OBJECTS */
1264
1265 /****************************************************************************
1266  * Simple event functions ;)
1267  ****************************************************************************/
1268 #ifdef HAVE_EVENT_OBJECTS
1269 void event_init(struct event *e, unsigned int flags)
1270 {
1271     e->queues[STATE_NONSIGNALED] = NULL;
1272     e->queues[STATE_SIGNALED] = NULL;
1273     e->state = flags & STATE_SIGNALED;
1274     e->automatic = (flags & EVENT_AUTOMATIC) ? 1 : 0;
1275     corelock_init(&e->cl);
1276 }
1277
1278 void event_wait(struct event *e, unsigned int for_state)
1279 {
1280     struct thread_entry *current;
1281
1282     corelock_lock(&e->cl);
1283
1284     if(e->automatic != 0)
1285     {
1286         /* wait for false always satisfied by definition
1287            or if it just changed to false */
1288         if(e->state == STATE_SIGNALED || for_state == STATE_NONSIGNALED)
1289         {
1290             /* automatic - unsignal */
1291             e->state = STATE_NONSIGNALED;
1292             corelock_unlock(&e->cl);
1293             return;
1294         }
1295         /* block until state matches */
1296     }
1297     else if(for_state == e->state)
1298     {
1299         /* the state being waited for is the current state */
1300         corelock_unlock(&e->cl);
1301         return;
1302     }
1303
1304     /* block until state matches what callers requests */
1305     current = cores[CURRENT_CORE].running;
1306
1307     IF_COP( current->obj_cl = &e->cl; )
1308     current->bqp = &e->queues[for_state];
1309
1310     disable_irq();
1311     block_thread(current);
1312
1313     corelock_unlock(&e->cl);
1314
1315     /* turn control over to next thread */
1316     switch_thread();
1317 }
1318
1319 void event_set_state(struct event *e, unsigned int state)
1320 {
1321     unsigned int result;
1322     int oldlevel;
1323
1324     corelock_lock(&e->cl);
1325
1326     if(e->state == state)
1327     {
1328         /* no change */
1329         corelock_unlock(&e->cl);
1330         return;
1331     }
1332
1333     IF_PRIO( result = THREAD_OK; )
1334
1335     oldlevel = disable_irq_save();
1336
1337     if(state == STATE_SIGNALED)
1338     {
1339         if(e->automatic != 0)
1340         {
1341             /* no thread should have ever blocked for nonsignaled */
1342             KERNEL_ASSERT(e->queues[STATE_NONSIGNALED] == NULL,
1343                           "set_event_state->queue[NS]:S\n");
1344             /* pass to next thread and keep unsignaled - "pulse" */
1345             result = wakeup_thread(&e->queues[STATE_SIGNALED]);
1346             e->state = (result & THREAD_OK) ? STATE_NONSIGNALED : STATE_SIGNALED;
1347         }
1348         else
1349         {
1350             /* release all threads waiting for signaled */
1351             e->state = STATE_SIGNALED;
1352             IF_PRIO( result = )
1353                 thread_queue_wake(&e->queues[STATE_SIGNALED]);
1354         }
1355     }
1356     else
1357     {
1358         /* release all threads waiting for nonsignaled */
1359
1360         /* no thread should have ever blocked if automatic */
1361         KERNEL_ASSERT(e->queues[STATE_NONSIGNALED] == NULL ||
1362                       e->automatic == 0, "set_event_state->queue[NS]:NS\n");
1363
1364         e->state = STATE_NONSIGNALED;
1365         IF_PRIO( result = )
1366             thread_queue_wake(&e->queues[STATE_NONSIGNALED]);
1367     }
1368
1369     restore_irq(oldlevel);
1370
1371     corelock_unlock(&e->cl);
1372
1373 #ifdef HAVE_PRIORITY_SCHEDULING
1374     if(result & THREAD_SWITCH)
1375         switch_thread();
1376 #endif
1377 }
1378 #endif /* HAVE_EVENT_OBJECTS */
1379
1380
1381 #ifdef HAVE_WAKEUP_OBJECTS
1382 /****************************************************************************
1383  * Lightweight IRQ-compatible wakeup object
1384  */
1385
1386 /* Initialize the wakeup object */
1387 void wakeup_init(struct wakeup *w)
1388 {
1389     w->queue = NULL;
1390     w->signalled = 0;
1391     IF_COP( corelock_init(&w->cl); )
1392 }
1393
1394 /* Wait for a signal blocking indefinitely or for a specified period */
1395 int wakeup_wait(struct wakeup *w, int timeout)
1396 {
1397     int ret = OBJ_WAIT_SUCCEEDED; /* Presume success */
1398     int oldlevel = disable_irq_save();
1399
1400     corelock_lock(&w->cl);
1401
1402     if(w->signalled == 0 && timeout != TIMEOUT_NOBLOCK)
1403     {
1404         struct thread_entry * current = cores[CURRENT_CORE].running;
1405
1406         IF_COP( current->obj_cl = &w->cl; )
1407         current->bqp = &w->queue;
1408
1409         if (timeout != TIMEOUT_BLOCK)
1410             block_thread_w_tmo(current, timeout);
1411         else
1412             block_thread(current);
1413
1414         corelock_unlock(&w->cl);
1415         switch_thread();
1416
1417         oldlevel = disable_irq_save();
1418         corelock_lock(&w->cl);
1419     }
1420
1421     if(w->signalled == 0)
1422     {
1423         /* Timed-out or failed */
1424         ret = (timeout != TIMEOUT_BLOCK) ?
1425             OBJ_WAIT_TIMEDOUT : OBJ_WAIT_FAILED;
1426     }
1427
1428     w->signalled = 0;  /* Reset */
1429
1430     corelock_unlock(&w->cl);
1431     restore_irq(oldlevel);
1432
1433     return ret;
1434 }
1435
1436 /* Signal the thread waiting or leave the signal if the thread hasn't
1437  * waited yet.
1438  *
1439  * returns THREAD_NONE or THREAD_OK
1440  */
1441 int wakeup_signal(struct wakeup *w)
1442 {
1443     int oldlevel = disable_irq_save();
1444     int ret;
1445
1446     corelock_lock(&w->cl);
1447
1448     w->signalled = 1;
1449     ret = wakeup_thread(&w->queue);
1450
1451     corelock_unlock(&w->cl);
1452     restore_irq(oldlevel);
1453
1454     return ret;
1455 }
1456 #endif /* HAVE_WAKEUP_OBJECTS */
1457