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

/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	rciCtrlTask
 *
 * DESCRIPTION:	 RCI control level on an Real-Time Linux  machine
 *
 * AUTHOR/DATE:	Matthew R. Stein Summer 2001 based on John Lloyd, Fall 1988
 *
 * MODIFICATION HISTORY:
 *
 *              MS  Summer 2001 replace read from clock device with read from
 *              real-time fifo
 *		John Lloyd, October 1989: Converted old 'object' package into
 *		the 'allocMem' package.
 *
 *--------------------------------------------------------------------------*/
/* Functions which implement the */

#ifdef LINUX_CFG

#include <stdio.h>
#include <signal.h>
#include <errno.h>

#include <sys/types.h>
#include <sys/time.h>
#include <sys/prctl.h>

#include "../h/cdefs.h"
#include "../h/errorCodes.h"

#include "../h/fifos.h"

#include "linCtrlTask.h"

#define CTRL_STACK_SIZE	0x40000
#define RET_ERROR	rciHostClose(); return (-1);

/* ----- Error Messages. Defined externally to save space ----- */

static char *badIoctlMsg	= "%s -- %s ioctl failed, errno %d\n";
static char *noResponseMsg	= "%s -- can't signal control task\n";
static char *cantOpenRciDev	= "%s -- can't open %s, errno %d\n";

int rciIsOpen;
int rciCtrlTaskPid;
int rciMainTaskPid;
int rciClockFd;
int taskCtrlFd;
int rciPlanCtrlFds[2];
int rciCtrlPlanFds[2];

FILE *rciStdout;

fd_set intrDevFds;
IntrDevDesc *intrDevList = NULL;

extern int rciSignalRefCount;
extern int rciSimOnly;
/* MS */
extern int errno;

static int (*alarmFxn)();
static int alarmArg;
static int alarmTime;

static int (*beatFxn)();
static int beatArg;
static int beatPeriod;
static int beatRunning = 0;

static int (*execFxn)();
static int execArg;

static growStack();
extern int rciClockTick();


static void onXcpu()
{
	/* Called when the planning level exits and sends the
	   control level an XCPU message. */
  rciCtrlTaskPid=0;
  exit (-1);
}
/* Added by MS */
static void onInt()
{
	/* Called when the planning level is killed by SIGINT
	   want also to ask JLS to stop and then exit control level. */
  if (rciStopJLS() < 0)
    { rciAbort (ENoDev, "Failed to stop JLS\n");
    }
  exit (-1);
}

osperror(s)
char *s;
{
	fprintf (stderr, "%s: %s\n", s, sys_errlist[errno]);
}

int pbset;
int first;

char rciPrintfBuf[1024];
char *rciPrintfBufp;
extern FILE *rciStdout;


static execCtrlFxn(fxn, arg)
int (*fxn)();
int arg;
{
	/* The planning level task is blocked to prevent conflicts 
	   when executing*/

	extern int rciMainTaskPid;
	int status;
	volatile int spin;

	pbset = 1;
	rciPrintfBufp = rciPrintfBuf;

	(*fxn)(arg);	
	pbset = 0;

	if (rciPrintfBufp != rciPrintfBuf)
	 { fprintf (rciStdout, "%s", rciPrintfBuf);
	   fflush (rciStdout);
	   rciPrintfBufp = rciPrintfBuf;
	 }
}

rciCtrlTask()
{
	fd_set rfds, intrDevSav;
	int time;
	int n, k;
	char com;

	/* Start up the control task, set priorites. */

	signal (SIGTSTP, SIG_IGN);

	/* MS signal (SIGINT,  SIG_IGN); */
	signal (SIGINT,  onInt);
		
	signal (SIGXCPU, onXcpu);
	
	alarmFxn = NULLF;
	beatFxn = NULLF;
	execFxn = NULLF;


	/* send acknowledge to main task that we are here. */

	com = RCI_CTRL_ACK;
	write (rciCtrlPlanFds[1], &com, 1);

	while (1)
	 { register IntrDevDesc *id;

	   /* The task sits in a select polling loop, looking for

	      1. input from the clock device, which indicates clock 
		 tick expiry and the need to run a timed task;

	      2. input on any of the RCI interrupt devices, which
		 indicates an interrupt has arrived and we should
		 run in the interrupt monitor;

	      3. input from the parent task, which indicates 
		 the parent wants us to execute an RCI function.
	    */

	   do
	    { rfds = intrDevSav = intrDevFds;
	      FD_SET (rciPlanCtrlFds[0], &rfds);
	      FD_SET (rciClockFd, &rfds);
	      n = select (FD_SETSIZE, &rfds, (fd_set*)0, (fd_set*)0,
					     (struct timeval*)0);
	      if (n<0 && memcmp(&intrDevSav,&intrDevFds,sizeof(fd_set)) == 0)
	       { /* we check intrDevFds because the parent may have closed
		    one of the files there, causing select to return EBADF;
		    this is OK and we should not exit then. */
		 osperror ("rciCtrlTask: error in select");
		 exit (-1);
	       }
	    }
	   while (n<0);
	   for (id=intrDevList; id; id=id->next)
	    { if (FD_ISSET(id->fd, &rfds))
	       { 
		 /* interrupt occured on device fd. */
		 if ((n = read_restart (id->fd, &com, 1)) < 0)
		  { osperror ("rciCtrlTask: read from intr -- ");
		    exit (-1);
		  }
		 if (n == 0)
		  { fprintf (stderr,
"rciCtrlTask: read from intr device returns wrong size\n");
		    exit (-1);
		  }
		 if (id->fxn)
		  { execCtrlFxn (id->fxn,id->arg);
		  }
	       }
	    }
	   if (FD_ISSET(rciClockFd, &rfds))
	     { RciTimerEvent tev;

	      if ((n = read_restart (rciClockFd, (char*)&tev, sizeof(tev))) < 0)
	       { osperror ("rciCtrlTask: read from clock -- ");
		 exit (-1);
	       }
	      if (n != sizeof(tev))
	       { fprintf (stderr,
"rciCtrlTask: read from clock device returns wrong size\n");
		 exit (-1);
	       }
	      if (tev.req & RCI_ALARM_READY)
	       { if (alarmFxn)
		  { execCtrlFxn (alarmFxn, alarmArg);
		  }
	       }
	      if (tev.req & RCI_BEAT_READY)
	       { 
		 rciClockTick();
		 if (beatFxn)
		  { execCtrlFxn (beatFxn, beatArg);
		  }
	       }

	    }
	   if (FD_ISSET(rciPlanCtrlFds[0], &rfds))
	    { /* parent task wants us to do something */

	      if ((n = read_restart (rciPlanCtrlFds[0], &com, 1)) < 0)
	       { osperror ("rciCtrlTask: read from plan -- ");
		 exit (-1);
	       }
	      if (n == 0)
	       { fprintf (stderr,
"rciCtrlTask: read from plan device returns wrong size\n");
		 exit (-1);
	       }
	      switch (com)
	       { case RCI_CTRL_EXEC:
		  { if (execFxn)
		     { execCtrlFxn (execFxn, execArg);
		       execFxn = 0;
		     }
		    break;
		  }
		 default:
		  { fprintf (stderr,
"rciCtrlTask: bogus command (0x%x)from planning task\n", com);
		    exit (-1);
		  }
	       }
	      com = RCI_CTRL_ACK;
	      write (rciCtrlPlanFds[1], &com, 1);
	    }
	 }		
}

rciStartJLS()
{
 char *fxnName = "rciSchedAst()";
  jlsMessageType msg;

  /* This function is always called from the control task context. */
  /* Start the JLS action of the real-time control process. Process is
     already running periodically, this just turns the joint-level
     servo loop on */
  msg.command=START_TASK;
  msg.arg=0;
  if (write(taskCtrlFd, &msg, sizeof(msg)) < 0) {
    errorMsg (EBadAccess, cantOpenRciDev, fxnName, 
	      "TaskConrolFd", errno);
    RET_ERROR;
  }
  
}

rciStopJLS()
{
 char *fxnName = "rciStopJLS()";
  jlsMessageType msg;

  /* This function is always called from the control task context. */
  /* Stop the JLS action of the real-time control process. */
  msg.command=STOP_TASK;
  msg.arg=0;
  if (write(taskCtrlFd, &msg, sizeof(msg)) < 0) {
    errorMsg (EBadAccess, cantOpenRciDev, fxnName, 
	      "TaskConrolFd", errno);
    RET_ERROR;
  }
  
}


rciSchedAst (clock, time, fxn, arg)
int clock, time;
int (*fxn)();
int arg;
{
  char *fxnName = "rciSchedAst()";
  jlsMessageType msg;

  /* This function is always called from the control task context. */
  /* clock is ignored for now */
  /* This function typically sets alarmFxn to monitorTimerHandler so that
     this function is activated on every alarm timer event */   
	alarmFxn = fxn;
	alarmArg = arg;
	alarmTime = time;
	   /* MS This is new code for real-time linux.  We have a real-time
	      process for joint-level-servo.  Send a message to this process
	      via real-time fifo that is should start sending us periodic
	      messages on another real-time fifo.  This does not yet activate
	      joint-level control but is should begin waking up the control
	      task "rciCtrlTask" periodically.
	   */
	/*if(time == 0) msg.command=STOP_ALRM_MSG;
	  else */
	msg.command=START_ALRM_MSG;
	msg.arg=time;
	if (write(taskCtrlFd, &msg, sizeof(msg)) < 0) {
	  errorMsg (EBadAccess, cantOpenRciDev, fxnName, 
		    "TaskConrolFd", errno);
	  RET_ERROR;
	}
	
}


rciClockExec (ticks, fxn, arg)
int ticks;
int (*fxn)();
int arg;
{
  /* Always called in the planning task context, when a monitor is
     being requested on the arbiter process.*/
  /* This function called by arbMonitor .c it sets beatFxn. In turn beatFxn
     is called when a beat timer event is received by rciCtrlTask, a spawned
     process that waits for data on the rtfifo.
  */
  char *fxnName = "rciClockExec()";
  jlsMessageType msg;

#if !defined(SIMONLY_CFG)
	if (rciSimOnly)
	 { return (1);
	 }
	beatArg = arg;
	beatPeriod = ticks;
	beatFxn = fxn;

	/* In Linux implementation Beat is always running when
	   rci is running, so when adding other beat finctions
	   using arbMonitor.c (e.g. rccl setp functions) do not
	   need to re-send the start beat command. */
	/* Currently turning the beat off is unimplemented, but this
	   should be fixed 


		if(ticks == 0) msg.command=STOP_BEAT_MSG;
		else msg.command=START_BEAT_MSG; 
		beatRunning=0;
           */

	if(!beatRunning) {
	  msg.command=START_BEAT_MSG;
	  msg.arg=ticks;
	  if (write(taskCtrlFd, &msg, sizeof(msg)) < 0) {
	    errorMsg (EBadAccess, cantOpenRciDev, fxnName, 
		      "TaskControlFd", errno);
	    RET_ERROR;
	  }
	  beatRunning = 1;
	}
	
#endif
	return(1);
}

rciTaskExec (fxn, arg)
int (*fxn)();
int arg;
{
	/* Always called in the planning task context. */

	char *fxnName = "rciTaskExec()";
	char com;
	int n;

	if (rciSimOnly)
	 { if (rciSignalRefCount > 0)
	    { blockRciSignals();
	    }
	   (*fxn)(arg);
	   if (rciSignalRefCount > 0)
	    { unblockRciSignals();
	    }
	 }
	else
	 { 
#ifndef SIMONLY_CFG
	   execFxn = fxn;
	   execArg = arg;
	   com = RCI_CTRL_EXEC;
	   if (write (rciPlanCtrlFds[1], &com, 1) < 0 ||
		read_restart (rciCtrlPlanFds[0], &com, 1) <= 0 ||
		com != RCI_CTRL_ACK)
	    { errorMsg (EInternal, noResponseMsg, fxnName);
	      return (-1);
	    }
#endif
	 }
	return(1);
}

#define CTRL_STACK_SIZE	0x40000

static growStack()
{
	char stack[CTRL_STACK_SIZE];
	int pagesize;
	char *cp;
	volatile char c;
	
	pagesize = getpagesize();
	for (cp=stack; cp<(&stack[CTRL_STACK_SIZE]); cp+=pagesize)
	 { c = *cp;
	 }
}


#else	/* !defined(LINUX_CFG) */
static int keepRanlibHappy;
#endif
