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);
}
}
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;
}
}
}
// 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;
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
//printf("mdlOutputs at %g\n", ssGetT(S));
rtsys = (RTsys*) ssGetUserData(S);
if (rtsys->init_phase) {
/* Failure during initialization */
return;
}
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *n = ssGetOutputPortRealSignal(S,1);
real_T *s = ssGetOutputPortRealSignal(S,2);
real_T *m = ssGetOutputPortRealSignal(S,3);
real_T *energyConsumption = ssGetOutputPortRealSignal(S,4);
int i, j, k, detected;
double dTime;
DataNode *dn;
Task* task;
UserTask* t;
InterruptHandler* hdl;
Monitor *mon;
if (!rtsys->started && ssGetT(S) == 0.0) {
rtsys->started = true;
return;
}
if (!rtsys->mdlzerocalled) {
printf("Zero crossing detection must be turned on in order to run TrueTime!\n");
ssSetErrorStatus(S, "Zero crossing detection must be turned on in order to run TrueTime!");
return;
}
/* Storing the time */
rtsys->time = ssGetT(S) * rtsys->clockDrift + rtsys->clockOffset;
detected = 0;
/* Check interrupts */
i = 0;
dn = (DataNode*) rtsys->triggerList->getFirst();
while (dn != NULL) {
if (fabs(rtsys->interruptinputs[i]-rtsys->oldinterruptinputs[i]) > 0.1) {
hdl = (InterruptHandler*) dn->data;
Trigger* trig = hdl->trigger;
if (rtsys->time - trig->prevHit > trig->latency) {
// Trigger interrupt handler
if (hdl->myList == rtsys->readyQ) {
// handler serving older interrupts
hdl->pending++;
} else {
hdl->moveToList(rtsys->readyQ);
detected = 1;
}
trig->prevHit = rtsys->time;
} else {
//printf("Call to interrupt handler %s ignored at time %f. Within interrupt latency!\n", hdl->name, rtsys->time);
}
rtsys->oldinterruptinputs[i] = rtsys->interruptinputs[i];
}
i++;
dn = (DataNode*) dn->getNext();
}
/* Check network */
dn = (DataNode*) rtsys->networkList->getFirst();
while (dn != NULL) {
hdl = (InterruptHandler*) dn->data;
Network* network = hdl->network;
i = network->networkID - 1;
//printf("mdlOutputs: checking network #%d inp: %d oldinp: %d\n",i,rtsys->networkinputs[i],rtsys->oldnetworkinputs[i]);
if (fabs(rtsys->networkinputs[i] - rtsys->oldnetworkinputs[i]) > 0.1) {
hdl->moveToList(rtsys->readyQ);
detected = 1;
rtsys->oldnetworkinputs[i] = rtsys->networkinputs[i];
}
dn = (DataNode*) dn->getNext();
}
/* Run kernel? */
double externTime = (rtsys->time- rtsys->clockOffset) / rtsys->clockDrift;
if ((externTime >= rtsys->nextHit) || (detected > 0)) {
dTime = runKernel(ssGetT(S));
if (rtsys->error) {
// Something went wrong executing a code function
mxArray *bn[1];
mexCallMATLAB(1, bn, 0, 0, "gcs"); // get current system
char buf[200];
mxGetString(bn[0], buf, 200);
for (unsigned int i=0; i<strlen(buf); i++) if (buf[i]=='\n') buf[i]=' ';
printf("In block ==> '%s'\nSimulation aborted!\n", buf);
ssSetStopRequested(S, 1);
} else {
rtsys->nextHit = (rtsys->time + dTime - rtsys->clockOffset) / rtsys->clockDrift;
}
}
/* Outputs */
for (i=0; i<rtsys->nbrOfOutputs; i++) {
y[i] = rtsys->outputs[i];
}
/* Network send */
for (i=0; i<rtsys->nbrOfNetworks; i++) {
n[i] = rtsys->nwSnd[i];
}
/* Task schedule */
i = 0;
j = 0;
dn = (DataNode*) rtsys->taskList->getFirst();
while (dn != NULL) {
t = (UserTask*) dn->data;
rtsys->taskSched[i] = (double) (j+1);
if (t->display) j++;
dn = (DataNode*) dn->getNext();
i++;
}
task = (Task*) rtsys->readyQ->getFirst();
while (task != NULL) {
if (task->isUserTask()) {
t = (UserTask*) task;
rtsys->taskSched[t->taskID - 1] += 0.25;
}
task = (Task*) task->getNext();
}
if ((rtsys->running != NULL) && (rtsys->running->isUserTask())) {
t = (UserTask*) rtsys->running;
rtsys->taskSched[t->taskID - 1] += 0.25;
}
i = 0;
j = 0;
dn = (DataNode*) rtsys->taskList->getFirst();
while (dn != NULL) {
t = (UserTask*) dn->data;
if (t->display) {
s[j] = rtsys->taskSched[i];
j++;
}
dn = (DataNode*) dn->getNext();
i++;
}
/* Handler schedule */
i = 0;
j = 0;
dn = (DataNode*) rtsys->handlerList->getFirst();
while (dn != NULL) {
rtsys->handlerSched[i] = (double) (j+rtsys->nbrOfSchedTasks+2);
if (i==0 && rtsys->contextSwitchTime > EPS) {
// Context switch schedule, move graph down to task level
rtsys->handlerSched[i] = rtsys->handlerSched[i] - 1;
}
hdl = (InterruptHandler*) dn->data;
if (hdl->display) j++;
dn = (DataNode*) dn->getNext();
i++;
}
task = (Task*) rtsys->readyQ->getFirst();
while (task != NULL) {
if (!(task->isUserTask())) {
hdl = (InterruptHandler*) task;
rtsys->handlerSched[hdl->handlerID - 1] += 0.25;
}
task = (Task*) task->getNext();
}
if ((rtsys->running != NULL) && (!(rtsys->running->isUserTask()))) {
hdl = (InterruptHandler*) rtsys->running;
rtsys->handlerSched[hdl->handlerID - 1] += 0.25;
}
i = 0;
j = 0;
dn = (DataNode*) rtsys->handlerList->getFirst();
while (dn != NULL) {
hdl = (InterruptHandler*) dn->data;
if (hdl->display) {
s[j+rtsys->nbrOfSchedTasks] = rtsys->handlerSched[i];
j++;
}
dn = (DataNode*) dn->getNext();
i++;
}
/* Monitor graph */
k = 0;
dn = (DataNode*) rtsys->monitorList->getFirst();
while (dn != NULL) {
mon = (Monitor*) dn->data;
for (j=0; j<rtsys->nbrOfTasks; j++)
rtsys->monitorGraph[j] = (double) (j+1+k*(1+rtsys->nbrOfTasks));
t = (UserTask*) mon->waitingQ->getFirst();
while (t != NULL) {
i = t->taskID;
rtsys->monitorGraph[i-1] += 0.25;
t = (UserTask*) t->getNext();
}
if (mon->heldBy != NULL) {
i = mon->heldBy->taskID;
rtsys->monitorGraph[i-1] += 0.5;
}
if (mon->display) {
for (j=0; j<rtsys->nbrOfTasks; j++)
m[j+k*rtsys->nbrOfTasks] = rtsys->monitorGraph[j];
k++;
}
dn = (DataNode*) dn->getNext();
}
/* Energy consumption */
energyConsumption[0] = rtsys->energyConsumption;
}
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;
}
