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