/*-=-=-=-=-=-=-=-=-=-=-=- 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:	monitorCom
 *
 * DESCRIPTION:	Routine which execute the various commands sent to the
 *		monitor from the planning 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 "../h/errorStack.h"
#include "../h/assignMem.h"
#include "../h/rciConfig.h"
#include "../h/robot_kynvar.h"

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

extern void *locateSharedMem();

/* ----- RCI global variables ----- */

extern int rciCpu;
extern int rciTaskId;
extern RCI_DESC rciTaskTable[];
extern int rciNumTasks;
extern unsigned char *rciBus;
extern unsigned char *rciIOpage;
extern void *rciMessagePool[MAX_CPUS];
extern RCI_RBT *rciRbtDescList;

extern int rciClockNum;		/* No. of the real time clock	*/
extern float rciClockRes;	/* Resolution of the real time clock */

extern int *rciMaskInterrupts;	/* masks interrupts when set */

/* ----- Communication variables between planning task and monitor ----- */

extern char moncomCmd[MAX_CPUS];
extern char moncomMsgBuf[MAX_CPUS][MON_MSGBUF_SIZE];
extern int moncomStartTime;

extern TASK_ENTRY *residentTaskQueue;

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

static char *outOfMemMsg	= "%s -- out of memory\n";
static char *cantLocateMemMsg	= "%s -- can't locate shared memory 0x%x\n";

monitorCommand(com)
int com;
{
#define COM_MSG(type)	(struct type*)&moncomMsgBuf[rciCpu][0]

	switch (com & ~MON_USERLEVEL)
	 { 
	   case MON_INITIALIZE:
	    { monitorInit();
	      break;
	    }
	   case MON_SETUP_TASK:
	    { monitorSetupTask (COM_MSG(setupTaskMsg));
	      rciNumTasks++;
	      break;
	    }
	   case MON_START_TASKS:
	    { monitorStartTasks (COM_MSG(startTasksMsg));
	      break;
	    }
	   case MON_SETUP_ROBOT:
	    { monitorSetupRobot (COM_MSG(setupRobotMsg));
	      break;
	    }
	   case MON_ATTACH_ROBOT:
	    { monitorAttachRobot (COM_MSG(attachRobotMsg));
	      break;
	    }
	   case MON_DETACH_ROBOT:
	    { monitorDetachRobot (COM_MSG(detachRobotMsg));
	      break;
	    }
	   case MON_START_CLOCK:
	    { monitorStartClock (COM_MSG(clockControlMsg));
	      break;
	    }
	   case MON_IMPORT_CLOCK:
	    { monitorImportClock (COM_MSG(clockControlMsg));
	      break;
	    }
	   case MON_EXPORT_CLOCK:
	    { monitorExportClock (COM_MSG(clockControlMsg));
	      break;
	    }
	   case MON_MSGMEM_ASSIGN:
	    { monitorMsgmemAssign (COM_MSG(msgMemoryMsg));
	      break;
	    }
	 }
}

#define ASSIGN_LOCAL(var,typ,size)  ASSIGN_MEM(var,typ,size,localp)
#define ASSIGN_SHARE(var,typ,size)  ASSIGN_MEM(var,typ,size,sharep)

static int clockExport[MAX_CPUS];

monitorInit()
{
	/* Called once whenever a monitor becomes active (just after a
	   CPU is loaded, in the case of an arbiter moniter, or during
	   the first call to rciCreate(), in the case of the arbiter
	   monitor.
	 */
	int k;

	for (k=0; k<MAX_CPUS; k++)
	 { clockExport[k] = 0;
	 }
	taskEntryInit();
	monitorTimerInit();
}

monitorSetupTask(msg)
register struct setupTaskMsg *msg;
{
	/* Called by the monitor whenever a task is created or imported.
	   Sets up the data structures associated with a task, as 
	   described in the 'msg' data structure */

	char *fxnName = "monitorSetupTask()";

	RCI_DESC *td;
	MSG_ROOT *mr;
	char *sharep;
	char *localp;
	int k;
	
	if ((sharep = (char*)locateSharedMem (msg->shareMem)) == NULL)
	 { rciAbort (EInternal, cantLocateMemMsg, fxnName, msg->shareMem);
	   return (-1);
	 }
	localp = (char*)msg->localMem;
	
	/* allocate task descriptor and set it up */

	td = &rciTaskTable[msg->id];
	td->taskID = msg->id;
	td->taskCode = (1<<msg->id);
	td->interruptNumber = UNDEF;	/* other numbers assigned later */
	td->signalNumber = 0;		/* signal number assigned later */
	td->cpu = msg->taskCpu;

	for (k=0; k<MAX_TASKROBOTS; k++)
	 { td->rbts[k] = NULL;
	 }

	/* now set up the tcb */

	if (msg->id == 0)		/* planning task doesn't have these */
	 { td->tcb = NULL;
	   td->dev = NULL;
	 }
	else
	 { ASSIGN_SHARE (td->tcb, RCI_TCB*, sizeof(RCI_TCB));
	   if (msg->flags & TASK_HOST)
	    { taskControlBlockInit (td->tcb);
	    }
	   ASSIGN_SHARE (td->dev, void*, sizeof(IO_DEVICE));
	 }

	/* set up the message areas */

	ASSIGN_LOCAL (mr, MSG_ROOT*, sizeof(MSG_ROOT));
	ASSIGN_SHARE (mr->free, NOTICE_BUF*, sizeof(NOTICE_BUF));
	ASSIGN_SHARE (mr->recv, NOTICE_BUF*, sizeof(NOTICE_BUF));
	if (msg->flags & TASK_HOST)
	 { void *pool;
	   ERROR_STACK_DESC *estk;

	   ASSIGN_LOCAL (mr->msgs, NOTICE_PACKET*, 
		   	 sizeof(NOTICE_PACKET)*MAX_RCI_TASKS);
	   taskMessageInit (mr);
	   td->msg = (void*)mr;

	   /* set up an error stack for the task */

	   ASSIGN_LOCAL (estk, ERROR_STACK_DESC*, sizeof(ERROR_STACK_DESC));
	   ASSIGN_LOCAL (pool, void*, ERROR_STACK_SIZE);
	   errStackInit (estk, pool, ERROR_STACK_SIZE);
	   td->estk = (void*)estk;
	 }
	else
	 { mr->msgs = NULL;
	   td->msg = (void*)mr;
	   td->estk = NULL;
	 }

	if (msg->flags & TASK_HOST)
	 { insertTaskEntry (td, residentTaskQueue);
	 }
	else
	 { td->estk = NULL;
	 }

	putRciName (msg->name, msg->id);
#if 0
	rciPrintf ("Task %s %s\n", msg->name, 
		   msg->flags & TASK_HOST ? "created" : "imported");
#endif

	return (1);
}


monitorStartTasks (msg)
register struct startTasksMsg *msg;
{
	/* Called on the arbiter monitor only, each time an rciStart()
	   request is issued that specifies clocked tasks (i.e., tasks
	   running ON_TRIGGER or ON_CLOCK). A common start time is
	   established for each of these tasks, and then start signals
	   are sent to their monitors. If some of the tasks are resident
	   on the arbiter cpu, then the start sugnal handler is called
	   directly at the end of this function.
	 */

	int cpu;
	int delay;

#define MINDELAY 50		/* delay start at least 50 msec */

	delay = ((MINDELAY-1)/(int)rciClockRes + 1)*rciClockRes;

	moncomStartTime = rciTime() + delay;
	for (cpu = 0; cpu < MAX_CPUS; cpu++)
	 { if (msg->activate[cpu] && cpu != ARBITER_CPU)
	    { rciSignalAux(cpu, MON_COMSIG);
	      msg->activate[cpu] = 0;
	    }
	 }
	if (msg->activate[ARBITER_CPU])
	 { monitorSignalHandler (0);
	   msg->activate[ARBITER_CPU] = 0;
	 }
}

monitorSetupRobot (msg)
register struct setupRobotMsg *msg;
{
	/* Called once on each monitor when a robot is first set up (which
	   will happen the first time a call to rciAttachRobot() is made
	   for each particular robot). It instantiates read-only parameters 
	   that describe the robot in question.
	 */

	char *fxnName = "monitorSetupRobot()";

	RCI_RBT *rbt;

	char *localp = (char*)msg->localMem;

	ASSIGN_LOCAL (rbt, RCI_RBT*, sizeof(RCI_RBT));
	rbt->id = msg->id;
	rbt->class = msg->class;

	rbt->kynSize = msg->kynSize;
	rbt->varSize = msg->varSize;

	rbt->startup = msg->comfxns.startup;
	rbt->release = msg->comfxns.release;
	rbt->input = msg->comfxns.input;
	rbt->output = msg->comfxns.output;

	ASSIGN_LOCAL (rbt->dev, void*, sizeof(IO_DEVICE));
	ASSIGN_LOCAL (rbt->kyn, KYN*, sizeof(KYN));
	ASSIGN_LOCAL (rbt->kyn->gen, GEN_KYN*, sizeof(GEN_KYN));
	ASSIGN_LOCAL (rbt->kyn->ext, void*, rbt->kynSize);
	ASSIGN_LOCAL (rbt->jls, JLS*, sizeof(JLS));
	ASSIGN_LOCAL (rbt->name, char*, msg->nameLength);

	bcopy (msg->kynGen, rbt->kyn->gen, sizeof(GEN_KYN));
	bcopy (msg->kynPar, rbt->kyn->ext, rbt->kynSize);
	bcopy (msg->jls, rbt->jls, sizeof(JLS));
	strcpy (rbt->name, msg->name);

	/* add to the list for this CPU */
	rbt->next = rciRbtDescList;				
	rciRbtDescList = rbt;
}

monitorAttachRobot (msg)
register struct attachRobotMsg *msg;
{
	/* Called on each monitor every time the user calls attaches
	   on robot to a particular task (using rciAttachRobot()).
	   This instantiates the RCI_RBT structure for the robot
	   in question, and places a pointer to it in the 'rbts' field
	   of the task descriptor 'td'. It also increments the count
	   of the number of robots attached to that task.
	 */
	char *fxnName = "monitorAttachRobot()";

	RCI_DESC *td;
	RCI_RBT *rbt;
	char *sharep;

	rbt = (RCI_RBT*)msg->localMem;
	*(IO_DEVICE*)rbt->dev = msg->dev;
	rbt->idev = msg->idev;		/* used by planner only */

	if ((sharep = (char*)locateSharedMem (msg->shareMem)) == NULL)
	 { rciAbort (EInternal, cantLocateMemMsg, fxnName, msg->shareMem);
	   return (-1);
	 }

	td = &rciTaskTable[msg->taskId];

	ASSIGN_SHARE (rbt->how, HOW*, sizeof(HOW));
	if (msg->flags & ROBOT_HOST)
	 { howInit (rbt->how);
	 }
	ASSIGN_SHARE (rbt->chg, CHG*, sizeof(CHG));
	if (msg->flags & ROBOT_HOST)
	 { chgInit (rbt->chg);
	 }
	ASSIGN_SHARE (rbt->var, void*, rbt->varSize);
	if (msg->flags & ROBOT_HOST)
	 { bzero (rbt->var, rbt->varSize);
	 }
	td->rbts[msg->num] = rbt;
	td->numRobots++;

	return (1);
}

monitorDetachRobot (msg)
register struct detachRobotMsg *msg;
{
	/* Called one each monitor every time a robot is detached from
	   a task (using the function rciDetachRobot()). All it does
	   is zero the entry in the 'rbts' field of the task descriptor
	   and decrement the count of robots attached to that task.
	 */

	RCI_DESC *td;

	td = &rciTaskTable[msg->taskId];
	td->rbts[msg->num] = NULL;
	td->numRobots--;
	return (1);
}


extern int moncomClockOffset;

monitorStartClock (msg)
register struct clockControlMsg *msg;
{
	/* Called on the RCI cpu which is running the system clock,
	   to initialize that clock to either 0 or the value
	   of the previous target.
	 */
	rciStartClock (msg->hostCpu, msg->resolution, msg->oldResolution);
	rciClockRes = msg->resolution;
	rciClockNum = rciCpu;
	return(1);
}

monitorExportClock (msg)
register struct clockControlMsg *msg;
{
	/* Invoked on the monitor of the CPU that is hosting the RCI system 
	   clock to setup a broadcast of that clock to another CPU */

	rciExportClock (msg->targetCpu, msg->exportCode);
	return (1);
}

monitorImportClock (msg)
register struct clockControlMsg *msg;
{
	/* Called on the monitor of a CPU which is not hosting the
	   RCI system clock. This sets the RCI internal parameters
	   necessary for the CPU to use the external clock */

	rciImportClock (msg->hostCpu, msg->resolution, msg->reInitClock);
	rciClockRes = msg->resolution;
	rciClockNum = msg->hostCpu;
	return (1);
}

monitorMsgmemAssign (msg)
register struct msgMemoryMsg *msg;
{
	char *fxnName = "monitorMsgmemAssign()";
	void *addr;

	if ((addr = locateSharedMem(msg->addr)) == NULL)
	 { rciAbort (EInternal, cantLocateMemMsg, fxnName, msg->addr);
	   return (-1);
	 }
	rciMessagePool[msg->cpu] = addr;
	if (msg->cpu == rciCpu)
	 { monitorMessageInit();
	 }
	return (1);
}
