ttkernel.cpp

一个很棒的网络控制系统仿真软件

#include "ttkernel.h"

// ----- Main data structure ------

RTsys *rtsys;

// ------- Internal functions used by kernel ------- 

#include "movetime.cpp"
#include "moveready.cpp"
#include "gettask.cpp"
#include "gethandler.cpp"
#include "runhandler.cpp"
#include "compfunctions.cpp"
#include "codefunctions.cpp"
#include "priofunctions.cpp"
#include "defaulthooks.cpp"
#include "initnetwork2.cpp"

#ifndef KERNEL_MATLAB

// --- Initialization and creation ----

#include "initkernel.cpp"
#include "createtask.cpp"
#include "createpertask.cpp"
#include "createhandler.cpp"
#include "createtrigger.cpp"
#include "initnetwork.cpp"
#include "createmonitor.cpp"
#include "createevent.cpp"
#include "createmailbox.cpp"
#include "attachdlhandler.cpp"
#include "attachwcethandler.cpp"
#include "attachpriofcn.cpp"
#include "attachhook.cpp"
#include "noschedule.cpp"
#include "nonpreemptable.cpp"

// ------- Real-time primitives ------- 

#include "createjob.cpp"
#include "killjob.cpp"
#include "createtimer.cpp"
#include "removetimer.cpp"
#include "analogin.cpp"
#include "analogout.cpp"
#include "sleep.cpp"
#include "entermonitor.cpp"
#include "exitmonitor.cpp"
#include "wait.cpp"
#include "notifyall.cpp"
#include "tryfetch.cpp"
#include "trypost.cpp"
#include "currenttime.cpp"
#include "invokingtask.cpp"
#include "setnextsegment.cpp"
#include "callblocksystem.cpp"
#include "sendmsg.cpp"
#include "getmsg.cpp"

// ---------- Sets and Gets ------------

#include "setdeadline.cpp"
#include "setabsdeadline.cpp"
#include "setpriority.cpp"
#include "setperiod.cpp"
#include "setbudget.cpp"
#include "setwcet.cpp"
#include "getrelease.cpp"
#include "getdeadline.cpp"
#include "getabsdeadline.cpp"
#include "getpriority.cpp"
#include "getperiod.cpp"
#include "getbudget.cpp"
#include "getwcet.cpp"

#endif

#ifdef KERNEL_MATLAB
// ----------------------------------------------
// ------ Executes an m-file code function ------
// ----------------------------------------------

// mxArray used to pass the segment to the code function
mxArray *segArray;
bool destroyed;

double executeCode(char *codeName, int seg, char* dataName) {
  double retval;
  mxArray *lhs[2];
  mxArray *rhs[2];

  *mxGetPr(segArray) = (double) seg;

  rhs[0] = segArray;
  if (dataName) {
    rhs[1] = mexGetVariable("global", dataName);
  } else {
    rhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL);
    *mxGetPr(rhs[1]) = 0.0;
  }

  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, codeName) != 0) {
    rtsys->error = true;
    return 0.0;
  }

  if (mxGetClassID(lhs[0]) == mxUNKNOWN_CLASS) {
    printf("??? executeCode: execution time not assigned\n\n");
    printf("Error in ==> code function '%s', segment %d\n", codeName, seg);
    rtsys->error = true;
    return 0.0;
  }

  if (!mxIsDoubleScalar(lhs[0])) {
    printf("??? executeCode: illegal execution time\n\n");
    printf("Error in ==> code function '%s', segment %d\n", codeName, seg);
    rtsys->error = true;
    return 0.0;
  }

  if (mxGetClassID(lhs[1]) == mxUNKNOWN_CLASS) {
    printf("??? executeCode: data not assigned\n\n");
    printf("Error in ==> code function '%s', segment %d\n", codeName, seg);
    rtsys->error = true;
    return 0.0;
  }

  if (dataName) {
    mexPutVariable("global", dataName, lhs[1]);
  }
  
  retval = *mxGetPr(lhs[0]);

  mxDestroyArray(rhs[1]);
  mxDestroyArray(lhs[0]);
  mxDestroyArray(lhs[1]);

  return retval;
}
#endif

// ----------------------------------------------
// -- Determines time for next clock interrupt --
// ----- used in the kernel function below ------
// ----------------------------------------------

double getNextInvocation() {

  double compTime;
  double nextHit = 1000.0;

  if (rtsys->timeQ->getLast() != NULL) {
    TaskNode* tn = (TaskNode*) rtsys->timeQ->getLast();
    nextHit = tn->getTask()->currentJob->release - rtsys->time;
  }

  if (rtsys->timerQ->getLast() != NULL) {
    TimerNode* tin = (TimerNode*) rtsys->timerQ->getLast();
    compTime = tin->getTimer()->time - rtsys->time;
    nextHit = (nextHit < compTime) ? nextHit : compTime;
  }

  if (rtsys->activeHandler != NULL) {
    compTime = rtsys->activeHandler->execTime;
    nextHit = (nextHit < compTime) ? nextHit : compTime;
  } else if (rtsys->running != NULL) {
    compTime = rtsys->running->currentJob->execTime;
    nextHit = (nextHit < compTime) ? nextHit : compTime;
  }

  return nextHit;
}


// -------------------------------------------------------------
// --------------------- Kernel Function -----------------------
// ----  Called from the Simulink callback functions during ----
// -- simulation and returns the time for its next invocation --
// -------------------------------------------------------------

double runKernel(void) {
  
  double nextHit, timeElapsed; 
  TaskNode *tn, *temp;
  Task *task;
  Job* job;
  JobNode* jn;
  Handler* handler;
  TimerNode *tin, *tmp;
  
#ifdef KERNEL_MATLAB
  mxArray* data;
#endif

  timeElapsed = rtsys->time - rtsys->prevHit; // time since last invocation
  rtsys->prevHit = rtsys->time;
  nextHit = 0.0;

  Task* lastTask = rtsys->running;  // running task at beginning of invocation
  Task* suspended = lastTask;       // task that may be suspended

  while (nextHit < EPS) {

    // Count down execution time for current task instance 
    // and check if it has finished its execution 

    task = rtsys->running;
    if (task != NULL) {
      job = task->currentJob;
      // Count down execution time 
      job->execTime -= timeElapsed;
      if (job->execTime < EPS) {
	// Execute next segment 
	job->segment++;

	if (job->segment == 1) {
	  // First segment, execute started-hook
	  task->started_hook(task);
	} else {
	  // Subsequent segment, update budget and lastStart variable
	  job->budget -= (rtsys->time - job->lastStart);
	  job->lastStart = rtsys->time;
	}

	// Execute next segment of the code function

	rtsys->executing = task; // Executing task, not changed by calls from code function. 

#ifndef KERNEL_MATLAB
	job->execTime = task->codeFcn(job->segment, task->data);
	if (rtsys->error) {
	  printf("Error in ==> task '%s', segment %d\n", task->name, job->segment);
	  return 0.0;
	}
#else
	job->execTime = executeCode(task->codeFcnMATLAB, job->segment, task->dataMATLAB);
	if (rtsys->error) {
	  return 0.0;
	}
#endif

	if (job->execTime < 0.0) { // Negative execution time = Job finished
	  // Remove task from readyQ
	  rtsys->readyQ->deleteNode(rtsys->readyQ->getLast());

	  // Execute finished-hook
	  task->finished_hook(task);
	  
	  suspended = NULL;
	  rtsys->executing = NULL;
	  
	  if (rtsys->simContextSwitch) {
	    // Simulate context switch
	    runHandler(rtsys->kernelHandler, NULL);
	  }

	  delete job;
	  // Serve pending jobs
	  jn = (JobNode*) task->jobQ->getLast();
	  if (jn != NULL) {
	    job = jn->getJob();
	    task->jobQ->removeNode(jn);
	    task->currentJob = job;
	    moveToTimeQ(task);
	  } else {
	    task->currentJob = NULL;
	  }
	}
      }
    }

    // Count down execution time for current interrupt handler and 
    // remove the handler if it has finished

    handler = rtsys->activeHandler;
    if (handler != NULL) {
      handler->execTime -= timeElapsed;
      if (handler->execTime < EPS) {
	// Execute next segment 
	handler->segment++;

#ifndef KERNEL_MATLAB
	handler->execTime = handler->codeFcn(handler->segment, handler->data);
	if (rtsys->error) {
	  printf("Error in ==> handler '%s', segment %d\n", handler->name, handler->segment);
	  return 0.0;
	}
#else
	if (handler->codeFcnMATLAB == NULL) {
	  handler->execTime = handler->codeFcn(handler->segment, handler->data);
	  if (rtsys->error) {
	    printf("Error in ==> handler '%s', segment %d\n", handler->name, handler->segment);
	    return 0.0;
	  }
	} else { // call M-file
	  handler->execTime = executeCode(handler->codeFcnMATLAB, handler->segment, handler->dataMATLAB);
	  if (rtsys->error) {
	    return 0.0;
	  }
	}
#endif
	
      }
      if (handler->execTime < 0.0) {
	// Finished
	handler->execTime = 0.0;
	handler->segment = 0;
	rtsys->activeHandler = NULL;
	rtsys->handlerQ->deleteNode(rtsys->handlerQ->getLast());
      }      
    }

    // Check time queue for possible releases

    tn = (TaskNode*) rtsys->timeQ->getLast();
    while (tn != NULL) {
      task = tn->getTask();
      if ((task->currentJob->release - rtsys->time) < EPS) {
	// Task to be released 
	moveToReadyQ(task);
	temp = tn;
	tn = (TaskNode*) tn->getPrev();
	rtsys->timeQ->deleteNode(temp);

	if (task->currentJob->segment == 0) {
	  // First release of job, execute released-hook
	  task->released_hook(task);
	} else {
	  // Job released after sleep, execute resumed-hook
	  task->resumed_hook(task);
	}
      } else {
	break;
      }
    }

    // Check for expired timers
    
    
    tin = (TimerNode*) rtsys->timerQ->getLast();
    while (tin != NULL) {
      Timer* t = tin->getTimer();
      if ((t->time - rtsys->time) < EPS) {
	// Timer expired, run interrupt handler
	runHandler(t->handler, t->task);
	tmp = tin;
	tin = (TimerNode*) tin->getPrev();
	rtsys->timerQ->deleteNode(tmp);
	
	if (t->isPeriodic) {
	  t->time += t->period;
	  rtsys->timerQ->insertSorted(new TimerNode(t));
	}
	else {
	  delete t;
	}
      } else {
	break;
      }
    }

    // Dispatching 

    if (rtsys->handlerQ->getLast() == NULL) { // No active handlers 

      // Determine task with highest priority and make it running task

      tn = (TaskNode*) rtsys->readyQ->getLast();
      if (tn != NULL) {
	rtsys->running = tn->getTask();

 	// Is this task being resumed?
	if (lastTask != NULL && rtsys->running != lastTask && rtsys->running->currentJob->segment != 0) {
	  // Execute resumed-hook
	  rtsys->running->resumed_hook(rtsys->running);
	} 
	lastTask = rtsys->running;

	// Is any other task being suspended?
	if (rtsys->running != suspended && suspended != NULL) { 

	  // Execute suspended-hook
	  suspended->suspended_hook(suspended);
	  suspended = rtsys->running;
	  if (rtsys->simContextSwitch) {
 	    // Simulate context switch
	    runHandler(rtsys->kernelHandler, NULL);
 	  }
	} else if (rtsys->running != lastTask && rtsys->simContextSwitch && rtsys->savedContext) { // no task suspended by new running task
	  // Simulate context switch 
	  runHandler(rtsys->kernelHandler, NULL);
	}
	// Suspended-hook may activate handlers
	if (rtsys->handlerQ->getLast() != NULL) {
	  HandlerNode* hn = (HandlerNode*) rtsys->handlerQ->getLast();
	  rtsys->activeHandler = hn->getHandler();
	  rtsys->running = NULL;
	}

      } else { // No tasks in readyQ
	if (rtsys->executing != NULL) { // task suspended itself
	  // Execute suspended-hook
	  rtsys->executing->suspended_hook(rtsys->executing);
	  suspended = NULL;
	}
	rtsys->running = NULL;
      }

    } else { // There exist active interrupt handlers

      // Make the highest-priority interrupt handler active

      HandlerNode* hn = (HandlerNode*) rtsys->handlerQ->getLast();
      rtsys->activeHandler = hn->getHandler();
      rtsys->running = NULL;
    }

    // Get time of next clock interrupt 

    nextHit = getNextInvocation();
    timeElapsed = 0.0;

  } // loop while nextHit = 0.0 

  rtsys->executing = NULL;
  return nextHit;
}


// ------- Simulink callback functions ------- 

#ifdef __cplusplus
extern "C" { // use the C fcn-call standard for all functions  
#endif       // defined within this scope   

#define S_FUNCTION_LEVEL 2
  
#include "simstruc.h"


static void mdlInitializeSizes(SimStruct *S)
{
/*  printf("-------------------------------------------------------\n");
  printf("                TrueTime, Version 1.13\n");            
  printf(" Copyright (c) 2003 by Dan Henriksson and Anton Cervin\n");
  printf("            Department of Automatic Control\n");        
  printf("          Lund Institute of Technology, Sweden\n");
 printf("-------------------------------------------------------\n");  */

#ifdef KERNEL_MATLAB
  mxArray *arg0; // Arguments to S-function
  mxArray *arg1;
  char initfun[100];
  static mxArray *lhs[1];
  static mxArray *rhs[3];
  char rtsysp[100];
  double *valuePtr;
  
  segArray = mxCreateScalarDouble(0.0);
  destroyed = false;
  mexMakeArrayPersistent(segArray);
#endif
  
  ssSetNumSFcnParams(S, 1);  /* Number of expected parameters */
  if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
    return; /* Parameter mismatch will be reported by Simulink */
  }