/*--------------------------------------------------------------------
|
/*--------------------------------------------------------------------
|
* TITLE: Plasma Real Time Operating System
|
* TITLE: Plasma Real Time Operating System
|
* AUTHOR: Steve Rhoads (rhoadss@yahoo.com)
|
* AUTHOR: Steve Rhoads (rhoadss@yahoo.com)
|
* DATE CREATED: 12/17/05
|
* DATE CREATED: 12/17/05
|
* FILENAME: rtos.c
|
* FILENAME: rtos.c
|
* PROJECT: Plasma CPU core
|
* PROJECT: Plasma CPU core
|
* COPYRIGHT: Software placed into the public domain by the author.
|
* COPYRIGHT: Software placed into the public domain by the author.
|
* Software 'as is' without warranty. Author liable for nothing.
|
* Software 'as is' without warranty. Author liable for nothing.
|
* DESCRIPTION:
|
* DESCRIPTION:
|
* Plasma Real Time Operating System
|
* Plasma Real Time Operating System
|
* Fully pre-emptive RTOS with support for:
|
* Fully pre-emptive RTOS with support for:
|
* Heaps, Threads, Semaphores, Mutexes, Message Queues, and Timers.
|
* Heaps, Threads, Semaphores, Mutexes, Message Queues, and Timers.
|
* This file tries to be hardware independent except for calls to:
|
* This file tries to be hardware independent except for calls to:
|
* MemoryRead() and MemoryWrite() for interrupts.
|
* MemoryRead() and MemoryWrite() for interrupts.
|
* Partial support for multiple CPUs using symmetric multiprocessing.
|
* Partial support for multiple CPUs using symmetric multiprocessing.
|
*--------------------------------------------------------------------*/
|
*--------------------------------------------------------------------*/
|
#include "plasma.h"
|
#include "plasma.h"
|
#include "rtos.h"
|
#include "rtos.h"
|
|
|
#define HEAP_MAGIC 0x1234abcd
|
#define HEAP_MAGIC 0x1234abcd
|
#define THREAD_MAGIC 0x4321abcd
|
#define THREAD_MAGIC 0x4321abcd
|
#define SEM_RESERVED_COUNT 2
|
#define SEM_RESERVED_COUNT 2
|
#define INFO_COUNT 4
|
#define INFO_COUNT 4
|
#define HEAP_COUNT 8
|
#define HEAP_COUNT 8
|
|
|
|
|
/*************** Structures ***************/
|
/*************** Structures ***************/
|
#ifdef WIN32
|
#ifdef WIN32
|
#define setjmp _setjmp
|
#define setjmp _setjmp
|
//x86 registers
|
//x86 registers
|
typedef struct jmp_buf2 {
|
typedef struct jmp_buf2 {
|
uint32 Ebp, Ebx, Edi, Esi, sp, pc, extra[10];
|
uint32 Ebp, Ebx, Edi, Esi, sp, pc, extra[10];
|
} jmp_buf2;
|
} jmp_buf2;
|
#elif defined(ARM_CPU)
|
#elif defined(ARM_CPU)
|
//ARM registers
|
//ARM registers
|
typedef struct jmp_buf2 {
|
typedef struct jmp_buf2 {
|
uint32 r[13], sp, lr, pc, cpsr, extra[5];
|
uint32 r[13], sp, lr, pc, cpsr, extra[5];
|
} jmp_buf2;
|
} jmp_buf2;
|
#else
|
#else
|
//Plasma registers
|
//Plasma registers
|
typedef struct jmp_buf2 {
|
typedef struct jmp_buf2 {
|
uint32 s[9], gp, sp, pc;
|
uint32 s[9], gp, sp, pc;
|
} jmp_buf2;
|
} jmp_buf2;
|
#endif
|
#endif
|
|
|
typedef struct HeapNode_s {
|
typedef struct HeapNode_s {
|
struct HeapNode_s *next;
|
struct HeapNode_s *next;
|
int size;
|
int size;
|
} HeapNode_t;
|
} HeapNode_t;
|
|
|
struct OS_Heap_s {
|
struct OS_Heap_s {
|
uint32 magic;
|
uint32 magic;
|
const char *name;
|
const char *name;
|
OS_Semaphore_t *semaphore;
|
OS_Semaphore_t *semaphore;
|
HeapNode_t *available;
|
HeapNode_t *available;
|
HeapNode_t base;
|
HeapNode_t base;
|
struct OS_Heap_s *alternate;
|
struct OS_Heap_s *alternate;
|
};
|
};
|
//typedef struct OS_Heap_s OS_Heap_t;
|
//typedef struct OS_Heap_s OS_Heap_t;
|
|
|
typedef enum {
|
typedef enum {
|
THREAD_PEND = 0, //Thread in semaphore's linked list
|
THREAD_PEND = 0, //Thread in semaphore's linked list
|
THREAD_READY = 1, //Thread in ThreadHead linked list
|
THREAD_READY = 1, //Thread in ThreadHead linked list
|
THREAD_RUNNING = 2 //Thread == ThreadCurrent[cpu]
|
THREAD_RUNNING = 2 //Thread == ThreadCurrent[cpu]
|
} OS_ThreadState_e;
|
} OS_ThreadState_e;
|
|
|
struct OS_Thread_s {
|
struct OS_Thread_s {
|
const char *name; //Name of thread
|
const char *name; //Name of thread
|
OS_ThreadState_e state; //Pending, ready, or running
|
OS_ThreadState_e state; //Pending, ready, or running
|
int cpuIndex; //Which CPU is running the thread
|
int cpuIndex; //Which CPU is running the thread
|
int cpuLock; //Lock the thread to a specific CPU
|
int cpuLock; //Lock the thread to a specific CPU
|
jmp_buf env; //Registers saved during context swap
|
jmp_buf env; //Registers saved during context swap
|
OS_FuncPtr_t funcPtr; //First function called
|
OS_FuncPtr_t funcPtr; //First function called
|
void *arg; //Argument to first function called
|
void *arg; //Argument to first function called
|
uint32 priority; //Priority of thread (0=low, 255=high)
|
uint32 priority; //Priority of thread (0=low, 255=high)
|
uint32 ticksTimeout; //Tick value when semaphore pend times out
|
uint32 ticksTimeout; //Tick value when semaphore pend times out
|
void *info[INFO_COUNT]; //User storage
|
void *info[INFO_COUNT]; //User storage
|
OS_Semaphore_t *semaphorePending; //Semaphore thread is blocked on
|
OS_Semaphore_t *semaphorePending; //Semaphore thread is blocked on
|
int returnCode; //Return value from semaphore pend
|
int returnCode; //Return value from semaphore pend
|
uint32 processId; //Process ID if using MMU
|
uint32 processId; //Process ID if using MMU
|
OS_Heap_t *heap; //Heap used if no heap specified
|
OS_Heap_t *heap; //Heap used if no heap specified
|
struct OS_Thread_s *next; //Linked list of threads by priority
|
struct OS_Thread_s *next; //Linked list of threads by priority
|
struct OS_Thread_s *prev;
|
struct OS_Thread_s *prev;
|
struct OS_Thread_s *nextTimeout; //Linked list of threads by timeout
|
struct OS_Thread_s *nextTimeout; //Linked list of threads by timeout
|
struct OS_Thread_s *prevTimeout;
|
struct OS_Thread_s *prevTimeout;
|
uint32 magic[1]; //Bottom of stack to detect stack overflow
|
uint32 magic[1]; //Bottom of stack to detect stack overflow
|
};
|
};
|
//typedef struct OS_Thread_s OS_Thread_t;
|
//typedef struct OS_Thread_s OS_Thread_t;
|
|
|
struct OS_Semaphore_s {
|
struct OS_Semaphore_s {
|
const char *name;
|
const char *name;
|
struct OS_Thread_s *threadHead; //threads pending on semaphore
|
struct OS_Thread_s *threadHead; //threads pending on semaphore
|
int count;
|
int count;
|
};
|
};
|
//typedef struct OS_Semaphore_s OS_Semaphore_t;
|
//typedef struct OS_Semaphore_s OS_Semaphore_t;
|
|
|
struct OS_Mutex_s {
|
struct OS_Mutex_s {
|
OS_Semaphore_t *semaphore;
|
OS_Semaphore_t *semaphore;
|
OS_Thread_t *thread;
|
OS_Thread_t *thread;
|
int count;
|
int count;
|
};
|
};
|
//typedef struct OS_Mutex_s OS_Mutex_t;
|
//typedef struct OS_Mutex_s OS_Mutex_t;
|
|
|
struct OS_MQueue_s {
|
struct OS_MQueue_s {
|
const char *name;
|
const char *name;
|
OS_Semaphore_t *semaphore;
|
OS_Semaphore_t *semaphore;
|
int count, size, used, read, write;
|
int count, size, used, read, write;
|
};
|
};
|
//typedef struct OS_MQueue_s OS_MQueue_t;
|
//typedef struct OS_MQueue_s OS_MQueue_t;
|
|
|
struct OS_Timer_s {
|
struct OS_Timer_s {
|
const char *name;
|
const char *name;
|
struct OS_Timer_s *next, *prev;
|
struct OS_Timer_s *next, *prev;
|
uint32 ticksTimeout;
|
uint32 ticksTimeout;
|
uint32 ticksRestart;
|
uint32 ticksRestart;
|
int active;
|
int active;
|
OS_TimerFuncPtr_t callback;
|
OS_TimerFuncPtr_t callback;
|
OS_MQueue_t *mqueue;
|
OS_MQueue_t *mqueue;
|
uint32 info;
|
uint32 info;
|
};
|
};
|
//typedef struct OS_Timer_s OS_Timer_t;
|
//typedef struct OS_Timer_s OS_Timer_t;
|
|
|
|
|
/*************** Globals ******************/
|
/*************** Globals ******************/
|
static OS_Heap_t *HeapArray[HEAP_COUNT];
|
static OS_Heap_t *HeapArray[HEAP_COUNT];
|
static int InterruptInside[OS_CPU_COUNT];
|
static int InterruptInside[OS_CPU_COUNT];
|
static int ThreadNeedReschedule[OS_CPU_COUNT];
|
static int ThreadNeedReschedule[OS_CPU_COUNT];
|
static OS_Thread_t *ThreadCurrent[OS_CPU_COUNT]; //Currently running thread(s)
|
static OS_Thread_t *ThreadCurrent[OS_CPU_COUNT]; //Currently running thread(s)
|
static OS_Thread_t *ThreadHead; //Linked list of threads sorted by priority
|
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 *TimeoutHead; //Linked list of threads sorted by timeout
|
static int ThreadSwapEnabled;
|
static int ThreadSwapEnabled;
|
static uint32 ThreadTime;
|
static uint32 ThreadTime;
|
static void *NeedToFree;
|
static void *NeedToFree;
|
static OS_Semaphore_t SemaphoreReserved[SEM_RESERVED_COUNT];
|
static OS_Semaphore_t SemaphoreReserved[SEM_RESERVED_COUNT];
|
static OS_Semaphore_t *SemaphoreSleep;
|
static OS_Semaphore_t *SemaphoreSleep;
|
static OS_Semaphore_t *SemaphoreRelease;
|
static OS_Semaphore_t *SemaphoreRelease;
|
static OS_Semaphore_t *SemaphoreLock;
|
static OS_Semaphore_t *SemaphoreLock;
|
static OS_Semaphore_t *SemaphoreTimer;
|
static OS_Semaphore_t *SemaphoreTimer;
|
static OS_Timer_t *TimerHead; //Linked list of timers sorted by timeout
|
static OS_Timer_t *TimerHead; //Linked list of timers sorted by timeout
|
static OS_FuncPtr_t Isr[32];
|
static OS_FuncPtr_t Isr[32];
|
|
|
|
|
/***************** Heap *******************/
|
/***************** Heap *******************/
|
/******************************************/
|
/******************************************/
|
OS_Heap_t *OS_HeapCreate(const char *name, void *memory, uint32 size)
|
OS_Heap_t *OS_HeapCreate(const char *name, void *memory, uint32 size)
|
{
|
{
|
OS_Heap_t *heap;
|
OS_Heap_t *heap;
|
|
|
assert(((uint32)memory & 3) == 0);
|
assert(((uint32)memory & 3) == 0);
|
heap = (OS_Heap_t*)memory;
|
heap = (OS_Heap_t*)memory;
|
heap->magic = HEAP_MAGIC;
|
heap->magic = HEAP_MAGIC;
|
heap->name = name;
|
heap->name = name;
|
heap->semaphore = OS_SemaphoreCreate(name, 1);
|
heap->semaphore = OS_SemaphoreCreate(name, 1);
|
heap->available = (HeapNode_t*)(heap + 1);
|
heap->available = (HeapNode_t*)(heap + 1);
|
heap->available->next = &heap->base;
|
heap->available->next = &heap->base;
|
heap->available->size = (size - sizeof(OS_Heap_t)) / sizeof(HeapNode_t);
|
heap->available->size = (size - sizeof(OS_Heap_t)) / sizeof(HeapNode_t);
|
heap->base.next = heap->available;
|
heap->base.next = heap->available;
|
heap->base.size = 0;
|
heap->base.size = 0;
|
return heap;
|
return heap;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_HeapDestroy(OS_Heap_t *heap)
|
void OS_HeapDestroy(OS_Heap_t *heap)
|
{
|
{
|
OS_SemaphoreDelete(heap->semaphore);
|
OS_SemaphoreDelete(heap->semaphore);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Modified from K&R
|
//Modified from K&R
|
void *OS_HeapMalloc(OS_Heap_t *heap, int bytes)
|
void *OS_HeapMalloc(OS_Heap_t *heap, int bytes)
|
{
|
{
|
HeapNode_t *node, *prevp;
|
HeapNode_t *node, *prevp;
|
int nunits;
|
int nunits;
|
|
|
if(heap == NULL && OS_ThreadSelf())
|
if(heap == NULL && OS_ThreadSelf())
|
heap = OS_ThreadSelf()->heap;
|
heap = OS_ThreadSelf()->heap;
|
if((uint32)heap < HEAP_COUNT)
|
if((uint32)heap < HEAP_COUNT)
|
heap = HeapArray[(int)heap];
|
heap = HeapArray[(int)heap];
|
nunits = (bytes + sizeof(HeapNode_t) - 1) / sizeof(HeapNode_t) + 1;
|
nunits = (bytes + sizeof(HeapNode_t) - 1) / sizeof(HeapNode_t) + 1;
|
OS_SemaphorePend(heap->semaphore, OS_WAIT_FOREVER);
|
OS_SemaphorePend(heap->semaphore, OS_WAIT_FOREVER);
|
prevp = heap->available;
|
prevp = heap->available;
|
for(node = prevp->next; ; prevp = node, node = node->next)
|
for(node = prevp->next; ; prevp = node, node = node->next)
|
{
|
{
|
if(node->size >= nunits) //Big enough?
|
if(node->size >= nunits) //Big enough?
|
{
|
{
|
if(node->size == nunits) //Exactly
|
if(node->size == nunits) //Exactly
|
prevp->next = node->next;
|
prevp->next = node->next;
|
else
|
else
|
{ //Allocate tail end
|
{ //Allocate tail end
|
node->size -= nunits;
|
node->size -= nunits;
|
node += node->size;
|
node += node->size;
|
node->size = nunits;
|
node->size = nunits;
|
}
|
}
|
heap->available = prevp;
|
heap->available = prevp;
|
node->next = (HeapNode_t*)heap;
|
node->next = (HeapNode_t*)heap;
|
OS_SemaphorePost(heap->semaphore);
|
OS_SemaphorePost(heap->semaphore);
|
return (void*)(node + 1);
|
return (void*)(node + 1);
|
}
|
}
|
if(node == heap->available) //Wrapped around free list
|
if(node == heap->available) //Wrapped around free list
|
{
|
{
|
OS_SemaphorePost(heap->semaphore);
|
OS_SemaphorePost(heap->semaphore);
|
if(heap->alternate)
|
if(heap->alternate)
|
return OS_HeapMalloc(heap->alternate, bytes);
|
return OS_HeapMalloc(heap->alternate, bytes);
|
return NULL;
|
return NULL;
|
}
|
}
|
}
|
}
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Modified from K&R
|
//Modified from K&R
|
void OS_HeapFree(void *block)
|
void OS_HeapFree(void *block)
|
{
|
{
|
OS_Heap_t *heap;
|
OS_Heap_t *heap;
|
HeapNode_t *bp, *node;
|
HeapNode_t *bp, *node;
|
|
|
assert(block);
|
assert(block);
|
bp = (HeapNode_t*)block - 1; //point to block header
|
bp = (HeapNode_t*)block - 1; //point to block header
|
heap = (OS_Heap_t*)bp->next;
|
heap = (OS_Heap_t*)bp->next;
|
assert(heap->magic == HEAP_MAGIC);
|
assert(heap->magic == HEAP_MAGIC);
|
if(heap->magic != HEAP_MAGIC)
|
if(heap->magic != HEAP_MAGIC)
|
return;
|
return;
|
OS_SemaphorePend(heap->semaphore, OS_WAIT_FOREVER);
|
OS_SemaphorePend(heap->semaphore, OS_WAIT_FOREVER);
|
for(node = heap->available; !(node < bp && bp < node->next); node = node->next)
|
for(node = heap->available; !(node < bp && bp < node->next); node = node->next)
|
{
|
{
|
if(node >= node->next && (bp > node || bp < node->next))
|
if(node >= node->next && (bp > node || bp < node->next))
|
break; //freed block at start or end of area
|
break; //freed block at start or end of area
|
}
|
}
|
|
|
if(bp + bp->size == node->next) //join to upper
|
if(bp + bp->size == node->next) //join to upper
|
{
|
{
|
bp->size += node->next->size;
|
bp->size += node->next->size;
|
bp->next = node->next->next;
|
bp->next = node->next->next;
|
}
|
}
|
else
|
else
|
{
|
{
|
bp->next = node->next;
|
bp->next = node->next;
|
}
|
}
|
|
|
if(node + node->size == bp) //join to lower
|
if(node + node->size == bp) //join to lower
|
{
|
{
|
node->size += bp->size;
|
node->size += bp->size;
|
node->next = bp->next;
|
node->next = bp->next;
|
}
|
}
|
else
|
else
|
node->next = bp;
|
node->next = bp;
|
heap->available = node;
|
heap->available = node;
|
OS_SemaphorePost(heap->semaphore);
|
OS_SemaphorePost(heap->semaphore);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_HeapAlternate(OS_Heap_t *heap, OS_Heap_t *alternate)
|
void OS_HeapAlternate(OS_Heap_t *heap, OS_Heap_t *alternate)
|
{
|
{
|
heap->alternate = alternate;
|
heap->alternate = alternate;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_HeapRegister(void *index, OS_Heap_t *heap)
|
void OS_HeapRegister(void *index, OS_Heap_t *heap)
|
{
|
{
|
if((uint32)index < HEAP_COUNT)
|
if((uint32)index < HEAP_COUNT)
|
HeapArray[(int)index] = heap;
|
HeapArray[(int)index] = heap;
|
}
|
}
|
|
|
|
|
|
|
/***************** Thread *****************/
|
/***************** Thread *****************/
|
/******************************************/
|
/******************************************/
|
//Linked list of threads sorted by priority
|
//Linked list of threads sorted by priority
|
//The listed list is either ThreadHead (ready to run threads not including
|
//The listed list is either ThreadHead (ready to run threads not including
|
//the currently running thread) or a list of threads waiting on a semaphore.
|
//the currently running thread) or a list of threads waiting on a semaphore.
|
//Must be called with interrupts disabled
|
//Must be called with interrupts disabled
|
static void OS_ThreadPriorityInsert(OS_Thread_t **head, OS_Thread_t *thread)
|
static void OS_ThreadPriorityInsert(OS_Thread_t **head, OS_Thread_t *thread)
|
{
|
{
|
OS_Thread_t *node, *prev;
|
OS_Thread_t *node, *prev;
|
|
|
prev = NULL;
|
prev = NULL;
|
for(node = *head; node; node = node->next)
|
for(node = *head; node; node = node->next)
|
{
|
{
|
if(node->priority < thread->priority)
|
if(node->priority < thread->priority)
|
break;
|
break;
|
prev = node;
|
prev = node;
|
}
|
}
|
|
|
if(prev == NULL)
|
if(prev == NULL)
|
{
|
{
|
thread->next = *head;
|
thread->next = *head;
|
thread->prev = NULL;
|
thread->prev = NULL;
|
if(*head)
|
if(*head)
|
(*head)->prev = thread;
|
(*head)->prev = thread;
|
*head = thread;
|
*head = thread;
|
}
|
}
|
else
|
else
|
{
|
{
|
if(prev->next)
|
if(prev->next)
|
prev->next->prev = thread;
|
prev->next->prev = thread;
|
thread->next = prev->next;
|
thread->next = prev->next;
|
thread->prev = prev;
|
thread->prev = prev;
|
prev->next = thread;
|
prev->next = thread;
|
}
|
}
|
assert(ThreadHead);
|
assert(ThreadHead);
|
thread->state = THREAD_READY;
|
thread->state = THREAD_READY;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Must be called with interrupts disabled
|
//Must be called with interrupts disabled
|
static void OS_ThreadPriorityRemove(OS_Thread_t **head, OS_Thread_t *thread)
|
static void OS_ThreadPriorityRemove(OS_Thread_t **head, OS_Thread_t *thread)
|
{
|
{
|
assert(thread->magic[0] == THREAD_MAGIC); //check stack overflow
|
assert(thread->magic[0] == THREAD_MAGIC); //check stack overflow
|
if(thread->prev == NULL)
|
if(thread->prev == NULL)
|
*head = thread->next;
|
*head = thread->next;
|
else
|
else
|
thread->prev->next = thread->next;
|
thread->prev->next = thread->next;
|
if(thread->next)
|
if(thread->next)
|
thread->next->prev = thread->prev;
|
thread->next->prev = thread->prev;
|
thread->next = NULL;
|
thread->next = NULL;
|
thread->prev = NULL;
|
thread->prev = NULL;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Linked list of threads sorted by timeout value
|
//Linked list of threads sorted by timeout value
|
//Must be called with interrupts disabled
|
//Must be called with interrupts disabled
|
static void OS_ThreadTimeoutInsert(OS_Thread_t *thread)
|
static void OS_ThreadTimeoutInsert(OS_Thread_t *thread)
|
{
|
{
|
OS_Thread_t *node, *prev;
|
OS_Thread_t *node, *prev;
|
int diff;
|
int diff;
|
|
|
prev = NULL;
|
prev = NULL;
|
for(node = TimeoutHead; node; node = node->nextTimeout)
|
for(node = TimeoutHead; node; node = node->nextTimeout)
|
{
|
{
|
diff = thread->ticksTimeout - node->ticksTimeout;
|
diff = thread->ticksTimeout - node->ticksTimeout;
|
if(diff <= 0)
|
if(diff <= 0)
|
break;
|
break;
|
prev = node;
|
prev = node;
|
}
|
}
|
|
|
if(prev == NULL)
|
if(prev == NULL)
|
{
|
{
|
thread->nextTimeout = TimeoutHead;
|
thread->nextTimeout = TimeoutHead;
|
thread->prevTimeout = NULL;
|
thread->prevTimeout = NULL;
|
if(TimeoutHead)
|
if(TimeoutHead)
|
TimeoutHead->prevTimeout = thread;
|
TimeoutHead->prevTimeout = thread;
|
TimeoutHead = thread;
|
TimeoutHead = thread;
|
}
|
}
|
else
|
else
|
{
|
{
|
if(prev->nextTimeout)
|
if(prev->nextTimeout)
|
prev->nextTimeout->prevTimeout = thread;
|
prev->nextTimeout->prevTimeout = thread;
|
thread->nextTimeout = prev->nextTimeout;
|
thread->nextTimeout = prev->nextTimeout;
|
thread->prevTimeout = prev;
|
thread->prevTimeout = prev;
|
prev->nextTimeout = thread;
|
prev->nextTimeout = thread;
|
}
|
}
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Must be called with interrupts disabled
|
//Must be called with interrupts disabled
|
static void OS_ThreadTimeoutRemove(OS_Thread_t *thread)
|
static void OS_ThreadTimeoutRemove(OS_Thread_t *thread)
|
{
|
{
|
if(thread->prevTimeout == NULL && TimeoutHead != thread)
|
if(thread->prevTimeout == NULL && TimeoutHead != thread)
|
return; //not in list
|
return; //not in list
|
if(thread->prevTimeout == NULL)
|
if(thread->prevTimeout == NULL)
|
TimeoutHead = thread->nextTimeout;
|
TimeoutHead = thread->nextTimeout;
|
else
|
else
|
thread->prevTimeout->nextTimeout = thread->nextTimeout;
|
thread->prevTimeout->nextTimeout = thread->nextTimeout;
|
if(thread->nextTimeout)
|
if(thread->nextTimeout)
|
thread->nextTimeout->prevTimeout = thread->prevTimeout;
|
thread->nextTimeout->prevTimeout = thread->prevTimeout;
|
thread->nextTimeout = NULL;
|
thread->nextTimeout = NULL;
|
thread->prevTimeout = NULL;
|
thread->prevTimeout = NULL;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Loads highest priority thread from the ThreadHead linked list
|
//Loads highest priority thread from the ThreadHead linked list
|
//The currently running thread isn't in the ThreadHead list
|
//The currently running thread isn't in the ThreadHead list
|
//Must be called with interrupts disabled
|
//Must be called with interrupts disabled
|
static void OS_ThreadReschedule(int roundRobin)
|
static void OS_ThreadReschedule(int roundRobin)
|
{
|
{
|
OS_Thread_t *threadNext, *threadCurrent;
|
OS_Thread_t *threadNext, *threadCurrent;
|
int rc, cpuIndex = OS_CpuIndex();
|
int rc, cpuIndex = OS_CpuIndex();
|
|
|
if(ThreadSwapEnabled == 0 || InterruptInside[cpuIndex])
|
if(ThreadSwapEnabled == 0 || InterruptInside[cpuIndex])
|
{
|
{
|
ThreadNeedReschedule[cpuIndex] |= 2 + roundRobin; //Reschedule later
|
ThreadNeedReschedule[cpuIndex] |= 2 + roundRobin; //Reschedule later
|
return;
|
return;
|
}
|
}
|
|
ThreadNeedReschedule[cpuIndex] = 0;
|
|
|
//Determine which thread should run
|
//Determine which thread should run
|
threadNext = ThreadHead;
|
threadNext = ThreadHead;
|
while(threadNext && threadNext->cpuLock != -1 &&
|
while(threadNext && threadNext->cpuLock != -1 &&
|
threadNext->cpuLock != cpuIndex)
|
threadNext->cpuLock != cpuIndex)
|
threadNext = threadNext->next;
|
threadNext = threadNext->next;
|
if(threadNext == NULL)
|
if(threadNext == NULL)
|
return;
|
return;
|
threadCurrent = ThreadCurrent[cpuIndex];
|
threadCurrent = ThreadCurrent[cpuIndex];
|
|
|
if(threadCurrent == NULL ||
|
if(threadCurrent == NULL ||
|
threadCurrent->state == THREAD_PEND ||
|
threadCurrent->state == THREAD_PEND ||
|
threadCurrent->priority < threadNext->priority ||
|
threadCurrent->priority < threadNext->priority ||
|
(roundRobin && threadCurrent->priority == threadNext->priority))
|
(roundRobin && threadCurrent->priority == threadNext->priority))
|
{
|
{
|
//Swap threads
|
//Swap threads
|
ThreadCurrent[cpuIndex] = threadNext;
|
ThreadCurrent[cpuIndex] = threadNext;
|
if(threadCurrent)
|
if(threadCurrent)
|
{
|
{
|
assert(threadCurrent->magic[0] == THREAD_MAGIC); //check stack overflow
|
assert(threadCurrent->magic[0] == THREAD_MAGIC); //check stack overflow
|
if(threadCurrent->state == THREAD_RUNNING)
|
if(threadCurrent->state == THREAD_RUNNING)
|
OS_ThreadPriorityInsert(&ThreadHead, threadCurrent);
|
OS_ThreadPriorityInsert(&ThreadHead, threadCurrent);
|
rc = setjmp(threadCurrent->env); //ANSI C call to save registers
|
rc = setjmp(threadCurrent->env); //ANSI C call to save registers
|
if(rc)
|
if(rc)
|
return; //Returned from longjmp()
|
return; //Returned from longjmp()
|
}
|
}
|
|
|
//Remove the new running thread from the ThreadHead linked list
|
//Remove the new running thread from the ThreadHead linked list
|
threadNext = ThreadCurrent[OS_CpuIndex()]; //removed warning
|
threadNext = ThreadCurrent[OS_CpuIndex()]; //removed warning
|
assert(threadNext->state == THREAD_READY);
|
assert(threadNext->state == THREAD_READY);
|
OS_ThreadPriorityRemove(&ThreadHead, threadNext);
|
OS_ThreadPriorityRemove(&ThreadHead, threadNext);
|
threadNext->state = THREAD_RUNNING;
|
threadNext->state = THREAD_RUNNING;
|
threadNext->cpuIndex = OS_CpuIndex();
|
threadNext->cpuIndex = OS_CpuIndex();
|
longjmp(threadNext->env, 1); //ANSI C call to restore registers
|
longjmp(threadNext->env, 1); //ANSI C call to restore registers
|
}
|
}
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_ThreadCpuLock(OS_Thread_t *thread, int cpuIndex)
|
void OS_ThreadCpuLock(OS_Thread_t *thread, int cpuIndex)
|
{
|
{
|
thread->cpuLock = cpuIndex;
|
thread->cpuLock = cpuIndex;
|
if(thread == OS_ThreadSelf() && cpuIndex != (int)OS_CpuIndex())
|
if(thread == OS_ThreadSelf() && cpuIndex != (int)OS_CpuIndex())
|
OS_ThreadSleep(1);
|
OS_ThreadSleep(1);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
static void OS_ThreadInit(void *arg)
|
static void OS_ThreadInit(void *arg)
|
{
|
{
|
uint32 cpuIndex = OS_CpuIndex();
|
uint32 cpuIndex = OS_CpuIndex();
|
(void)arg;
|
(void)arg;
|
|
|
OS_CriticalEnd(1);
|
OS_CriticalEnd(1);
|
ThreadCurrent[cpuIndex]->funcPtr(ThreadCurrent[cpuIndex]->arg);
|
ThreadCurrent[cpuIndex]->funcPtr(ThreadCurrent[cpuIndex]->arg);
|
OS_ThreadExit();
|
OS_ThreadExit();
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Stops warning "argument X might be clobbered by `longjmp'"
|
//Stops warning "argument X might be clobbered by `longjmp'"
|
static void OS_ThreadRegsInit(jmp_buf env)
|
static void OS_ThreadRegsInit(jmp_buf env)
|
{
|
{
|
setjmp(env); //ANSI C call to save registers
|
setjmp(env); //ANSI C call to save registers
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
OS_Thread_t *OS_ThreadCreate(const char *name,
|
OS_Thread_t *OS_ThreadCreate(const char *name,
|
OS_FuncPtr_t funcPtr,
|
OS_FuncPtr_t funcPtr,
|
void *arg,
|
void *arg,
|
uint32 priority,
|
uint32 priority,
|
uint32 stackSize)
|
uint32 stackSize)
|
{
|
{
|
OS_Thread_t *thread;
|
OS_Thread_t *thread;
|
uint8 *stack;
|
uint8 *stack;
|
jmp_buf2 *env;
|
jmp_buf2 *env;
|
uint32 state;
|
uint32 state;
|
|
|
OS_SemaphorePend(SemaphoreRelease, OS_WAIT_FOREVER);
|
OS_SemaphorePend(SemaphoreRelease, OS_WAIT_FOREVER);
|
if(NeedToFree)
|
if(NeedToFree)
|
OS_HeapFree(NeedToFree);
|
OS_HeapFree(NeedToFree);
|
NeedToFree = NULL;
|
NeedToFree = NULL;
|
OS_SemaphorePost(SemaphoreRelease);
|
OS_SemaphorePost(SemaphoreRelease);
|
|
|
if(stackSize == 0)
|
if(stackSize == 0)
|
stackSize = STACK_SIZE_DEFAULT;
|
stackSize = STACK_SIZE_DEFAULT;
|
if(stackSize < STACK_SIZE_MINIMUM)
|
if(stackSize < STACK_SIZE_MINIMUM)
|
stackSize = STACK_SIZE_MINIMUM;
|
stackSize = STACK_SIZE_MINIMUM;
|
thread = (OS_Thread_t*)OS_HeapMalloc(NULL, sizeof(OS_Thread_t) + stackSize);
|
thread = (OS_Thread_t*)OS_HeapMalloc(NULL, sizeof(OS_Thread_t) + stackSize);
|
assert(thread);
|
assert(thread);
|
if(thread == NULL)
|
if(thread == NULL)
|
return NULL;
|
return NULL;
|
memset(thread, 0, sizeof(OS_Thread_t));
|
memset(thread, 0, sizeof(OS_Thread_t));
|
stack = (uint8*)(thread + 1);
|
stack = (uint8*)(thread + 1);
|
memset(stack, 0xcd, stackSize);
|
memset(stack, 0xcd, stackSize);
|
|
|
thread->name = name;
|
thread->name = name;
|
thread->state = THREAD_READY;
|
thread->state = THREAD_READY;
|
thread->cpuLock = -1;
|
thread->cpuLock = -1;
|
thread->funcPtr = funcPtr;
|
thread->funcPtr = funcPtr;
|
thread->arg = arg;
|
thread->arg = arg;
|
thread->priority = priority;
|
thread->priority = priority;
|
thread->semaphorePending = NULL;
|
thread->semaphorePending = NULL;
|
thread->returnCode = 0;
|
thread->returnCode = 0;
|
if(OS_ThreadSelf())
|
if(OS_ThreadSelf())
|
{
|
{
|
thread->processId = OS_ThreadSelf()->processId;
|
thread->processId = OS_ThreadSelf()->processId;
|
thread->heap = OS_ThreadSelf()->heap;
|
thread->heap = OS_ThreadSelf()->heap;
|
}
|
}
|
else
|
else
|
{
|
{
|
thread->processId = 0;
|
thread->processId = 0;
|
thread->heap = NULL;
|
thread->heap = NULL;
|
}
|
}
|
thread->next = NULL;
|
thread->next = NULL;
|
thread->prev = NULL;
|
thread->prev = NULL;
|
thread->nextTimeout = NULL;
|
thread->nextTimeout = NULL;
|
thread->prevTimeout = NULL;
|
thread->prevTimeout = NULL;
|
thread->magic[0] = THREAD_MAGIC;
|
thread->magic[0] = THREAD_MAGIC;
|
|
|
OS_ThreadRegsInit(thread->env);
|
OS_ThreadRegsInit(thread->env);
|
env = (jmp_buf2*)thread->env;
|
env = (jmp_buf2*)thread->env;
|
env->sp = (uint32)stack + stackSize - 24; //minimum stack frame size
|
env->sp = (uint32)stack + stackSize - 24; //minimum stack frame size
|
env->pc = (uint32)OS_ThreadInit;
|
env->pc = (uint32)OS_ThreadInit;
|
|
|
state = OS_CriticalBegin();
|
state = OS_CriticalBegin();
|
OS_ThreadPriorityInsert(&ThreadHead, thread);
|
OS_ThreadPriorityInsert(&ThreadHead, thread);
|
OS_ThreadReschedule(0);
|
OS_ThreadReschedule(0);
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
return thread;
|
return thread;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_ThreadExit(void)
|
void OS_ThreadExit(void)
|
{
|
{
|
uint32 state, cpuIndex = OS_CpuIndex();
|
uint32 state, cpuIndex = OS_CpuIndex();
|
|
|
for(;;)
|
for(;;)
|
{
|
{
|
OS_SemaphorePend(SemaphoreRelease, OS_WAIT_FOREVER);
|
OS_SemaphorePend(SemaphoreRelease, OS_WAIT_FOREVER);
|
if(NeedToFree)
|
if(NeedToFree)
|
OS_HeapFree(NeedToFree);
|
OS_HeapFree(NeedToFree);
|
NeedToFree = NULL;
|
NeedToFree = NULL;
|
OS_SemaphorePost(SemaphoreRelease);
|
OS_SemaphorePost(SemaphoreRelease);
|
|
|
state = OS_CriticalBegin();
|
state = OS_CriticalBegin();
|
if(NeedToFree)
|
if(NeedToFree)
|
{
|
{
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
continue;
|
continue;
|
}
|
}
|
ThreadCurrent[cpuIndex]->state = THREAD_PEND;
|
ThreadCurrent[cpuIndex]->state = THREAD_PEND;
|
NeedToFree = ThreadCurrent[cpuIndex];
|
NeedToFree = ThreadCurrent[cpuIndex];
|
OS_ThreadReschedule(0);
|
OS_ThreadReschedule(0);
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
}
|
}
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
OS_Thread_t *OS_ThreadSelf(void)
|
OS_Thread_t *OS_ThreadSelf(void)
|
{
|
{
|
return ThreadCurrent[OS_CpuIndex()];
|
return ThreadCurrent[OS_CpuIndex()];
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_ThreadSleep(int ticks)
|
void OS_ThreadSleep(int ticks)
|
{
|
{
|
OS_SemaphorePend(SemaphoreSleep, ticks);
|
OS_SemaphorePend(SemaphoreSleep, ticks);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
uint32 OS_ThreadTime(void)
|
uint32 OS_ThreadTime(void)
|
{
|
{
|
return ThreadTime;
|
return ThreadTime;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_ThreadInfoSet(OS_Thread_t *thread, uint32 index, void *Info)
|
void OS_ThreadInfoSet(OS_Thread_t *thread, uint32 index, void *Info)
|
{
|
{
|
if(index < INFO_COUNT)
|
if(index < INFO_COUNT)
|
thread->info[index] = Info;
|
thread->info[index] = Info;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void *OS_ThreadInfoGet(OS_Thread_t *thread, uint32 index)
|
void *OS_ThreadInfoGet(OS_Thread_t *thread, uint32 index)
|
{
|
{
|
if(index < INFO_COUNT)
|
if(index < INFO_COUNT)
|
return thread->info[index];
|
return thread->info[index];
|
return NULL;
|
return NULL;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
uint32 OS_ThreadPriorityGet(OS_Thread_t *thread)
|
uint32 OS_ThreadPriorityGet(OS_Thread_t *thread)
|
{
|
{
|
return thread->priority;
|
return thread->priority;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_ThreadPrioritySet(OS_Thread_t *thread, uint32 priority)
|
void OS_ThreadPrioritySet(OS_Thread_t *thread, uint32 priority)
|
{
|
{
|
uint32 state;
|
uint32 state;
|
state = OS_CriticalBegin();
|
state = OS_CriticalBegin();
|
thread->priority = priority;
|
thread->priority = priority;
|
if(thread->state == THREAD_READY)
|
if(thread->state == THREAD_READY)
|
{
|
{
|
OS_ThreadPriorityRemove(&ThreadHead, thread);
|
OS_ThreadPriorityRemove(&ThreadHead, thread);
|
OS_ThreadPriorityInsert(&ThreadHead, thread);
|
OS_ThreadPriorityInsert(&ThreadHead, thread);
|
OS_ThreadReschedule(0);
|
OS_ThreadReschedule(0);
|
}
|
}
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_ThreadProcessId(OS_Thread_t *thread, uint32 processId, OS_Heap_t *heap)
|
void OS_ThreadProcessId(OS_Thread_t *thread, uint32 processId, OS_Heap_t *heap)
|
{
|
{
|
thread->processId = processId;
|
thread->processId = processId;
|
thread->heap = heap;
|
thread->heap = heap;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Must be called with interrupts disabled
|
//Must be called with interrupts disabled
|
void OS_ThreadTick(void *Arg)
|
void OS_ThreadTick(void *Arg)
|
{
|
{
|
OS_Thread_t *thread;
|
OS_Thread_t *thread;
|
OS_Semaphore_t *semaphore;
|
OS_Semaphore_t *semaphore;
|
int diff;
|
int diff;
|
(void)Arg;
|
(void)Arg;
|
|
|
++ThreadTime;
|
++ThreadTime;
|
while(TimeoutHead)
|
while(TimeoutHead)
|
{
|
{
|
thread = TimeoutHead;
|
thread = TimeoutHead;
|
diff = ThreadTime - thread->ticksTimeout;
|
diff = ThreadTime - thread->ticksTimeout;
|
if(diff < 0)
|
if(diff < 0)
|
break;
|
break;
|
OS_ThreadTimeoutRemove(thread);
|
OS_ThreadTimeoutRemove(thread);
|
semaphore = thread->semaphorePending;
|
semaphore = thread->semaphorePending;
|
++semaphore->count;
|
++semaphore->count;
|
thread->semaphorePending = NULL;
|
thread->semaphorePending = NULL;
|
thread->returnCode = -1;
|
thread->returnCode = -1;
|
OS_ThreadPriorityRemove(&semaphore->threadHead, thread);
|
OS_ThreadPriorityRemove(&semaphore->threadHead, thread);
|
OS_ThreadPriorityInsert(&ThreadHead, thread);
|
OS_ThreadPriorityInsert(&ThreadHead, thread);
|
}
|
}
|
OS_ThreadReschedule(1);
|
OS_ThreadReschedule(1);
|
}
|
}
|
|
|
|
|
|
|
/***************** Semaphore **************/
|
/***************** Semaphore **************/
|
/******************************************/
|
/******************************************/
|
OS_Semaphore_t *OS_SemaphoreCreate(const char *name, uint32 count)
|
OS_Semaphore_t *OS_SemaphoreCreate(const char *name, uint32 count)
|
{
|
{
|
OS_Semaphore_t *semaphore;
|
OS_Semaphore_t *semaphore;
|
static int semCount = 0;
|
static int semCount = 0;
|
|
|
if(semCount < SEM_RESERVED_COUNT)
|
if(semCount < SEM_RESERVED_COUNT)
|
semaphore = &SemaphoreReserved[semCount++]; //Heap not ready yet
|
semaphore = &SemaphoreReserved[semCount++]; //Heap not ready yet
|
else
|
else
|
semaphore = (OS_Semaphore_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_Semaphore_t));
|
semaphore = (OS_Semaphore_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_Semaphore_t));
|
assert(semaphore);
|
assert(semaphore);
|
if(semaphore == NULL)
|
if(semaphore == NULL)
|
return NULL;
|
return NULL;
|
|
|
semaphore->name = name;
|
semaphore->name = name;
|
semaphore->threadHead = NULL;
|
semaphore->threadHead = NULL;
|
semaphore->count = count;
|
semaphore->count = count;
|
return semaphore;
|
return semaphore;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_SemaphoreDelete(OS_Semaphore_t *semaphore)
|
void OS_SemaphoreDelete(OS_Semaphore_t *semaphore)
|
{
|
{
|
while(semaphore->threadHead)
|
while(semaphore->threadHead)
|
OS_SemaphorePost(semaphore);
|
OS_SemaphorePost(semaphore);
|
OS_HeapFree(semaphore);
|
OS_HeapFree(semaphore);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
int OS_SemaphorePend(OS_Semaphore_t *semaphore, int ticks)
|
int OS_SemaphorePend(OS_Semaphore_t *semaphore, int ticks)
|
{
|
{
|
uint32 state, cpuIndex;
|
uint32 state, cpuIndex;
|
OS_Thread_t *thread;
|
OS_Thread_t *thread;
|
int returnCode=0;
|
int returnCode=0;
|
|
|
assert(semaphore);
|
assert(semaphore);
|
assert(InterruptInside[OS_CpuIndex()] == 0);
|
assert(InterruptInside[OS_CpuIndex()] == 0);
|
state = OS_CriticalBegin();
|
state = OS_CriticalBegin();
|
if(--semaphore->count < 0)
|
if(--semaphore->count < 0)
|
{
|
{
|
if(ticks == 0)
|
if(ticks == 0)
|
{
|
{
|
++semaphore->count;
|
++semaphore->count;
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
return -1;
|
return -1;
|
}
|
}
|
cpuIndex = OS_CpuIndex();
|
cpuIndex = OS_CpuIndex();
|
thread = ThreadCurrent[cpuIndex];
|
thread = ThreadCurrent[cpuIndex];
|
assert(thread);
|
assert(thread);
|
thread->semaphorePending = semaphore;
|
thread->semaphorePending = semaphore;
|
thread->ticksTimeout = ticks + OS_ThreadTime();
|
thread->ticksTimeout = ticks + OS_ThreadTime();
|
//FYI: The current thread isn't in the ThreadHead linked list
|
//FYI: The current thread isn't in the ThreadHead linked list
|
OS_ThreadPriorityInsert(&semaphore->threadHead, thread);
|
OS_ThreadPriorityInsert(&semaphore->threadHead, thread);
|
thread->state = THREAD_PEND;
|
thread->state = THREAD_PEND;
|
if(ticks != OS_WAIT_FOREVER)
|
if(ticks != OS_WAIT_FOREVER)
|
OS_ThreadTimeoutInsert(thread);
|
OS_ThreadTimeoutInsert(thread);
|
assert(ThreadHead);
|
assert(ThreadHead);
|
OS_ThreadReschedule(0);
|
OS_ThreadReschedule(0);
|
returnCode = thread->returnCode;
|
returnCode = thread->returnCode;
|
}
|
}
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
return returnCode;
|
return returnCode;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_SemaphorePost(OS_Semaphore_t *semaphore)
|
void OS_SemaphorePost(OS_Semaphore_t *semaphore)
|
{
|
{
|
uint32 state;
|
uint32 state;
|
OS_Thread_t *thread;
|
OS_Thread_t *thread;
|
|
|
assert(semaphore);
|
assert(semaphore);
|
state = OS_CriticalBegin();
|
state = OS_CriticalBegin();
|
if(++semaphore->count <= 0)
|
if(++semaphore->count <= 0)
|
{
|
{
|
thread = semaphore->threadHead;
|
thread = semaphore->threadHead;
|
OS_ThreadTimeoutRemove(thread);
|
OS_ThreadTimeoutRemove(thread);
|
OS_ThreadPriorityRemove(&semaphore->threadHead, thread);
|
OS_ThreadPriorityRemove(&semaphore->threadHead, thread);
|
OS_ThreadPriorityInsert(&ThreadHead, thread);
|
OS_ThreadPriorityInsert(&ThreadHead, thread);
|
thread->semaphorePending = NULL;
|
thread->semaphorePending = NULL;
|
thread->returnCode = 0;
|
thread->returnCode = 0;
|
OS_ThreadReschedule(0);
|
OS_ThreadReschedule(0);
|
}
|
}
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
}
|
}
|
|
|
|
|
|
|
/***************** Mutex ******************/
|
/***************** Mutex ******************/
|
/******************************************/
|
/******************************************/
|
OS_Mutex_t *OS_MutexCreate(const char *name)
|
OS_Mutex_t *OS_MutexCreate(const char *name)
|
{
|
{
|
OS_Mutex_t *mutex;
|
OS_Mutex_t *mutex;
|
|
|
mutex = (OS_Mutex_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_Mutex_t));
|
mutex = (OS_Mutex_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_Mutex_t));
|
if(mutex == NULL)
|
if(mutex == NULL)
|
return NULL;
|
return NULL;
|
mutex->semaphore = OS_SemaphoreCreate(name, 1);
|
mutex->semaphore = OS_SemaphoreCreate(name, 1);
|
if(mutex->semaphore == NULL)
|
if(mutex->semaphore == NULL)
|
return NULL;
|
return NULL;
|
mutex->thread = NULL;
|
mutex->thread = NULL;
|
mutex->count = 0;
|
mutex->count = 0;
|
return mutex;
|
return mutex;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_MutexDelete(OS_Mutex_t *mutex)
|
void OS_MutexDelete(OS_Mutex_t *mutex)
|
{
|
{
|
OS_SemaphoreDelete(mutex->semaphore);
|
OS_SemaphoreDelete(mutex->semaphore);
|
OS_HeapFree(mutex);
|
OS_HeapFree(mutex);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_MutexPend(OS_Mutex_t *mutex)
|
void OS_MutexPend(OS_Mutex_t *mutex)
|
{
|
{
|
OS_Thread_t *thread;
|
OS_Thread_t *thread;
|
|
|
assert(mutex);
|
assert(mutex);
|
thread = OS_ThreadSelf();
|
thread = OS_ThreadSelf();
|
if(thread == mutex->thread)
|
if(thread == mutex->thread)
|
{
|
{
|
++mutex->count;
|
++mutex->count;
|
return;
|
return;
|
}
|
}
|
OS_SemaphorePend(mutex->semaphore, OS_WAIT_FOREVER);
|
OS_SemaphorePend(mutex->semaphore, OS_WAIT_FOREVER);
|
mutex->thread = thread;
|
mutex->thread = thread;
|
mutex->count = 1;
|
mutex->count = 1;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_MutexPost(OS_Mutex_t *mutex)
|
void OS_MutexPost(OS_Mutex_t *mutex)
|
{
|
{
|
assert(mutex);
|
assert(mutex);
|
assert(mutex->thread == OS_ThreadSelf());
|
assert(mutex->thread == OS_ThreadSelf());
|
assert(mutex->count > 0);
|
assert(mutex->count > 0);
|
if(--mutex->count <= 0)
|
if(--mutex->count <= 0)
|
{
|
{
|
mutex->thread = NULL;
|
mutex->thread = NULL;
|
OS_SemaphorePost(mutex->semaphore);
|
OS_SemaphorePost(mutex->semaphore);
|
}
|
}
|
}
|
}
|
|
|
|
|
|
|
/***************** MQueue *****************/
|
/***************** MQueue *****************/
|
/******************************************/
|
/******************************************/
|
OS_MQueue_t *OS_MQueueCreate(const char *name,
|
OS_MQueue_t *OS_MQueueCreate(const char *name,
|
int messageCount,
|
int messageCount,
|
int messageBytes)
|
int messageBytes)
|
{
|
{
|
OS_MQueue_t *queue;
|
OS_MQueue_t *queue;
|
int size;
|
int size;
|
|
|
size = messageBytes / sizeof(uint32);
|
size = messageBytes / sizeof(uint32);
|
queue = (OS_MQueue_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_MQueue_t) +
|
queue = (OS_MQueue_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_MQueue_t) +
|
messageCount * size * 4);
|
messageCount * size * 4);
|
if(queue == NULL)
|
if(queue == NULL)
|
return queue;
|
return queue;
|
queue->name = name;
|
queue->name = name;
|
queue->semaphore = OS_SemaphoreCreate(name, 0);
|
queue->semaphore = OS_SemaphoreCreate(name, 0);
|
if(queue->semaphore == NULL)
|
if(queue->semaphore == NULL)
|
return NULL;
|
return NULL;
|
queue->count = messageCount;
|
queue->count = messageCount;
|
queue->size = size;
|
queue->size = size;
|
queue->used = 0;
|
queue->used = 0;
|
queue->read = 0;
|
queue->read = 0;
|
queue->write = 0;
|
queue->write = 0;
|
return queue;
|
return queue;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_MQueueDelete(OS_MQueue_t *mQueue)
|
void OS_MQueueDelete(OS_MQueue_t *mQueue)
|
{
|
{
|
OS_SemaphoreDelete(mQueue->semaphore);
|
OS_SemaphoreDelete(mQueue->semaphore);
|
OS_HeapFree(mQueue);
|
OS_HeapFree(mQueue);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
int OS_MQueueSend(OS_MQueue_t *mQueue, void *message)
|
int OS_MQueueSend(OS_MQueue_t *mQueue, void *message)
|
{
|
{
|
uint32 state, *dst, *src;
|
uint32 state, *dst, *src;
|
int i;
|
int i;
|
|
|
assert(mQueue);
|
assert(mQueue);
|
src = (uint32*)message;
|
src = (uint32*)message;
|
state = OS_CriticalBegin();
|
state = OS_CriticalBegin();
|
if(++mQueue->used > mQueue->count)
|
if(++mQueue->used > mQueue->count)
|
{
|
{
|
--mQueue->used;
|
--mQueue->used;
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
return -1;
|
return -1;
|
}
|
}
|
dst = (uint32*)(mQueue + 1) + mQueue->write * mQueue->size;
|
dst = (uint32*)(mQueue + 1) + mQueue->write * mQueue->size;
|
for(i = 0; i < mQueue->size; ++i)
|
for(i = 0; i < mQueue->size; ++i)
|
dst[i] = src[i];
|
dst[i] = src[i];
|
if(++mQueue->write >= mQueue->count)
|
if(++mQueue->write >= mQueue->count)
|
mQueue->write = 0;
|
mQueue->write = 0;
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
OS_SemaphorePost(mQueue->semaphore);
|
OS_SemaphorePost(mQueue->semaphore);
|
return 0;
|
return 0;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
int OS_MQueueGet(OS_MQueue_t *mQueue, void *message, int ticks)
|
int OS_MQueueGet(OS_MQueue_t *mQueue, void *message, int ticks)
|
{
|
{
|
uint32 state, *dst, *src;
|
uint32 state, *dst, *src;
|
int i, rc;
|
int i, rc;
|
|
|
assert(mQueue);
|
assert(mQueue);
|
dst = (uint32*)message;
|
dst = (uint32*)message;
|
rc = OS_SemaphorePend(mQueue->semaphore, ticks);
|
rc = OS_SemaphorePend(mQueue->semaphore, ticks);
|
if(rc)
|
if(rc)
|
return rc;
|
return rc;
|
state = OS_CriticalBegin();
|
state = OS_CriticalBegin();
|
--mQueue->used;
|
--mQueue->used;
|
src = (uint32*)(mQueue + 1) + mQueue->read * mQueue->size;
|
src = (uint32*)(mQueue + 1) + mQueue->read * mQueue->size;
|
for(i = 0; i < mQueue->size; ++i)
|
for(i = 0; i < mQueue->size; ++i)
|
dst[i] = src[i];
|
dst[i] = src[i];
|
if(++mQueue->read >= mQueue->count)
|
if(++mQueue->read >= mQueue->count)
|
mQueue->read = 0;
|
mQueue->read = 0;
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
return 0;
|
return 0;
|
}
|
}
|
|
|
|
|
|
|
/***************** Jobs *******************/
|
/***************** Jobs *******************/
|
/******************************************/
|
/******************************************/
|
typedef void (*JobFunc_t)();
|
typedef void (*JobFunc_t)();
|
static OS_MQueue_t *jobQueue;
|
static OS_MQueue_t *jobQueue;
|
static OS_Thread_t *jobThread;
|
static OS_Thread_t *jobThread;
|
|
|
static void JobThread(void *arg)
|
static void JobThread(void *arg)
|
{
|
{
|
uint32 message[4];
|
uint32 message[4];
|
JobFunc_t funcPtr;
|
JobFunc_t funcPtr;
|
(void)arg;
|
(void)arg;
|
for(;;)
|
for(;;)
|
{
|
{
|
OS_MQueueGet(jobQueue, message, OS_WAIT_FOREVER);
|
OS_MQueueGet(jobQueue, message, OS_WAIT_FOREVER);
|
funcPtr = (JobFunc_t)message[0];
|
funcPtr = (JobFunc_t)message[0];
|
funcPtr(message[1], message[2], message[3]);
|
funcPtr(message[1], message[2], message[3]);
|
}
|
}
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_Job(void (*funcPtr)(), void *arg0, void *arg1, void *arg2)
|
void OS_Job(void (*funcPtr)(), void *arg0, void *arg1, void *arg2)
|
{
|
{
|
uint32 message[4];
|
uint32 message[4];
|
int rc;
|
int rc;
|
|
|
OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
|
OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
|
if(jobThread == NULL)
|
if(jobThread == NULL)
|
{
|
{
|
jobQueue = OS_MQueueCreate("job", 100, 16);
|
jobQueue = OS_MQueueCreate("job", 100, 16);
|
jobThread = OS_ThreadCreate("job", JobThread, NULL, 150, 4000);
|
jobThread = OS_ThreadCreate("job", JobThread, NULL, 150, 4000);
|
}
|
}
|
OS_SemaphorePost(SemaphoreLock);
|
OS_SemaphorePost(SemaphoreLock);
|
|
|
message[0] = (uint32)funcPtr;
|
message[0] = (uint32)funcPtr;
|
message[1] = (uint32)arg0;
|
message[1] = (uint32)arg0;
|
message[2] = (uint32)arg1;
|
message[2] = (uint32)arg1;
|
message[3] = (uint32)arg2;
|
message[3] = (uint32)arg2;
|
rc = OS_MQueueSend(jobQueue, message);
|
rc = OS_MQueueSend(jobQueue, message);
|
}
|
}
|
|
|
|
|
/***************** Timer ******************/
|
/***************** Timer ******************/
|
/******************************************/
|
/******************************************/
|
static void OS_TimerThread(void *arg)
|
static void OS_TimerThread(void *arg)
|
{
|
{
|
uint32 timeNow;
|
uint32 timeNow;
|
int diff, ticks;
|
int diff, ticks;
|
uint32 message[8];
|
uint32 message[8];
|
OS_Timer_t *timer;
|
OS_Timer_t *timer;
|
(void)arg;
|
(void)arg;
|
|
|
timeNow = OS_ThreadTime();
|
timeNow = OS_ThreadTime();
|
for(;;)
|
for(;;)
|
{
|
{
|
//Determine how long to sleep
|
//Determine how long to sleep
|
OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
|
OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
|
if(TimerHead)
|
if(TimerHead)
|
ticks = TimerHead->ticksTimeout - timeNow;
|
ticks = TimerHead->ticksTimeout - timeNow;
|
else
|
else
|
ticks = OS_WAIT_FOREVER;
|
ticks = OS_WAIT_FOREVER;
|
OS_SemaphorePost(SemaphoreLock);
|
OS_SemaphorePost(SemaphoreLock);
|
OS_SemaphorePend(SemaphoreTimer, ticks);
|
OS_SemaphorePend(SemaphoreTimer, ticks);
|
|
|
//Send messages for all timed out timers
|
//Send messages for all timed out timers
|
timeNow = OS_ThreadTime();
|
timeNow = OS_ThreadTime();
|
for(;;)
|
for(;;)
|
{
|
{
|
timer = NULL;
|
timer = NULL;
|
OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
|
OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
|
if(TimerHead)
|
if(TimerHead)
|
{
|
{
|
diff = timeNow - TimerHead->ticksTimeout;
|
diff = timeNow - TimerHead->ticksTimeout;
|
if(diff >= 0)
|
if(diff >= 0)
|
timer = TimerHead;
|
timer = TimerHead;
|
}
|
}
|
OS_SemaphorePost(SemaphoreLock);
|
OS_SemaphorePost(SemaphoreLock);
|
if(timer == NULL)
|
if(timer == NULL)
|
break;
|
break;
|
if(timer->ticksRestart)
|
if(timer->ticksRestart)
|
OS_TimerStart(timer, timer->ticksRestart, timer->ticksRestart);
|
OS_TimerStart(timer, timer->ticksRestart, timer->ticksRestart);
|
else
|
else
|
OS_TimerStop(timer);
|
OS_TimerStop(timer);
|
|
|
if(timer->callback)
|
if(timer->callback)
|
timer->callback(timer, timer->info);
|
timer->callback(timer, timer->info);
|
else
|
else
|
{
|
{
|
//Send message
|
//Send message
|
message[0] = MESSAGE_TYPE_TIMER;
|
message[0] = MESSAGE_TYPE_TIMER;
|
message[1] = (uint32)timer;
|
message[1] = (uint32)timer;
|
message[2] = timer->info;
|
message[2] = timer->info;
|
OS_MQueueSend(timer->mqueue, message);
|
OS_MQueueSend(timer->mqueue, message);
|
}
|
}
|
}
|
}
|
}
|
}
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
OS_Timer_t *OS_TimerCreate(const char *name, OS_MQueue_t *mQueue, uint32 info)
|
OS_Timer_t *OS_TimerCreate(const char *name, OS_MQueue_t *mQueue, uint32 info)
|
{
|
{
|
OS_Timer_t *timer;
|
OS_Timer_t *timer;
|
|
|
OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
|
OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
|
if(SemaphoreTimer == NULL)
|
if(SemaphoreTimer == NULL)
|
{
|
{
|
SemaphoreTimer = OS_SemaphoreCreate("Timer", 0);
|
SemaphoreTimer = OS_SemaphoreCreate("Timer", 0);
|
OS_ThreadCreate("Timer", OS_TimerThread, NULL, 250, 2000);
|
OS_ThreadCreate("Timer", OS_TimerThread, NULL, 250, 2000);
|
}
|
}
|
OS_SemaphorePost(SemaphoreLock);
|
OS_SemaphorePost(SemaphoreLock);
|
|
|
timer = (OS_Timer_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_Timer_t));
|
timer = (OS_Timer_t*)OS_HeapMalloc(HEAP_SYSTEM, sizeof(OS_Timer_t));
|
if(timer == NULL)
|
if(timer == NULL)
|
return NULL;
|
return NULL;
|
timer->name = name;
|
timer->name = name;
|
timer->callback = NULL;
|
timer->callback = NULL;
|
timer->mqueue = mQueue;
|
timer->mqueue = mQueue;
|
timer->next = NULL;
|
timer->next = NULL;
|
timer->prev = NULL;
|
timer->prev = NULL;
|
timer->info = info;
|
timer->info = info;
|
timer->active = 0;
|
timer->active = 0;
|
return timer;
|
return timer;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_TimerDelete(OS_Timer_t *timer)
|
void OS_TimerDelete(OS_Timer_t *timer)
|
{
|
{
|
OS_TimerStop(timer);
|
OS_TimerStop(timer);
|
OS_HeapFree(timer);
|
OS_HeapFree(timer);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_TimerCallback(OS_Timer_t *timer, OS_TimerFuncPtr_t callback)
|
void OS_TimerCallback(OS_Timer_t *timer, OS_TimerFuncPtr_t callback)
|
{
|
{
|
timer->callback = callback;
|
timer->callback = callback;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Must not be called from an ISR
|
//Must not be called from an ISR
|
void OS_TimerStart(OS_Timer_t *timer, uint32 ticks, uint32 ticksRestart)
|
void OS_TimerStart(OS_Timer_t *timer, uint32 ticks, uint32 ticksRestart)
|
{
|
{
|
OS_Timer_t *node, *prev;
|
OS_Timer_t *node, *prev;
|
int diff, check=0;
|
int diff, check=0;
|
|
|
assert(timer);
|
assert(timer);
|
assert(InterruptInside[OS_CpuIndex()] == 0);
|
assert(InterruptInside[OS_CpuIndex()] == 0);
|
ticks += OS_ThreadTime();
|
ticks += OS_ThreadTime();
|
if(timer->active)
|
if(timer->active)
|
OS_TimerStop(timer);
|
OS_TimerStop(timer);
|
OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
|
OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
|
if(timer->active)
|
if(timer->active)
|
{
|
{
|
//Prevent race condition
|
//Prevent race condition
|
OS_SemaphorePost(SemaphoreLock);
|
OS_SemaphorePost(SemaphoreLock);
|
return;
|
return;
|
}
|
}
|
timer->ticksTimeout = ticks;
|
timer->ticksTimeout = ticks;
|
timer->ticksRestart = ticksRestart;
|
timer->ticksRestart = ticksRestart;
|
timer->active = 1;
|
timer->active = 1;
|
prev = NULL;
|
prev = NULL;
|
for(node = TimerHead; node; node = node->next)
|
for(node = TimerHead; node; node = node->next)
|
{
|
{
|
diff = ticks - node->ticksTimeout;
|
diff = ticks - node->ticksTimeout;
|
if(diff <= 0)
|
if(diff <= 0)
|
break;
|
break;
|
prev = node;
|
prev = node;
|
}
|
}
|
timer->next = node;
|
timer->next = node;
|
timer->prev = prev;
|
timer->prev = prev;
|
if(node)
|
if(node)
|
node->prev = timer;
|
node->prev = timer;
|
if(prev == NULL)
|
if(prev == NULL)
|
{
|
{
|
TimerHead = timer;
|
TimerHead = timer;
|
check = 1;
|
check = 1;
|
}
|
}
|
else
|
else
|
prev->next = timer;
|
prev->next = timer;
|
OS_SemaphorePost(SemaphoreLock);
|
OS_SemaphorePost(SemaphoreLock);
|
if(check)
|
if(check)
|
OS_SemaphorePost(SemaphoreTimer);
|
OS_SemaphorePost(SemaphoreTimer);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Must not be called from an ISR
|
//Must not be called from an ISR
|
void OS_TimerStop(OS_Timer_t *timer)
|
void OS_TimerStop(OS_Timer_t *timer)
|
{
|
{
|
assert(timer);
|
assert(timer);
|
assert(InterruptInside[OS_CpuIndex()] == 0);
|
assert(InterruptInside[OS_CpuIndex()] == 0);
|
OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
|
OS_SemaphorePend(SemaphoreLock, OS_WAIT_FOREVER);
|
if(timer->active)
|
if(timer->active)
|
{
|
{
|
timer->active = 0;
|
timer->active = 0;
|
if(timer->prev == NULL)
|
if(timer->prev == NULL)
|
TimerHead = timer->next;
|
TimerHead = timer->next;
|
else
|
else
|
timer->prev->next = timer->next;
|
timer->prev->next = timer->next;
|
if(timer->next)
|
if(timer->next)
|
timer->next->prev = timer->prev;
|
timer->next->prev = timer->prev;
|
}
|
}
|
OS_SemaphorePost(SemaphoreLock);
|
OS_SemaphorePost(SemaphoreLock);
|
}
|
}
|
|
|
|
|
/***************** ISR ********************/
|
/***************** ISR ********************/
|
/******************************************/
|
/******************************************/
|
void OS_InterruptServiceRoutine(uint32 status, uint32 *stack)
|
void OS_InterruptServiceRoutine(uint32 status, uint32 *stack)
|
{
|
{
|
int i;
|
int i;
|
uint32 state, cpuIndex = OS_CpuIndex();
|
uint32 state, cpuIndex = OS_CpuIndex();
|
|
|
if(status == 0 && Isr[31])
|
if(status == 0 && Isr[31])
|
Isr[31](stack); //SYSCALL or BREAK
|
Isr[31](stack); //SYSCALL or BREAK
|
|
|
InterruptInside[cpuIndex] = 1;
|
InterruptInside[cpuIndex] = 1;
|
i = 0;
|
i = 0;
|
do
|
do
|
{
|
{
|
if(status & 1)
|
if(status & 1)
|
{
|
{
|
if(Isr[i])
|
if(Isr[i])
|
Isr[i](stack);
|
Isr[i](stack);
|
else
|
else
|
OS_InterruptMaskClear(1 << i);
|
OS_InterruptMaskClear(1 << i);
|
}
|
}
|
status >>= 1;
|
status >>= 1;
|
++i;
|
++i;
|
} while(status);
|
} while(status);
|
InterruptInside[cpuIndex] = 0;
|
InterruptInside[cpuIndex] = 0;
|
|
|
state = OS_SpinLock();
|
state = OS_SpinLock();
|
if(ThreadNeedReschedule[cpuIndex])
|
if(ThreadNeedReschedule[cpuIndex])
|
OS_ThreadReschedule(ThreadNeedReschedule[cpuIndex] & 1);
|
OS_ThreadReschedule(ThreadNeedReschedule[cpuIndex] & 1);
|
OS_SpinUnlock(state);
|
OS_SpinUnlock(state);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_InterruptRegister(uint32 mask, OS_FuncPtr_t funcPtr)
|
void OS_InterruptRegister(uint32 mask, OS_FuncPtr_t funcPtr)
|
{
|
{
|
int i;
|
int i;
|
|
|
for(i = 0; i < 32; ++i)
|
for(i = 0; i < 32; ++i)
|
{
|
{
|
if(mask & (1 << i))
|
if(mask & (1 << i))
|
Isr[i] = funcPtr;
|
Isr[i] = funcPtr;
|
}
|
}
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Plasma hardware dependent
|
//Plasma hardware dependent
|
uint32 OS_InterruptStatus(void)
|
uint32 OS_InterruptStatus(void)
|
{
|
{
|
return MemoryRead(IRQ_STATUS);
|
return MemoryRead(IRQ_STATUS);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Plasma hardware dependent
|
//Plasma hardware dependent
|
uint32 OS_InterruptMaskSet(uint32 mask)
|
uint32 OS_InterruptMaskSet(uint32 mask)
|
{
|
{
|
uint32 state;
|
uint32 state;
|
state = OS_CriticalBegin();
|
state = OS_CriticalBegin();
|
mask |= MemoryRead(IRQ_MASK);
|
mask |= MemoryRead(IRQ_MASK);
|
MemoryWrite(IRQ_MASK, mask);
|
MemoryWrite(IRQ_MASK, mask);
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
return mask;
|
return mask;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Plasma hardware dependent
|
//Plasma hardware dependent
|
uint32 OS_InterruptMaskClear(uint32 mask)
|
uint32 OS_InterruptMaskClear(uint32 mask)
|
{
|
{
|
uint32 state;
|
uint32 state;
|
state = OS_CriticalBegin();
|
state = OS_CriticalBegin();
|
mask = MemoryRead(IRQ_MASK) & ~mask;
|
mask = MemoryRead(IRQ_MASK) & ~mask;
|
MemoryWrite(IRQ_MASK, mask);
|
MemoryWrite(IRQ_MASK, mask);
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
return mask;
|
return mask;
|
}
|
}
|
|
|
|
|
/**************** Init ********************/
|
/**************** Init ********************/
|
/******************************************/
|
/******************************************/
|
static volatile uint32 IdleCount;
|
static volatile uint32 IdleCount;
|
static void OS_IdleThread(void *arg)
|
static void OS_IdleThread(void *arg)
|
{
|
{
|
(void)arg;
|
(void)arg;
|
|
|
//Don't block in the idle thread!
|
//Don't block in the idle thread!
|
for(;;)
|
for(;;)
|
{
|
{
|
++IdleCount;
|
++IdleCount;
|
}
|
}
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
#ifndef DISABLE_IRQ_SIM
|
#ifndef DISABLE_IRQ_SIM
|
static void OS_IdleSimulateIsr(void *arg)
|
static void OS_IdleSimulateIsr(void *arg)
|
{
|
{
|
uint32 count=0, value;
|
uint32 count=0, value;
|
(void)arg;
|
(void)arg;
|
|
|
for(;;)
|
for(;;)
|
{
|
{
|
MemoryRead(IRQ_MASK + 4); //calls Sleep(10)
|
MemoryRead(IRQ_MASK + 4); //calls Sleep(10)
|
#if WIN32
|
#if WIN32
|
while(OS_InterruptMaskSet(0) & IRQ_UART_WRITE_AVAILABLE)
|
while(OS_InterruptMaskSet(0) & IRQ_UART_WRITE_AVAILABLE)
|
OS_InterruptServiceRoutine(IRQ_UART_WRITE_AVAILABLE, 0);
|
OS_InterruptServiceRoutine(IRQ_UART_WRITE_AVAILABLE, 0);
|
#endif
|
#endif
|
value = OS_InterruptMaskSet(0) & 0xf;
|
value = OS_InterruptMaskSet(0) & 0xf;
|
if(value)
|
if(value)
|
OS_InterruptServiceRoutine(value, 0);
|
OS_InterruptServiceRoutine(value, 0);
|
++count;
|
++count;
|
}
|
}
|
}
|
}
|
#endif //DISABLE_IRQ_SIM
|
#endif //DISABLE_IRQ_SIM
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Plasma hardware dependent
|
//Plasma hardware dependent
|
static void OS_ThreadTickToggle(void *arg)
|
static void OS_ThreadTickToggle(void *arg)
|
{
|
{
|
uint32 status, mask, state;
|
uint32 status, mask, state;
|
|
|
//Toggle looking for IRQ_COUNTER18 or IRQ_COUNTER18_NOT
|
//Toggle looking for IRQ_COUNTER18 or IRQ_COUNTER18_NOT
|
state = OS_SpinLock();
|
state = OS_SpinLock();
|
status = MemoryRead(IRQ_STATUS) & (IRQ_COUNTER18 | IRQ_COUNTER18_NOT);
|
status = MemoryRead(IRQ_STATUS) & (IRQ_COUNTER18 | IRQ_COUNTER18_NOT);
|
mask = MemoryRead(IRQ_MASK) | IRQ_COUNTER18 | IRQ_COUNTER18_NOT;
|
mask = MemoryRead(IRQ_MASK) | IRQ_COUNTER18 | IRQ_COUNTER18_NOT;
|
mask &= ~status;
|
mask &= ~status;
|
MemoryWrite(IRQ_MASK, mask);
|
MemoryWrite(IRQ_MASK, mask);
|
OS_ThreadTick(arg);
|
OS_ThreadTick(arg);
|
OS_SpinUnlock(state);
|
OS_SpinUnlock(state);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_Init(uint32 *heapStorage, uint32 bytes)
|
void OS_Init(uint32 *heapStorage, uint32 bytes)
|
{
|
{
|
int i;
|
int i;
|
if((int)OS_Init > 0x10000000) //Running from DDR?
|
if((int)OS_Init > 0x10000000) //Running from DDR?
|
OS_AsmInterruptInit(); //Patch interrupt vector
|
OS_AsmInterruptInit(); //Patch interrupt vector
|
OS_InterruptMaskClear(0xffffffff); //Disable interrupts
|
OS_InterruptMaskClear(0xffffffff); //Disable interrupts
|
HeapArray[0] = OS_HeapCreate("Default", heapStorage, bytes);
|
HeapArray[0] = OS_HeapCreate("Default", heapStorage, bytes);
|
HeapArray[1] = HeapArray[0];
|
HeapArray[1] = HeapArray[0];
|
SemaphoreSleep = OS_SemaphoreCreate("Sleep", 0);
|
SemaphoreSleep = OS_SemaphoreCreate("Sleep", 0);
|
SemaphoreRelease = OS_SemaphoreCreate("Release", 1);
|
SemaphoreRelease = OS_SemaphoreCreate("Release", 1);
|
SemaphoreLock = OS_SemaphoreCreate("Lock", 1);
|
SemaphoreLock = OS_SemaphoreCreate("Lock", 1);
|
for(i = 0; i < OS_CPU_COUNT; ++i)
|
for(i = 0; i < OS_CPU_COUNT; ++i)
|
OS_ThreadCreate("Idle", OS_IdleThread, NULL, 0, 256);
|
OS_ThreadCreate("Idle", OS_IdleThread, NULL, 0, 256);
|
#ifndef DISABLE_IRQ_SIM
|
#ifndef DISABLE_IRQ_SIM
|
if((OS_InterruptStatus() & (IRQ_COUNTER18 | IRQ_COUNTER18_NOT)) == 0)
|
if((OS_InterruptStatus() & (IRQ_COUNTER18 | IRQ_COUNTER18_NOT)) == 0)
|
{
|
{
|
//Detected that running in simulator so create SimIsr thread
|
//Detected that running in simulator so create SimIsr thread
|
UartPrintfCritical("SimIsr\n");
|
UartPrintfCritical("SimIsr\n");
|
OS_ThreadCreate("SimIsr", OS_IdleSimulateIsr, NULL, 1, 0);
|
OS_ThreadCreate("SimIsr", OS_IdleSimulateIsr, NULL, 1, 0);
|
}
|
}
|
#endif //DISABLE_IRQ_SIM
|
#endif //DISABLE_IRQ_SIM
|
//Plasma hardware dependent
|
//Plasma hardware dependent
|
OS_InterruptRegister(IRQ_COUNTER18 | IRQ_COUNTER18_NOT, OS_ThreadTickToggle);
|
OS_InterruptRegister(IRQ_COUNTER18 | IRQ_COUNTER18_NOT, OS_ThreadTickToggle);
|
OS_InterruptMaskSet(IRQ_COUNTER18 | IRQ_COUNTER18_NOT);
|
OS_InterruptMaskSet(IRQ_COUNTER18 | IRQ_COUNTER18_NOT);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_Start(void)
|
void OS_Start(void)
|
{
|
{
|
ThreadSwapEnabled = 1;
|
ThreadSwapEnabled = 1;
|
(void)OS_SpinLock();
|
(void)OS_SpinLock();
|
OS_ThreadReschedule(1);
|
OS_ThreadReschedule(1);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Place breakpoint here
|
//Place breakpoint here
|
void OS_Assert(void)
|
void OS_Assert(void)
|
{
|
{
|
}
|
}
|
|
|
|
|
#if OS_CPU_COUNT > 1
|
#if OS_CPU_COUNT > 1
|
static uint8 SpinLockArray[OS_CPU_COUNT];
|
static uint8 SpinLockArray[OS_CPU_COUNT];
|
/******************************************/
|
/******************************************/
|
uint32 OS_CpuIndex(void)
|
uint32 OS_CpuIndex(void)
|
{
|
{
|
return 0; //0 to OS_CPU_COUNT-1
|
return 0; //0 to OS_CPU_COUNT-1
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Symmetric Multiprocessing Spin Lock Mutex
|
//Symmetric Multiprocessing Spin Lock Mutex
|
uint32 OS_SpinLock(void)
|
uint32 OS_SpinLock(void)
|
{
|
{
|
uint32 state, cpuIndex, i, j, ok, delay;
|
uint32 state, cpuIndex, i, j, ok, delay;
|
|
|
cpuIndex = OS_CpuIndex();
|
cpuIndex = OS_CpuIndex();
|
delay = cpuIndex + 8;
|
delay = cpuIndex + 8;
|
state = OS_AsmInterruptEnable(0);
|
state = OS_AsmInterruptEnable(0);
|
do
|
do
|
{
|
{
|
ok = 1;
|
ok = 1;
|
if(++SpinLockArray[cpuIndex] == 1)
|
if(++SpinLockArray[cpuIndex] == 1)
|
{
|
{
|
for(i = 0; i < OS_CPU_COUNT; ++i)
|
for(i = 0; i < OS_CPU_COUNT; ++i)
|
{
|
{
|
if(i != cpuIndex && SpinLockArray[i])
|
if(i != cpuIndex && SpinLockArray[i])
|
ok = 0;
|
ok = 0;
|
}
|
}
|
if(ok == 0)
|
if(ok == 0)
|
{
|
{
|
SpinLockArray[cpuIndex] = 0;
|
SpinLockArray[cpuIndex] = 0;
|
for(j = 0; j < delay; ++j) //wait a bit
|
for(j = 0; j < delay; ++j) //wait a bit
|
++i;
|
++i;
|
if(delay < 128)
|
if(delay < 128)
|
delay <<= 1;
|
delay <<= 1;
|
}
|
}
|
}
|
}
|
} while(ok == 0);
|
} while(ok == 0);
|
return state;
|
return state;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_SpinUnlock(uint32 state)
|
void OS_SpinUnlock(uint32 state)
|
{
|
{
|
uint32 cpuIndex;
|
uint32 cpuIndex;
|
cpuIndex = OS_CpuIndex();
|
cpuIndex = OS_CpuIndex();
|
if(--SpinLockArray[cpuIndex] == 0)
|
if(--SpinLockArray[cpuIndex] == 0)
|
OS_AsmInterruptEnable(state);
|
OS_AsmInterruptEnable(state);
|
|
|
assert(SpinLockArray[cpuIndex] < 10);
|
assert(SpinLockArray[cpuIndex] < 10);
|
}
|
}
|
#endif //OS_CPU_COUNT > 1
|
#endif //OS_CPU_COUNT > 1
|
|
|
|
|
/************** WIN32/Linux Support *************/
|
/************** WIN32/Linux Support *************/
|
#ifdef WIN32
|
#ifdef WIN32
|
#ifdef LINUX
|
#ifdef LINUX
|
#define putch putchar
|
#define putch putchar
|
#undef _LIBC
|
#undef _LIBC
|
#undef kbhit
|
#undef kbhit
|
#undef getch
|
#undef getch
|
#define UartPrintf UartPrintf2
|
#define UartPrintf UartPrintf2
|
#define UartScanf UartScanf2
|
#define UartScanf UartScanf2
|
#include <stdio.h>
|
#include <stdio.h>
|
#include <stdlib.h>
|
#include <stdlib.h>
|
#include <termios.h>
|
#include <termios.h>
|
#include <unistd.h>
|
#include <unistd.h>
|
void Sleep(unsigned int value)
|
void Sleep(unsigned int value)
|
{
|
{
|
usleep(value * 1000);
|
usleep(value * 1000);
|
}
|
}
|
|
|
int kbhit(void)
|
int kbhit(void)
|
{
|
{
|
struct termios oldt, newt;
|
struct termios oldt, newt;
|
struct timeval tv;
|
struct timeval tv;
|
fd_set read_fd;
|
fd_set read_fd;
|
|
|
tcgetattr(STDIN_FILENO, &oldt);
|
tcgetattr(STDIN_FILENO, &oldt);
|
newt = oldt;
|
newt = oldt;
|
newt.c_lflag &= ~(ICANON | ECHO);
|
newt.c_lflag &= ~(ICANON | ECHO);
|
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
|
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
|
tv.tv_sec=0;
|
tv.tv_sec=0;
|
tv.tv_usec=0;
|
tv.tv_usec=0;
|
FD_ZERO(&read_fd);
|
FD_ZERO(&read_fd);
|
FD_SET(0,&read_fd);
|
FD_SET(0,&read_fd);
|
if(select(1, &read_fd, NULL, NULL, &tv) == -1)
|
if(select(1, &read_fd, NULL, NULL, &tv) == -1)
|
return 0;
|
return 0;
|
if(FD_ISSET(0,&read_fd))
|
if(FD_ISSET(0,&read_fd))
|
return 1;
|
return 1;
|
return 0;
|
return 0;
|
}
|
}
|
|
|
int getch(void)
|
int getch(void)
|
{
|
{
|
struct termios oldt, newt;
|
struct termios oldt, newt;
|
int ch;
|
int ch;
|
|
|
tcgetattr(STDIN_FILENO, &oldt);
|
tcgetattr(STDIN_FILENO, &oldt);
|
newt = oldt;
|
newt = oldt;
|
newt.c_lflag &= ~(ICANON | ECHO);
|
newt.c_lflag &= ~(ICANON | ECHO);
|
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
|
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
|
ch = getchar();
|
ch = getchar();
|
return ch;
|
return ch;
|
}
|
}
|
#else
|
#else
|
//Support RTOS inside Windows
|
//Support RTOS inside Windows
|
#undef kbhit
|
#undef kbhit
|
#undef getch
|
#undef getch
|
#undef putch
|
#undef putch
|
extern int kbhit();
|
extern int kbhit();
|
extern int getch(void);
|
extern int getch(void);
|
extern int putch(int);
|
extern int putch(int);
|
extern void __stdcall Sleep(unsigned long value);
|
extern void __stdcall Sleep(unsigned long value);
|
#endif
|
#endif
|
|
|
static uint32 Memory[8];
|
static uint32 Memory[8];
|
|
|
uint32 MemoryRead(uint32 address)
|
uint32 MemoryRead(uint32 address)
|
{
|
{
|
Memory[2] |= IRQ_UART_WRITE_AVAILABLE; //IRQ_STATUS
|
Memory[2] |= IRQ_UART_WRITE_AVAILABLE; //IRQ_STATUS
|
switch(address)
|
switch(address)
|
{
|
{
|
case UART_READ:
|
case UART_READ:
|
if(kbhit())
|
if(kbhit())
|
Memory[0] = getch(); //UART_READ
|
Memory[0] = getch(); //UART_READ
|
Memory[2] &= ~IRQ_UART_READ_AVAILABLE; //clear bit
|
Memory[2] &= ~IRQ_UART_READ_AVAILABLE; //clear bit
|
return Memory[0];
|
return Memory[0];
|
case IRQ_MASK:
|
case IRQ_MASK:
|
return Memory[1]; //IRQ_MASK
|
return Memory[1]; //IRQ_MASK
|
case IRQ_MASK + 4:
|
case IRQ_MASK + 4:
|
Sleep(10);
|
Sleep(10);
|
return 0;
|
return 0;
|
case IRQ_STATUS:
|
case IRQ_STATUS:
|
if(kbhit())
|
if(kbhit())
|
Memory[2] |= IRQ_UART_READ_AVAILABLE;
|
Memory[2] |= IRQ_UART_READ_AVAILABLE;
|
return Memory[2];
|
return Memory[2];
|
}
|
}
|
return 0;
|
return 0;
|
}
|
}
|
|
|
void MemoryWrite(uint32 address, uint32 value)
|
void MemoryWrite(uint32 address, uint32 value)
|
{
|
{
|
switch(address)
|
switch(address)
|
{
|
{
|
case UART_WRITE:
|
case UART_WRITE:
|
putch(value);
|
putch(value);
|
break;
|
break;
|
case IRQ_MASK:
|
case IRQ_MASK:
|
Memory[1] = value;
|
Memory[1] = value;
|
break;
|
break;
|
case IRQ_STATUS:
|
case IRQ_STATUS:
|
Memory[2] = value;
|
Memory[2] = value;
|
break;
|
break;
|
}
|
}
|
}
|
}
|
|
|
uint32 OS_AsmInterruptEnable(uint32 enableInterrupt)
|
uint32 OS_AsmInterruptEnable(uint32 enableInterrupt)
|
{
|
{
|
return enableInterrupt;
|
return enableInterrupt;
|
}
|
}
|
|
|
void OS_AsmInterruptInit(void)
|
void OS_AsmInterruptInit(void)
|
{
|
{
|
}
|
}
|
#endif //WIN32
|
#endif //WIN32
|
|
|
|
|
/**************** Example *****************/
|
/**************** Example *****************/
|
#ifndef NO_MAIN
|
#ifndef NO_MAIN
|
#ifdef WIN32
|
#ifdef WIN32
|
static uint8 HeapSpace[1024*512];
|
static uint8 HeapSpace[1024*512];
|
#endif
|
#endif
|
|
|
int main(int programEnd, char *argv[])
|
int main(int programEnd, char *argv[])
|
{
|
{
|
(void)programEnd; //Pointer to end of used memory
|
(void)programEnd; //Pointer to end of used memory
|
(void)argv;
|
(void)argv;
|
|
|
UartPrintfCritical("Starting RTOS\n");
|
UartPrintfCritical("Starting RTOS\n");
|
#ifdef WIN32
|
#ifdef WIN32
|
OS_Init((uint32*)HeapSpace, sizeof(HeapSpace));
|
OS_Init((uint32*)HeapSpace, sizeof(HeapSpace));
|
#else
|
#else
|
//Remaining space after program in 1MB external RAM
|
//Remaining space after program in 1MB external RAM
|
OS_Init((uint32*)programEnd,
|
OS_Init((uint32*)programEnd,
|
RAM_EXTERNAL_BASE + RAM_EXTERNAL_SIZE - programEnd);
|
RAM_EXTERNAL_BASE + RAM_EXTERNAL_SIZE - programEnd);
|
#endif
|
#endif
|
UartInit();
|
UartInit();
|
OS_ThreadCreate("Main", MainThread, NULL, 100, 4000);
|
OS_ThreadCreate("Main", MainThread, NULL, 100, 4000);
|
OS_Start();
|
OS_Start();
|
return 0;
|
return 0;
|
}
|
}
|
#endif //NO_MAIN
|
#endif //NO_MAIN
|
|
|
|
|
