void ttGive(const char *nameOfSemaphore) { Semaphore* sem; UserTask* task; DataNode* dn = getNode(nameOfSemaphore, rtsys->semaphoreList); if (dn == NULL) { // Semaphore does not exist char buf[200]; sprintf(buf, "ttGive: Non-existent semaphore '%s'!", nameOfSemaphore); TT_MEX_ERROR(buf); return; } task = (UserTask*) rtsys->running; sem = (Semaphore*) dn->data; if (sem->value >= sem->maxval) { // Check if maximum already reached return; // In that case, do nothing } sem->value++; if (sem->value <= 0) { // This test is probably unnecessary // Move first waiting task to readyQ task = (UserTask*) sem->waitingQ->getFirst(); if (task != NULL) { task->moveToList(rtsys->readyQs[task->affinity]); task->state = READY; } } }
void ttTake(char *nameOfSemaphore) { Semaphore* sem; UserTask* task; DataNode* dn = getNode(nameOfSemaphore, rtsys->semaphoreList); if (dn == NULL) { // Semaphore does not exist char buf[200]; sprintf(buf, "ttTake: Non-existent semaphore '%s'!", nameOfSemaphore); MEX_ERROR(buf); return; } task = (UserTask*) rtsys->running; sem = (Semaphore*) dn->data; sem->value--; if (sem->value < 0) { // Not free task->moveToList(sem->waitingQ); task->state = WAITING; // Execute suspend hook task->suspend_hook(task); } }
// TT dispatcher handler -- called at the Time-Triggered slot boundaries double ttDispatcherCodeFcn(int segment, void *data) { char buf[200]; Task *task, *next; UserTask *userTask; TimeTriggeredDispatcher *dispatcher = (TimeTriggeredDispatcher*) data; int slotIndex=((int)((float)(rtsys->time/dispatcher->slotLength)) % dispatcher->schedulingTable.size()); // Remove all tasks from readyQ belonging to TTDispatcher and move to tmpQ task = (Task*) rtsys->readyQs[dispatcher->affinity]->getFirst(); while (task != NULL) { next = (Task*) task->getNext(); if (task->isUserTask()) { userTask = (UserTask*) task; if (userTask->ttdisp == dispatcher) { task->moveToList(dispatcher->tmpQ); } } task = next; } if (dispatcher->schedulingTable[slotIndex] > 0) { // Select the task to execute bool taskMoved = false; task = (Task*) dispatcher->tmpQ->getFirst(); while (task != NULL) { next = (Task*) task->getNext(); userTask = (UserTask*) task; // Select one of the tasks to execute if (userTask->taskIdentifier == dispatcher->schedulingTable[slotIndex]) // Current slot is for selected task { userTask->moveToList(rtsys->readyQs[dispatcher->affinity]); taskMoved = true; //OUT("Task %s moved to readyQ\n", userTask->name); break; } task = next; } // If job is not yet released, notify user if (!taskMoved) { sprintf(buf, "disp(\'@time %f: TTDispatcher Info: Task with identifier %d is not ready to execute\')", rtsys->time, dispatcher->schedulingTable[slotIndex]); mexEvalString(buf); } } // Schedule dispatcher timer for next slot dispatcher->slotTimer->time = rtsys->time + dispatcher->slotLength; dispatcher->slotTimer->moveToList(rtsys->timeQ); return FINISHED; }
void ttExitMonitor(const char *nameOfMonitor) { Monitor* mon; UserTask* task; DataNode* dn = getNode(nameOfMonitor, rtsys->monitorList); if (dn == NULL) { // Monitor does not exist char buf[200]; sprintf(buf, "ttExitMonitor: Non-existent monitor '%s'!", nameOfMonitor); TT_MEX_ERROR(buf); return; } task = (UserTask*) rtsys->running; mon = (Monitor*) dn->data; if (mon->heldBy != task) { char buf[200]; sprintf(buf, "ttExitMonitor: Task '%s' not holding monitor '%s'!", task->name, nameOfMonitor); TT_MEX_ERROR(buf); return; } // Priority Inheritance, reset task->tempPrio = 0.0; task->prioRaised = false; // Reshuffle readyQ if (task->myList == rtsys->readyQs[rtsys->currentCPU]) { task->moveToList(rtsys->readyQs[rtsys->currentCPU]); task->state = READY; } mon->heldBy = NULL; // Move first waiting task to readyQ task = (UserTask*) mon->waitingQ->getFirst(); if (task != NULL) { task->moveToList(rtsys->readyQs[task->affinity]); task->state = READY; mon->heldBy = task; } }
void ttPost(const char* mailbox, void* msg) { DataNode* dn = getNode(mailbox, rtsys->mailboxList); if (dn == NULL) { char buf[200]; sprintf(buf, "ttPost: Non-existent mailbox '%s'!", mailbox); TT_MEX_ERROR(buf); return; } Mailbox* m = (Mailbox*) dn->data; UserTask* task = (UserTask*) rtsys->running; if (m->count == m->maxSize) { //mexPrintf("ttPost: Mailbox '%s' is full, blocking\n", mailbox); // block the posting task task->moveToList(m->writerQ); task->state = WAITING; // Execute suspend hook task->suspend_hook(task); // Store the msg pointer in the task struct task->mb_data = msg; } else { m->buffer->appendNode(new DataNode(msg, NULL)); m->count++; // number of messages // release first waiting reader, if any task = (UserTask*) m->readerQ->getFirst(); if (task != NULL) { dn = (DataNode*) m->buffer->getFirst(); task->mb_data = (void*)dn->data; // save it for later (ttRetrieve) m->buffer->deleteNode(dn); m->count--; task->moveToList(rtsys->readyQs[task->affinity]); task->state = READY; } } }
void invoke_task(Task *task, const char *invoker) { debugPrintf("'%s': task_invoke('%s','%s') at %f\n", rtsys->blockName, task->name, invoker, rtsys->time); if (task->isUserTask()) { UserTask *usertask = (UserTask*)task; usertask->arrival_hook(usertask); if (usertask->nbrInvocations == 0) { usertask->arrival = rtsys->time; usertask->release = rtsys->time; usertask->release_hook(usertask); usertask->moveToList(rtsys->readyQs[usertask->affinity]); usertask->state = READY; } else { TaskInvocation *ti = new TaskInvocation(); ti->timestamp = rtsys->time; strncpy(ti->invoker, invoker, MAXCHARS); // not used usertask->pending->appendNode(new DataNode(ti, NULL)); } usertask->nbrInvocations++; } else { InterruptHandler *handler = (InterruptHandler*)task; if (handler->nbrInvocations == 0) { handler->timestamp = rtsys->time; strncpy(handler->invoker, invoker, MAXCHARS); handler->moveToList(rtsys->readyQs[handler->affinity]); handler->state = READY; } else { TaskInvocation *ti = new TaskInvocation(); ti->timestamp = rtsys->time; strncpy(ti->invoker, invoker, MAXCHARS); handler->pending->appendNode(new DataNode(ti, NULL)); } handler->nbrInvocations++; } }
bool ttTryPost(const char* mailbox, void* msg) { DataNode* dn; Mailbox* m; dn = getNode(mailbox, rtsys->mailboxList); if (dn == NULL) { char buf[200]; sprintf(buf, "ttTryPost: Non-existent mailbox '%s'!", mailbox); TT_MEX_ERROR(buf); return false; } m = (Mailbox*) dn->data; if (m->count == m->maxSize) { //mexPrintf("ttTryPost: Mailbox '%s' is full\n", mailbox); return false; } else { m->buffer->appendNode(new DataNode(msg, NULL)); m->count++; } // release first waiting reader, if any UserTask* task = (UserTask*) m->readerQ->getFirst(); if (task != NULL) { dn = (DataNode*) m->buffer->getFirst(); task->mb_data = (void*)dn->data; // save it for later (ttRetrieve) m->buffer->deleteNode(dn); m->count--; task->moveToList(rtsys->readyQs[task->affinity]); } return true; }
double runKernel(double externalTime) { double nextHit, timeElapsed; Task *task, *temp, *oldrunning, *newrunning; UserTask *usertask; InterruptHandler *hdl; DataNode* dn; // If no energy, then we can not run if (rtsys->energyLevel <= 0) { //printf("Energy is out at time: %f\n", rtsys->time); return INF; } timeElapsed = externalTime - rtsys->prevHit; // time since last invocation rtsys->prevHit = externalTime; // update previous invocation time nextHit = 0.0; //printf("runkernel at %f\n", rtsys->time); #ifdef KERNEL_MATLAB /* Write rtsys pointer to global workspace */ *((long *)mxGetPr(rtsys->rtsysptr)) = (long)rtsys; #endif while (nextHit < EPS) { // Count down execution time for current task (usertask or handler) // and check if it has finished its execution task = rtsys->running; if (task != NULL) { // Count down execution time task->execTime -= timeElapsed * rtsys->cpuScaling; if (task->execTime < EPS) { // Execute next segment task->segment++; if (task->isUserTask()) { usertask = (UserTask*) task; // Update budget and lastStart variable at segment change usertask->budget -= (rtsys->time - usertask->lastStart); usertask->lastStart = rtsys->time; } // Execute next segment of the code function #ifndef KERNEL_MATLAB task->execTime = task->codeFcn(task->segment, task->data); if (rtsys->error) { printf("Error in ==> task '%s', segment %d\n", task->name, task->segment); return 0.0; } #else if (task->codeFcnMATLAB == NULL) { task->execTime = task->codeFcn(task->segment, task->data); } else { task->execTime = executeCode(task->codeFcnMATLAB, task->segment, task); } if (rtsys->error) { printf("Error in ==> task '%s', segment %d\n", task->name, task->segment); return 0.0; } #endif if (task->execTime < 0.0) { // Negative execution time = task finished task->execTime = 0.0; task->segment = 0; if (task->myList == rtsys->readyQ) { // Remove task from readyQ task->remove(); } if (!(task->isUserTask())) { hdl = (InterruptHandler*) task; if (hdl->type == TIMER) { if (hdl->timer->isPeriodic) { // if periodic timer put back in timeQ hdl->timer->time += hdl->timer->period; hdl->moveToList(rtsys->timeQ); } else { // Remove timer and free up handler dn = getNode(hdl->timer->name, rtsys->timerList); rtsys->timerList->deleteNode(dn); delete hdl->timer; hdl->timer = NULL; hdl->type = UNUSED; } } if (hdl->type == EXTERNAL) { if (hdl->pending > 0) { // new external interrupt occured before handler finished hdl->pending--; hdl->moveToList(rtsys->readyQ); } } } else { // the finished task is a usertask usertask = (UserTask*) task; // Execute finish-hook usertask->finish_hook(usertask); usertask->state = IDLE; // Release next job if any usertask->nbrJobs--; if (usertask->nbrJobs > 0) { // next pending release dn = (DataNode*) usertask->pending->getFirst(); double* release = (double*) dn->data; usertask->release = *release; usertask->absDeadline = *release + usertask->deadline; usertask->moveToList(rtsys->timeQ); usertask->pending->deleteNode(dn); delete release; // Execute release-hook usertask->release_hook(usertask); usertask->state = SLEEPING; } } } } } // end: counting down execution time of running task // Check time queue for possible releases task = (Task*) rtsys->timeQ->getFirst(); while (task != NULL) { if ((task->wakeupTime() - rtsys->time) < EPS) { // Task to be released temp = task; task = (Task*) task->getNext(); temp->moveToList(rtsys->readyQ); if (temp->isUserTask()) { usertask = (UserTask*) temp; usertask->state = READY; } } else { break; } } // end: checking timeQ for releases // Determine task with highest priority and make it running task newrunning = (Task*) rtsys->readyQ->getFirst(); oldrunning = rtsys->running; if (newrunning != NULL) { // Check for suspend- and resume-hooks if (oldrunning != NULL) { // Is oldrunning being suspended? if (oldrunning->isUserTask()) { if (newrunning != oldrunning && ((UserTask*) oldrunning)->state == RUNNING) { usertask = (UserTask*) oldrunning; usertask->state = SUSPENDED; usertask->suspend_hook(usertask); } } } // invocation of hooks may have triggered kernelHandler newrunning = (Task*) rtsys->readyQ->getFirst(); // Is newrunning being resumed? if (newrunning->isUserTask()) { if ( (((UserTask*) newrunning)->state == READY) || (((UserTask*) newrunning)->state == SUSPENDED) ) { // newrunning is being resumed or started usertask = (UserTask*) newrunning; usertask->state = RUNNING; if (usertask->segment == 0) { usertask->start_hook(usertask); } else { usertask->resume_hook(usertask); } } } // invocation of hooks may have triggered kernelHandler rtsys->running = (Task*) rtsys->readyQ->getFirst(); } else { // No tasks in readyQ rtsys->running = NULL; } // end: task dispatching // Determine next invocation of kernel nextHit = getNextInvocation(); timeElapsed = 0.0; } // end: loop while nextHit < EPS return nextHit; }
double runKernel(double externalTime) { Task *task, *temp, *newrunning; UserTask *usertask; InterruptHandler *handler; DataNode* dn; // If no energy, then we can not run if (rtsys->energyLevel <= 0) { debugPrintf("'%s': Energy is out at time: %f\n", rtsys->blockName, rtsys->time); return TT_MAX_TIMESTEP; } double timeElapsed = externalTime - rtsys->prevHit; // time since last invocation rtsys->prevHit = externalTime; // update previous invocation time debugPrintf("'%s': runkernel at %.16f\n", rtsys->blockName, rtsys->time); #ifdef KERNEL_MATLAB // Write rtsys pointer to global workspace so that MATLAB kernel // primitives can access it *((long *)mxGetPr(rtsys->rtsysptr)) = (long)rtsys; #endif double timestep = 0.0; int niter = 0; while (timestep < TT_TIME_RESOLUTION) { if (++niter == TT_MAX_ITER) { mexPrintf("??? Fatal kernel error: maximum number of iterations reached!\n"); rtsys->error = 1; return 0.0; } // For each CPU, count down execution time for the current task for (int i=0; i<rtsys->nbrOfCPUs; i++) { debugPrintf("running core %d\n", i); rtsys->currentCPU = i; rtsys->running = rtsys->runnings[i]; task = rtsys->running; if (task != NULL) { if (task->state == RUNNING) { task->state = READY; } double duration = timeElapsed * rtsys->cpuScaling; // Decrease remaining execution time for current segment and increase total CPU time task->execTime -= duration; task->CPUTime += duration; // If user task, call runkernel hook (to e.g. update budgets) if (task->isUserTask()) { usertask = (UserTask*)task; usertask->runkernel_hook(usertask,duration); } // Check if task has finished current segment or not yet started if (task->execTime / rtsys->cpuScaling < TT_TIME_RESOLUTION) { // Execute next segment task->segment = task->nextSegment; task->nextSegment++; // default, can later be changed by ttSetNextSegment #ifndef KERNEL_MATLAB debugPrintf("'%s': executing code segment %d of task '%s'\n", rtsys->blockName, task->segment, task->name); task->execTime = task->codeFcn(task->segment, task->data); if (rtsys->error) { TT_RUNKERNEL_ERROR(errbuf); mexPrintf("In task ==> '%s', code segment %d\n", task->name, task->segment); return 0.0; } #else if (task->codeFcnMATLAB == NULL) { task->execTime = task->codeFcn(task->segment, task->data); } else { mxArray *lhs[2]; mxArray *rhs[2]; debugPrintf("'%s': executing code function '%s'\n", rtsys->blockName, task->codeFcnMATLAB); *mxGetPr(rtsys->segArray) = (double)task->segment; rhs[0] = rtsys->segArray; if (task->dataMATLAB) { rhs[1] = task->dataMATLAB; } else { rhs[1] = mxCreateDoubleMatrix(0, 0, mxREAL); } mexSetTrapFlag(1); // return control to the MEX file after an error lhs[0] = NULL; // needed not to crash Matlab after an error lhs[1] = NULL; // needed not to crash Matlab after an error if (mexCallMATLAB(2, lhs, 2, rhs, task->codeFcnMATLAB) != 0) { rtsys->error = true; return 0.0; } if (mxGetClassID(lhs[0]) == mxUNKNOWN_CLASS) { snprintf(errbuf, MAXERRBUF, "Execution time not assigned in code function '%s'", task->codeFcnMATLAB); TT_RUNKERNEL_ERROR(errbuf); rtsys->error = true; return 0.0; } if (!mxIsDoubleScalar(lhs[0])) { snprintf(errbuf, MAXERRBUF, "Illegal execution time returned by code function '%s'", task->codeFcnMATLAB); TT_RUNKERNEL_ERROR(errbuf); rtsys->error = true; return 0.0; } if (mxGetClassID(lhs[1]) == mxUNKNOWN_CLASS) { snprintf(errbuf, MAXERRBUF, "Data not assigned in code function '%s'", task->codeFcnMATLAB); TT_RUNKERNEL_ERROR(errbuf); rtsys->error = true; return 0.0; } //if ( task->dataMATLAB ) { if ( task->dataMATLAB != lhs[1] ) { mxDestroyArray(task->dataMATLAB); //task->dataMATLAB = mxDuplicateArray(lhs[1]); task->dataMATLAB = lhs[1]; mexMakeArrayPersistent(task->dataMATLAB); } task->execTime = *mxGetPr(lhs[0]); //mxDestroyArray(rhs[1]); mxDestroyArray(lhs[0]); //mxDestroyArray(lhs[1]); } #endif if (task->execTime < 0.0) { // Negative execution time = task is finished debugPrintf("'%s': task '%s' finished\n", rtsys->blockName, task->name); task->execTime = 0.0; task->segment = 0; task->nextSegment = 1; // Remove task from readyQ and set running to NULL if (task->state == READY) { task->remove(); task->state = SLEEPING; } else { snprintf(errbuf, MAXERRBUF, "Finished task '%s' not in ReadyQ.", task->name); TT_RUNKERNEL_ERROR(errbuf); rtsys->error = true; return 0.0; } rtsys->runnings[i] = NULL; rtsys->running = NULL; if (task->isUserTask()) { // Execute finish-hook usertask = (UserTask*)task; usertask->finish_hook(usertask); rtsys->runningUserTasks[i] = NULL; } task->nbrInvocations--; if (task->nbrInvocations > 0) { // There are queued invocations, release the next one dn = (DataNode*) task->pending->getFirst(); TaskInvocation *ti = (TaskInvocation *)dn->data; if (task->isUserTask()) { usertask = (UserTask*)task; usertask->arrival = ti->timestamp; usertask->release = rtsys->time; usertask->release_hook(usertask); // could affect task prio } debugPrintf("'%s': releasing task '%s'\n", rtsys->blockName, task->name); task->moveToList(rtsys->readyQs[task->affinity]); // re-insert task into readyQ task->state = READY; if (task->isHandler()) { handler = (InterruptHandler*)task; strncpy(handler->invoker, ti->invoker, MAXCHARS); handler->timestamp = ti->timestamp; } task->pending->deleteNode(dn); delete ti; } } } } } // Check time queue for possible releases and expired timers task = (Task*) rtsys->timeQ->getFirst(); while (task != NULL) { if ((task->wakeupTime() - rtsys->time) >= TT_TIME_RESOLUTION) { break; // timeQ is sorted by time, no use to go further } // Task to be released temp = task; task = (Task*) task->getNext(); if (temp->isTimer()) { Timer *timer = (Timer*)temp; debugPrintf("'%s': timer '%s' expired at %f\n", rtsys->blockName, timer->name, rtsys->time); invoke_task(timer->task, timer->name); if (timer->isPeriodic) { // if periodic timer put back in timeQ timer->time += timer->period; timer->moveToList(rtsys->timeQ); } else { timer->remove(); // remove timer from timeQ if (!timer->isOverrunTimer) { // delete the timer dn = getNode(timer->name, rtsys->timerList); rtsys->timerList->deleteNode(dn); delete timer; } } } else if (temp->isUserTask()) { usertask = (UserTask*)temp; debugPrintf("'%s': releasing task '%s'\n", rtsys->blockName, usertask->name); usertask->moveToList(rtsys->readyQs[usertask->affinity]); usertask->state = READY; } else if (temp->isHandler()) { mexPrintf("??? Fatal kernel error: interrupt handler in TimeQ!\n"); rtsys->error = 1; return 0.0; } } // end: checking timeQ for releases // For each core, determine the task with highest priority and make it running task for (int i=0; i<rtsys->nbrOfCPUs; i++) { debugPrintf("scheduling core %d\n", i); rtsys->currentCPU = i; newrunning = (Task*) rtsys->readyQs[i]->getFirst(); // If old running has been preempted, execute suspend_hook if (rtsys->runnings[i] != NULL && rtsys->runnings[i] != newrunning) { if (rtsys->runnings[i]->isUserTask()) { usertask = (UserTask*)rtsys->runnings[i]; usertask->suspend_hook(usertask); } } // If new running != old running, execute start_hook or resume_hook if (newrunning != NULL && newrunning != rtsys->runnings[i]) { if (newrunning->isUserTask()) { usertask = (UserTask*)newrunning; if (usertask->segment == 0) { usertask->segment = 1; usertask->start_hook(usertask); } else { usertask->resume_hook(usertask); } rtsys->runningUserTasks[i] = usertask; } } rtsys->runnings[i] = (Task*) rtsys->readyQs[i]->getFirst(); // hooks may have released handlers if (rtsys->runnings[i] != NULL) { rtsys->runnings[i]->state = RUNNING; } } // Determine next invocation of kernel double compTime; timestep = TT_MAX_TIMESTEP; // Next release from timeQ (user task or timer) if (rtsys->timeQ->getFirst() != NULL) { Task* t = (Task*) rtsys->timeQ->getFirst(); timestep = t->wakeupTime() - rtsys->time; } // Remaining execution time of running tasks for (int i=0; i<rtsys->nbrOfCPUs; i++) { if (rtsys->runnings[i] != NULL) { compTime = rtsys->runnings[i]->execTime / rtsys->cpuScaling; timestep = (timestep < compTime) ? timestep : compTime; } } timeElapsed = 0.0; } // end: loop while timestep < TT_TIME_RESOLUTION return timestep; }