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

#include <sys/time.h>	/* for setitimer() */
#include <vxWorks.h>
#include <semLib.h>
#include <signal.h>

#include "../h/cdefs.h"
#include "vxwCtrlTask.h"

extern SEM_ID *rciCtrlSem;

EXEC_REQ taskExr;
EXEC_REQ clockExr;
EXEC_REQ timerExr;
EXEC_REQ *intrExrList = NULL;
int clockReqCount;
int clockReqPeriod;
int timerReqCount;
int taskReqRval;
int rciClockCount;

#define POLL_AND_EXECUTE(e)	\
	if ((e).request)	\
	 { (e).request = 0;	\
	   (*(e).fxn)((e).arg);	\
	 }

#define CLEAR_EXEC_REQ(e)	\
	(e).request = 0;	\
	(e).fxn = NULLF;	\
	(e).arg = 0;

void rciCtrlTask(sig)
{
	int rval;

	clockReqCount = UNDEF;
	clockReqPeriod = UNDEF;
	timerReqCount = UNDEF;
	taskReqRval = 0;

	setItimerValues(0,0);

	CLEAR_EXEC_REQ(taskExr);
	CLEAR_EXEC_REQ(clockExr);
	CLEAR_EXEC_REQ(timerExr);

	intrExrList = NULL;

	vxwCrashInit(sig);

	while (1)
	 { EXEC_REQ *exr;

	   semTake(rciCtrlSem);

	   POLL_AND_EXECUTE (timerExr);
	   for (exr=intrExrList; exr; exr=exr->next)
	    { POLL_AND_EXECUTE (*exr);
	    }
	   POLL_AND_EXECUTE (clockExr);
	   if (taskExr.request)
	    { /* request is cleared AFTER to provide an ack */
	      taskReqRval = (*taskExr.fxn)(taskExr.arg);
	      taskExr.request = 0;
	    }
	 }
}

#define USEC_TO_SEC	(1.0e-6)
#define SEC_TO_USEC	(1.0e6)

timevalToTicks(tv,rate)
struct timeval *tv;
int rate;
{
	return (rate*((double)tv->tv_sec + USEC_TO_SEC*tv->tv_usec));
}

ticksToTimeval(tv,ticks,rate)
struct timeval *tv;
int ticks, rate;
{
	tv->tv_sec = ticks/rate;
	tv->tv_usec = (ticks%rate)/(double)rate*SEC_TO_USEC;
}

setitimer(which, value, ovalue)
int which;
struct itimerval *value, *ovalue;
{
	/* Substitute for the setitimer routine on UNIX. Have the
	   RCI system clock deliver a SIGALRM to the planning task
	   at a specified time or rate.
	 */
	int oldValue;
	int oldInterval;
	int newValue;
	int newInterval;
	int rate;
	int s;
	
	rate = sysClkRateGet();
	newValue = timevalToTicks (&value->it_value, rate);
	newInterval = timevalToTicks (&value->it_interval, rate);

	s = VXINT_LOCK();
	getItimerValues (&oldInterval, &oldValue);
	setItimerValues (newInterval, newValue);
	VXINT_UNLOCK(s);
	if (ovalue)
	 { ticksToTimeval (&ovalue->it_value, oldValue, rate);
	   ticksToTimeval (&ovalue->it_interval, oldInterval, rate);
	 }
	return(1);
}

#endif
