rtos.c

Plasma IP Core 你可以利用这个组件在FPGA中设计MIPS结构的CPU

   {
      threadNext = threadBest;
   }
   else if(roundRobin)
   {
      //Find the next ready thread
      for(threadAlt = threadCurrent->next; threadAlt; threadAlt = threadAlt->next)
      {
         if(threadAlt->state == THREAD_READY &&
            (threadAlt->cpuLock == -1 || threadAlt->cpuLock == (int)cpuIndex))
            break;
      }
      if(threadAlt && threadAlt->priority == threadCurrent->priority)
         threadNext = threadAlt;
      else if(threadBest && threadBest->priority >= threadCurrent->priority)
         threadNext = threadBest;
   }

   if(threadNext != threadCurrent)
   {
      //Swap threads
      ThreadCurrent[cpuIndex] = threadNext;
      assert(threadNext);
      if(threadCurrent)
      {
         assert(threadCurrent->magic[0] == THREAD_MAGIC); //check stack overflow
         threadCurrent->state = THREAD_READY;
         threadCurrent->spinLocks = OS_SpinCountGet();
         threadCurrent->cpuIndex = -1;
         rc = setjmp(threadCurrent->env);   //ANSI C call to save registers
         if(rc)
         {
            //Returned from longjmp()
            return;
         }
      }

      //Restore spin lock count
      cpuIndex = OS_CpuIndex();             //removed warning
      threadNext = ThreadCurrent[cpuIndex]; //removed warning
      threadNext->state = THREAD_RUNNING;
      OS_SpinCountSet(threadNext->spinLocks);
      threadNext->cpuIndex = (int)cpuIndex;
      OS_ThreadRescheduleCheck();
      longjmp(threadNext->env, 1);          //ANSI C call to restore registers
   }
   OS_ThreadRescheduleCheck();
}


/******************************************/
void OS_ThreadCpuLock(OS_Thread_t *Thread, int CpuIndex)
{
   Thread->cpuLock = CpuIndex;
   if(Thread == OS_ThreadSelf() && CpuIndex != (int)OS_CpuIndex())
      OS_ThreadSleep(1);
}

#endif //#if OS_CPU_COUNT <= 1


/******************************************/
static void OS_ThreadInit(void *arg)
{
   uint32 cpuIndex = OS_CpuIndex();
   (void)arg;

   OS_CriticalEnd(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;
   memset(thread, 0, sizeof(OS_Thread_t));
   stack = (uint8*)(thread + 1);
   memset(stack, 0xcd, stackSize);

   thread->name = name;
   thread->state = THREAD_READY;
   thread->cpuLock = -1;
   thread->funcPtr = funcPtr;
   thread->arg = arg;
   thread->priority = priority;
   thread->info = NULL;
   thread->semaphorePending = NULL;
   thread->returnCode = 0;
   thread->spinLocks = 1;
   if(OS_ThreadSelf())
   {
      thread->processId = OS_ThreadSelf()->processId;
      thread->heap = OS_ThreadSelf()->heap;
   }
   else
   {
      thread->processId = 0;
      thread->heap = NULL;
   }
   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 - 24; //minimum stack frame size
   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);
   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();
   thread->priority = priority;
   if(thread->state != THREAD_PEND)
   {
      OS_ThreadPriorityRemove(&ThreadHead, thread);
      OS_ThreadPriorityInsert(&ThreadHead, thread);
      OS_ThreadReschedule(0);
   }
   OS_CriticalEnd(state);
}


/******************************************/
void OS_ThreadProcessId(OS_Thread_t *thread, uint32 processId, OS_Heap_t *heap)
{
   thread->processId = processId;
   thread->heap = heap;
}


/******************************************/
//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 = 0;

   if(semCount < SEM_RESERVED_COUNT)
      semaphore = &SemaphoreReserved[semCount++];  //Heap not ready yet
   else
      semaphore = (OS_Semaphore_t*)OS_HeapMalloc(HEAP_SYSTEM, 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)
   {
      if(ticks == 0)
      {
         ++semaphore->count;
         OS_CriticalEnd(state);
         return -1;
      }
      cpuIndex = OS_CpuIndex();
      thread = ThreadCurrent[cpuIndex];
      assert(thread);
      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);
      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(HEAP_SYSTEM, 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(HEAP_SYSTEM, 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;