rtos.c

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

/*--------------------------------------------------------------------
 * TITLE: Plasma Real Time Operating System
 * AUTHOR: Steve Rhoads ([e-mail])
 * 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;
#elif defined(ARM_CPU)
   //ARM registers
   typedef struct jmp_buf2 {  
      uint32 r[13], sp, lr, pc, cpsr, extra[5];
   } 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;

typedef enum {
   THREAD_PEND    = 0,
   THREAD_READY   = 1,
   THREAD_RUNNING = 2
} OS_ThreadState_e;

struct OS_Thread_s {
   const char *name;
   OS_ThreadState_e state;
   int cpuIndex;
   int cpuLock;
   jmp_buf env;
   OS_FuncPtr_t funcPtr;
   void *arg;
   uint32 priority;
   uint32 ticksTimeout;
   void *info;
   OS_Semaphore_t *semaphorePending;
   int returnCode;
   uint32 spinLocks;
   uint32 processId;
   OS_Heap_t *heap;
   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_Thread_t *ThreadCurrent[OS_CPU_COUNT];
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 int ThreadSwapEnabled;
static int ThreadNeedReschedule;
static uint32 ThreadTime;
static void *NeedToFree;
static OS_Semaphore_t SemaphoreReserved[SEM_RESERVED_COUNT];
static OS_Semaphore_t *SemaphoreSleep;
static OS_Semaphore_t *SemaphoreLock;
static OS_Semaphore_t *SemaphoreTimer;
static OS_Timer_t *TimerHead;     //Linked list of timers sorted by timeout
static OS_FuncPtr_t Isr[32];
static int InterruptInside;
int InitStack[128];               //Used by boot.asm


/***************** 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(heap == NULL && OS_ThreadSelf())
      heap = OS_ThreadSelf()->heap;
   if((uint32)heap < HEAP_COUNT)
      heap = HeapArray[(int)heap];
   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);
         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);
   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((uint32)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);
   if(*head == ThreadHead)
      thread->state = THREAD_READY;
}


/******************************************/
//Must be called with interrupts disabled
static void OS_ThreadPriorityRemove(OS_Thread_t **head, OS_Thread_t *thread)
{
   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;
   thread->state = THREAD_PEND;
}


/******************************************/
//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;
}


#if OS_CPU_COUNT <= 1
/******************************************/
//Loads a new thread 
//Must be called with interrupts disabled
static void OS_ThreadReschedule(int roundRobin)
{
   OS_Thread_t *threadNext, *threadCurrent, *threadTry;
   int rc;

   if(ThreadSwapEnabled == 0 || InterruptInside)
   {
      ThreadNeedReschedule |= 2 + roundRobin;  //Reschedule later
      return;
   }

   //Determine which thread should run
   threadCurrent = ThreadCurrent[0];
   threadNext = threadCurrent;
   if(threadCurrent == NULL || threadCurrent->state == THREAD_PEND)
      threadNext = ThreadHead;
   else if(threadCurrent->priority < ThreadHead->priority)
      threadNext = ThreadHead;
   else if(roundRobin)
   {
      //Determine next ready thread with same priority
      threadTry = threadCurrent->next;
      if(threadTry && threadTry->priority == threadCurrent->priority)
         threadNext = threadTry;
      else
         threadNext = ThreadHead;
   }

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

      threadNext = ThreadCurrent[0];       //removed warning
      longjmp(threadNext->env, 1);         //ANSI C call to restore registers
   }
}

#else //OS_CPU_COUNT > 1

/******************************************/
//Check if a different CPU needs to swap threads
static void OS_ThreadRescheduleCheck(void)
{
   OS_Thread_t *threadBest;
   uint32 i, priorityLow, cpuIndex = OS_CpuIndex();
   int cpuLow;

   //Find the CPU running the lowest priority thread
   cpuLow = 0;
   priorityLow = 0xffffffff;
   for(i = 0; i < OS_CPU_COUNT; ++i)
   {
      if(i != cpuIndex && (ThreadCurrent[i] == NULL || 
         ThreadCurrent[i]->priority < priorityLow))
      {
         cpuLow = i;
         if(ThreadCurrent[i])
            priorityLow = ThreadCurrent[i]->priority;
         else
            priorityLow = 0;
      }
   }

   //Determine highest priority ready thread for other CPUs
   for(threadBest = ThreadHead; threadBest && threadBest->priority > priorityLow; 
      threadBest = threadBest->next)
   {
      if(threadBest->state == THREAD_READY)
      { 
         if(threadBest->cpuLock == -1)
         {
            OS_CpuInterrupt(cpuLow, 1);  //Reschedule on the other CPU
            break;
         }
         else if(threadBest->priority > ThreadCurrent[threadBest->cpuLock]->priority)
         {
            OS_CpuInterrupt(threadBest->cpuLock, 1);  //Reschedule on the other CPU
            break;
         }
      }
   }
}


/******************************************/
//Loads a new thread in a multiprocessor environment
//Must be called with interrupts disabled
static void OS_ThreadReschedule(int roundRobin)
{
   OS_Thread_t *threadNext, *threadCurrent, *threadBest, *threadAlt;
   uint32 cpuIndex = OS_CpuIndex();
   int rc;

   if(ThreadSwapEnabled == 0 || InterruptInside)
   {
      ThreadNeedReschedule |= 2 + roundRobin;  //Reschedule later
      return;
   }

   //Determine highest priority ready thread
   for(threadBest = ThreadHead; threadBest; threadBest = threadBest->next)
   {
      if(threadBest->state == THREAD_READY && 
         (threadBest->cpuLock == -1 || threadBest->cpuLock == (int)cpuIndex))
         break;
   }

   //Determine which thread should run
   threadCurrent = ThreadCurrent[cpuIndex];
   threadNext = threadCurrent;
   if(threadCurrent == NULL || threadCurrent->state == THREAD_PEND)
   {
      threadNext = threadBest;
   }
   else if(threadBest && threadCurrent->priority < threadBest->priority)