kernel/rtos.c

   1 /*--------------------------------------------------------------------
   2  * TITLE: Plasma Real Time Operating System
   3  * AUTHOR: Steve Rhoads (rhoadss@yahoo.com)
   4  * DATE CREATED: 12/17/05
   5  * FILENAME: rtos.c
   6  * PROJECT: Plasma CPU core
   7  * COPYRIGHT: Software placed into the public domain by the author.
   8  *    Software 'as is' without warranty.  Author liable for nothing.
   9  * DESCRIPTION:
  10  *    Plasma Real Time Operating System
  11  *    Fully pre-emptive RTOS with support for:
  12  *       Heaps, Threads, Semaphores, Mutexes, Message Queues, and Timers.
  13  *    This file tries to be hardware independent except for calls to:
  14  *       MemoryRead() and MemoryWrite() for interrupts.
  15  *    Partial support for multiple CPUs using symmetric multiprocessing.
  16  *--------------------------------------------------------------------*/
  17 #include "plasma.h"
  18 #include "rtos.h"
  19
  20 #define HEAP_MAGIC 0x1234abcd
  21 #define THREAD_MAGIC 0x4321abcd
  22 #define SEM_RESERVED_COUNT 2
  23 #define INFO_COUNT 4
  24 #define HEAP_COUNT 8
  25
  26
  27 /*************** Structures ***************/
  28 #ifdef WIN32
  29    #define setjmp _setjmp
  30    //x86 registers
  31    typedef struct jmp_buf2 {
  32       uint32 Ebp, Ebx, Edi, Esi, sp, pc, extra[10];
  33    } jmp_buf2;
  34 #elif defined(ARM_CPU)
  35    //ARM registers
  36    typedef struct jmp_buf2 {
  37       uint32 r[13], sp, lr, pc, cpsr, extra[5];
  38    } jmp_buf2;
  39 #else
  40    //Plasma registers
  41    typedef struct jmp_buf2 {
  42       uint32 s[9], gp, sp, pc;
  43    } jmp_buf2;
  44 #endif
  45
  46 typedef struct HeapNode_s {
  47    struct HeapNode_s *next;
  48    int size;
  49 } HeapNode_t;
  50
  51 struct OS_Heap_s {
  52    uint32 magic;
  53    const char *name;
  54    OS_Semaphore_t *semaphore;
  55    HeapNode_t *available;
  56    HeapNode_t base;
  57    struct OS_Heap_s *alternate;
  58 };
  59 //typedef struct OS_Heap_s OS_Heap_t;
  60
  61 typedef enum {
  62    THREAD_PEND    = 0,       //Thread in semaphore's linked list
  63    THREAD_READY   = 1,       //Thread in ThreadHead linked list
  64    THREAD_RUNNING = 2        //Thread == ThreadCurrent[cpu]
  65 } OS_ThreadState_e;
  66
  67 struct OS_Thread_s {
  68    const char *name;         //Name of thread
  69    OS_ThreadState_e state;   //Pending, ready, or running
  70    int cpuIndex;             //Which CPU is running the thread
  71    int cpuLock;              //Lock the thread to a specific CPU
  72    jmp_buf env;              //Registers saved during context swap
  73    OS_FuncPtr_t funcPtr;     //First function called
  74    void *arg;                //Argument to first function called
  75    uint32 priority;          //Priority of thread (0=low, 255=high)
  76    uint32 ticksTimeout;      //Tick value when semaphore pend times out
  77    void *info[INFO_COUNT];   //User storage
  78    OS_Semaphore_t *semaphorePending;  //Semaphore thread is blocked on
  79    int returnCode;           //Return value from semaphore pend
  80    uint32 processId;         //Process ID if using MMU
  81    OS_Heap_t *heap;          //Heap used if no heap specified
  82    struct OS_Thread_s *next; //Linked list of threads by priority
  83    struct OS_Thread_s *prev;
  84    struct OS_Thread_s *nextTimeout; //Linked list of threads by timeout
  85    struct OS_Thread_s *prevTimeout;
  86    uint32 magic[1];          //Bottom of stack to detect stack overflow
  87 };
  88 //typedef struct OS_Thread_s OS_Thread_t;
  89
  90 struct OS_Semaphore_s {
  91    const char *name;
  92    struct OS_Thread_s *threadHead; //threads pending on semaphore
  93    int count;
  94 };
  95 //typedef struct OS_Semaphore_s OS_Semaphore_t;
  96
  97 struct OS_Mutex_s {
  98    OS_Semaphore_t *semaphore;
  99    OS_Thread_t *thread;
 100    int count;
 101 };
 102 //typedef struct OS_Mutex_s OS_Mutex_t;
 103
 104 struct OS_MQueue_s {
 105    const char *name;
 106    OS_Semaphore_t *semaphore;
 107    int count, size, used, read, write;
 108 };
 109 //typedef struct OS_MQueue_s OS_MQueue_t;
 110
 111 struct OS_Timer_s {
 112    const char *name;
 113    struct OS_Timer_s *next, *prev;
 114    uint32 ticksTimeout;
 115    uint32 ticksRestart;
 116    int active;
 117    OS_TimerFuncPtr_t callback;
 118    OS_MQueue_t *mqueue;
 119    uint32 info;
 120 };
 121 //typedef struct OS_Timer_s OS_Timer_t;
 122
 123
 124 /*************** Globals ******************/
 125 static OS_Heap_t *HeapArray[HEAP_COUNT];
 126 static int InterruptInside[OS_CPU_COUNT];
 127 static int ThreadNeedReschedule[OS_CPU_COUNT];
 128 static OS_Thread_t *ThreadCurrent[OS_CPU_COUNT];  //Currently running thread(s)
 129 static OS_Thread_t *ThreadHead;   //Linked list of threads sorted by priority
 130 static OS_Thread_t *TimeoutHead;  //Linked list of threads sorted by timeout
 131 static int ThreadSwapEnabled;
 132 static uint32 ThreadTime;         //Number of ~10ms ticks since reboot
 133 static void *NeedToFree;          //Closed but not yet freed thread
 134 static OS_Semaphore_t SemaphoreReserved[SEM_RESERVED_COUNT];
 135 static OS_Semaphore_t *SemaphoreSleep;
 136 static OS_Semaphore_t *SemaphoreRelease;
 137 static OS_Semaphore_t *SemaphoreLock;
 138 static OS_Semaphore_t *SemaphoreTimer;
 139 static OS_Timer_t *TimerHead;     //Linked list of timers sorted by timeout
 140 static OS_FuncPtr_t Isr[32];
 141
 142
 143 /***************** Heap *******************/
 144 /******************************************/
 145 OS_Heap_t *OS_HeapCreate(const char *name, void *memory, uint32 size)
 146 {
 147    OS_Heap_t *heap;
 148
 149    assert(((uint32)memory & 3) == 0);
 150    heap = (OS_Heap_t*)memory;
 151    heap->magic = HEAP_MAGIC;
 152    heap->name = name;
 153    heap->semaphore = OS_SemaphoreCreate(name, 1);
 154    heap->available = (HeapNode_t*)(heap + 1);
 155    heap->available->next = &heap->base;
 156    heap->available->size = (size - sizeof(OS_Heap_t)) / sizeof(HeapNode_t);
 157    heap->base.next = heap->available;
 158    heap->base.size = 0;
 159    return heap;
 160 }
 161
 162
 163 /******************************************/
 164 void OS_HeapDestroy(OS_Heap_t *heap)
 165 {
 166    OS_SemaphoreDelete(heap->semaphore);
 167 }
 168
 169
 170 /******************************************/
 171 //Modified from Kernighan & Ritchie "The C Programming Language"
 172 void *OS_HeapMalloc(OS_Heap_t *heap, int bytes)
 173 {
 174    HeapNode_t *node, *prevp;
 175    int nunits;
 176
 177    if(heap == NULL && OS_ThreadSelf())
 178       heap = OS_ThreadSelf()->heap;
 179    if((uint32)heap < HEAP_COUNT)
 180       heap = HeapArray[(int)heap];
 181    nunits = (bytes + sizeof(HeapNode_t) - 1) / sizeof(HeapNode_t) + 1;
 182    OS_SemaphorePend(heap->semaphore, OS_WAIT_FOREVER);
 183    prevp = heap->available;
 184    for(node = prevp->next; ; prevp = node, node = node->next)
 185    {
 186       if(node->size >= nunits)       //Big enough?
 187       {
 188          if(node->size == nunits)    //Exactly
 189             prevp->next = node->next;
 190          else
 191          {                           //Allocate tail end
 192             node->size -= nunits;
 193             node += node->size;
 194             node->size = nunits;
 195          }
 196          heap->available = prevp;
 197          node->next = (HeapNode_t*)heap;
 198          OS_SemaphorePost(heap->semaphore);
 199          return (void*)(node + 1);
 200       }
 201       if(node == heap->available)   //Wrapped around free list
 202       {
 203          OS_SemaphorePost(heap->semaphore);
 204          if(heap->alternate)
 205             return OS_HeapMalloc(heap->alternate, bytes);
 206          return NULL;
 207       }
 208    }
 209 }
 210
 211
 212 /******************************************/
 213 //Modified from K&R
 214 void OS_HeapFree(void *block)
 215 {
 216    OS_Heap_t *heap;
 217    HeapNode_t *bp, *node;
 218
 219    assert(block);
 220    bp = (HeapNode_t*)block - 1;   //point to block header
 221    heap = (OS_Heap_t*)bp->next;
 222    assert(heap->magic == HEAP_MAGIC);
 223    if(heap->magic != HEAP_MAGIC)
 224       return;
 225    OS_SemaphorePend(heap->semaphore, OS_WAIT_FOREVER);
 226    for(node = heap->available; !(node < bp && bp < node->next); node = node->next)
 227    {
 228       if(node >= node->next && (bp > node || bp < node->next))
 229          break;               //freed block at start or end of area
 230    }
 231
 232    if(bp + bp->size == node->next)   //join to upper
 233    {
 234       bp->size += node->next->size;
 235       bp->next = node->next->next;
 236    }
 237    else
 238    {
 239       bp->next = node->next;
 240    }
 241
 242    if(node + node->size == bp)       //join to lower
 243    {
 244       node->size += bp->size;
 245       node->next = bp->next;
 246    }
 247    else
 248       node->next = bp;
 249    heap->available = node;
 250    OS_SemaphorePost(heap->semaphore);
 251 }
 252
 253
 254 /******************************************/
 255 void OS_HeapAlternate(OS_Heap_t *heap, OS_Heap_t *alternate)
 256 {
 257    heap->alternate = alternate;
 258 }
 259
 260
 261 /******************************************/
 262 void OS_HeapRegister(void *index, OS_Heap_t *heap)
 263 {
 264    if((uint32)index < HEAP_COUNT)
 265       HeapArray[(int)index] = heap;
 266 }
 267
 268
 269
 270 /***************** Thread *****************/
 271 /******************************************/
 272 //Linked list of threads sorted by priority
 273 //The listed list is either ThreadHead (ready to run threads not including
 274 //the currently running thread) or a list of threads waiting on a semaphore.
 275 //Must be called with interrupts disabled
 276 static void OS_ThreadPriorityInsert(OS_Thread_t **head, OS_Thread_t *thread)
 277 {
 278    OS_Thread_t *node, *prev;
 279
 280    prev = NULL;
 281    for(node = *head; node; node = node->next)
 282    {
 283       if(node->priority < thread->priority)
 284          break;
 285       prev = node;
 286    }
 287
 288    if(prev == NULL)
 289    {
 290       thread->next = *head;
 291       thread->prev = NULL;
 292       if(*head)
 293          (*head)->prev = thread;
 294       *head = thread;
 295    }
 296    else
 297    {
 298       if(prev->next)
 299          prev->next->prev = thread;
 300       thread->next = prev->next;
 301       thread->prev = prev;
 302       prev->next = thread;
 303    }
 304    assert(ThreadHead);
 305    thread->state = THREAD_READY;
 306 }
 307
 308
 309 /******************************************/
 310 //Must be called with interrupts disabled
 311 static void OS_ThreadPriorityRemove(OS_Thread_t **head, OS_Thread_t *thread)
 312 {
 313    assert(thread->magic[0] == THREAD_MAGIC);  //check stack overflow
 314    if(thread->prev == NULL)
 315       *head = thread->next;
 316    else
 317       thread->prev->next = thread->next;
 318    if(thread->next)
 319       thread->next->prev = thread->prev;
 320    thread->next = NULL;
 321    thread->prev = NULL;
 322 }
 323
 324
 325 /******************************************/
 326 //Linked list of threads sorted by timeout value
 327 //Must be called with interrupts disabled
 328 static void OS_ThreadTimeoutInsert(OS_Thread_t *thread)
 329 {
 330    OS_Thread_t *node, *prev;
 331    int diff;
 332
 333    prev = NULL;
 334    for(node = TimeoutHead; node; node = node->nextTimeout)
 335    {
 336       diff = thread->ticksTimeout - node->ticksTimeout;
 337       if(diff <= 0)
 338          break;
 339       prev = node;
 340    }
 341
 342    if(prev == NULL)
 343    {
 344       thread->nextTimeout = TimeoutHead;
 345       thread->prevTimeout = NULL;
 346       if(TimeoutHead)
 347          TimeoutHead->prevTimeout = thread;
 348       TimeoutHead = thread;
 349    }
 350    else
 351    {
 352       if(prev->nextTimeout)
 353          prev->nextTimeout->prevTimeout = thread;
 354       thread->nextTimeout = prev->nextTimeout;
 355       thread->prevTimeout = prev;
 356       prev->nextTimeout = thread;
 357    }
 358 }
 359
 360
 361 /******************************************/
 362 //Must be called with interrupts disabled
 363 static void OS_ThreadTimeoutRemove(OS_Thread_t *thread)
 364 {
 365    if(thread->prevTimeout == NULL && TimeoutHead != thread)
 366       return;         //not in list
 367    if(thread->prevTimeout == NULL)
 368       TimeoutHead = thread->nextTimeout;
 369    else
 370       thread->prevTimeout->nextTimeout = thread->nextTimeout;
 371    if(thread->nextTimeout)
 372       thread->nextTimeout->prevTimeout = thread->prevTimeout;
 373    thread->nextTimeout = NULL;
 374    thread->prevTimeout = NULL;
 375 }
 376
 377
 378 /******************************************/
 379 //Loads highest priority thread from the ThreadHead linked list
 380 //The currently running thread isn't in the ThreadHead list
 381 //Must be called with interrupts disabled
 382 static void OS_ThreadReschedule(int roundRobin)
 383 {
 384    OS_Thread_t *threadNext, *threadCurrent;
 385    int rc, cpuIndex = OS_CpuIndex();
 386
 387    if(ThreadSwapEnabled == 0 || InterruptInside[cpuIndex])
 388    {
 389       ThreadNeedReschedule[cpuIndex] |= 2 + roundRobin;  //Reschedule later
 390       return;
 391    }\r
 392    ThreadNeedReschedule[cpuIndex] = 0;
 393
 394    //Determine which thread should run
 395    threadNext = ThreadHead;
 396    while(threadNext && threadNext->cpuLock != -1 &&
 397          threadNext->cpuLock != cpuIndex)
 398       threadNext = threadNext->next;         //Skip CPU locked threads
 399    if(threadNext == NULL)
 400       return;
 401    threadCurrent = ThreadCurrent[cpuIndex];
 402
 403    if(threadCurrent == NULL ||
 404       threadCurrent->state == THREAD_PEND ||
 405       threadCurrent->priority < threadNext->priority ||
 406       (roundRobin && threadCurrent->priority == threadNext->priority))
 407    {
 408       //Swap threads
 409       ThreadCurrent[cpuIndex] = threadNext;
 410       if(threadCurrent)
 411       {
 412          assert(threadCurrent->magic[0] == THREAD_MAGIC); //check stack overflow
 413          if(threadCurrent->state == THREAD_RUNNING)
 414             OS_ThreadPriorityInsert(&ThreadHead, threadCurrent);
 415          rc = setjmp(threadCurrent->env);  //ANSI C call to save registers
 416          if(rc)
 417             return;  //Returned from longjmp()
 418       }
 419
 420       //Remove the new running thread from the ThreadHead linked list
 421       threadNext = ThreadCurrent[OS_CpuIndex()]; //removed warning
 422       assert(threadNext->state == THREAD_READY);
 423       OS_ThreadPriorityRemove(&ThreadHead, threadNext);
 424       threadNext->state = THREAD_RUNNING;
 425       threadNext->cpuIndex = OS_CpuIndex();
 426       longjmp(threadNext->env, 1);         //ANSI C call to restore registers
 427    }
 428 }
 429
 430
 431 /******************************************/
 432 void OS_ThreadCpuLock(OS_Thread_t *thread, int cpuIndex)
 433 {
 434    thread->cpuLock = cpuIndex;
 435    if(thread == OS_ThreadSelf() && cpuIndex != (int)OS_CpuIndex())
 436       OS_ThreadSleep(1);
 437 }
 438
 439
 440 /******************************************/
 441 static void OS_ThreadInit(void *arg)
 442 {
 443    uint32 cpuIndex = OS_CpuIndex();
 444    (void)arg;
 445
 446    OS_CriticalEnd(1);
 447    ThreadCurrent[cpuIndex]->funcPtr(ThreadCurrent[cpuIndex]->arg);
 448    OS_ThreadExit();
 449 }
 450
 451
 452 /******************************************/
 453 //Stops warning "argument X might be clobbered by `longjmp'"
 454 static void OS_ThreadRegsInit(jmp_buf env)
 455 {
 456    setjmp(env); //ANSI C call to save registers
 457 }
 458
 459
 460 /******************************************/
 461 OS_Thread_t *OS_ThreadCreate(const char *name,
 462                              OS_FuncPtr_t funcPtr,
 463                              void *arg,
 464                              uint32 priority,
 465                              uint32 stackSize)
 466 {
 467    OS_Thread_t *thread;
 468    uint8 *stack;
 469    jmp_buf2 *env;
 470    uint32 state;
 471
 472    OS_SemaphorePend(SemaphoreRelease, OS_WAIT_FOREVER);
 473    if(NeedToFree)
 474       OS_HeapFree(NeedToFree);
 475    NeedToFree = NULL;
 476    OS_SemaphorePost(SemaphoreRelease);
 477
 478    if(stackSize == 0)
 479       stackSize = STACK_SIZE_DEFAULT;
 480    if(stackSize < STACK_SIZE_MINIMUM)
 481       stackSize = STACK_SIZE_MINIMUM;
 482    thread = (OS_Thread_t*)OS_HeapMalloc(NULL, sizeof(OS_Thread_t) + stackSize);
 483    assert(thread);
 484    if(thread == NULL)
 485       return NULL;
 486    memset(thread, 0, sizeof(OS_Thread_t));
 487    stack = (uint8*)(thread + 1);
 488    memset(stack, 0xcd, stackSize);
 489
 490    thread->name = name;
 491    thread->state = THREAD_READY;
 492    thread->cpuLock = -1;
 493    thread->funcPtr = funcPtr;
 494    thread->arg = arg;
 495    thread->priority = priority;
 496    thread->semaphorePending = NULL;
 497    thread->returnCode = 0;
 498    if(OS_ThreadSelf())
 499    {
 500       thread->processId = OS_ThreadSelf()->processId;
 501       thread->heap = OS_ThreadSelf()->heap;
 502    }
 503    else
 504    {
 505       thread->processId = 0;
 506       thread->heap = NULL;
 507    }
 508    thread->next = NULL;
 509    thread->prev = NULL;
 510    thread->nextTimeout = NULL;
 511    thread->prevTimeout = NULL;
 512    thread->magic[0] = THREAD_MAGIC;
 513
 514    OS_ThreadRegsInit(thread->env);
 515    env = (jmp_buf2*)thread->env;
 516    env->sp = (uint32)stack + stackSize - 24; //minimum stack frame size
 517    env->pc = (uint32)OS_ThreadInit;
 518
 519    //Add thread to linked list of ready to run threads
 520    state = OS_CriticalBegin();
 521    OS_ThreadPriorityInsert(&ThreadHead, thread);
 522    OS_ThreadReschedule(0);                   //run highest priority thread
 523    OS_CriticalEnd(state);
 524    return thread;
 525 }
 526
 527
 528 /******************************************/
 529 void OS_ThreadExit(void)
 530 {
 531    uint32 state, cpuIndex = OS_CpuIndex();
 532
 533    for(;;)
 534    {
 535       //Free the memory for closed but not yet freed threads
 536       OS_SemaphorePend(SemaphoreRelease, OS_WAIT_FOREVER);
 537       if(NeedToFree)
 538          OS_HeapFree(NeedToFree);
 539       NeedToFree = NULL;
 540       OS_SemaphorePost(SemaphoreRelease);
 541
 542       state = OS_CriticalBegin();
 543       if(NeedToFree)
 544       {
 545          OS_CriticalEnd(state);
 546          continue;
 547       }
 548       ThreadCurrent[cpuIndex]->state = THREAD_PEND;
 549       NeedToFree = ThreadCurrent[cpuIndex];
 550       OS_ThreadReschedule(0);
 551       OS_CriticalEnd(state);
 552    }
 553 }
 554
 555
 556 /******************************************/
 557 //Return currently running thread
 558 OS_Thread_t *OS_ThreadSelf(void)
 559 {
 560    return ThreadCurrent[OS_CpuIndex()];
 561 }
 562
 563
 564 /******************************************/
 565 //Sleep for ~10 msecs ticks
 566 void OS_ThreadSleep(int ticks)
 567 {
 568    OS_SemaphorePend(SemaphoreSleep, ticks);
 569 }
 570
 571
 572 /******************************************/
 573 //Return the number of ~10 msecs ticks since reboot
 574 uint32 OS_ThreadTime(void)
 575 {
 576    return ThreadTime;
 577 }
 578
 579
 580 /******************************************/
 581 //Save thread unique values
 582 void OS_ThreadInfoSet(OS_Thread_t *thread, uint32 index, void *Info)
 583 {
 584    if(index < INFO_COUNT)
 585       thread->info[index] = Info;
 586 }
 587
 588
 589 /******************************************/
 590 void *OS_ThreadInfoGet(OS_Thread_t *thread, uint32 index)
 591 {
 592    if(index < INFO_COUNT)
 593       return thread->info[index];
 594    return NULL;
 595 }
 596
 597
 598 /******************************************/
 599 uint32 OS_ThreadPriorityGet(OS_Thread_t *thread)
 600 {
 601    return thread->priority;
 602 }
 603
 604
 605 /******************************************/
 606 void OS_ThreadPrioritySet(OS_Thread_t *thread, uint32 priority)
 607 {
 608    uint32 state;
 609    state = OS_CriticalBegin();
 610    thread->priority = priority;
 611    if(thread->state == THREAD_READY)
 612    {
 613       OS_ThreadPriorityRemove(&ThreadHead, thread);
 614       OS_ThreadPriorityInsert(&ThreadHead, thread);
 615       OS_ThreadReschedule(0);
 616    }
 617    OS_CriticalEnd(state);
 618 }
 619
 620
 621 /******************************************/
 622 void OS_ThreadProcessId(OS_Thread_t *thread, uint32 processId, OS_Heap_t *heap)
 623 {
 624    thread->processId = processId;
 625    thread->heap = heap;
 626 }
 627
 628
 629 /******************************************/
 630 //Must be called with interrupts disabled every ~10 msecs
 631 //Will wake up threads that have timed out waiting on a semaphore
 632 void OS_ThreadTick(void *Arg)
 633 {
 634    OS_Thread_t *thread;
 635    OS_Semaphore_t *semaphore;
 636    int diff;
 637    (void)Arg;
 638
 639    ++ThreadTime;         //Number of ~10 msec ticks since reboot
 640    while(TimeoutHead)
 641    {
 642       thread = TimeoutHead;
 643       diff = ThreadTime - thread->ticksTimeout;
 644       if(diff < 0)
 645          break;
 646
 647       //The thread has timed out waiting for a semaphore
 648       OS_ThreadTimeoutRemove(thread);
 649       semaphore = thread->semaphorePending;
 650       ++semaphore->count;
 651       thread->semaphorePending = NULL;
 652       thread->returnCode = -1;
 653       OS_ThreadPriorityRemove(&semaphore->threadHead, thread);
 654       OS_ThreadPriorityInsert(&ThreadHead, thread);
 655    }
 656    OS_ThreadReschedule(1);    //Run highest priority thread
 657 }
 658
 659
 660
 661 /***************** Semaphore **************/
 662 /******************************************/
 663 //Create a counting semaphore
 664 OS_Semaphore_t *OS_SemaphoreCreate(const char *name, uint32 count)
 665 {
 666    OS_Semaphore_t *semaphore;
 667    static int semCount = 0;
 668
 669    if(semCount < SEM_RESERVED_COUNT)
 670       semaphore = &SemaphoreReserved[semCount++];  //Heap not ready yet
 671    else
 672       semaphore = (OS_Semaphore_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_Semaphore_t));
 673    assert(semaphore);
 674    if(semaphore == NULL)
 675       return NULL;
 676
 677    semaphore->name = name;
 678    semaphore->threadHead = NULL;
 679    semaphore->count = count;
 680    return semaphore;
 681 }
 682
 683
 684 /******************************************/
 685 void OS_SemaphoreDelete(OS_Semaphore_t *semaphore)
 686 {
 687    while(semaphore->threadHead)
 688       OS_SemaphorePost(semaphore);
 689    OS_HeapFree(semaphore);
 690 }
 691
 692
 693 /******************************************/
 694 //Sleep the number of ticks (~10ms) until the semaphore is acquired
 695 int OS_SemaphorePend(OS_Semaphore_t *semaphore, int ticks)
 696 {
 697    uint32 state, cpuIndex;
 698    OS_Thread_t *thread;
 699    int returnCode=0;
 700
 701    assert(semaphore);
 702    assert(InterruptInside[OS_CpuIndex()] == 0);
 703    state = OS_CriticalBegin();    //Disable interrupts
 704    if(--semaphore->count < 0)
 705    {
 706       //Semaphore not available
 707       if(ticks == 0)
 708       {
 709          ++semaphore->count;
 710          OS_CriticalEnd(state);
 711          return -1;
 712       }
 713
 714       //Need to sleep until the semaphore is available
 715       cpuIndex = OS_CpuIndex();
 716       thread = ThreadCurrent[cpuIndex];
 717       assert(thread);
 718       thread->semaphorePending = semaphore;
 719       thread->ticksTimeout = ticks + OS_ThreadTime();
 720
 721       //FYI: The current thread isn't in the ThreadHead linked list
 722       //Place the thread into a sorted linked list of pending threads
 723       OS_ThreadPriorityInsert(&semaphore->threadHead, thread);
 724       thread->state = THREAD_PEND;
 725       if(ticks != OS_WAIT_FOREVER)
 726          OS_ThreadTimeoutInsert(thread); //Check every ~10ms for timeouts
 727       assert(ThreadHead);
 728       OS_ThreadReschedule(0);           //Run highest priority thread
 729       returnCode = thread->returnCode;  //Will be -1 if timed out
 730    }
 731    OS_CriticalEnd(state);               //Re-enable interrupts
 732    return returnCode;
 733 }
 734
 735
 736 /******************************************/
 737 //Release a semaphore and possibly wake up a blocked thread
 738 void OS_SemaphorePost(OS_Semaphore_t *semaphore)
 739 {
 740    uint32 state;
 741    OS_Thread_t *thread;
 742
 743    assert(semaphore);
 744    state = OS_CriticalBegin();
 745    if(++semaphore->count <= 0)
 746    {
 747       //Wake up a thread that was waiting for this semaphore
 748       thread = semaphore->threadHead;
 749       OS_ThreadTimeoutRemove(thread);
 750       OS_ThreadPriorityRemove(&semaphore->threadHead, thread);
 751       OS_ThreadPriorityInsert(&ThreadHead, thread);
 752       thread->semaphorePending = NULL;
 753       thread->returnCode = 0;
 754       OS_ThreadReschedule(0);
 755    }
 756    OS_CriticalEnd(state);
 757 }
 758
 759
 760
 761 /***************** Mutex ******************/
 762 /******************************************/
 763 //Create a mutex (a thread aware semaphore)
 764 OS_Mutex_t *OS_MutexCreate(const char *name)
 765 {
 766    OS_Mutex_t *mutex;
 767
 768    mutex = (OS_Mutex_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_Mutex_t));
 769    if(mutex == NULL)
 770       return NULL;
 771    mutex->semaphore = OS_SemaphoreCreate(name, 1);
 772    if(mutex->semaphore == NULL)
 773       return NULL;
 774    mutex->thread = NULL;
 775    mutex->count = 0;
 776    return mutex;
 777 }
 778
 779
 780 /******************************************/
 781 void OS_MutexDelete(OS_Mutex_t *mutex)
 782 {
 783    OS_SemaphoreDelete(mutex->semaphore);
 784    OS_HeapFree(mutex);
 785 }
 786
 787
 788 /******************************************/
 789 void OS_MutexPend(OS_Mutex_t *mutex)
 790 {
 791    OS_Thread_t *thread;
 792
 793    assert(mutex);
 794    thread = OS_ThreadSelf();
 795    if(thread == mutex->thread)
 796    {
 797       ++mutex->count;
 798       return;
 799    }
 800    OS_SemaphorePend(mutex->semaphore, OS_WAIT_FOREVER);
 801    mutex->thread = thread;
 802    mutex->count = 1;
 803 }
 804
 805
 806 /******************************************/
 807 void OS_MutexPost(OS_Mutex_t *mutex)
 808 {
 809    assert(mutex);
 810    assert(mutex->thread == OS_ThreadSelf());
 811    assert(mutex->count > 0);
 812    if(--mutex->count <= 0)
 813    {
 814       mutex->thread = NULL;
 815       OS_SemaphorePost(mutex->semaphore);
 816    }
 817 }
 818
 819
 820
 821 /***************** MQueue *****************/
 822 /******************************************/
 823 //Create a message queue
 824 OS_MQueue_t *OS_MQueueCreate(const char *name,
 825                              int messageCount,
 826                              int messageBytes)
 827 {
 828    OS_MQueue_t *queue;
 829    int size;
 830
 831    assert((messageBytes & 3) == 0);
 832    size = messageBytes / sizeof(uint32);
 833    queue = (OS_MQueue_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_MQueue_t) +
 834       messageCount * size * 4);
 835    if(queue == NULL)
 836       return queue;
 837    queue->name = name;
 838    queue->semaphore = OS_SemaphoreCreate(name, 0);
 839    if(queue->semaphore == NULL)
 840       return NULL;
 841    queue->count = messageCount;
 842    queue->size = size;
 843    queue->used = 0;
 844    queue->read = 0;
 845    queue->write = 0;
 846    return queue;
 847 }
 848
 849
 850 /******************************************/
 851 void OS_MQueueDelete(OS_MQueue_t *mQueue)
 852 {
 853    OS_SemaphoreDelete(mQueue->semaphore);
 854    OS_HeapFree(mQueue);
 855 }
 856
 857
 858 /******************************************/
 859 //Send a message that is messageBytes long (defined during create)
 860 int OS_MQueueSend(OS_MQueue_t *mQueue, void *message)
 861 {
 862    uint32 state, *dst, *src;
 863    int i;
 864
 865    assert(mQueue);
 866    src = (uint32*)message;
 867    state = OS_CriticalBegin();           //Disable interrupts
 868    if(++mQueue->used > mQueue->count)
 869    {
 870       //The message queue is full so discard the message
 871       --mQueue->used;
 872       OS_CriticalEnd(state);
 873       return -1;
 874    }
 875    dst = (uint32*)(mQueue + 1) + mQueue->write * mQueue->size;
 876    for(i = 0; i < mQueue->size; ++i)     //Copy the message into the queue
 877       dst[i] = src[i];
 878    if(++mQueue->write >= mQueue->count)
 879       mQueue->write = 0;
 880    OS_CriticalEnd(state);                //Re-enable interrupts
 881    OS_SemaphorePost(mQueue->semaphore);  //Wakeup the receiving thread
 882    return 0;
 883 }
 884
 885
 886 /******************************************/
 887 //Receive a message that is messageBytes long (defined during create)
 888 //Wait at most ~10 msec ticks
 889 int OS_MQueueGet(OS_MQueue_t *mQueue, void *message, int ticks)
 890 {
 891    uint32 state, *dst, *src;
 892    int i, rc;
 893
 894    assert(mQueue);
 895    rc = OS_SemaphorePend(mQueue->semaphore, ticks); //Wait for message
 896    if(rc)
 897       return rc;                         //Request timed out so rc = -1
 898    state = OS_CriticalBegin();           //Disable interrupts
 899    --mQueue->used;
 900    dst = (uint32*)message;
 901    src = (uint32*)(mQueue + 1) + mQueue->read * mQueue->size;
 902    for(i = 0; i < mQueue->size; ++i)     //Copy message from the queue
 903       dst[i] = src[i];
 904    if(++mQueue->read >= mQueue->count)
 905       mQueue->read = 0;
 906    OS_CriticalEnd(state);                //Re-enable interrupts
 907    return 0;
 908 }
 909
 910
 911
 912 /***************** Jobs *******************/
 913 /******************************************/
 914 typedef void (*JobFunc_t)();
 915 static OS_MQueue_t *jobQueue;
 916 static OS_Thread_t *jobThread;
 917
 918 //This thread waits for jobs that request a function to be called
 919 static void JobThread(void *arg)
 920 {
 921    uint32 message[4];
 922    JobFunc_t funcPtr;
 923    (void)arg;
 924    for(;;)
 925    {
 926       OS_MQueueGet(jobQueue, message, OS_WAIT_FOREVER);
 927       funcPtr = (JobFunc_t)message[0];
 928       funcPtr(message[1], message[2], message[3]);
 929    }
 930 }
 931
 932
 933 /******************************************/
 934 //Call a function using the job thread so the caller won't be blocked
 935 void OS_Job(void (*funcPtr)(), void *arg0, void *arg1, void *arg2)
 936 {
 937    uint32 message[4];
 938    int rc;
 939
 940    OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
 941    if(jobThread == NULL)
 942    {
 943       jobQueue = OS_MQueueCreate("job", 100, 16);
 944       jobThread = OS_ThreadCreate("job", JobThread, NULL, 150, 4000);
 945    }
 946    OS_SemaphorePost(SemaphoreLock);
 947
 948    message[0] = (uint32)funcPtr;
 949    message[1] = (uint32)arg0;
 950    message[2] = (uint32)arg1;
 951    message[3] = (uint32)arg2;
 952    rc = OS_MQueueSend(jobQueue, message);
 953 }
 954
 955
 956 /***************** Timer ******************/
 957 /******************************************/
 958 //This thread polls the list of timers to see if any have timed out
 959 static void OS_TimerThread(void *arg)
 960 {
 961    uint32 timeNow;
 962    int diff, ticks;
 963    uint32 message[8];
 964    OS_Timer_t *timer;
 965    (void)arg;
 966
 967    timeNow = OS_ThreadTime();  //Number of ~10 msec ticks since reboot
 968    for(;;)
 969    {
 970       //Determine how long to sleep
 971       OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
 972       if(TimerHead)
 973          ticks = TimerHead->ticksTimeout - timeNow;
 974       else
 975          ticks = OS_WAIT_FOREVER;
 976       OS_SemaphorePost(SemaphoreLock);
 977       OS_SemaphorePend(SemaphoreTimer, ticks);
 978
 979       //Send messages for all timed out timers
 980       timeNow = OS_ThreadTime();
 981       for(;;)
 982       {
 983          timer = NULL;
 984          OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
 985          if(TimerHead)
 986          {
 987             diff = timeNow - TimerHead->ticksTimeout;
 988             if(diff >= 0)
 989                timer = TimerHead;
 990          }
 991          OS_SemaphorePost(SemaphoreLock);
 992          if(timer == NULL)
 993             break;
 994          if(timer->ticksRestart)
 995             OS_TimerStart(timer, timer->ticksRestart, timer->ticksRestart);
 996          else
 997             OS_TimerStop(timer);
 998
 999          if(timer->callback)
1000             timer->callback(timer, timer->info);
1001          else
1002          {
1003             //Send message
1004             message[0] = MESSAGE_TYPE_TIMER;
1005             message[1] = (uint32)timer;
1006             message[2] = timer->info;
1007             OS_MQueueSend(timer->mqueue, message);
1008          }
1009       }
1010    }
1011 }
1012
1013
1014 /******************************************/
1015 //Create a timer that will send a message upon timeout
1016 OS_Timer_t *OS_TimerCreate(const char *name, OS_MQueue_t *mQueue, uint32 info)
1017 {
1018    OS_Timer_t *timer;
1019
1020    OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
1021    if(SemaphoreTimer == NULL)
1022    {
1023       SemaphoreTimer = OS_SemaphoreCreate("Timer", 0);
1024       OS_ThreadCreate("Timer", OS_TimerThread, NULL, 250, 2000);
1025    }
1026    OS_SemaphorePost(SemaphoreLock);
1027
1028    timer = (OS_Timer_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_Timer_t));
1029    if(timer == NULL)
1030       return NULL;
1031    timer->name = name;
1032    timer->callback = NULL;
1033    timer->mqueue = mQueue;
1034    timer->next = NULL;
1035    timer->prev = NULL;
1036    timer->info = info;
1037    timer->active = 0;
1038    return timer;
1039 }
1040
1041
1042 /******************************************/
1043 void OS_TimerDelete(OS_Timer_t *timer)
1044 {
1045    OS_TimerStop(timer);
1046    OS_HeapFree(timer);
1047 }
1048
1049
1050 /******************************************/
1051 void OS_TimerCallback(OS_Timer_t *timer, OS_TimerFuncPtr_t callback)
1052 {
1053    timer->callback = callback;
1054 }
1055
1056
1057 /******************************************/
1058 //Must not be called from an ISR
1059 //In ~10 msec ticks send a message (or callback)
1060 void OS_TimerStart(OS_Timer_t *timer, uint32 ticks, uint32 ticksRestart)
1061 {
1062    OS_Timer_t *node, *prev;
1063    int diff, check=0;
1064
1065    assert(timer);
1066    assert(InterruptInside[OS_CpuIndex()] == 0);
1067    ticks += OS_ThreadTime();
1068    if(timer->active)
1069       OS_TimerStop(timer);
1070    OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
1071    if(timer->active)
1072    {
1073       //Prevent race condition
1074       OS_SemaphorePost(SemaphoreLock);
1075       return;
1076    }
1077    timer->ticksTimeout = ticks;
1078    timer->ticksRestart = ticksRestart;
1079    timer->active = 1;
1080    prev = NULL;
1081    for(node = TimerHead; node; node = node->next)
1082    {
1083       diff = ticks - node->ticksTimeout;
1084       if(diff <= 0)
1085          break;
1086       prev = node;
1087    }
1088    timer->next = node;
1089    timer->prev = prev;
1090    if(node)
1091       node->prev = timer;
1092    if(prev == NULL)
1093    {
1094       TimerHead = timer;
1095       check = 1;
1096    }
1097    else
1098       prev->next = timer;
1099    OS_SemaphorePost(SemaphoreLock);
1100    if(check)
1101       OS_SemaphorePost(SemaphoreTimer);  //Wakeup OS_TimerThread
1102 }
1103
1104
1105 /******************************************/
1106 //Must not be called from an ISR
1107 void OS_TimerStop(OS_Timer_t *timer)
1108 {
1109    assert(timer);
1110    assert(InterruptInside[OS_CpuIndex()] == 0);
1111    OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
1112    if(timer->active)
1113    {
1114       timer->active = 0;
1115       if(timer->prev == NULL)
1116          TimerHead = timer->next;
1117       else
1118          timer->prev->next = timer->next;
1119       if(timer->next)
1120          timer->next->prev = timer->prev;
1121    }
1122    OS_SemaphorePost(SemaphoreLock);
1123 }
1124
1125
1126 /***************** ISR ********************/
1127 /******************************************/
1128 void OS_InterruptServiceRoutine(uint32 status, uint32 *stack)
1129 {
1130    int i;
1131    uint32 state, cpuIndex = OS_CpuIndex();
1132
1133    if(status == 0 && Isr[31])
1134       Isr[31](stack);                   //SYSCALL or BREAK
1135
1136    InterruptInside[cpuIndex] = 1;
1137    i = 0;
1138    do
1139    {
1140       if(status & 1)
1141       {
1142          if(Isr[i])
1143             Isr[i](stack);
1144          else
1145             OS_InterruptMaskClear(1 << i);
1146       }
1147       status >>= 1;
1148       ++i;
1149    } while(status);
1150    InterruptInside[cpuIndex] = 0;
1151
1152    state = OS_SpinLock();
1153    if(ThreadNeedReschedule[cpuIndex])
1154       OS_ThreadReschedule(ThreadNeedReschedule[cpuIndex] & 1);
1155    OS_SpinUnlock(state);
1156 }
1157
1158
1159 /******************************************/
1160 void OS_InterruptRegister(uint32 mask, OS_FuncPtr_t funcPtr)
1161 {
1162    int i;
1163
1164    for(i = 0; i < 32; ++i)
1165    {
1166       if(mask & (1 << i))
1167          Isr[i] = funcPtr;
1168    }
1169 }
1170
1171
1172 /******************************************/
1173 //Plasma hardware dependent
1174 uint32 OS_InterruptStatus(void)
1175 {
1176    return MemoryRead(IRQ_STATUS);
1177 }
1178
1179
1180 /******************************************/
1181 //Plasma hardware dependent
1182 uint32 OS_InterruptMaskSet(uint32 mask)
1183 {
1184    uint32 state;
1185    state = OS_CriticalBegin();
1186    mask |= MemoryRead(IRQ_MASK);
1187    MemoryWrite(IRQ_MASK, mask);
1188    OS_CriticalEnd(state);
1189    return mask;
1190 }
1191
1192
1193 /******************************************/
1194 //Plasma hardware dependent
1195 uint32 OS_InterruptMaskClear(uint32 mask)
1196 {
1197    uint32 state;
1198    state = OS_CriticalBegin();
1199    mask = MemoryRead(IRQ_MASK) & ~mask;
1200    MemoryWrite(IRQ_MASK, mask);
1201    OS_CriticalEnd(state);
1202    return mask;
1203 }
1204
1205
1206 /**************** Init ********************/
1207 /******************************************/
1208 //If there aren't any other ready to run threads then spin here
1209 static volatile uint32 IdleCount;
1210 static void OS_IdleThread(void *arg)
1211 {
1212    (void)arg;
1213
1214    //Don't block in the idle thread!
1215    for(;;)
1216    {
1217       ++IdleCount;
1218    }
1219 }
1220
1221
1222 /******************************************/
1223 #ifndef DISABLE_IRQ_SIM
1224 //Simulate the hardware interrupts
1225 static void OS_IdleSimulateIsr(void *arg)
1226 {
1227    uint32 count=0, value;
1228    (void)arg;
1229
1230    for(;;)
1231    {
1232       MemoryRead(IRQ_MASK + 4);       //calls Sleep(10)
1233 #if WIN32
1234       while(OS_InterruptMaskSet(0) & IRQ_UART_WRITE_AVAILABLE)
1235          OS_InterruptServiceRoutine(IRQ_UART_WRITE_AVAILABLE, 0);
1236 #endif
1237       value = OS_InterruptMaskSet(0) & 0xf;
1238       if(value)
1239          OS_InterruptServiceRoutine(value, 0);
1240       ++count;
1241    }
1242 }
1243 #endif //DISABLE_IRQ_SIM
1244
1245
1246 /******************************************/
1247 //Plasma hardware dependent
1248 //ISR called every ~10 msecs when bit 18 of the counter register toggles
1249 static void OS_ThreadTickToggle(void *arg)
1250 {
1251    uint32 status, mask, state;
1252
1253    //Toggle looking for IRQ_COUNTER18 or IRQ_COUNTER18_NOT
1254    state = OS_SpinLock();
1255    status = MemoryRead(IRQ_STATUS) & (IRQ_COUNTER18 | IRQ_COUNTER18_NOT);
1256    mask = MemoryRead(IRQ_MASK) | IRQ_COUNTER18 | IRQ_COUNTER18_NOT;
1257    mask &= ~status;
1258    MemoryWrite(IRQ_MASK, mask);
1259    OS_ThreadTick(arg);
1260    OS_SpinUnlock(state);
1261 }
1262
1263
1264 /******************************************/
1265 //Initialize the OS by setting up the system heap and the tick interrupt
1266 void OS_Init(uint32 *heapStorage, uint32 bytes)
1267 {
1268    int i;\r
1269    if((int)OS_Init > 0x10000000)        //Running from DDR?
1270       OS_AsmInterruptInit();            //Patch interrupt vector
1271    OS_InterruptMaskClear(0xffffffff);   //Disable interrupts
1272    HeapArray[0] = OS_HeapCreate("Default", heapStorage, bytes);
1273    HeapArray[1] = HeapArray[0];
1274    SemaphoreSleep = OS_SemaphoreCreate("Sleep", 0);
1275    SemaphoreRelease = OS_SemaphoreCreate("Release", 1);
1276    SemaphoreLock = OS_SemaphoreCreate("Lock", 1);
1277    for(i = 0; i < OS_CPU_COUNT; ++i)
1278       OS_ThreadCreate("Idle", OS_IdleThread, NULL, 0, 256);
1279 #ifndef DISABLE_IRQ_SIM
1280    if((OS_InterruptStatus() & (IRQ_COUNTER18 | IRQ_COUNTER18_NOT)) == 0)
1281    {
1282       //Detected that running in simulator so create SimIsr thread
1283       UartPrintfCritical("SimIsr\n");
1284       OS_ThreadCreate("SimIsr", OS_IdleSimulateIsr, NULL, 1, 0);
1285    }
1286 #endif //DISABLE_IRQ_SIM
1287    //Plasma hardware dependent (register for OS tick interrupt every ~10 msecs)
1288    OS_InterruptRegister(IRQ_COUNTER18 | IRQ_COUNTER18_NOT, OS_ThreadTickToggle);
1289    OS_InterruptMaskSet(IRQ_COUNTER18 | IRQ_COUNTER18_NOT);
1290 }
1291
1292
1293 /******************************************/
1294 //Start thread swapping
1295 void OS_Start(void)
1296 {
1297    ThreadSwapEnabled = 1;
1298    (void)OS_SpinLock();
1299    OS_ThreadReschedule(1);
1300 }
1301
1302
1303 /******************************************/
1304 //Place breakpoint here
1305 void OS_Assert(void)
1306 {
1307 }
1308
1309
1310 #if OS_CPU_COUNT > 1
1311 static uint8 SpinLockArray[OS_CPU_COUNT];
1312 /******************************************/
1313 uint32 OS_CpuIndex(void)
1314 {
1315    return 0; //0 to OS_CPU_COUNT-1
1316 }
1317
1318
1319 /******************************************/
1320 //Symmetric Multiprocessing Spin Lock Mutex
1321 uint32 OS_SpinLock(void)
1322 {
1323    uint32 state, cpuIndex, i, j, ok, delay;
1324
1325    cpuIndex = OS_CpuIndex();
1326    delay = cpuIndex + 8;
1327    state = OS_AsmInterruptEnable(0);
1328    //Spin until only this CPU has the spin lock
1329    do
1330    {
1331       ok = 1;
1332       if(++SpinLockArray[cpuIndex] == 1)
1333       {
1334          for(i = 0; i < OS_CPU_COUNT; ++i)
1335          {
1336             if(i != cpuIndex && SpinLockArray[i])
1337                ok = 0;   //Another CPU has the spin lock
1338          }
1339          if(ok == 0)
1340          {
1341             SpinLockArray[cpuIndex] = 0;
1342             for(j = 0; j < delay; ++j)  //wait a bit
1343                ++i;
1344             if(delay < 128)
1345                delay <<= 1;
1346          }
1347       }
1348    } while(ok == 0);
1349    return state;
1350 }
1351
1352
1353 /******************************************/
1354 void OS_SpinUnlock(uint32 state)
1355 {
1356    uint32 cpuIndex;
1357    cpuIndex = OS_CpuIndex();
1358    if(--SpinLockArray[cpuIndex] == 0)
1359       OS_AsmInterruptEnable(state);
1360
1361    assert(SpinLockArray[cpuIndex] < 10);
1362 }
1363 #endif  //OS_CPU_COUNT > 1
1364
1365
1366 /************** WIN32/Linux Support *************/
1367 #ifdef WIN32
1368 #ifdef LINUX
1369 #define putch putchar
1370 #undef _LIBC
1371 #undef kbhit
1372 #undef getch
1373 #define UartPrintf UartPrintf2
1374 #define UartScanf UartScanf2
1375 #include <stdio.h>
1376 #include <stdlib.h>
1377 #include <termios.h>
1378 #include <unistd.h>
1379 //Support RTOS inside Linux
1380 void Sleep(unsigned int value)
1381 {
1382    usleep(value * 1000);
1383 }
1384
1385 int kbhit(void)
1386 {
1387    struct termios oldt, newt;
1388    struct timeval tv;
1389    fd_set read_fd;
1390
1391    tcgetattr(STDIN_FILENO, &oldt);
1392    newt = oldt;
1393    newt.c_lflag &= ~(ICANON | ECHO);
1394    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
1395    tv.tv_sec=0;
1396    tv.tv_usec=0;
1397    FD_ZERO(&read_fd);
1398    FD_SET(0,&read_fd);
1399    if(select(1, &read_fd, NULL, NULL, &tv) == -1)
1400       return 0;
1401    if(FD_ISSET(0,&read_fd))
1402       return 1;
1403    return 0;
1404 }
1405
1406 int getch(void)
1407 {
1408    struct termios oldt, newt;
1409    int ch;
1410
1411    tcgetattr(STDIN_FILENO, &oldt);
1412    newt = oldt;
1413    newt.c_lflag &= ~(ICANON | ECHO);
1414    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
1415    ch = getchar();
1416    return ch;
1417 }
1418 #else
1419 //Support RTOS inside Windows
1420 #undef kbhit
1421 #undef getch
1422 #undef putch
1423 extern int kbhit();
1424 extern int getch(void);
1425 extern int putch(int);
1426 extern void __stdcall Sleep(unsigned long value);
1427 #endif
1428
1429 static uint32 Memory[8];
1430
1431 //Simulates device register memory reads
1432 uint32 MemoryRead(uint32 address)
1433 {
1434    Memory[2] |= IRQ_UART_WRITE_AVAILABLE;    //IRQ_STATUS
1435    switch(address)
1436    {
1437    case UART_READ:
1438       if(kbhit())
1439          Memory[0] = getch();                //UART_READ
1440       Memory[2] &= ~IRQ_UART_READ_AVAILABLE; //clear bit
1441       return Memory[0];
1442    case IRQ_MASK:
1443       return Memory[1];                      //IRQ_MASK
1444    case IRQ_MASK + 4:
1445       Sleep(10);
1446       return 0;
1447    case IRQ_STATUS:
1448       if(kbhit())
1449          Memory[2] |= IRQ_UART_READ_AVAILABLE;
1450       return Memory[2];
1451    }
1452    return 0;
1453 }
1454
1455 //Simulates device register memory writes
1456 void MemoryWrite(uint32 address, uint32 value)
1457 {
1458    switch(address)
1459    {
1460    case UART_WRITE:
1461       putch(value);
1462       break;
1463    case IRQ_MASK:
1464       Memory[1] = value;
1465       break;
1466    case IRQ_STATUS:
1467       Memory[2] = value;
1468       break;
1469    }
1470 }
1471
1472 uint32 OS_AsmInterruptEnable(uint32 enableInterrupt)
1473 {
1474    return enableInterrupt;
1475 }
1476
1477 void OS_AsmInterruptInit(void)
1478 {
1479 }
1480 #endif  //WIN32
1481
1482
1483 /**************** Example *****************/
1484 #ifndef NO_MAIN
1485 #ifdef WIN32
1486 static uint8 HeapSpace[1024*512];  //For simulation on a PC
1487 #endif
1488
1489 int main(int programEnd, char *argv[])
1490 {
1491    (void)programEnd;  //Pointer to end of used memory
1492    (void)argv;
1493
1494    UartPrintfCritical("Starting RTOS\n");
1495 #ifdef WIN32
1496    OS_Init((uint32*)HeapSpace, sizeof(HeapSpace));  //For PC simulation
1497 #else
1498    //Create heap in remaining space after program in 1MB external RAM
1499    OS_Init((uint32*)programEnd,
1500            RAM_EXTERNAL_BASE + RAM_EXTERNAL_SIZE - programEnd);
1501 #endif
1502    UartInit();
1503    OS_ThreadCreate("Main", MainThread, NULL, 100, 4000);
1504    OS_Start();
1505    return 0;
1506 }
1507 #endif  //NO_MAIN
1508