URL https://opencores.org/ocsvn/plasma/plasma/trunk

Subversion Repositories plasma

[/] [plasma/] [trunk/] [kernel/] [rtos.c] - Blame information for rev 167

Go to most recent revision | Details | Compare with Previous | View Log


/*--------------------------------------------------------------------
 * TITLE: Plasma Real Time Operating System
 * AUTHOR: Steve Rhoads (rhoadss@yahoo.com)
 * DATE CREATED: 12/17/05
 * FILENAME: rtos.c
 * PROJECT: Plasma CPU core
 * COPYRIGHT: Software placed into the public domain by the author.
 *    Software 'as is' without warranty.  Author liable for nothing.
 * DESCRIPTION:
 *    Plasma Real Time Operating System
 *    Fully pre-emptive RTOS with support for:
 *       Heaps, Threads, Semaphores, Mutexes, Message Queues, and Timers.
 *    This file tries to be hardware independent except for calls to:
 *       MemoryRead() and MemoryWrite() for interrupts.
 *    Partial support for multiple CPUs using symmetric multiprocessing.
 *--------------------------------------------------------------------*/
#include "plasma.h"
#include "rtos.h"
 
#define HEAP_MAGIC 0x1234abcd
#define THREAD_MAGIC 0x4321abcd
#define SEM_RESERVED_COUNT 2
#define HEAP_COUNT 8
 
 
/*************** Structures ***************/
#ifdef WIN32
#define setjmp _setjmp
   //x86 registers
   typedef struct jmp_buf2 {
      uint32 Ebp, Ebx, Edi, Esi, sp, pc, extra[10];
   } jmp_buf2;
#else  
   //Plasma registers
   typedef struct jmp_buf2 {
      uint32 s[9], gp, sp, pc;
   } jmp_buf2;
#endif
 
typedef struct HeapNode_s {
   struct HeapNode_s *next;
   int size;
} HeapNode_t;
 
struct OS_Heap_s {
   uint32 magic;
   const char *name;
   OS_Semaphore_t *semaphore;
   HeapNode_t *available;
   HeapNode_t base;
   struct OS_Heap_s *alternate;
};
//typedef struct OS_Heap_s OS_Heap_t;
 
struct OS_Thread_s {
   const char *name;
   jmp_buf env;
   OS_FuncPtr_t funcPtr;
   void *arg;
   uint32 priority;
   uint32 ticksTimeout;
   void *info;
   OS_Semaphore_t *semaphorePending;
   int returnCode;
   uint32 spinLocks;
   struct OS_Thread_s *next, *prev;
   struct OS_Thread_s *nextTimeout, *prevTimeout;
   uint32 magic[1];
};
//typedef struct OS_Thread_s OS_Thread_t;
 
struct OS_Semaphore_s {
   const char *name;
   struct OS_Thread_s *threadHead;
   int count;
};
//typedef struct OS_Semaphore_s OS_Semaphore_t;
 
struct OS_Mutex_s {
   OS_Semaphore_t *semaphore;
   OS_Thread_t *thread;
   int count;
};
//typedef struct OS_Mutex_s OS_Mutex_t;
 
struct OS_MQueue_s {
   const char *name;
   OS_Semaphore_t *semaphore;
   int count, size, used, read, write;
};
//typedef struct OS_MQueue_s OS_MQueue_t;
 
struct OS_Timer_s {
   const char *name;
   struct OS_Timer_s *next, *prev;
   uint32 ticksTimeout;
   uint32 ticksRestart;
   int active;
   OS_MQueue_t *mqueue;
   uint32 info;
};
//typedef struct OS_Timer_s OS_Timer_t;
 
 
/*************** Globals ******************/
static OS_Heap_t *HeapArray[HEAP_COUNT];
static OS_Semaphore_t *SemaphoreSleep;
static OS_Semaphore_t *SemaphoreLock;
static int ThreadSwapEnabled;
static int ThreadCurrentActive[OS_CPU_COUNT];
static int ThreadNeedReschedule;
static uint32 ThreadTime;
static OS_Thread_t *ThreadHead;   //Linked list of threads sorted by priority
static OS_Thread_t *TimeoutHead;  //Linked list of threads sorted by timeout
static OS_Thread_t *ThreadCurrent[OS_CPU_COUNT];
static void *NeedToFree;
static OS_Semaphore_t SemaphoreReserved[SEM_RESERVED_COUNT];
static OS_Timer_t *TimerHead;     //Linked list of timers sorted by timeout
static OS_Semaphore_t *SemaphoreTimer;
static OS_FuncPtr_t Isr[32];
static int InterruptInside;
 
 
/***************** Heap *******************/
/******************************************/
OS_Heap_t *OS_HeapCreate(const char *Name, void *Memory, uint32 Size)
{
   OS_Heap_t *heap;
 
   assert(((uint32)Memory & 3) == 0);
   heap = (OS_Heap_t*)Memory;
   heap->magic = HEAP_MAGIC;
   heap->name = Name;
   heap->semaphore = OS_SemaphoreCreate(Name, 1);
   heap->available = (HeapNode_t*)(heap + 1);
   heap->available->next = &heap->base;
   heap->available->size = (Size - sizeof(OS_Heap_t)) / sizeof(HeapNode_t);
   heap->base.next = heap->available;
   heap->base.size = 0;
   return heap;
}
 
 
/******************************************/
void OS_HeapDestroy(OS_Heap_t *Heap)
{
   OS_SemaphoreDelete(Heap->semaphore);
}
 
 
/******************************************/
//Modified from K&R
void *OS_HeapMalloc(OS_Heap_t *Heap, int Bytes)
{
   HeapNode_t *node, *prevp;
   int nunits;
 
   if((int)Heap < HEAP_COUNT)
      Heap = HeapArray[(int)Heap];
   //printf("OS_HeapMalloc(%s, %d)\n", Heap->name, Bytes);
   nunits = (Bytes + sizeof(HeapNode_t) - 1) / sizeof(HeapNode_t) + 1;
   OS_SemaphorePend(Heap->semaphore, OS_WAIT_FOREVER);
   prevp = Heap->available;
   for(node = prevp->next; ; prevp = node, node = node->next)
   {
      if(node->size >= nunits)       //Big enough?
      {
         if(node->size == nunits)    //Exactly
            prevp->next = node->next;
         else
         {                           //Allocate tail end
            node->size -= nunits;
            node += node->size;
            node->size = nunits;
         }
         Heap->available = prevp;
         node->next = (HeapNode_t*)Heap;
         OS_SemaphorePost(Heap->semaphore);
         //printf("ptr=0x%x\n", (uint32)(node + 1));
         return (void*)(node + 1);
      }
      if(node == Heap->available)   //Wrapped around free list
      {
         OS_SemaphorePost(Heap->semaphore);
         if(Heap->alternate)
            return OS_HeapMalloc(Heap->alternate, Bytes);
         return NULL;
      }
   }
}
 
 
/******************************************/
//Modified from K&R
void OS_HeapFree(void *Block)
{
   OS_Heap_t *heap;
   HeapNode_t *bp, *node;
 
   assert(Block);
   //printf("OS_HeapFree(0x%x)\n", Block);
   bp = (HeapNode_t*)Block - 1;   //point to block header
   heap = (OS_Heap_t*)bp->next;
   assert(heap->magic == HEAP_MAGIC);
   if(heap->magic != HEAP_MAGIC)
      return;
   OS_SemaphorePend(heap->semaphore, OS_WAIT_FOREVER);
   for(node = heap->available; !(node < bp && bp < node->next); node = node->next)
   {
      if(node >= node->next && (bp > node || bp < node->next))
         break;               //freed block at start or end of area
   }
 
   if(bp + bp->size == node->next)   //join to upper
   {
      bp->size += node->next->size;
      bp->next = node->next->next;
   }
   else
   {
      bp->next = node->next;
   }
 
   if(node + node->size == bp)       //join to lower
   {
      node->size += bp->size;
      node->next = bp->next;
   }
   else
      node->next = bp;
   heap->available = node;
   OS_SemaphorePost(heap->semaphore);
}
 
 
/******************************************/
void OS_HeapAlternate(OS_Heap_t *Heap, OS_Heap_t *Alternate)
{
   Heap->alternate = Alternate;
}
 
 
/******************************************/
void OS_HeapRegister(void *Index, OS_Heap_t *Heap)
{
   if((int)Index < HEAP_COUNT)
      HeapArray[(int)Index] = Heap;
}
 
 
 
/***************** Thread *****************/
/******************************************/
//Linked list of threads sorted by priority
//Must be called with interrupts disabled
static void OS_ThreadPriorityInsert(OS_Thread_t **head, OS_Thread_t *thread)
{
   OS_Thread_t *node, *prev;
 
   prev = NULL;
   for(node = *head; node; node = node->next)
   {
      if(node->priority <= thread->priority)
         break;
      prev = node;
   }
 
   if(prev == NULL)
   {
      thread->next = *head;
      thread->prev = NULL;
      *head = thread;
   }
   else
   {
      if(prev->next)
         prev->next->prev = thread;
      thread->next = prev->next;
      thread->prev = prev;
      prev->next = thread;
   }
   assert(ThreadHead);
}
 
 
/******************************************/
//Must be called with interrupts disabled
static void OS_ThreadPriorityRemove(OS_Thread_t **head, OS_Thread_t *thread)
{
   uint32 cpuIndex = OS_CpuIndex();
   //printf("r(%d) ", thread->priority);
   assert(thread->magic[0] == THREAD_MAGIC);  //check stack overflow
   if(thread->prev == NULL)
      *head = thread->next;
   else
      thread->prev->next = thread->next;
   if(thread->next)
      thread->next->prev = thread->prev;
   thread->next = NULL;
   thread->prev = NULL;
   if(head == &ThreadHead && ThreadCurrent[cpuIndex] == thread)
      ThreadCurrentActive[cpuIndex] = 0;
}
 
 
/******************************************/
//Linked list of threads sorted by timeout value
//Must be called with interrupts disabled
static void OS_ThreadTimeoutInsert(OS_Thread_t *thread)
{
   OS_Thread_t *node, *prev;
   int diff;
 
   prev = NULL;
   for(node = TimeoutHead; node; node = node->nextTimeout)
   {
      diff = thread->ticksTimeout - node->ticksTimeout;
      if(diff <= 0)
         break;
      prev = node;
   }
 
   if(prev == NULL)
   {
      thread->nextTimeout = TimeoutHead;
      thread->prevTimeout = NULL;
      if(TimeoutHead)
         TimeoutHead->prevTimeout = thread;
      TimeoutHead = thread;
   }
   else
   {
      if(prev->nextTimeout)
         prev->nextTimeout->prevTimeout = thread;
      thread->nextTimeout = prev->nextTimeout;
      thread->prevTimeout = prev;
      prev->nextTimeout = thread;
   }
}
 
 
/******************************************/
//Must be called with interrupts disabled
static void OS_ThreadTimeoutRemove(OS_Thread_t *thread)
{
   if(thread->prevTimeout == NULL && TimeoutHead != thread)
      return;         //not in list
   if(thread->prevTimeout == NULL)
      TimeoutHead = thread->nextTimeout;
   else
      thread->prevTimeout->nextTimeout = thread->nextTimeout;
   if(thread->nextTimeout)
      thread->nextTimeout->prevTimeout = thread->prevTimeout;
   thread->nextTimeout = NULL;
   thread->prevTimeout = NULL;
}
 
 
/******************************************/
//Loads a new thread 
//Must be called with interrupts disabled
static void OS_ThreadReschedule(int RoundRobin)
{
   OS_Thread_t *threadNext, *threadPrev, *threadCurrent;
   uint32 cpuIndex = OS_CpuIndex();
   int rc;
 
   if(ThreadSwapEnabled == 0 || InterruptInside)
   {
      ThreadNeedReschedule |= 2 + RoundRobin;
      return;
   }
 
   //Determine which thread should run
   threadCurrent = ThreadCurrent[cpuIndex];
   threadNext = threadCurrent;
   assert(ThreadHead);
   if(ThreadCurrentActive[cpuIndex] == 0 || threadCurrent == NULL)
      threadNext = ThreadHead;
   else if(threadCurrent->priority < ThreadHead->priority)
      threadNext = ThreadHead;
   else if(RoundRobin)
   {
      if(threadCurrent->next &&
         threadCurrent->next->priority == ThreadHead->priority)
         threadNext = threadCurrent->next;
      else
         threadNext = ThreadHead;
   }
 
   if(threadNext != threadCurrent)
   {
      //Swap threads
      threadPrev = threadCurrent;
      ThreadCurrent[cpuIndex] = threadNext;
      assert(threadNext);
      if(threadPrev)
      {
         assert(threadPrev->magic[0] == THREAD_MAGIC); //check stack overflow
         threadPrev->spinLocks = OS_SpinLockGet();
         //printf("OS_ThreadRescheduleSave(%s)\n", threadPrev->name);
         rc = setjmp(threadPrev->env);  //ANSI C call to save registers
         if(rc)
         {
            //Returned from longjmp()
            return;
         }
      }
      cpuIndex = OS_CpuIndex();             //removed warning
      threadNext = ThreadCurrent[cpuIndex]; //removed warning
      ThreadCurrentActive[cpuIndex] = 1;
      //printf("OS_ThreadRescheduleRestore(%s)\n", threadNext->name);
      OS_SpinLockSet(threadNext->spinLocks);
      longjmp(threadNext->env, 1);      //ANSI C call to restore registers
   }
}
 
 
/******************************************/
static void OS_ThreadInit(void *Arg)
{
   uint32 cpuIndex = OS_CpuIndex();
   (void)Arg;
 
   OS_AsmInterruptEnable(1);
   ThreadCurrent[cpuIndex]->funcPtr(ThreadCurrent[cpuIndex]->arg);
   OS_ThreadExit();
}
 
 
/******************************************/
//Stops warning "argument X might be clobbered by `longjmp'"
static void OS_ThreadRegsInit(jmp_buf env)
{
   setjmp(env); //ANSI C call to save registers
}
 
 
/******************************************/
OS_Thread_t *OS_ThreadCreate(const char *Name,
                             OS_FuncPtr_t FuncPtr,
                             void *Arg,
                             uint32 Priority,
                             uint32 StackSize)
{
   OS_Thread_t *thread;
   uint8 *stack;
   jmp_buf2 *env;
   uint32 state;
 
   OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
   if(NeedToFree)
      OS_HeapFree(NeedToFree);
   NeedToFree = NULL;
   OS_SemaphorePost(SemaphoreLock);
 
   if(StackSize == 0)
      StackSize = STACK_SIZE_DEFAULT;
   if(StackSize < STACK_SIZE_MINIMUM)
      StackSize = STACK_SIZE_MINIMUM;
   thread = (OS_Thread_t*)OS_HeapMalloc(NULL, sizeof(OS_Thread_t) + StackSize);
   assert(thread);
   if(thread == NULL)
      return NULL;
   stack = (uint8*)(thread + 1);
   memset(stack, 0xcd, StackSize);
 
   thread->name = Name;
   thread->funcPtr = FuncPtr;
   thread->arg = Arg;
   thread->priority = Priority;
   thread->info = NULL;
   thread->semaphorePending = NULL;
   thread->returnCode = 0;
   thread->spinLocks = 1;
   thread->next = NULL;
   thread->prev = NULL;
   thread->nextTimeout = NULL;
   thread->prevTimeout = NULL;
   thread->magic[0] = THREAD_MAGIC;
 
   OS_ThreadRegsInit(thread->env);
   env = (jmp_buf2*)thread->env;
   env->sp = (uint32)stack + StackSize - 4;
   env->pc = (uint32)OS_ThreadInit;
 
   state = OS_CriticalBegin();
   OS_ThreadPriorityInsert(&ThreadHead, thread);
   OS_ThreadReschedule(0);
   OS_CriticalEnd(state);
   return thread;
}
 
 
/******************************************/
void OS_ThreadExit(void)
{
   uint32 state, cpuIndex = OS_CpuIndex();
   OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
   if(NeedToFree)
      OS_HeapFree(NeedToFree);
   NeedToFree = NULL;
   OS_SemaphorePost(SemaphoreLock);
 
   state = OS_CriticalBegin();
   OS_ThreadPriorityRemove(&ThreadHead, ThreadCurrent[cpuIndex]);
   NeedToFree = ThreadCurrent[cpuIndex];
   OS_ThreadReschedule(0);
   assert(ThreadHead == NULL);
   OS_CriticalEnd(state);
}
 
 
/******************************************/
OS_Thread_t *OS_ThreadSelf(void)
{
   return ThreadCurrent[OS_CpuIndex()];
}
 
 
/******************************************/
void OS_ThreadSleep(int Ticks)
{
   OS_SemaphorePend(SemaphoreSleep, Ticks);
}
 
 
/******************************************/
uint32 OS_ThreadTime(void)
{
   return ThreadTime;
}
 
 
/******************************************/
void OS_ThreadInfoSet(OS_Thread_t *Thread, void *Info)
{
   Thread->info = Info;
}
 
 
/******************************************/
void *OS_ThreadInfoGet(OS_Thread_t *Thread)
{
   return Thread->info;
}
 
 
/******************************************/
uint32 OS_ThreadPriorityGet(OS_Thread_t *Thread)
{
   return Thread->priority;
}
 
 
/******************************************/
void OS_ThreadPrioritySet(OS_Thread_t *Thread, uint32 Priority)
{
   uint32 state;
   state = OS_CriticalBegin();
   OS_ThreadPriorityRemove(&ThreadHead, Thread);
   Thread->priority = Priority;
   OS_ThreadPriorityInsert(&ThreadHead, Thread);
   OS_ThreadReschedule(0);
   OS_CriticalEnd(state);
}
 
 
/******************************************/
//Must be called with interrupts disabled
void OS_ThreadTick(void *Arg)
{
   OS_Thread_t *thread;
   OS_Semaphore_t *semaphore;
   int diff;
   (void)Arg;
 
   ++ThreadTime;
   while(TimeoutHead)
   {
      thread = TimeoutHead;
      diff = ThreadTime - thread->ticksTimeout;
      if(diff < 0)
         break;
      OS_ThreadTimeoutRemove(thread);
      semaphore = thread->semaphorePending;
      ++semaphore->count;
      thread->semaphorePending = NULL;
      thread->returnCode = -1;
      OS_ThreadPriorityRemove(&semaphore->threadHead, thread);
      OS_ThreadPriorityInsert(&ThreadHead, thread);
   }
   OS_ThreadReschedule(1);
}
 
 
 
/***************** Semaphore **************/
/******************************************/
OS_Semaphore_t *OS_SemaphoreCreate(const char *Name, uint32 Count)
{
   OS_Semaphore_t *semaphore;
   static int semCount;
 
   if(semCount < SEM_RESERVED_COUNT)
      semaphore = &SemaphoreReserved[semCount++];
   else
      semaphore = (OS_Semaphore_t*)OS_HeapMalloc(NULL, sizeof(OS_Semaphore_t));
   assert(semaphore);
   if(semaphore == NULL)
      return NULL;
 
   semaphore->name = Name;
   semaphore->threadHead = NULL;
   semaphore->count = Count;
   return semaphore;
}
 
 
/******************************************/
void OS_SemaphoreDelete(OS_Semaphore_t *Semaphore)
{
   while(Semaphore->threadHead)
      OS_SemaphorePost(Semaphore);
   OS_HeapFree(Semaphore);
}
 
 
/******************************************/
int OS_SemaphorePend(OS_Semaphore_t *Semaphore, int Ticks)
{
   uint32 state, cpuIndex;
   OS_Thread_t *thread;
   int returnCode=0;
 
   assert(Semaphore);
   assert(InterruptInside == 0);
   state = OS_CriticalBegin();
   if(--Semaphore->count < 0)
   {
      cpuIndex = OS_CpuIndex();
      assert(ThreadCurrent[cpuIndex]);
      if(Ticks == 0)
      {
         ++Semaphore->count;
         OS_CriticalEnd(state);
         return -1;
      }
      thread = ThreadCurrent[cpuIndex];
      thread->semaphorePending = Semaphore;
      thread->ticksTimeout = Ticks + OS_ThreadTime();
      OS_ThreadPriorityRemove(&ThreadHead, thread);
      OS_ThreadPriorityInsert(&Semaphore->threadHead, thread);
      if(Ticks != OS_WAIT_FOREVER)
         OS_ThreadTimeoutInsert(thread);
      ThreadCurrentActive[cpuIndex] = 0;
      assert(ThreadHead);
      OS_ThreadReschedule(0);
      returnCode = thread->returnCode;
   }
   OS_CriticalEnd(state);
   return returnCode;
}
 
 
/******************************************/
void OS_SemaphorePost(OS_Semaphore_t *Semaphore)
{
   uint32 state;
   OS_Thread_t *thread;
 
   assert(Semaphore);
   state = OS_CriticalBegin();
   if(++Semaphore->count <= 0)
   {
      thread = Semaphore->threadHead;
      OS_ThreadTimeoutRemove(thread);
      OS_ThreadPriorityRemove(&Semaphore->threadHead, thread);
      OS_ThreadPriorityInsert(&ThreadHead, thread);
      thread->semaphorePending = NULL;
      thread->returnCode = 0;
      OS_ThreadReschedule(0);
   }
   OS_CriticalEnd(state);
}
 
 
 
/***************** Mutex ******************/
/******************************************/
OS_Mutex_t *OS_MutexCreate(const char *Name)
{
   OS_Mutex_t *mutex;
 
   mutex = (OS_Mutex_t*)OS_HeapMalloc(NULL, sizeof(OS_Mutex_t));
   if(mutex == NULL)
      return NULL;
   mutex->semaphore = OS_SemaphoreCreate(Name, 1);
   if(mutex->semaphore == NULL)
      return NULL;
   mutex->thread = NULL;
   mutex->count = 0;
   return mutex;
}
 
 
/******************************************/
void OS_MutexDelete(OS_Mutex_t *Mutex)
{
   OS_SemaphoreDelete(Mutex->semaphore);
   OS_HeapFree(Mutex);
}
 
 
/******************************************/
void OS_MutexPend(OS_Mutex_t *Mutex)
{
   OS_Thread_t *thread;
 
   assert(Mutex);
   thread = OS_ThreadSelf();
   if(thread == Mutex->thread)
   {
      ++Mutex->count;
      return;
   }
   OS_SemaphorePend(Mutex->semaphore, OS_WAIT_FOREVER);
   Mutex->thread = thread;
   Mutex->count = 1;
}
 
 
/******************************************/
void OS_MutexPost(OS_Mutex_t *Mutex)
{
   assert(Mutex);
   assert(Mutex->thread == OS_ThreadSelf());
   assert(Mutex->count > 0);
   if(--Mutex->count <= 0)
   {
      Mutex->thread = NULL;
      OS_SemaphorePost(Mutex->semaphore);
   }
}
 
 
 
/***************** MQueue *****************/
/******************************************/
OS_MQueue_t *OS_MQueueCreate(const char *Name,
                             int MessageCount,
                             int MessageBytes)
{
   OS_MQueue_t *queue;
   int size;
 
   size = MessageBytes / sizeof(uint32);
   queue = (OS_MQueue_t*)OS_HeapMalloc(NULL, sizeof(OS_MQueue_t) +
      MessageCount * size * 4);
   if(queue == NULL)
      return queue;
   queue->name = Name;
   queue->semaphore = OS_SemaphoreCreate(Name, 0);
   if(queue->semaphore == NULL)
      return NULL;
   queue->count = MessageCount;
   queue->size = size;
   queue->used = 0;
   queue->read = 0;
   queue->write = 0;
   return queue;
}
 
 
/******************************************/
void OS_MQueueDelete(OS_MQueue_t *MQueue)
{
   OS_SemaphoreDelete(MQueue->semaphore);
   OS_HeapFree(MQueue);
}
 
 
/******************************************/
int OS_MQueueSend(OS_MQueue_t *MQueue, void *Message)
{
   uint32 state, *dst, *src;
   int i;
 
   assert(MQueue);
   src = (uint32*)Message;
   state = OS_CriticalBegin();
   if(++MQueue->used > MQueue->count)
   {
      --MQueue->used;
      OS_CriticalEnd(state);
      return -1;
   }
   dst = (uint32*)(MQueue + 1) + MQueue->write * MQueue->size;
   for(i = 0; i < MQueue->size; ++i)
      dst[i] = src[i];
   if(++MQueue->write >= MQueue->count)
      MQueue->write = 0;
   OS_CriticalEnd(state);
   OS_SemaphorePost(MQueue->semaphore);
   return 0;
}
 
 
/******************************************/
int OS_MQueueGet(OS_MQueue_t *MQueue, void *Message, int Ticks)
{
   uint32 state, *dst, *src;
   int i, rc;
 
   assert(MQueue);
   dst = (uint32*)Message;
   rc = OS_SemaphorePend(MQueue->semaphore, Ticks);
   if(rc)
      return rc;
   state = OS_CriticalBegin();
   --MQueue->used;
   src = (uint32*)(MQueue + 1) + MQueue->read * MQueue->size;
   for(i = 0; i < MQueue->size; ++i)
      dst[i] = src[i];
   if(++MQueue->read >= MQueue->count)
      MQueue->read = 0;
   OS_CriticalEnd(state);
   return 0;
}
 
 
 
/***************** Timer ******************/
/******************************************/
static void OS_TimerThread(void *Arg)
{
   uint32 timeNow;
   int diff, ticks;
   uint32 message[8];
   OS_Timer_t *timer;
   (void)Arg;
 
   timeNow = OS_ThreadTime();
   for(;;)
   {
      //Determine how long to sleep
      OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
      if(TimerHead)
         ticks = TimerHead->ticksTimeout - timeNow;
      else
         ticks = OS_WAIT_FOREVER;
      OS_SemaphorePost(SemaphoreLock);
      OS_SemaphorePend(SemaphoreTimer, ticks);
 
      //Send messages for all timed out timers
      timeNow = OS_ThreadTime();
      for(;;)
      {
         OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
         if(TimerHead == NULL)
         {
            OS_SemaphorePost(SemaphoreLock);
            break;
         }
         diff = timeNow - TimerHead->ticksTimeout;
         if(diff < 0)
         {
            OS_SemaphorePost(SemaphoreLock);
            break;
         }
         timer = TimerHead;
         OS_SemaphorePost(SemaphoreLock);
         if(timer->ticksRestart)
            OS_TimerStart(timer, timer->ticksRestart, timer->ticksRestart);
         else
            OS_TimerStop(timer);
 
         //Send message
         message[0] = MESSAGE_TYPE_TIMER;
         message[1] = (uint32)timer;
         message[2] = (uint32)timer->info;
         OS_MQueueSend(timer->mqueue, message);
      }
   }
}
 
 
/******************************************/
OS_Timer_t *OS_TimerCreate(const char *Name, OS_MQueue_t *MQueue, uint32 Info)
{
   OS_Timer_t *timer;
   int startThread=0;
 
   OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
   if(SemaphoreTimer == NULL)
   {
      SemaphoreTimer = OS_SemaphoreCreate("Timer", 0);
      startThread = 1;
   }
   OS_SemaphorePost(SemaphoreLock);
   if(startThread)
      OS_ThreadCreate("Timer", OS_TimerThread, NULL, 250, 2000);
 
   timer = (OS_Timer_t*)OS_HeapMalloc(NULL, sizeof(OS_Timer_t));
   if(timer == NULL)
      return NULL;
   timer->name = Name;
   timer->mqueue = MQueue;
   timer->next = NULL;
   timer->prev = NULL;
   timer->info = Info;
   timer->active = 0;
   return timer;
}
 
 
/******************************************/
void OS_TimerDelete(OS_Timer_t *Timer)
{
   OS_TimerStop(Timer);
   OS_HeapFree(Timer);
}
 
 
/******************************************/
//Must not be called from an ISR
void OS_TimerStart(OS_Timer_t *Timer, uint32 Ticks, uint32 TicksRestart)
{
   OS_Timer_t *node, *prev;
   int diff, check=0;
 
   assert(Timer);
   assert(InterruptInside == 0);
   Ticks += OS_ThreadTime();
   if(Timer->active)
      OS_TimerStop(Timer);
   OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
   Timer->ticksTimeout = Ticks;
   Timer->ticksRestart = TicksRestart;
   Timer->active = 1;
   prev = NULL;
   for(node = TimerHead; node; node = node->next)
   {
      diff = Ticks - node->ticksTimeout;
      if(diff <= 0)
         break;
      prev = node;
   }
   Timer->next = node;
   Timer->prev = prev;
   if(node)
      node->prev = Timer;
   if(prev == NULL)
   {
      TimerHead = Timer;
      check = 1;
   }
   else
      prev->next = Timer;
   OS_SemaphorePost(SemaphoreLock);
   if(check)
      OS_SemaphorePost(SemaphoreTimer);
}
 
 
/******************************************/
//Must not be called from an ISR
void OS_TimerStop(OS_Timer_t *Timer)
{
   assert(Timer);
   assert(InterruptInside == 0);
   OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
   if(Timer->active)
   {
      Timer->active = 0;
      if(Timer->prev == NULL)
         TimerHead = Timer->next;
      else
         Timer->prev->next = Timer->next;
      if(Timer->next)
         Timer->next->prev = Timer->prev;
   }
   OS_SemaphorePost(SemaphoreLock);
}
 
 
/***************** ISR ********************/
/******************************************/
void OS_InterruptServiceRoutine(uint32 Status)
{
   int i;
   uint32 state;
 
   //MemoryWrite(GPIO0_OUT, Status);  //Change LEDs
   InterruptInside = 1;
   i = 0;
   do
   {
      if(Status & 1)
      {
         if(Isr[i])
            Isr[i]((uint32*)i);
         else
            OS_InterruptMaskClear(1 << i);
      }
      Status >>= 1;
      ++i;
   } while(Status);
   InterruptInside = 0;
   //MemoryWrite(GPIO0_OUT, 0);
 
   state = OS_SpinLock();
   if(ThreadNeedReschedule)
      OS_ThreadReschedule(ThreadNeedReschedule & 1);
   OS_SpinUnlock(state);
}
 
 
/******************************************/
void OS_InterruptRegister(uint32 Mask, OS_FuncPtr_t FuncPtr)
{
   int i;
 
   for(i = 0; i < 32; ++i)
   {
      if(Mask & (1 << i))
         Isr[i] = FuncPtr;
   }
}
 
 
/******************************************/
//Plasma hardware dependent
uint32 OS_InterruptStatus(void)
{
   return MemoryRead(IRQ_STATUS);
}
 
 
/******************************************/
//Plasma hardware dependent
uint32 OS_InterruptMaskSet(uint32 Mask)
{
   uint32 mask, state;
   state = OS_CriticalBegin();
   mask = MemoryRead(IRQ_MASK) | Mask;
   MemoryWrite(IRQ_MASK, mask);
   OS_CriticalEnd(state);
   return mask;
}
 
 
/******************************************/
//Plasma hardware dependent
uint32 OS_InterruptMaskClear(uint32 Mask)
{
   uint32 mask, state;
   state = OS_CriticalBegin();
   mask = MemoryRead(IRQ_MASK) & ~Mask;
   MemoryWrite(IRQ_MASK, mask);
   OS_CriticalEnd(state);
   return mask;
}
 
 
/**************** Init ********************/
/******************************************/
static volatile uint32 IdleCount;
static void OS_IdleThread(void *Arg)
{
   (void)Arg;
 
   //Don't block in the idle thread!
   for(;;)
   {
      ++IdleCount;
   }
}
 
 
/******************************************/
#ifndef DISABLE_IRQ_SIM
static void OS_IdleSimulateIsr(void *Arg)
{
   uint32 count=0, value;
   (void)Arg;
 
   for(;;)
   {
      MemoryRead(IRQ_MASK + 4);       //calls Sleep(10)
#if WIN32
      while(OS_InterruptMaskSet(0) & IRQ_UART_WRITE_AVAILABLE)
         OS_InterruptServiceRoutine(IRQ_UART_WRITE_AVAILABLE);
#endif
      value = OS_InterruptMaskSet(0);
      OS_InterruptServiceRoutine(value);
      ++count;
   }
}
#endif //DISABLE_IRQ_SIM
 
 
/******************************************/
//Plasma hardware dependent
void OS_ThreadTick2(void *Arg)
{
   uint32 status, mask, state;
 
   state = OS_SpinLock();
   status = MemoryRead(IRQ_STATUS) & (IRQ_COUNTER18 | IRQ_COUNTER18_NOT);
   mask = MemoryRead(IRQ_MASK) | IRQ_COUNTER18 | IRQ_COUNTER18_NOT;
   mask &= ~status;
   MemoryWrite(IRQ_MASK, mask);
   OS_ThreadTick(Arg);
   OS_SpinUnlock(state);
}
 
 
/******************************************/
void OS_Init(uint32 *HeapStorage, uint32 Bytes)
{
   OS_AsmInterruptInit();               //Patch interrupt vector
   OS_InterruptMaskClear(0xffffffff);   //Disable interrupts
   HeapArray[0] = OS_HeapCreate("Default", HeapStorage, Bytes);
   SemaphoreSleep = OS_SemaphoreCreate("Sleep", 0);
   SemaphoreLock = OS_SemaphoreCreate("Lock", 1);
   OS_ThreadCreate("Idle", OS_IdleThread, NULL, 0, 256);
#ifndef DISABLE_IRQ_SIM
   if((OS_InterruptStatus() & (IRQ_COUNTER18 | IRQ_COUNTER18_NOT)) == 0)
   {
      //Detected that running in simulator so create SimIsr thread
      UartPrintfCritical("SimIsr\n");
      OS_ThreadCreate("SimIsr", OS_IdleSimulateIsr, NULL, 1, 0);
   }
#endif //DISABLE_IRQ_SIM
 
   //Plasma hardware dependent
   OS_InterruptRegister(IRQ_COUNTER18 | IRQ_COUNTER18_NOT, OS_ThreadTick2);
   OS_InterruptMaskSet(IRQ_COUNTER18 | IRQ_COUNTER18_NOT);
}
 
 
/******************************************/
void OS_Start(void)
{
   ThreadSwapEnabled = 1;
   (void)OS_SpinLock();
   OS_ThreadReschedule(1);
}
 
 
/******************************************/
//Place breakpoint here
void OS_Assert(void)
{
}
 
 
#if OS_CPU_COUNT > 1
static uint8 SpinLockArray[OS_CPU_COUNT];
/******************************************/
uint32 OS_CpuIndex(void)
{
   return 0; //0 to OS_CPU_COUNT-1
}
 
 
/******************************************/
//Symmetric Multiprocessing Spin Lock Mutex
uint32 OS_SpinLock(void)
{
   uint32 state, cpu, i, j, ok;
 
   cpu = OS_CpuIndex();
   state = OS_AsmInterruptEnable(0);
   do
   {
      ok = 1;
      if(++SpinLockArray[cpu] == 1)
      {
         for(i = 0; i < OS_CPU_COUNT; ++i)
         {
            if(i != cpu && SpinLockArray[i])
               ok = 0;
         }
         if(ok == 0)
         {
            SpinLockArray[cpu] = 0;
            for(j = 0; j < 100*cpu; ++j)  //wait a bit
               ++i;
         }
      }
   } while(ok == 0);
   return state;
}
 
 
/******************************************/
void OS_SpinUnlock(uint32 state)
{
   uint32 cpu;
   cpu = OS_CpuIndex();
   if(--SpinLockArray[cpu] == 0)
      OS_AsmInterruptEnable(state);
}
 
 
/******************************************/
//Must be called with interrupts disabled and spin locked
uint32 OS_SpinLockGet(void)
{
   uint32 cpu, count;
   cpu = OS_CpuIndex();
   count = SpinLockArray[cpu];
   return count;
}
 
 
/******************************************/
//Must be called with interrupts disabled and spin locked
void OS_SpinLockSet(uint32 count)
{
   uint32 cpu;
   cpu = OS_CpuIndex();
   SpinLockArray[cpu] = (uint8)count;
}
#endif
 
 
/************** WIN32 Support *************/
#ifdef WIN32
//Support RTOS inside Windows
extern int kbhit();
extern int getch(void);
extern int putch(int);
extern void __stdcall Sleep(unsigned long value);
 
static uint32 Memory[8];
 
uint32 MemoryRead(uint32 Address)
{
   Memory[2] |= IRQ_UART_WRITE_AVAILABLE;
   switch(Address)
   {
   case UART_READ:
      if(kbhit())
         Memory[0] = getch();
      Memory[2] &= ~IRQ_UART_READ_AVAILABLE; //clear bit
      return Memory[0];
   case IRQ_MASK:
      return Memory[1];
   case IRQ_MASK + 4:
      Sleep(10);
      return 0;
   case IRQ_STATUS:
      if(kbhit())
         Memory[2] |= IRQ_UART_READ_AVAILABLE;
      return Memory[2];
   }
   return 0;
}
 
void MemoryWrite(uint32 Address, uint32 Value)
{
   switch(Address)
   {
   case UART_WRITE:
      putch(Value);
      break;
   case IRQ_MASK:
      Memory[1] = Value;
      break;
   case IRQ_STATUS:
      Memory[2] = Value;
      break;
   }
}
 
uint32 OS_AsmInterruptEnable(uint32 EnableInterrupt)
{
   return EnableInterrupt;
}
 
void OS_AsmInterruptInit(void)
{
}
#endif  //WIN32
 
 
/**************** Example *****************/
#ifndef NO_MAIN
static uint8 HeapSpace[1024*512];
 
int main(void)
{
   UartPrintfCritical("Starting RTOS\n");
   OS_Init((uint32*)HeapSpace, sizeof(HeapSpace));
   UartInit();
   OS_ThreadCreate("Main", MainThread, 0, 100, 64000);
   OS_Start();
   return 0;
}
#endif
 

Line No.	Rev	Author	Line
1	138	rhoads	`/*--------------------------------------------------------------------`
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	167	rhoads	`* Partial support for multiple CPUs using symmetric multiprocessing.`
16	138	rhoads	`--------------------------------------------------------------------/`
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 HEAP_COUNT 8`
24
25	167	rhoads
26	138	rhoads	`/************* Structures *************/`
27			`#ifdef WIN32`
28			`#define setjmp _setjmp`
29			`//x86 registers`
30			`typedef struct jmp_buf2 {`
31			`uint32 Ebp, Ebx, Edi, Esi, sp, pc, extra[10];`
32			`} jmp_buf2;`
33			`#else`
34			`//Plasma registers`
35			`typedef struct jmp_buf2 {`
36			`uint32 s[9], gp, sp, pc;`
37			`} jmp_buf2;`
38			`#endif`
39
40			`typedef struct HeapNode_s {`
41			`struct HeapNode_s *next;`
42			`int size;`
43			`} HeapNode_t;`
44
45			`struct OS_Heap_s {`
46			`uint32 magic;`
47			`const char *name;`
48			`OS_Semaphore_t *semaphore;`
49			`HeapNode_t *available;`
50			`HeapNode_t base;`
51			`struct OS_Heap_s *alternate;`
52			`};`
53			`//typedef struct OS_Heap_s OS_Heap_t;`
54
55			`struct OS_Thread_s {`
56			`const char *name;`
57			`jmp_buf env;`
58			`OS_FuncPtr_t funcPtr;`
59			`void *arg;`
60			`uint32 priority;`
61			`uint32 ticksTimeout;`
62			`void *info;`
63			`OS_Semaphore_t *semaphorePending;`
64			`int returnCode;`
65	167	rhoads	`uint32 spinLocks;`
66	138	rhoads	`struct OS_Thread_s next, prev;`
67			`struct OS_Thread_s nextTimeout, prevTimeout;`
68			`uint32 magic[1];`
69			`};`
70			`//typedef struct OS_Thread_s OS_Thread_t;`
71
72			`struct OS_Semaphore_s {`
73			`const char *name;`
74			`struct OS_Thread_s *threadHead;`
75			`int count;`
76			`};`
77			`//typedef struct OS_Semaphore_s OS_Semaphore_t;`
78
79			`struct OS_Mutex_s {`
80			`OS_Semaphore_t *semaphore;`
81			`OS_Thread_t *thread;`
82			`int count;`
83			`};`
84			`//typedef struct OS_Mutex_s OS_Mutex_t;`
85
86			`struct OS_MQueue_s {`
87			`const char *name;`
88			`OS_Semaphore_t *semaphore;`
89			`int count, size, used, read, write;`
90			`};`
91			`//typedef struct OS_MQueue_s OS_MQueue_t;`
92
93			`struct OS_Timer_s {`
94			`const char *name;`
95			`struct OS_Timer_s next, prev;`
96			`uint32 ticksTimeout;`
97			`uint32 ticksRestart;`
98			`int active;`
99			`OS_MQueue_t *mqueue;`
100			`uint32 info;`
101			`};`
102			`//typedef struct OS_Timer_s OS_Timer_t;`
103
104
105			`/************* Globals ****************/`
106			`static OS_Heap_t *HeapArray[HEAP_COUNT];`
107			`static OS_Semaphore_t *SemaphoreSleep;`
108			`static OS_Semaphore_t *SemaphoreLock;`
109			`static int ThreadSwapEnabled;`
110	167	rhoads	`static int ThreadCurrentActive[OS_CPU_COUNT];`
111	138	rhoads	`static int ThreadNeedReschedule;`
112			`static uint32 ThreadTime;`
113			`static OS_Thread_t *ThreadHead; //Linked list of threads sorted by priority`
114			`static OS_Thread_t *TimeoutHead; //Linked list of threads sorted by timeout`
115	167	rhoads	`static OS_Thread_t *ThreadCurrent[OS_CPU_COUNT];`
116	138	rhoads	`static void *NeedToFree;`
117			`static OS_Semaphore_t SemaphoreReserved[SEM_RESERVED_COUNT];`
118			`static OS_Timer_t *TimerHead; //Linked list of timers sorted by timeout`
119			`static OS_Semaphore_t *SemaphoreTimer;`
120			`static OS_FuncPtr_t Isr[32];`
121			`static int InterruptInside;`
122
123
124			`/*************** Heap *****************/`
125			`/******************************************/`
126			`OS_Heap_t OS_HeapCreate(const char Name, void *Memory, uint32 Size)`
127			`{`
128			`OS_Heap_t *heap;`
129
130			`assert(((uint32)Memory & 3) == 0);`
131			`heap = (OS_Heap_t*)Memory;`
132			`heap->magic = HEAP_MAGIC;`
133			`heap->name = Name;`
134			`heap->semaphore = OS_SemaphoreCreate(Name, 1);`
135			`heap->available = (HeapNode_t*)(heap + 1);`
136			`heap->available->next = &heap->base;`
137			`heap->available->size = (Size - sizeof(OS_Heap_t)) / sizeof(HeapNode_t);`
138			`heap->base.next = heap->available;`
139			`heap->base.size = 0;`
140			`return heap;`
141			`}`
142
143
144			`/******************************************/`
145			`void OS_HeapDestroy(OS_Heap_t *Heap)`
146			`{`
147			`OS_SemaphoreDelete(Heap->semaphore);`
148			`}`
149
150
151			`/******************************************/`
152			`//Modified from K&R`
153			`void OS_HeapMalloc(OS_Heap_t Heap, int Bytes)`
154			`{`
155			`HeapNode_t node, prevp;`
156			`int nunits;`
157
158			`if((int)Heap < HEAP_COUNT)`
159			`Heap = HeapArray[(int)Heap];`
160			`//printf("OS_HeapMalloc(%s, %d)\n", Heap->name, Bytes);`
161			`nunits = (Bytes + sizeof(HeapNode_t) - 1) / sizeof(HeapNode_t) + 1;`
162			`OS_SemaphorePend(Heap->semaphore, OS_WAIT_FOREVER);`
163			`prevp = Heap->available;`
164			`for(node = prevp->next; ; prevp = node, node = node->next)`
165			`{`
166			`if(node->size >= nunits) //Big enough?`
167			`{`
168			`if(node->size == nunits) //Exactly`
169			`prevp->next = node->next;`
170			`else`
171			`{ //Allocate tail end`
172			`node->size -= nunits;`
173			`node += node->size;`
174			`node->size = nunits;`
175			`}`
176			`Heap->available = prevp;`
177			`node->next = (HeapNode_t*)Heap;`
178			`OS_SemaphorePost(Heap->semaphore);`
179			`//printf("ptr=0x%x\n", (uint32)(node + 1));`
180			`return (void*)(node + 1);`
181			`}`
182			`if(node == Heap->available) //Wrapped around free list`
183			`{`
184			`OS_SemaphorePost(Heap->semaphore);`
185			`if(Heap->alternate)`
186			`return OS_HeapMalloc(Heap->alternate, Bytes);`
187			`return NULL;`
188			`}`
189			`}`
190			`}`
191
192
193			`/******************************************/`
194			`//Modified from K&R`
195			`void OS_HeapFree(void *Block)`
196			`{`
197			`OS_Heap_t *heap;`
198			`HeapNode_t bp, node;`
199
200			`assert(Block);`
201			`//printf("OS_HeapFree(0x%x)\n", Block);`
202			`bp = (HeapNode_t*)Block - 1; //point to block header`
203			`heap = (OS_Heap_t*)bp->next;`
204			`assert(heap->magic == HEAP_MAGIC);`
205			`if(heap->magic != HEAP_MAGIC)`
206			`return;`
207			`OS_SemaphorePend(heap->semaphore, OS_WAIT_FOREVER);`
208			`for(node = heap->available; !(node < bp && bp < node->next); node = node->next)`
209			`{`
210			`if(node >= node->next && (bp > node \|\| bp < node->next))`
211			`break; //freed block at start or end of area`
212			`}`
213
214			`if(bp + bp->size == node->next) //join to upper`
215			`{`
216			`bp->size += node->next->size;`
217			`bp->next = node->next->next;`
218			`}`
219			`else`
220			`{`
221			`bp->next = node->next;`
222			`}`
223
224			`if(node + node->size == bp) //join to lower`
225			`{`
226			`node->size += bp->size;`
227			`node->next = bp->next;`
228			`}`
229			`else`
230			`node->next = bp;`
231			`heap->available = node;`
232			`OS_SemaphorePost(heap->semaphore);`
233			`}`
234
235
236			`/******************************************/`
237			`void OS_HeapAlternate(OS_Heap_t Heap, OS_Heap_t Alternate)`
238			`{`
239			`Heap->alternate = Alternate;`
240			`}`
241
242
243			`/******************************************/`
244			`void OS_HeapRegister(void Index, OS_Heap_t Heap)`
245			`{`
246			`if((int)Index < HEAP_COUNT)`
247			`HeapArray[(int)Index] = Heap;`
248			`}`
249
250
251
252			`/*************** Thread ***************/`
253			`/******************************************/`
254			`//Linked list of threads sorted by priority`
255			`//Must be called with interrupts disabled`
256			`static void OS_ThreadPriorityInsert(OS_Thread_t *head, OS_Thread_t thread)`
257			`{`
258			`OS_Thread_t node, prev;`
259
260			`prev = NULL;`
261			`for(node = *head; node; node = node->next)`
262			`{`
263			`if(node->priority <= thread->priority)`
264			`break;`
265			`prev = node;`
266			`}`
267
268			`if(prev == NULL)`
269			`{`
270			`thread->next = *head;`
271			`thread->prev = NULL;`
272			`*head = thread;`
273			`}`
274			`else`
275			`{`
276			`if(prev->next)`
277			`prev->next->prev = thread;`
278			`thread->next = prev->next;`
279			`thread->prev = prev;`
280			`prev->next = thread;`
281			`}`
282			`assert(ThreadHead);`
283			`}`
284
285
286			`/******************************************/`
287			`//Must be called with interrupts disabled`
288			`static void OS_ThreadPriorityRemove(OS_Thread_t *head, OS_Thread_t thread)`
289			`{`
290	167	rhoads	`uint32 cpuIndex = OS_CpuIndex();`
291	138	rhoads	`//printf("r(%d) ", thread->priority);`
292			`assert(thread->magic[0] == THREAD_MAGIC); //check stack overflow`
293			`if(thread->prev == NULL)`
294			`*head = thread->next;`
295			`else`
296			`thread->prev->next = thread->next;`
297			`if(thread->next)`
298			`thread->next->prev = thread->prev;`
299			`thread->next = NULL;`
300			`thread->prev = NULL;`
301	167	rhoads	`if(head == &ThreadHead && ThreadCurrent[cpuIndex] == thread)`
302			`ThreadCurrentActive[cpuIndex] = 0;`
303	138	rhoads	`}`
304
305
306			`/******************************************/`
307			`//Linked list of threads sorted by timeout value`
308			`//Must be called with interrupts disabled`
309			`static void OS_ThreadTimeoutInsert(OS_Thread_t *thread)`
310			`{`
311			`OS_Thread_t node, prev;`
312			`int diff;`
313
314			`prev = NULL;`
315			`for(node = TimeoutHead; node; node = node->nextTimeout)`
316			`{`
317			`diff = thread->ticksTimeout - node->ticksTimeout;`
318			`if(diff <= 0)`
319			`break;`
320			`prev = node;`
321			`}`
322
323			`if(prev == NULL)`
324			`{`
325			`thread->nextTimeout = TimeoutHead;`
326			`thread->prevTimeout = NULL;`
327	151	rhoads	`if(TimeoutHead)`
328			`TimeoutHead->prevTimeout = thread;`
329	138	rhoads	`TimeoutHead = thread;`
330			`}`
331			`else`
332			`{`
333			`if(prev->nextTimeout)`
334			`prev->nextTimeout->prevTimeout = thread;`
335			`thread->nextTimeout = prev->nextTimeout;`
336			`thread->prevTimeout = prev;`
337			`prev->nextTimeout = thread;`
338			`}`
339			`}`
340
341
342			`/******************************************/`
343			`//Must be called with interrupts disabled`
344			`static void OS_ThreadTimeoutRemove(OS_Thread_t *thread)`
345			`{`
346			`if(thread->prevTimeout == NULL && TimeoutHead != thread)`
347			`return; //not in list`
348			`if(thread->prevTimeout == NULL)`
349			`TimeoutHead = thread->nextTimeout;`
350			`else`
351			`thread->prevTimeout->nextTimeout = thread->nextTimeout;`
352			`if(thread->nextTimeout)`
353			`thread->nextTimeout->prevTimeout = thread->prevTimeout;`
354			`thread->nextTimeout = NULL;`
355			`thread->prevTimeout = NULL;`
356			`}`
357
358
359			`/******************************************/`
360	151	rhoads	`//Loads a new thread`
361	138	rhoads	`//Must be called with interrupts disabled`
362			`static void OS_ThreadReschedule(int RoundRobin)`
363			`{`
364	167	rhoads	`OS_Thread_t threadNext, threadPrev, *threadCurrent;`
365			`uint32 cpuIndex = OS_CpuIndex();`
366	138	rhoads	`int rc;`
367
368			`if(ThreadSwapEnabled == 0 \|\| InterruptInside)`
369			`{`
370			`ThreadNeedReschedule \|= 2 + RoundRobin;`
371			`return;`
372			`}`
373
374			`//Determine which thread should run`
375	167	rhoads	`threadCurrent = ThreadCurrent[cpuIndex];`
376			`threadNext = threadCurrent;`
377	138	rhoads	`assert(ThreadHead);`
378	167	rhoads	`if(ThreadCurrentActive[cpuIndex] == 0 \|\| threadCurrent == NULL)`
379	138	rhoads	`threadNext = ThreadHead;`
380	167	rhoads	`else if(threadCurrent->priority < ThreadHead->priority)`
381	138	rhoads	`threadNext = ThreadHead;`
382			`else if(RoundRobin)`
383			`{`
384	167	rhoads	`if(threadCurrent->next &&`
385			`threadCurrent->next->priority == ThreadHead->priority)`
386			`threadNext = threadCurrent->next;`
387	138	rhoads	`else`
388			`threadNext = ThreadHead;`
389			`}`
390
391	167	rhoads	`if(threadNext != threadCurrent)`
392	138	rhoads	`{`
393			`//Swap threads`
394	167	rhoads	`threadPrev = threadCurrent;`
395			`ThreadCurrent[cpuIndex] = threadNext;`
396			`assert(threadNext);`
397	138	rhoads	`if(threadPrev)`
398			`{`
399			`assert(threadPrev->magic[0] == THREAD_MAGIC); //check stack overflow`
400	167	rhoads	`threadPrev->spinLocks = OS_SpinLockGet();`
401	138	rhoads	`//printf("OS_ThreadRescheduleSave(%s)\n", threadPrev->name);`
402	167	rhoads	`rc = setjmp(threadPrev->env); //ANSI C call to save registers`
403	138	rhoads	`if(rc)`
404			`{`
405			`//Returned from longjmp()`
406			`return;`
407			`}`
408			`}`
409	167	rhoads	`cpuIndex = OS_CpuIndex(); //removed warning`
410			`threadNext = ThreadCurrent[cpuIndex]; //removed warning`
411			`ThreadCurrentActive[cpuIndex] = 1;`
412			`//printf("OS_ThreadRescheduleRestore(%s)\n", threadNext->name);`
413			`OS_SpinLockSet(threadNext->spinLocks);`
414			`longjmp(threadNext->env, 1); //ANSI C call to restore registers`
415	138	rhoads	`}`
416			`}`
417
418
419			`/******************************************/`
420			`static void OS_ThreadInit(void *Arg)`
421			`{`
422	167	rhoads	`uint32 cpuIndex = OS_CpuIndex();`
423	138	rhoads	`(void)Arg;`
424
425			`OS_AsmInterruptEnable(1);`
426	167	rhoads	`ThreadCurrent[cpuIndex]->funcPtr(ThreadCurrent[cpuIndex]->arg);`
427	138	rhoads	`OS_ThreadExit();`
428			`}`
429
430
431			`/******************************************/`
432			//Stops warning "argument X might be clobbered by `longjmp'"
433			`static void OS_ThreadRegsInit(jmp_buf env)`
434			`{`
435			`setjmp(env); //ANSI C call to save registers`
436			`}`
437
438
439			`/******************************************/`
440			`OS_Thread_t OS_ThreadCreate(const char Name,`
441			`OS_FuncPtr_t FuncPtr,`
442			`void *Arg,`
443			`uint32 Priority,`
444			`uint32 StackSize)`
445			`{`
446			`OS_Thread_t *thread;`
447			`uint8 *stack;`
448			`jmp_buf2 *env;`
449			`uint32 state;`
450
451			`OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);`
452			`if(NeedToFree)`
453			`OS_HeapFree(NeedToFree);`
454			`NeedToFree = NULL;`
455			`OS_SemaphorePost(SemaphoreLock);`
456
457			`if(StackSize == 0)`
458			`StackSize = STACK_SIZE_DEFAULT;`
459			`if(StackSize < STACK_SIZE_MINIMUM)`
460			`StackSize = STACK_SIZE_MINIMUM;`
461			`thread = (OS_Thread_t*)OS_HeapMalloc(NULL, sizeof(OS_Thread_t) + StackSize);`
462			`assert(thread);`
463			`if(thread == NULL)`
464			`return NULL;`
465			`stack = (uint8*)(thread + 1);`
466			`memset(stack, 0xcd, StackSize);`
467
468			`thread->name = Name;`
469			`thread->funcPtr = FuncPtr;`
470			`thread->arg = Arg;`
471			`thread->priority = Priority;`
472			`thread->info = NULL;`
473			`thread->semaphorePending = NULL;`
474			`thread->returnCode = 0;`
475	167	rhoads	`thread->spinLocks = 1;`
476	138	rhoads	`thread->next = NULL;`
477			`thread->prev = NULL;`
478			`thread->nextTimeout = NULL;`
479			`thread->prevTimeout = NULL;`
480			`thread->magic[0] = THREAD_MAGIC;`
481
482			`OS_ThreadRegsInit(thread->env);`
483			`env = (jmp_buf2*)thread->env;`
484			`env->sp = (uint32)stack + StackSize - 4;`
485			`env->pc = (uint32)OS_ThreadInit;`
486
487			`state = OS_CriticalBegin();`
488			`OS_ThreadPriorityInsert(&ThreadHead, thread);`
489			`OS_ThreadReschedule(0);`
490			`OS_CriticalEnd(state);`
491			`return thread;`
492			`}`
493
494
495			`/******************************************/`
496			`void OS_ThreadExit(void)`
497			`{`
498	167	rhoads	`uint32 state, cpuIndex = OS_CpuIndex();`
499	138	rhoads	`OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);`
500			`if(NeedToFree)`

Browse

Tools

Subversion Repositories plasma

[/] [plasma/] [trunk/] [kernel/] [rtos.c] - Blame information for rev 167