OPENCORES.ORG

From: cvs at opencores.org<cvs@o...>
Date: Thu Sep 14 07:07:52 CEST 2006
Subject: [cvs-checkins] MODIFIED: mlite ...

Date: 00/06/09 14:07:07

Modified: mlite/kernel rtos.c Log: Improved multi-cpu support Revision Changes Path 1.6 mlite/kernel/rtos.c http://www.opencores.org/cvsweb.shtml/mlite/kernel/rtos.c.diff?r1=1.5&r2=1.6 (In the diff below, changes in quantity of whitespace are not shown.) Index: rtos.c =================================================================== RCS file: /cvsroot/rhoads/mlite/kernel/rtos.c,v retrieving revision 1.5 retrieving revision 1.6 diff -u -b -r1.5 -r1.6 --- rtos.c 11 Sep 2006 01:39:19 -0000 1.5 +++ rtos.c 14 Sep 2006 05:07:52 -0000 1.6 @@ -53,7 +53,7 @@ //typedef struct OS_Heap_s OS_Heap_t; typedef enum { - THREAD_BLOCKED = 0, + THREAD_PEND = 0, THREAD_READY = 1, THREAD_RUNNING = 2 } OS_ThreadState_e; @@ -304,7 +304,7 @@ thread->next->prev = thread->prev; thread->next = NULL; thread->prev = NULL; - thread->state = THREAD_BLOCKED; + thread->state = THREAD_PEND; } @@ -361,13 +361,13 @@ } +#if OS_CPU_COUNT <= 1 /******************************************/ //Loads a new thread //Must be called with interrupts disabled static void OS_ThreadReschedule(int RoundRobin) { - OS_Thread_t *threadNext, *threadCurrent, *threadBest, *threadTry; - uint32 cpuIndex = OS_CpuIndex(); + OS_Thread_t *threadNext, *threadCurrent, *threadTry; int rc; if(ThreadSwapEnabled == 0 || InterruptInside) @@ -377,30 +377,133 @@ } //Determine which thread should run - threadCurrent = ThreadCurrent[cpuIndex]; + threadCurrent = ThreadCurrent[0]; threadNext = threadCurrent; + if(threadCurrent == NULL || threadCurrent->state == THREAD_PEND) + threadNext = ThreadHead; + else if(threadCurrent->priority < ThreadHead->priority) + threadNext = ThreadHead; + else if(RoundRobin) + { + //Determine next ready thread with same priority + threadTry = threadCurrent->next; + if(threadTry && threadTry->priority == threadCurrent->priority) + threadNext = threadTry; + else + threadNext = ThreadHead; + } + + if(threadNext != threadCurrent) + { + //Swap threads + ThreadCurrent[0] = threadNext; + assert(threadNext); + if(threadCurrent) + { + assert(threadCurrent->magic[0] == THREAD_MAGIC); //check stack overflow + //printf("OS_ThreadRescheduleSave(%s)\n", threadPrev->name); + rc = setjmp(threadCurrent->env); //ANSI C call to save registers + if(rc) + { + //Returned from longjmp() + return; + } + } + + threadNext = ThreadCurrent[0]; //removed warning + //printf("OS_ThreadRescheduleRestore(%s)\n", threadNext->name); + longjmp(threadNext->env, 1); //ANSI C call to restore registers + } +} + +#else //#if OS_CPU_COUNT <= 1 + +/******************************************/ +//Loads a new thread in a multiprocessor environment +//Must be called with interrupts disabled +static void OS_ThreadReschedule(int RoundRobin) +{ + OS_Thread_t *threadNext, *threadCurrent, *threadBest, *threadAlt; + uint32 i, cpuLow, priorityLow, cpuIndex = OS_CpuIndex(); + int rc; + static volatile int CpuReschedule; //remove warning + + if(ThreadSwapEnabled == 0 || InterruptInside) + { + ThreadNeedReschedule |= 2 + RoundRobin; + return; + } + + //Find which CPU is running the lowest priority thread + CpuReschedule = -1; + cpuLow = 0; + priorityLow = 0xffffffff; + for(i = 0; i < OS_CPU_COUNT; ++i) + { + if(i != cpuIndex && (ThreadCurrent[i] == NULL || + ThreadCurrent[i]->priority < priorityLow)) + { + cpuLow = i; + if(ThreadCurrent[i]) + priorityLow = ThreadCurrent[i]->priority; + else + priorityLow = 0; + } + } + + //Determine highest priority ready threads for(threadBest = ThreadHead; threadBest; threadBest = threadBest->next) { if(threadBest->state == THREAD_READY) break; } - if(threadBest == NULL && threadCurrent && threadCurrent->state == THREAD_RUNNING) - threadBest = threadCurrent; - assert(threadBest); - if(threadCurrent == NULL || threadCurrent->state == THREAD_BLOCKED) + threadAlt = NULL; + if(threadBest) + { + for(threadAlt = threadBest->next; threadAlt; threadAlt = threadAlt->next) + { + if(threadAlt->state == THREAD_READY) + break; + } + } + + //Determine if a CPU needs to reschedule + if(threadAlt && threadAlt->priority > priorityLow) + CpuReschedule = cpuLow; + + //Determine which thread should run + threadCurrent = ThreadCurrent[cpuIndex]; + threadNext = threadCurrent; + if(threadCurrent == NULL || threadCurrent->state == THREAD_PEND) + { threadNext = threadBest; - else if(threadCurrent->priority < threadBest->priority) + } + else if(threadBest && threadCurrent->priority < threadBest->priority) + { threadNext = threadBest; + + if(threadAlt == NULL || threadCurrent->priority > threadAlt->priority) + { + CpuReschedule = cpuLow; + + //Only enable this if there really are multiple CPUs + //Try to minimize thread swapping by detecting if the other + //CPU would just start running threadCurrent +// if(threadCurrent->priority > priorityLow) +// threadNext = threadCurrent; + } + } else if(RoundRobin) { - for(threadTry = threadCurrent->next; threadTry; threadTry = threadTry->next) + //Find the next ready thread + for(threadAlt = threadCurrent->next; threadAlt; threadAlt = threadAlt->next) { - if(threadTry->state == THREAD_READY) + if(threadAlt->state == THREAD_READY) break; } - if(threadTry && threadTry->priority == threadCurrent->priority) - threadNext = threadTry; - else if(threadBest->priority == threadCurrent->priority) + if(threadAlt && threadAlt->priority == threadCurrent->priority) + threadNext = threadAlt; + else if(threadBest && threadBest->priority >= threadCurrent->priority) threadNext = threadBest; } @@ -420,19 +523,29 @@ if(rc) { //Returned from longjmp() + if(CpuReschedule >= 0) + OS_CpuInterrupt(CpuReschedule, 1); return; } } + //Restore spin lock count cpuIndex = OS_CpuIndex(); //removed warning threadNext = ThreadCurrent[cpuIndex]; //removed warning - //printf("OS_ThreadRescheduleRestore(%s)\n", threadNext->name); threadNext->state = THREAD_RUNNING; OS_SpinLockSet(threadNext->spinLocks); + + //printf("OS_ThreadRescheduleRestore(%s)\n", threadNext->name); longjmp(threadNext->env, 1); //ANSI C call to restore registers } + + //Check if a different CPU needs to swap threads + if(CpuReschedule >= 0) + OS_CpuInterrupt(CpuReschedule, 1); } +#endif //#if OS_CPU_COUNT <= 1 + /******************************************/ static void OS_ThreadInit(void *Arg) @@ -578,7 +691,7 @@ uint32 state; state = OS_CriticalBegin(); Thread->priority = Priority; - if(Thread->state != THREAD_BLOCKED) + if(Thread->state != THREAD_PEND) { OS_ThreadPriorityRemove(&ThreadHead, Thread); OS_ThreadPriorityInsert(&ThreadHead, Thread); @@ -1257,6 +1370,25 @@ SpinLockArray[cpuIndex] = (uint8)count; assert(count); } + + +/******************************************/ +void OS_CpuInterrupt(uint32 cpuIndex, uint32 bitfield) +{ + (void)cpuIndex; + (void)bitfield; +} + + +/******************************************/ +void OS_CpuInterruptServiceRoutine(void *Arg) +{ + uint32 state; + (void)Arg; + state = OS_SpinLock(); + OS_ThreadReschedule(0); + OS_SpinUnlock(state); +} #endif