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;
}
void run() override final
{
typedef typename make_index_tuple<sizeof...(Args)>::type index_t;
invoke_task(index_t());
}
static void mdlZeroCrossings(SimStruct *S)
{
rtsys = (RTsys*) ssGetUserData(S);
debugPrintf("'%s': mdlZeroCrossings at %.16f\n", rtsys->blockName, ssGetT(S));
int i;
if (rtsys->init_phase) {
/* Failure during initialization */
return;
}
/* Copy analog inputs */
InputRealPtrsType inputs = ssGetInputPortRealSignalPtrs(S,0);
for (i=0; i<rtsys->nbrOfInputs; i++) {
rtsys->inputs[i] = *inputs[i];
}
/* Check trigger input port for events */
inputs = ssGetInputPortRealSignalPtrs(S,1);
for (i=0; i<rtsys->nbrOfTriggers; i++) {
Trigger* trig = &(rtsys->triggers[i]);
// Only check for events if there is a handler attached
if (trig->handler != NULL) {
double input = *inputs[i];
double oldinput = rtsys->oldtriggerinputs[i];
int event = 0;
switch (trig->state) {
case ZERO:
if (input > 0.0) {
trig->state = POSITIVE;
event = RISING;
} else if (input < 0.0) {
trig->state = NEGATIVE;
event = FALLING;
}
break;
case NEGATIVE:
if ((input > 0.0 && oldinput <= 0.0) || (input == 0.0 && oldinput < 0.0)) {
trig->state = POSITIVE;
event = RISING;
}
break;
case POSITIVE:
if ((input < 0.0 && oldinput >= 0.0) || (input == 0.0 && oldinput > 0.0)) {
trig->state = NEGATIVE;
event = FALLING;
}
break;
}
if (event & rtsys->trigType) {
if (trig->minimumInterval <= 0.0 || rtsys->time - trig->prevHit >= trig->minimumInterval) {
debugPrintf("'%s': external trigger %d activated at %.14f\n", rtsys->blockName, i+1, rtsys->time);
// Trigger interrupt handler
invoke_task(trig->handler, trig->trigName);
if (trig->handler->nbrInvocations == 1) {
rtsys->nextHit = ssGetT(S);
}
trig->prevHit = rtsys->time;
}
}
rtsys->oldtriggerinputs[i] = *inputs[i];
}
}
/* Check network input port for events */
inputs = ssGetInputPortRealSignalPtrs(S,2);
for (i=0; i<rtsys->nbrOfNetworks; i++) {
NetworkInterface *nwi = &(rtsys->networkInterfaces[i]);
// Only check for events if there is a handler attached
if (nwi->handler != NULL) {
double input = *inputs[i];
double oldinput = rtsys->oldnetworkinputs[i];
if (input != oldinput) {
debugPrintf("'%s': incoming packet from network %d at %.14f\n", rtsys->blockName, nwi->networkNbr, rtsys->time);
char trigname[MAXCHARS];
snprintf(trigname, MAXCHARS, "network:%d", nwi->networkNbr);
invoke_task(nwi->handler, trigname);
if (nwi->handler->nbrInvocations == 1) {
rtsys->nextHit = ssGetT(S);
}
}
rtsys->oldnetworkinputs[i] = input;
}
}
/* Read the energy level input */
rtsys->energyLevel = *ssGetInputPortRealSignalPtrs(S,3)[0];
// Schedule next major time step according to rtsys->nextHit
ssGetNonsampledZCs(S)[0] = rtsys->nextHit - ssGetT(S);
}
