/* Functions which implement the RCI control level on an SGI Irix machine */

#ifdef SGI_CFG

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

#include <sys/types.h>
#include <sys/time.h>
#include <sys/prctl.h>
#include <sys/schedctl.h>
#include <sys/sysmips.h>
#include <sys/sysmp.h>
#include <sys/lock.h>

#include "../sys/rciext/rciioctl.h"

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

#include "sgiCtrlTask.h"

#define CTRL_STACK_SIZE	0x40000

/* ----- 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";

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

FILE *rciStdout;

fd_set intrDevFds;
IntrDevDesc *intrDevList = NULL;

extern int rciSignalRefCount;
extern int rciSimOnly;

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

static int (*beatFxn)();
static int beatArg;
static int beatPeriod;

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

static growStack();

static int onXcpu()
{
	/* Called when the planning level exits and sends the
	   control level an XCPU message. */

	exit (-1);
}

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

int pbset;
int first;

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

#define BLOCK_PROC	0

static execCtrlFxn(fxn, arg)
int (*fxn)();
int arg;
{
	/* The planning level task is blocked to prevent conflicts 
	   when executing on a multi-processor SGI CPU */

	extern int rciMainTaskPid;
	int status;
	volatile int spin;
#if BLOCK_PROC
	blockproc(rciMainTaskPid);
	spin=0;
	do
	 { status = prctl(PR_ISBLOCKED, rciMainTaskPid);
	 }
	while (status == 0 && ++spin < 1000);
	if (spin >= 1000)
	 { printf ("Control level: timeout suspending planning task\n");
	   exit (-1);
	 }
#endif
	pbset = 1;
	rciPrintfBufp = rciPrintfBuf;
/*	sysmips(FLUSH_CACHE); */
	(*fxn)(arg);	
	pbset = 0;
/*	sysmips(FLUSH_CACHE); */
	if (rciPrintfBufp != rciPrintfBuf)
	 { fprintf (rciStdout, "%s", rciPrintfBuf);
	   fflush (rciStdout);
	   rciPrintfBufp = rciPrintfBuf;
	 }
#if BLOCK_PROC
	unblockproc(rciMainTaskPid);
#endif
}

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

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

	if (plock(PROCLOCK) < 0)    /* not sure if we inherit plock() */
	 { osperror("plock:");	    /* from the parent or not. */
	   exit (-1);
	 }
  	sgiLockStack(CTRL_STACK_SIZE);

#if 1
	if (schedctl(NDPRI, 0, NDPHIMAX) < 0)
	 { osperror("schedctl:");
	   exit (-1);
	 }
#endif
#if 1
	signal (SIGTSTP, SIG_IGN);
	signal (SIGINT,  SIG_IGN);
#endif
	signal (SIGXCPU, onXcpu);

	prctl (PR_RESIDENT);

	alarmFxn = NULLF;
	beatFxn = NULLF;
	execFxn = NULLF;

	nap (0.02);		    /* wait for all this to sink in */

	/* 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)
	       { 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);
	    }
	 }		
}

rciSchedAst (clock, time, fxn, arg)
int clock, time;
int (*fxn)();
int arg;
{
#if 0
extern int lastschedast_tick;
lastschedast_tick = time;
#endif

	/* This function is always called from the control task context. */
	/* clock is ignored for now */

	alarmFxn = fxn;
	alarmArg = arg;
	alarmTime = time;

	if (ioctl (rciClockFd, RCIIOC_ALARM_REQ, time) < 0)
	 { osperror ("rciCtrlTask: ALARM_REQ ioctl -- ");
	   exit (-1);
	 }
}


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

	char *fxnName = "rciClockExec()";

#if !defined(SIMONLY_CFG)
	if (rciSimOnly)
	 { return (1);
	 }
	beatPeriod = ticks;
	beatFxn = fxn;
	beatArg = arg;
	if (ioctl (rciClockFd, RCIIOC_BEAT_REQ, ticks) < 0)
	 { errorMsg (EBadDev, badIoctlMsg, fxnName, "BEAT_REQ", oserror());
	   return (-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;
	 }
}

sgiLockStack(size)
int size;
{
	int pagesize;
	char *cp, *stackbase;
	volatile char c;

	pagesize = getpagesize();
	/* this SHOULD be PRDA->usrstk; */
	stackbase = (char*)&pagesize;
	for (cp=stackbase; cp>=stackbase-(size+pagesize)+1; cp-=pagesize)
	 { c = *cp;
	 }
	if (mpin (stackbase-size+1, size) < 0)
	 { osperror ("lockStack: mpin -- ");
	   exit (-1);
	 }
}

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