| Line 50... |
Line 50... |
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 {
|
|
THREAD_BLOCKED = 0,
|
|
THREAD_READY = 1,
|
|
THREAD_RUNNING = 2
|
|
} OS_ThreadState_e;
|
|
|
struct OS_Thread_s {
|
struct OS_Thread_s {
|
const char *name;
|
const char *name;
|
|
OS_ThreadState_e state;
|
jmp_buf env;
|
jmp_buf env;
|
OS_FuncPtr_t funcPtr;
|
OS_FuncPtr_t funcPtr;
|
void *arg;
|
void *arg;
|
uint32 priority;
|
uint32 priority;
|
uint32 ticksTimeout;
|
uint32 ticksTimeout;
|
| Line 105... |
Line 112... |
/*************** Globals ******************/
|
/*************** Globals ******************/
|
static OS_Heap_t *HeapArray[HEAP_COUNT];
|
static OS_Heap_t *HeapArray[HEAP_COUNT];
|
static OS_Semaphore_t *SemaphoreSleep;
|
static OS_Semaphore_t *SemaphoreSleep;
|
static OS_Semaphore_t *SemaphoreLock;
|
static OS_Semaphore_t *SemaphoreLock;
|
static int ThreadSwapEnabled;
|
static int ThreadSwapEnabled;
|
static int ThreadCurrentActive[OS_CPU_COUNT];
|
|
static int ThreadNeedReschedule;
|
static int ThreadNeedReschedule;
|
static uint32 ThreadTime;
|
static uint32 ThreadTime;
|
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 OS_Thread_t *ThreadCurrent[OS_CPU_COUNT];
|
static OS_Thread_t *ThreadCurrent[OS_CPU_COUNT];
|
| Line 278... |
Line 284... |
thread->next = prev->next;
|
thread->next = prev->next;
|
thread->prev = prev;
|
thread->prev = prev;
|
prev->next = thread;
|
prev->next = thread;
|
}
|
}
|
assert(ThreadHead);
|
assert(ThreadHead);
|
|
if(*head == ThreadHead)
|
|
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)
|
{
|
{
|
uint32 cpuIndex = OS_CpuIndex();
|
|
//printf("r(%d) ", thread->priority);
|
|
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;
|
if(head == &ThreadHead && ThreadCurrent[cpuIndex] == thread)
|
thread->state = THREAD_BLOCKED;
|
ThreadCurrentActive[cpuIndex] = 0;
|
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Linked list of threads sorted by timeout value
|
//Linked list of threads sorted by timeout value
|
| Line 359... |
Line 364... |
/******************************************/
|
/******************************************/
|
//Loads a new thread
|
//Loads a new thread
|
//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, *threadPrev, *threadCurrent;
|
OS_Thread_t *threadNext, *threadCurrent, *threadBest, *threadTry;
|
uint32 cpuIndex = OS_CpuIndex();
|
uint32 cpuIndex = OS_CpuIndex();
|
int rc;
|
int rc;
|
|
|
if(ThreadSwapEnabled == 0 || InterruptInside)
|
if(ThreadSwapEnabled == 0 || InterruptInside)
|
{
|
{
|
| Line 372... |
Line 377... |
}
|
}
|
|
|
//Determine which thread should run
|
//Determine which thread should run
|
threadCurrent = ThreadCurrent[cpuIndex];
|
threadCurrent = ThreadCurrent[cpuIndex];
|
threadNext = threadCurrent;
|
threadNext = threadCurrent;
|
assert(ThreadHead);
|
for(threadBest = ThreadHead; threadBest; threadBest = threadBest->next)
|
if(ThreadCurrentActive[cpuIndex] == 0 || threadCurrent == NULL)
|
{
|
threadNext = ThreadHead;
|
if(threadBest->state == THREAD_READY)
|
else if(threadCurrent->priority < ThreadHead->priority)
|
break;
|
threadNext = ThreadHead;
|
}
|
|
if(threadBest == NULL && threadCurrent && threadCurrent->state == THREAD_RUNNING)
|
|
threadBest = threadCurrent;
|
|
assert(threadBest);
|
|
if(threadCurrent == NULL || threadCurrent->state == THREAD_BLOCKED)
|
|
threadNext = threadBest;
|
|
else if(threadCurrent->priority < threadBest->priority)
|
|
threadNext = threadBest;
|
else if(RoundRobin)
|
else if(RoundRobin)
|
{
|
{
|
if(threadCurrent->next &&
|
for(threadTry = threadCurrent->next; threadTry; threadTry = threadTry->next)
|
threadCurrent->next->priority == ThreadHead->priority)
|
{
|
threadNext = threadCurrent->next;
|
if(threadTry->state == THREAD_READY)
|
else
|
break;
|
threadNext = ThreadHead;
|
}
|
|
if(threadTry && threadTry->priority == threadCurrent->priority)
|
|
threadNext = threadTry;
|
|
else if(threadBest->priority == threadCurrent->priority)
|
|
threadNext = threadBest;
|
}
|
}
|
|
|
if(threadNext != threadCurrent)
|
if(threadNext != threadCurrent)
|
{
|
{
|
//Swap threads
|
//Swap threads
|
threadPrev = threadCurrent;
|
|
ThreadCurrent[cpuIndex] = threadNext;
|
ThreadCurrent[cpuIndex] = threadNext;
|
assert(threadNext);
|
assert(threadNext);
|
if(threadPrev)
|
if(threadCurrent)
|
{
|
{
|
assert(threadPrev->magic[0] == THREAD_MAGIC); //check stack overflow
|
assert(threadCurrent->magic[0] == THREAD_MAGIC); //check stack overflow
|
threadPrev->spinLocks = OS_SpinLockGet();
|
threadCurrent->state = THREAD_READY;
|
|
threadCurrent->spinLocks = OS_SpinLockGet();
|
|
|
//printf("OS_ThreadRescheduleSave(%s)\n", threadPrev->name);
|
//printf("OS_ThreadRescheduleSave(%s)\n", threadPrev->name);
|
rc = setjmp(threadPrev->env); //ANSI C call to save registers
|
rc = setjmp(threadCurrent->env); //ANSI C call to save registers
|
if(rc)
|
if(rc)
|
{
|
{
|
//Returned from longjmp()
|
//Returned from longjmp()
|
return;
|
return;
|
}
|
}
|
}
|
}
|
|
|
cpuIndex = OS_CpuIndex(); //removed warning
|
cpuIndex = OS_CpuIndex(); //removed warning
|
threadNext = ThreadCurrent[cpuIndex]; //removed warning
|
threadNext = ThreadCurrent[cpuIndex]; //removed warning
|
ThreadCurrentActive[cpuIndex] = 1;
|
|
//printf("OS_ThreadRescheduleRestore(%s)\n", threadNext->name);
|
//printf("OS_ThreadRescheduleRestore(%s)\n", threadNext->name);
|
|
threadNext->state = THREAD_RUNNING;
|
OS_SpinLockSet(threadNext->spinLocks);
|
OS_SpinLockSet(threadNext->spinLocks);
|
longjmp(threadNext->env, 1); //ANSI C call to restore registers
|
longjmp(threadNext->env, 1); //ANSI C call to restore registers
|
}
|
}
|
}
|
}
|
|
|
| Line 420... |
Line 438... |
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_AsmInterruptEnable(1);
|
OS_CriticalEnd(1);
|
ThreadCurrent[cpuIndex]->funcPtr(ThreadCurrent[cpuIndex]->arg);
|
ThreadCurrent[cpuIndex]->funcPtr(ThreadCurrent[cpuIndex]->arg);
|
OS_ThreadExit();
|
OS_ThreadExit();
|
}
|
}
|
|
|
|
|
| Line 464... |
Line 482... |
return NULL;
|
return NULL;
|
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->funcPtr = FuncPtr;
|
thread->funcPtr = FuncPtr;
|
thread->arg = Arg;
|
thread->arg = Arg;
|
thread->priority = Priority;
|
thread->priority = Priority;
|
thread->info = NULL;
|
thread->info = NULL;
|
thread->semaphorePending = NULL;
|
thread->semaphorePending = NULL;
|
| Line 556... |
Line 575... |
/******************************************/
|
/******************************************/
|
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();
|
OS_ThreadPriorityRemove(&ThreadHead, Thread);
|
|
Thread->priority = Priority;
|
Thread->priority = Priority;
|
|
if(Thread->state != THREAD_BLOCKED)
|
|
{
|
|
OS_ThreadPriorityRemove(&ThreadHead, Thread);
|
OS_ThreadPriorityInsert(&ThreadHead, Thread);
|
OS_ThreadPriorityInsert(&ThreadHead, Thread);
|
OS_ThreadReschedule(0);
|
OS_ThreadReschedule(0);
|
|
}
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
| Line 631... |
Line 653... |
{
|
{
|
uint32 state, cpuIndex;
|
uint32 state, cpuIndex;
|
OS_Thread_t *thread;
|
OS_Thread_t *thread;
|
int returnCode=0;
|
int returnCode=0;
|
|
|
|
//assert(OS_SpinLockGet() == 0);
|
assert(Semaphore);
|
assert(Semaphore);
|
assert(InterruptInside == 0);
|
assert(InterruptInside == 0);
|
state = OS_CriticalBegin();
|
state = OS_CriticalBegin();
|
if(--Semaphore->count < 0)
|
if(--Semaphore->count < 0)
|
{
|
{
|
| Line 651... |
Line 674... |
thread->ticksTimeout = Ticks + OS_ThreadTime();
|
thread->ticksTimeout = Ticks + OS_ThreadTime();
|
OS_ThreadPriorityRemove(&ThreadHead, thread);
|
OS_ThreadPriorityRemove(&ThreadHead, thread);
|
OS_ThreadPriorityInsert(&Semaphore->threadHead, thread);
|
OS_ThreadPriorityInsert(&Semaphore->threadHead, thread);
|
if(Ticks != OS_WAIT_FOREVER)
|
if(Ticks != OS_WAIT_FOREVER)
|
OS_ThreadTimeoutInsert(thread);
|
OS_ThreadTimeoutInsert(thread);
|
ThreadCurrentActive[cpuIndex] = 0;
|
|
assert(ThreadHead);
|
assert(ThreadHead);
|
OS_ThreadReschedule(0);
|
OS_ThreadReschedule(0);
|
returnCode = thread->returnCode;
|
returnCode = thread->returnCode;
|
}
|
}
|
OS_CriticalEnd(state);
|
OS_CriticalEnd(state);
|
|
//assert(OS_SpinLockGet() == 0);
|
return returnCode;
|
return returnCode;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
| Line 1117... |
Line 1140... |
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_Init(uint32 *HeapStorage, uint32 Bytes)
|
void OS_Init(uint32 *HeapStorage, uint32 Bytes)
|
{
|
{
|
|
int i;
|
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);
|
SemaphoreSleep = OS_SemaphoreCreate("Sleep", 0);
|
SemaphoreSleep = OS_SemaphoreCreate("Sleep", 0);
|
SemaphoreLock = OS_SemaphoreCreate("Lock", 1);
|
SemaphoreLock = OS_SemaphoreCreate("Lock", 1);
|
|
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
|
| Line 1167... |
Line 1192... |
|
|
/******************************************/
|
/******************************************/
|
//Symmetric Multiprocessing Spin Lock Mutex
|
//Symmetric Multiprocessing Spin Lock Mutex
|
uint32 OS_SpinLock(void)
|
uint32 OS_SpinLock(void)
|
{
|
{
|
uint32 state, cpu, i, j, ok;
|
uint32 state, cpuIndex, i, j, ok, delay;
|
|
|
cpu = OS_CpuIndex();
|
cpuIndex = OS_CpuIndex();
|
|
delay = cpuIndex + 8;
|
state = OS_AsmInterruptEnable(0);
|
state = OS_AsmInterruptEnable(0);
|
do
|
do
|
{
|
{
|
ok = 1;
|
ok = 1;
|
if(++SpinLockArray[cpu] == 1)
|
if(++SpinLockArray[cpuIndex] == 1)
|
{
|
{
|
for(i = 0; i < OS_CPU_COUNT; ++i)
|
for(i = 0; i < OS_CPU_COUNT; ++i)
|
{
|
{
|
if(i != cpu && SpinLockArray[i])
|
if(i != cpuIndex && SpinLockArray[i])
|
ok = 0;
|
ok = 0;
|
}
|
}
|
if(ok == 0)
|
if(ok == 0)
|
{
|
{
|
SpinLockArray[cpu] = 0;
|
SpinLockArray[cpuIndex] = 0;
|
for(j = 0; j < 100*cpu; ++j) //wait a bit
|
for(j = 0; j < delay; ++j) //wait a bit
|
++i;
|
++i;
|
|
if(delay < 128)
|
|
delay <<= 1;
|
}
|
}
|
}
|
}
|
} while(ok == 0);
|
} while(ok == 0);
|
return state;
|
return state;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
void OS_SpinUnlock(uint32 state)
|
void OS_SpinUnlock(uint32 state)
|
{
|
{
|
uint32 cpu;
|
uint32 cpuIndex;
|
cpu = OS_CpuIndex();
|
cpuIndex = OS_CpuIndex();
|
if(--SpinLockArray[cpu] == 0)
|
if(--SpinLockArray[cpuIndex] == 0)
|
OS_AsmInterruptEnable(state);
|
OS_AsmInterruptEnable(state);
|
|
|
|
assert(SpinLockArray[cpuIndex] < 10);
|
|
assert(!(state && SpinLockArray[cpuIndex]));
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Must be called with interrupts disabled and spin locked
|
//Must be called with interrupts disabled and spin locked
|
uint32 OS_SpinLockGet(void)
|
uint32 OS_SpinLockGet(void)
|
{
|
{
|
uint32 cpu, count;
|
uint32 cpuIndex, count;
|
cpu = OS_CpuIndex();
|
cpuIndex = OS_CpuIndex();
|
count = SpinLockArray[cpu];
|
count = SpinLockArray[cpuIndex];
|
|
//assert(count);
|
return count;
|
return count;
|
}
|
}
|
|
|
|
|
/******************************************/
|
/******************************************/
|
//Must be called with interrupts disabled and spin locked
|
//Must be called with interrupts disabled and spin locked
|
void OS_SpinLockSet(uint32 count)
|
void OS_SpinLockSet(uint32 count)
|
{
|
{
|
uint32 cpu;
|
uint32 cpuIndex;
|
cpu = OS_CpuIndex();
|
cpuIndex = OS_CpuIndex();
|
SpinLockArray[cpu] = (uint8)count;
|
SpinLockArray[cpuIndex] = (uint8)count;
|
|
assert(count);
|
}
|
}
|
#endif
|
#endif
|
|
|
|
|
/************** WIN32 Support *************/
|
/************** WIN32 Support *************/
|
