| Line 21... |
Line 21... |
#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
|
|
|
|
#define PRINTF_DEBUG(STRING, A, B)
|
|
//#define PRINTF_DEBUG(STRING, A, B) printf(STRING, A, B)
|
|
|
/*************** Structures ***************/
|
/*************** Structures ***************/
|
#ifdef WIN32
|
#ifdef WIN32
|
#define setjmp _setjmp
|
#define setjmp _setjmp
|
//x86 registers
|
//x86 registers
|
| Line 96... |
Line 98... |
//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;
|
|
uint32 priorityRestore;
|
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 {
|
| Line 420... |
Line 423... |
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);
|
//printf("Pause(%d,%s) ", OS_CpuIndex(), threadCurrent->name);
|
PRINTF_DEBUG("Pause(%d,%s) ", OS_CpuIndex(), threadCurrent->name);
|
rc = setjmp(threadCurrent->env); //ANSI C call to save registers
|
rc = setjmp(threadCurrent->env); //ANSI C call to save registers
|
if(rc)
|
if(rc)
|
{
|
{
|
//threadCurrent = ThreadCurrent[OS_CpuIndex()];
|
PRINTF_DEBUG("Resume(%d,%s) ", OS_CpuIndex(),
|
//printf("Resume(%d,%s) ", OS_CpuIndex(), threadCurrent->name);
|
ThreadCurrent[OS_CpuIndex()]->name);
|
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
|
| Line 460... |
Line 463... |
static void OS_ThreadInit(void *arg)
|
static void OS_ThreadInit(void *arg)
|
{
|
{
|
uint32 cpuIndex = OS_CpuIndex();
|
uint32 cpuIndex = OS_CpuIndex();
|
(void)arg;
|
(void)arg;
|
|
|
|
PRINTF_DEBUG("Starting(%d,%s) ", cpuIndex, OS_ThreadSelf()->name);
|
OS_CriticalEnd(1);
|
OS_CriticalEnd(1);
|
ThreadCurrent[cpuIndex]->funcPtr(ThreadCurrent[cpuIndex]->arg);
|
ThreadCurrent[cpuIndex]->funcPtr(ThreadCurrent[cpuIndex]->arg);
|
OS_ThreadExit();
|
OS_ThreadExit();
|
}
|
}
|
|
|
| Line 724... |
Line 728... |
{
|
{
|
uint32 state, cpuIndex;
|
uint32 state, cpuIndex;
|
OS_Thread_t *thread;
|
OS_Thread_t *thread;
|
int returnCode=0;
|
int returnCode=0;
|
|
|
|
PRINTF_DEBUG("SemPend(%d,%s) ", OS_CpuIndex(), semaphore->name);
|
assert(semaphore);
|
assert(semaphore);
|
assert(InterruptInside[OS_CpuIndex()] == 0);
|
assert(InterruptInside[OS_CpuIndex()] == 0);
|
state = OS_CriticalBegin(); //Disable interrupts
|
state = OS_CriticalBegin(); //Disable interrupts
|
if(--semaphore->count < 0)
|
if(--semaphore->count < 0)
|
{
|
{
|
| Line 766... |
Line 771... |
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;
|
|
|
|
PRINTF_DEBUG("SemPost(%d,%s) ", OS_CpuIndex(), semaphore->name);
|
assert(semaphore);
|
assert(semaphore);
|
state = OS_CriticalBegin();
|
state = OS_CriticalBegin();
|
if(++semaphore->count <= 0)
|
if(++semaphore->count <= 0)
|
{
|
{
|
//Wake up a thread that was waiting for this semaphore
|
//Wake up a thread that was waiting for this semaphore
|
| Line 815... |
Line 821... |
|
|
/******************************************/
|
/******************************************/
|
void OS_MutexPend(OS_Mutex_t *mutex)
|
void OS_MutexPend(OS_Mutex_t *mutex)
|
{
|
{
|
OS_Thread_t *thread;
|
OS_Thread_t *thread;
|
|
uint32 state;
|
|
|
assert(mutex);
|
assert(mutex);
|
thread = OS_ThreadSelf();
|
thread = OS_ThreadSelf();
|
if(thread == mutex->thread)
|
if(thread == mutex->thread)
|
{
|
{
|
++mutex->count;
|
++mutex->count;
|
return;
|
return;
|
}
|
}
|
|
|
|
state = OS_CriticalBegin();
|
|
//Priority inheritance to prevent priority inversion
|
|
if(mutex->thread && mutex->thread->priority < thread->priority)
|
|
OS_ThreadPrioritySet(mutex->thread, thread->priority);
|
|
|
OS_SemaphorePend(mutex->semaphore, OS_WAIT_FOREVER);
|
OS_SemaphorePend(mutex->semaphore, OS_WAIT_FOREVER);
|
|
mutex->priorityRestore = thread->priority;
|
mutex->thread = thread;
|
mutex->thread = thread;
|
mutex->count = 1;
|
mutex->count = 1;
|
|
OS_CriticalEnd(state);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_MutexPost(OS_Mutex_t *mutex)
|
void OS_MutexPost(OS_Mutex_t *mutex)
|
{
|
{
|
|
OS_Thread_t *thread = OS_ThreadSelf();
|
|
uint32 state, priorityRestore;
|
|
|
assert(mutex);
|
assert(mutex);
|
assert(mutex->thread == OS_ThreadSelf());
|
assert(mutex->thread == thread);
|
assert(mutex->count > 0);
|
assert(mutex->count > 0);
|
if(--mutex->count <= 0)
|
if(--mutex->count <= 0)
|
{
|
{
|
|
state = OS_CriticalBegin();
|
mutex->thread = NULL;
|
mutex->thread = NULL;
|
|
priorityRestore = mutex->priorityRestore;
|
OS_SemaphorePost(mutex->semaphore);
|
OS_SemaphorePost(mutex->semaphore);
|
|
if(priorityRestore < thread->priority)
|
|
OS_ThreadPrioritySet(thread, priorityRestore);
|
|
OS_CriticalEnd(state);
|
}
|
}
|
}
|
}
|
|
|
|
|
|
|
/***************** MQueue *****************/
|
/***************** MQueue *****************/
|
/******************************************/
|
/******************************************/
|
//Create a message queue
|
//Create a message queue
|
OS_MQueue_t *OS_MQueueCreate(const char *name,
|
OS_MQueue_t *OS_MQueueCreate(const char *name,
|
int messageCount,
|
int messageCount,
|
| Line 1231... |
Line 1253... |
|
|
|
|
/**************** Init ********************/
|
/**************** Init ********************/
|
/******************************************/
|
/******************************************/
|
//If there aren't any other ready to run threads then spin here
|
//If there aren't any other ready to run threads then spin here
|
static volatile uint32 IdleCount;
|
|
static int SimulateIsr;
|
static int SimulateIsr;
|
static void OS_IdleThread(void *arg)
|
static void OS_IdleThread(void *arg)
|
{
|
{
|
|
uint32 IdleCount=0;
|
(void)arg;
|
(void)arg;
|
|
|
//Don't block in the idle thread!
|
//Don't block in the idle thread!
|
for(;;)
|
for(;;)
|
{
|
{
|
| Line 1261... |
Line 1283... |
#if OS_CPU_COUNT > 1
|
#if OS_CPU_COUNT > 1
|
if((int)arg < OS_CPU_COUNT - 1)
|
if((int)arg < OS_CPU_COUNT - 1)
|
{
|
{
|
unsigned int state;
|
unsigned int state;
|
#ifndef WIN32
|
#ifndef WIN32
|
for(IdleCount = 0; IdleCount < 100000; ++IdleCount) ;
|
for(IdleCount = 0; IdleCount < 25000; ++IdleCount) ;
|
#endif
|
#endif
|
state = OS_SpinLock();
|
state = OS_SpinLock();
|
OS_ThreadReschedule(1);
|
OS_ThreadReschedule(1);
|
OS_SpinUnlock(state);
|
OS_SpinUnlock(state);
|
}
|
}
|
| Line 1298... |
Line 1320... |
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("Heap", 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);
|
if((MemoryRead(IRQ_STATUS) & (IRQ_COUNTER18 | IRQ_COUNTER18_NOT)) == 0)
|
if((MemoryRead(IRQ_STATUS) & (IRQ_COUNTER18 | IRQ_COUNTER18_NOT)) == 0)
|
