/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	monitorSubs
 *
 * DESCRIPTION:	Support routines used by the RCI monitor task.
 *
 * AUTHOR/DATE:	John Lloyd, Fall 1987
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include "../h/cdefs.h"
#include "../h/rciTask.h"
#include "../h/rciMonitor.h"
#include "../h/errorCodes.h"
#include "../h/IOdevice.h"

#include "../rci/create.h"
#include "../rci/messageDefs.h"
#include "../rci/monitorDefs.h"
#include "../rci/link.h"

extern int rciCpu;
extern int rciTaskId;
extern RCI_DESC rciTaskTable[];

abortIfNecessary (td, code)
RCI_DESC *td;
int code;
{
	extern int rciAbortFlag;

	/* Examine the code sent to the doRelease() function and 
	   set an rciAbort condition if it is appropriate and it has
	   not already been done */

	if (!rciAbortFlag)
  	 {
	   switch (code)
	    { case EXECUTE_FXN_ERR:
	       { rciAbort (ETaskFxn,
	   "monitor %d -- error in execute function, task '%s'\n",
		 rciCpu, rciTaskName(td));
		 break;
	       }
	      case CONTROL_FXN_ERR:
	       { rciAbort (ETaskFxn,
	   "monitor %d -- error in control function, task '%s'\n",
		 rciCpu, rciTaskName(td));
		 break;
	       }
	      case RELEASE_FXN_ERR:
	       { rciAbort (ETaskFxn,
	   "monitor %d -- error in release function, task '%s'\n",
		 rciCpu, rciTaskName(td));
		 break;
	       }
	      case RUN_TIMEOUT:
	       { rciAbort (ERunTimeout,
	   "monitor %d -- run timeout on task '%s'\n",
		 rciCpu, rciTaskName(td));
		 break;
	       }
	      case BLK_TIMEOUT:
	       { rciAbort (EBlkTimeout,
	   "monitor %d -- blk timeout on task '%s'\n",
		 rciCpu, rciTaskName(td));
		 break;
	       }
	      case LAG_TIMEOUT:
	       { rciAbort (ELagTimeout,
	   "monitor %d -- lag timeout on task '%s'\n",
		 rciCpu, rciTaskName(td));
		 break;
	       }
	    }
	 }
}

checkLagTimeout (tcb, time)
RCI_TCB *tcb;
int time;
{	
	/* use information in the task control block and the present
	   physical time to determine if the task has fallen far
	   enough behind to warrant calling a lag timeout abort */

	float cycleTime;
	float alpha;
	float window;
	float lagAv;
	float interval;

	if (tcb->interval != UNDEF && tcb->cycleCount != 1 && 
	    rciInKernelMode())
	 { cycleTime = time - tcb->lastTime;
	   interval = tcb->interval;
	   window = interval*tcb->lagWindow;
	   alpha = MIN (cycleTime/window, 1.0);

	   lagAv = alpha*(cycleTime - interval) + (1.0-alpha)*tcb->lagAverage;
	   if ((window/(interval+lagAv)*lagAv) >=
	       tcb->maxLagCount*interval*.63)
	    { return (1);
	    }
	   tcb->lagAverage = lagAv;
	 }
	else
	 { tcb->lagAverage = 0.0;
	 }
	return (0);
}

taskControlBlockInit (tcb)
register RCI_TCB *tcb;
{	
	/* initialize the task control block (done once when the
	   task is created */

	tcb->controlMode	= IDLE;
 	tcb->cycleCount 	= 0;
 	
	tcb->scheduleDisp 	= UNDEF;
	tcb->maxBlkCount 	= DEFAULT_BLK_COUNT;
	tcb->maxLagCount	= DEFAULT_LAG_COUNT;
	tcb->lagWindow		= DEFAULT_LAG_WINDOW;

 	tcb->interval 		= UNDEF;
 	tcb->offset 		= UNDEF;

	tcb->triggerFxn 	= NULLF;
	tcb->triggerArg		= UNDEF;
	tcb->wakeupFxn 		= NULLF;
	tcb->wakeupArg		= UNDEF;

	tcb->startupFxn 	= NULLF;
	tcb->controlFxn		= NULLF;
	tcb->releaseFxn 	= NULLF;
	tcb->controlArg		= UNDEF;

	tcb->lastTime		= UNDEF;
 	tcb->newRate 		= NO;

	tcb->sharedMem		= NULL;
	tcb->globalMem		= NULL;
	tcb->privateMem		= NULL;
	tcb->fxnNotice 		= 0;
 	
	tcb->executeFxn		= NULLF;
	tcb->executeArg		= 0;
	tcb->executeRet		= 0;

	tcb->cmodeReq 		= NO;
 	tcb->abortReq 		= NO;
	tcb->nrateReq		= NO;
	tcb->blockReq		= 0;

 	tcb->blkTimeout 	= 0;
	tcb->lagAverage		= 0.0;

	tcb->newInterval 	= UNDEF;
	tcb->newOffset 		= UNDEF;

	tcb->runTime		= UNDEF;
	tcb->startTime		= UNDEF;

	tcb->triggerAst		= 0;
	tcb->timerAst		= 0;
	tcb->interruptSet	= 0;

	tcb->msg[0]		= 0;
	tcb->msg[1]		= 0;
	tcb->msg[2]		= 0;
	tcb->msg[3]		= 0;
	tcb->msg[4]		= 0;
	tcb->msg[5]		= 0;
	tcb->msg[6]		= 0;
	tcb->msg[7]		= 0;

	tcb->msg[8]		= 0;
	tcb->msg[9]		= 0;
	tcb->msg[10]		= 0;
	tcb->msg[11]		= 0;
	tcb->msg[12]		= 0;
	tcb->msg[13]		= 0;
	tcb->msg[14]		= 0;
	tcb->msg[15]		= 0;
}

taskControlBlockReset (tcb)
register RCI_TCB *tcb;
{	
	/* reset the task control block (done every time a task
	   is released) */

	tcb->controlMode;	/* set outside this function */
 	tcb->cycleCount;	/* clear this later */
 	
	tcb->scheduleDisp;
	tcb->maxBlkCount;
	tcb->maxLagCount;
	tcb->lagWindow;

 	tcb->interval;
 	tcb->offset;

	tcb->triggerFxn;
	tcb->triggerArg;
	tcb->wakeupFxn;
	tcb->wakeupArg;

	tcb->startupFxn;
	tcb->controlFxn;
	tcb->releaseFxn;
	tcb->controlArg;

	tcb->lastTime		= UNDEF;
 	tcb->newRate 		= NO;

	tcb->sharedMem		= NULL;
	tcb->globalMem		= NULL;
	tcb->privateMem		= NULL;
	tcb->fxnNotice 		= 0;

	tcb->executeFxn;
	tcb->executeArg;
	tcb->executeRet;

	tcb->cmodeReq 		= NO;
 	tcb->abortReq 		= NO;
	tcb->nrateReq		= NO;
	tcb->blockReq		= 0;

 	tcb->blkTimeout 	= 0;
	tcb->lagAverage		= 0.0;

	/* update new parameters, just in case they didn't make it */

	if (tcb->newInterval != UNDEF)
	 { tcb->interval = tcb->newInterval;
	 }
	if (tcb->newOffset != UNDEF)
	 { tcb->offset = tcb->newOffset;
	 }
	tcb->newInterval 	= UNDEF;
	tcb->newOffset 		= UNDEF;

	tcb->runTime		= UNDEF;
	tcb->startTime		= UNDEF;

	if (tcb->triggerAst)
	 { monitorTimerDelete (&(tcb->triggerAst));
	 }
	if (tcb->timerAst)
	 { monitorTimerDelete (&(tcb->timerAst));
	 }
	tcb->interruptSet	= 0;

	tcb->msg[0]		= 0;
	tcb->msg[1]		= 0;
	tcb->msg[2]		= 0;
	tcb->msg[3]		= 0;
	tcb->msg[4]		= 0;
	tcb->msg[5]		= 0;
	tcb->msg[6]		= 0;
	tcb->msg[7]		= 0;
}

/* =-=-=-=-=-=-=- Functions for maintaining the task queue -=-=-=-=-=-=-=- */

static TASK_ENTRY *freeTaskEntries;
static TASK_ENTRY taskEntryPool[MAX_RCI_TASKS*4];

static TASK_ENTRY _residentTaskQueue;
static TASK_ENTRY _signalTaskQueue;

TASK_ENTRY *residentTaskQueue = &_residentTaskQueue;
TASK_ENTRY *signalTaskQueue = &_signalTaskQueue;

TASK_ENTRY *getTaskEntry()
{
	TASK_ENTRY *new;

	if ((new = freeTaskEntries) == NULL)
	 { return (NULL);
	 }
	freeTaskEntries = new->next;
	return (new);
}

freeTaskEntry(old)
TASK_ENTRY *old;
{
	old->next = freeTaskEntries;
	freeTaskEntries = old;
}

insertTaskEntry (td, queue)
RCI_DESC *td;
TASK_ENTRY *queue;
{
	/* Insert the task into the indicated queue if it is not
	   already there. 
	*/
		
	TASK_ENTRY *entry;

	for (entry = queue->next; entry != queue; entry = entry->next)
	 { if (entry->td == td)
	    { return (0);
	    }
	 }
	if ((entry = getTaskEntry()) == NULL)
	 { return (-1);
	 }
	entry->td = td;
	LINK (entry, queue);
	return (1);
}

deleteTaskEntry (td, queue)
RCI_DESC *td;
TASK_ENTRY *queue;
{
	/* Delete the task from the indicated queue if it is there */

	TASK_ENTRY *entry;

	for (entry = queue->next; entry != queue; entry = entry->next)
	 { if (entry->td == td)
	    { UNLINK (entry);
	      freeTaskEntry (entry);
	      return (1);
	    }
	 }
	return (0);
}		

taskEntryInit()
{
	int k;

	/* called once by monitorInit() when the monitor is set up. 
	   Initializes the task queue (to NULL) and sets up the list of 
	   free task entries.  The task queue is the list of tasks
	   under the jurisdiction of the monitor in question. It is
	   examined by 'monitorRun()', and entries are placed in it by
	   'taskSetup()'.
	*/

	residentTaskQueue->next = residentTaskQueue->prev = residentTaskQueue;
	signalTaskQueue->next   = signalTaskQueue->prev   = signalTaskQueue;
	for (k=0; k<MAX_RCI_TASKS-1; k++)
	 { taskEntryPool[k].next = &taskEntryPool[k+1];
	 }
	freeTaskEntries = &taskEntryPool[0];
	taskEntryPool[MAX_TIMER_REQS-1].next = NULL;
}

rciSetTrigger (fxn, arg)
int (*fxn)();
int arg;
{
	register RCI_TCB *tcb;

	if (rciTaskId == 0)
	 { rciAbort (EBadTask,
	   	"rciSetTrigger() -- called from planning level\n");
	   return (-1);
	 }
	tcb = rciTaskTable[rciTaskId].tcb;

	if (fxn == NULLF)
	 { if (tcb->triggerAst)
	    { monitorTimerDelete (&tcb->triggerAst);
	    }
	 }
	else
	 { if (tcb->controlMode == IDLE)
	    { tcb->triggerFxn = fxn;
	      tcb->triggerArg = arg;
	    }
	   else
	    { int time;
   
	      if ((time = tcb->runTime) == UNDEF)
	       { time = 0;
	       }
	      if (tcb->triggerAst)
	       { monitorTimerDelete (&tcb->triggerAst);
	       }
	      monitorTimerRequest (&tcb->triggerAst, time, fxn, arg);
	    }
	 }
	return (1);
}

rciWakeupOn (td, fxn, arg)
RCI_DESC *td;
int (*fxn)();
int arg;
{
	RCI_TCB *tcb;

	if ((tcb = td->tcb) == NULL)
	 { rciAbort (EDescriptor,
	   	"rciWakeupOn() -- invalid task descriptor\n");
	   return (-1);
	 }
	tcb->wakeupFxn = fxn;
	tcb->wakeupArg = arg;
	return (1);
}

rciTimerAst (time, fxn, arg)
int time;
int (*fxn)();
int arg;
{
	register RCI_TCB *tcb;

	if (rciTaskId == 0)
	 { rciAbort (EBadTask, 
		"rciTimerAst() -- called from planning level\n");
	   return (-1);
	 }
	tcb = rciTaskTable[rciTaskId].tcb;

	if (tcb->timerAst)
	 { monitorTimerDelete (&tcb->timerAst);
	 }
	monitorTimerRequest (&tcb->timerAst, time, fxn, arg);

	return (1);
}
