/*-=-=-=-=-=-=-=-=-=-=-=- 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:	attachRobot
 *
 * DESCRIPTION:	Routines for connecting (or disconnecting) a robot
 *		to (or from) an RCI control task.
 * 
 * AUTHOR/DATE:	John Lloyd, Fall 1987
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#ifndef VXWORKS_CFG
#include <sys/file.h>
#endif
#include <errno.h>

#include "../h/cdefs.h"
#include "../h/errorCodes.h"
#include "../h/rciMacros.h"
#include "../h/robot_jls.h"
#include "../h/robot_kynvar.h"
#include "../h/robotComfxns.h"
#include "../h/IOdevice.h"
#include "../h/assignMem.h"
#include "../h/rciConfig.h"
#include "../h/rciTask.h"

#include "../rci/robotDefs.h"
#include "../rci/link.h"
#include "../rci/loadRobot.h"

#include "../rci/rciAutoAbort.h"

#ifdef SGI_CFG
#include "../sys/rciext/rciioctl.h"
#endif

ROBOT_DESC *robotList = NULL;

static int pageSize = 0;

extern void *malloc();
extern char *copyofstr();
extern unsigned long rciControlCpus;	/* active cpu mask */

static initRobotList();

static initDone = 0;
#define INIT_CHECK()	if (!initDone) doInit();
static doInit()
{
	initDone = 1;
	if (robotList)
	 { ROBOT_DESC *rdesc, *rdesc_next;
	   for (rdesc=robotList->next; rdesc!=robotList; rdesc=rdesc_next)
	    { rdesc_next = rdesc->next;
	      cleanupRobotDesc(rdesc);
	      FREE(rdesc);
	    }
	   FREE(robotList);
	   robotList = NULL;
	 }
	requestFlagInit(&initDone);
	return (1);
}

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

static char *nullMsg		= "%s --\n";
static char *taskIsActive	= "%s -- task %s is presently active\n";
static char *tooManyRobots	= "%s -- can only attach %d robots\n"; 
static char *rbtIsBusy		= "%s -- rbt %s: already attached to task\n";
static char *noAddressSpec	= "%s -- rbt %s: can't get address spec.\n";
static char *readIntrInfo	= "%s -- rbt %s: can't read interrupt info\n";
static char *badIntrInfo	= "%s -- rbt %s: bad interrupt info\n";
static char *cantOpenIntrDev	= "%s -- rbt %s: can't open interrupt dev.\n";
static char *rbtNotDef		= "%s -- rbt %s: not defined in RCI tables\n";
static char *rbtClassNotDef	= "%s -- rbt %s: class unknown\n";
static char *rbtIfxnsNotDef	= "%s -- rbt %s: no interface fxns defined\n";
static char *rbtNoJLS		= "%s -- rbt %s: can't get JLS structure\n";
static char *rbtNoKyn		= "%s -- kyn %s: can't get KYN structure\n";
static char *rbtNoVar		= "%s -- kyn %s: can't get VAR structure\n";
static char *outOfMemory	= "%s -- out of memory\n";
static char *cantSetupRbt	= "%s -- can't set up robot %s\n";
static char *rbtBadAddr		= "%s -- rbt %s: I/O device badly defined\n";
static char *rbtAllocIO		= "%s -- rbt %s: can't alloc I/O device\n";
static char *rbtIODevBusy	= "%s -- rbt %s: I/O device (%s,0x%x) busy\n";
static char *rbtCantAttach	= "%s -- rbt %s: can't attach to task\n";
static char *rbtNotAttached	= "%s -- robot not attached to task %s\n";
static char *rbtCantDetach	= "%s -- rbt %s: can't detach from task\n";
static char *cantAttachRbt	= "%s -- can't attach robot on CPU %d\n";
static char *missingIntrInfo	= "%s -- missing ipl, iaddr, or vector info\n";
static char *badIntrName	= "%s -- malformed interrupt name\n";
static char *rbtIntrDevBusy	= "%s -- rbt %s: interrupt device busy\n";
static char *cantDereferIntr	= "%s -- can't dereference interrupt\n";

ROBOT_DESC *createRobotDesc(name)
char *name;
{
	char *fxnName = "createRobotDesc()";

	ROBOT_DESC desc;	/* tmp allocation */
	ROBOT_DESC *rdesc;

	bzero (&desc, sizeof(ROBOT_DESC));
	
	desc.nameLength = strlen(name)+1;
	desc.name = copyofstr(name);

	if (!(desc.id = find_robot(name)) )
	 { errorMsg (ENotDefined, rbtNotDef, fxnName, name);
	   goto rdescCreateError;
	 }
	if (!(desc.class = robot_class(name)) )
	 { errorMsg (ENotDefined, rbtClassNotDef, fxnName, name);
	   goto rdescCreateError;
	 }
	if (!(get_comfxns (&desc.comfxns, name)) )
	 { errorMsg (EBadFxn, rbtIfxnsNotDef, fxnName, name);
	   goto rdescCreateError;
	 }
	if ((desc.jls = getJls(name)) == NULL)
	 { errorMsg (getErrorCode(0), rbtNoJLS, fxnName, name);
	   goto rdescCreateError;
	 }
	if ((desc.kyn = getKyn(name, &desc.kynSize)) == NULL)
	 { if (getErrorCode(0) != EBadFxn)
	    { errorMsg (getErrorCode(0), rbtNoKyn, fxnName, name);
	      goto rdescCreateError;
	    }
	   else
	    { clearErrors();
	      desc.kynSize = 0;
	    }
	 }
	desc.varSize = getVarSize(name);
	if ((desc.var = malloc(desc.varSize)) == NULL)
	 { errorMsg (EMemory, outOfMemory, fxnName);
	   goto rdescCreateError;
	 }
	if (pageSize == 0)
	 { pageSize = getpagesize();
	 }

	/* compute sizes of the robot memory regions */

	desc.privSize =  ALIGN_SIZE(sizeof(RCI_RBT)) +
			 ALIGN_SIZE(sizeof(IO_DEVICE)) +
			 ALIGN_SIZE(sizeof(JLS)) +
			 ALIGN_SIZE(desc.kynSize) + 
			 ALIGN_SIZE(sizeof(KYN)) +
			 ALIGN_SIZE(sizeof(GEN_KYN)) + 
			 ALIGN_SIZE(desc.nameLength);

	desc.shareSize = ALIGN_SIZE(sizeof(HOW)) +
			 ALIGN_SIZE(sizeof(CHG)) +
			 ALIGN_SIZE(desc.varSize);

	if (desc.privSize % pageSize)
	 { desc.privSize += pageSize - (desc.privSize % pageSize);
	 }
	if (desc.shareSize % pageSize)
	 { desc.shareSize += pageSize - (desc.shareSize % pageSize);
	 }

	desc.td = 0;
	
	if ((rdesc = ALLOC(ROBOT_DESC)) == NULL)
	 { errorMsg (EMemory, outOfMemory, fxnName);
	   goto rdescCreateError;
	 }

	/* now set up the robot information in all the control cpus */

	if (setupRobot (&desc, rciControlCpus|ARBITER_MASK) < 0)
	 { errorMsg (getErrorCode(0), cantSetupRbt, fxnName, name);
	   free (rdesc);
	   goto rdescCreateError;
	 }

	/* interrupt and device info will be assigned later */

	desc.intrDesc = (RobotIdevDesc*)0;

	*rdesc = desc;

	/* allocate space for name and copy it */

	if ((rdesc->name = (char*)malloc(desc.nameLength)) == NULL)
	 { errorMsg (EMemory, outOfMemory, fxnName);
	   free (rdesc);
	   goto rdescCreateError;
	 }
	strcpy (rdesc->name, name);

	/* information in the descriptor checks out. Link it in */

	LINK (rdesc, robotList);
	return (rdesc);
rdescCreateError:
	cleanupRobotDesc(&desc);
	return (NULL);
}

rciAttachRobot (td, name, rdev)
RCI_DESC *td;
const char *name;
struct devAddr *rdev;
{
	char *fxnName = "rciAttachRobot()";

	ROBOT_DESC *rdesc;
	IO_DEVICE dev;
	int status;
	int k;

	INIT_CHECK();

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (!robotList)
	 { if (initRobotList(fxnName) < 0)
	    { RETURN_ERROR(-1);
	    }
	 }

	/* --- make sure that the task is not active --- */

	if (td->tcb->controlMode == ACTIVE)
	 { errorMsg (ETaskActive, taskIsActive, fxnName, rciTaskName(td));
	   RETURN_ERROR(-1);
	 }

	/* check that we can attach another robot to this task */

	if (TASKNUMROBOTS(td) >= MAX_TASKROBOTS)
	 { errorMsg (ETooMany, tooManyRobots, fxnName, MAX_TASKROBOTS);
	   RETURN_ERROR(-1);
	 }

	/* look through the robot list and make sure that this robot
	   is not already attached */

	for (rdesc = robotList->next; rdesc != robotList; rdesc = rdesc->next)
	 { if (strcmp (rdesc->name, name) == 0)
	    { if (rdesc->td)
	       { errorMsg (EBusy, rbtIsBusy, fxnName, name);
		 RETURN_ERROR(-1);
	       }
	      break;
	    }
	 }

	/* If there is no descriptor for this robot, create one */

	if (rdesc == robotList)
	 { if ((rdesc = createRobotDesc (name)) == NULL)
	    { errorMsg (getErrorCode(0), nullMsg, fxnName);
	      RETURN_ERROR (-1);
	    }
	 }

	/* get the robot device information, if necessary */

	if (!rdesc->deviceInfoSet)
	 { if (getRobotDevice (&rdesc->devAddr, name) < 0)
	    { errorMsg (getErrorCode(0), noAddressSpec, fxnName, name);
	      RETURN_ERROR (-1);
	    }

	   if ((getRobotVector(&rdesc->vector,name) < 0) ||
	       (getRobotIpl(&rdesc->ipl,name) < 0) ||
	       (getRobotIntraddr(&rdesc->iaddr,name) < 0) ||
	       (getRobotIdev(&rdesc->idevName,name) < 0))
	    { errorMsg (getErrorCode(0), readIntrInfo, fxnName, name);
	      RETURN_ERROR (-1);
	    }

	/* If an interupt devivce is specified, see if any other robot
	   is using the same one. If so, then use the same reference
	   structure. Otherwise, allocate our own reference structure. */

	   if (rdesc->idevName != NULL)
	    { ROBOT_DESC *rd;
	      for (rd=robotList->next; rd!=robotList; rd=rd->next)
	       { 
#ifdef VXWORKS_CFG
		 if (rd->intrDesc && 
		     (strcmp(rd->idevName,rdesc->idevName) == 0) &&
		     (rd->vector == rdesc->vector) &&
		     (rd->iaddr == rdesc->iaddr))
		  { rdesc->intrDesc = rd->intrDesc;
		    break;
		  }
#else
		 if (rd->intrDesc && 
		     (strcmp(rd->idevName,rdesc->idevName) == 0))
		  { rdesc->intrDesc = rd->intrDesc;
		    break;
		  }
#endif
	       }
	      if (rd == robotList)
	       { /* allocate our own intrDesc */
		 
		 rdesc->intrDesc = ALLOC(RobotIdevDesc);
		 rdesc->intrDesc->fd = -1;
		 rdesc->intrDesc->openCnt = 0;
		 rdesc->intrDesc->refCnt = 0;
	       }
	      rdesc->intrDesc->refCnt++;
	    }
	   else
	    { rdesc->intrDesc = (RobotIdevDesc*)NULL;
	    }
	   rdesc->deviceInfoSet = 1;
	 }

	/* At this point, we have a valid pointer to a descriptor for
	   our robot ('rdesc'), and we know that no other task has this
	   robot attached. We now chose a number for the robot (an
	   index into the 'rbts' field of the task descriptor), and then
	   instruct all monitors to attach the robot */

	for (k=0; k<MAX_TASKROBOTS; k++)
	 { if (!td->rbts[k])
	    { rdesc->num = k;
	      break;
	    }
	 }

	/* --- if no com device specified, use the default --- */
	if (!rdev)
	 { rdev = &rdesc->devAddr;
	 }

	/* --- open the robot interrupt device, if necessary --- */

	if (rdesc->intrDesc && (strcmp(rdev->name,"sim") != 0))
	 { int fd;
	   /* open the device if no one else already has */
	   if (rdesc->intrDesc->fd == -1)
	    { 
#ifdef VXWORKS_CFG
	      IO_INTERRUPT intr;

	      if (vxwGetRobotInterrupt(&intr, rdesc) < 0)
	       { errorMsg (ENoInterDev, badIntrInfo, fxnName, name);
		 RETURN_ERROR (-1);
	       }
	      rciMaskAutoAbort();
	      if ((fd = rciInterruptOpen (&intr, rdesc->vector)) < 0)
	       { errorMsg (EBadAccess, nullMsg, fxnName);
		 RETURN_ERROR (-1);
	       }
	      rciUnmaskAutoAbort();
#else
#if 0
	      /* GRRR! It seems there is a delay between when close()
		 returns and the driver gets called! */
	       { int cnt;
		 fd = -1;
		 for (cnt=1; cnt<16 && fd<0; cnt*=2)
		  { if ((fd = open (rdesc->idevName, O_RDWR)) < 0)
		     { if (errno != EBUSY)
			{ sysErrorMsg (EBadAccess, fxnName, "open");
			  errorMsg (EBadAccess,cantOpenIntrDev,fxnName,name);
			  RETURN_ERROR(-1);
			}
		       sleep (1000);
		       nap (cnt*0.1);
		     }
		  }
		 if (fd<0)
		  { errorMsg (EBusy, rbtIntrDevBusy, fxnName, name);
		    RETURN_ERROR(-1);
		  }
	       }
#else
	      if ((fd = open (rdesc->idevName, O_RDWR)) < 0)
	       { if (errno == EBUSY)
		  { errorMsg (EBusy, rbtIntrDevBusy, fxnName, name);
		  }
		 else
		  { sysErrorMsg (EBadAccess, fxnName, "open");
		    errorMsg (EBadAccess,cantOpenIntrDev,fxnName,name);
		  }
		 RETURN_ERROR(-1);
	       }
#endif
#endif
	      rdesc->intrDesc->fd = fd;
	    }
	   rdesc->intrDesc->openCnt++;
	   rdesc->idev = rdesc->intrDesc->fd;
	 }
	else
	 { rdesc->idev = -1;
	 }

	/* ---- alloc the specfied device for the robot ---- */

	if (rdev->address == UNDEF || rdev->name == NULL)
	 { errorMsg (ENotDefined, rbtBadAddr, fxnName, name);
	   RETURN_ERROR(-1);
	 }
	if ((rdesc->devHandle = allocIOdevice (&dev, rdev, 0)) < 0)
	 { errorMsg (getErrorCode(0), rbtAllocIO, fxnName, name);
	   RETURN_ERROR(-1);
	 }
	if (rdesc->devHandle == 0)
	 { errorMsg (EBusy, rbtIODevBusy, fxnName, name, 
			    rdev->name, rdev->address);
	   RETURN_ERROR(-1);
	 }

	if (attachRobot (rdesc, td, &dev, rciControlCpus|ARBITER_MASK) < 0)
	 { errorMsg (getErrorCode(0), rbtCantAttach, fxnName, name);
	   RETURN_ERROR(-1);
	 }
	rdesc->td = td;
	return (rdesc->num);
}

rciDetachRobot (td, num)
RCI_DESC *td;
int num;
{
	char *fxnName = "rciDetachRobot()";
	ROBOT_DESC *rdesc;

	/* --- make sure that the task is not active --- */

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (td->tcb->controlMode == ACTIVE)
	 { errorMsg (ETaskActive, taskIsActive, fxnName, rciTaskName(td));
	   RETURN_ERROR(-1);
	 }

	if (robotList)
	 { for (rdesc=robotList->next; rdesc!=robotList; rdesc=rdesc->next)
	    { if (rdesc->td == td && rdesc->num == num)
	       { break;
	       }
	    }
	 }
	if (!robotList || rdesc == robotList)
	 { errorMsg (EDescriptor, rbtNotAttached, fxnName, rciTaskName(td));
	   RETURN_ERROR(-1);
	 }
	deallocIOdevice (rdesc->devHandle);
	if (detachRobot (rdesc, td, rciControlCpus|ARBITER_MASK) < 0)
	 { errorMsg (getErrorCode(0), rbtCantDetach, fxnName, rdesc->name);
	   RETURN_ERROR(-1);
	 }
	rdesc->td = NULL;

	/* ---- close down the interrupt device ---- */

	if (rdesc->intrDesc)
	 { if (rdesc->intrDesc->fd != -1 && --rdesc->intrDesc->openCnt == 0)
	    { 
	      /* If the task is currently referencing this interrupt, 
		 dereference the interrupt for it. This bit of code
		 was copied from part of setupScheduling(). */

	      if (td->interruptNumber == rdesc->intrDesc->fd)
	       { if (dereferenceInterrupt (td, td->interruptNumber) < 0)
		  { errorMsg (EInternal, cantDereferIntr, fxnName);
		    RETURN_ERROR (-1);
		  }
		 td->interruptNumber = UNDEF;
	       }
#ifdef VXWORKS_CFG
	      rciInterruptClose (rdesc->intrDesc->fd);
#else
#ifdef SGI_CFG
	      ioctl (rdesc->intrDesc->fd, RCIIOC_SOFT_CLOSE, 0);
#endif
	      close (rdesc->intrDesc->fd);
#endif
	       rdesc->intrDesc->fd = -1;
	    }
	 }	

	return (1);
}

importAllRobots (cpu)
int cpu;
{
	/* Called when a new CPU is configured into the system.
	   The monitor on that CPU is first asked to setup all the
	   necessary information about all presently defined robots.
	   If is then instructde to attach all robots which are currently
	   attached.
	*/

	char *fxnName = "importAllRobots()";
	ROBOT_DESC *rdesc;
	IO_DEVICE dev;

	if (!robotList)
	 { return (1);
	 }
	
	bzero (&dev, sizeof(IO_DEVICE));

	/* go through the list of all robots */

	for (rdesc = robotList->next; rdesc != robotList; rdesc = rdesc->next)
	 { if (setupRobot (rdesc, (1<<cpu)) < 0)
	    { errorMsg (getErrorCode(0), cantSetupRbt, fxnName, rdesc->name);
	      return (-1);
	    }
	   if (rdesc->td)
	    { if (attachRobot (rdesc, rdesc->td, &dev, (1<<cpu)) < 0)
	       { errorMsg (getErrorCode(0), rbtCantAttach, fxnName, 
					    rdesc->name);
		 return (-1);
	       }
	    }
	 }
	return (1);
}

detachAllRobots ()
{
	ROBOT_DESC *rdesc;

	/* called by rciClose() */

	if (robotList)
	 { for (rdesc=robotList->next; rdesc!=robotList; rdesc=rdesc->next)
	    { deallocIOdevice (rdesc->devHandle);
	      detachRobot (rdesc, rdesc->td, rciControlCpus|ARBITER_MASK);
	      rdesc->td = NULL;
	    }
	 }
}

static initRobotList(fxnName)
char *fxnName;
{
	/* Called the first time rciAttachRobot() is called.
	   Initializes the robot list */

	/* --- set up the robotList if necessary --- */

       	if ((robotList = ALLOC(ROBOT_DESC)) == NULL)
	 { errorMsg (EMemory, outOfMemory, fxnName);
#ifdef LINUX_CFG
	   return (-1);
#else
	   return (NULL);
#endif
	 }
	robotList->next = robotList->prev = robotList;

	return (1);
}

cleanupRobotDesc (rd)
ROBOT_DESC *rd;
{
	RobotIdevDesc *idesc;

	if (rd->devHandle && rd->devHandle != -1)
	 { deallocIOdevice (rd->devHandle);
	 }
	if (rd->name)
	 { FREE(rd->name);
	 }
	if (idesc = rd->intrDesc)
	 { if (idesc->fd != -1)
	    { if (--idesc->openCnt == 0)
	       { 
#ifdef VXWORKS_CFG
		 rciInterruptClose (idesc->fd);
#else
#ifdef SGI_CFG
		 ioctl (idesc->fd, RCIIOC_SOFT_CLOSE, 0);
#endif
		 close (idesc->fd);
#endif
		 idesc->fd = -1;
	       }
	    }
	   if (--idesc->refCnt == 0)
	    { rd->intrDesc = (RobotIdevDesc*)0;
	      FREE(idesc);
	    }
	 }
	if (rd->jls)
	 { FREE(rd->jls);
	 }
	if (rd->kyn)
	 { FREE(rd->kyn);
	 }
	if (rd->var)
	 { FREE(rd->var);
	 }
}

#ifdef VXWORKS_CFG
vxwGetRobotInterrupt(intr, rdesc)
IO_INTERRUPT *intr;
ROBOT_DESC *rdesc;
{
	char *fxnName = "vxwGetRobotInterrupt()";

	struct devAddr addr;
	int i;

	/* Parse the information describing the vector. Make sure
	   we have an IPL, a vector number, and a full address. */

	if (rdesc->ipl == UNDEF || 
	    rdesc->vector == UNDEF ||
	    rdesc->iaddr == UNDEF)
	 { errorMsg (ENoSpecify, missingIntrInfo, fxnName);
	   return (-1);
	 }
	addr.name = copyofstr(rdesc->idevName);
	addr.address = rdesc->iaddr;
	for (i=0; i<MAX_DEVARGS; i++)
	 { addr.args[i] = UNDEF;
	 }
	addr.addrStr = NULL;

	if (getIOinterrupt (intr, &addr, rdesc->vector, rdesc->ipl) < 0)
	 { errorMsg (getErrorCode(0), nullMsg, fxnName);
	   free (addr.name);
	   return (-1);
	 }
	free (addr.name);
	return (1);
}
#endif
