/*-=-=-=-=-=-=-=-=-=-=-=- 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: queue
 *
 * DESCRIPTION:	Routines to insert items into the RCCL motion queue
 *		(called by the user level, or inserting process).
 *
 * 		The RCCL motion queue is implemented as a circular buffer, 
 *		which may be used to store variable sized packets of 
 *		information which are passed off to the receiving process.
 *	
 *		'init_queue()' initializes all the data structures used
 *		by the queueing routines.
 *
 *		'reserve_queue_insert()' verifies that there is 'size' 
 *		contiguous bytes available in the buffer, and returns a
 *		pointer to the space. The region is not actually queued until
 *		a call is put in to 'confirm_queue_request()', which must be
 *		called before 'reserve_queue_insert()' can be called again.
 *
 *		Usage hence looks like this:
 *
 *			ptr = reserve_queue_insert (10);
 *		
 *			  ... store things in the space ...
 *
 *			confirm_queue_insert();
 * 
 *		'motions_queued()' returns the number of items currently
 *		on the queue, and can actually be called by either the
 *		queueing or deleting process.
 *
 * 		'reserve_queue_insert()' returns either a pointer to the
 *			allocated space, or NULL if it failed.
 *
 *		'confirm_queue_insert()' returns 1 if successful and 0 
 *			otherwise.
 *
 * NOTES:	Queue integrity between tasks is maintained in the
 *		following way: The insertion routines do not modify the tail 
 *		of the queue, only the head of it. Likewise, the deletion
 *		routines (below) only modify the tail. It is assumed that
 *		these variables can be read and written indivisibly.
 *
 * AUTHOR/DATE:	John Lloyd, May, 1986
 *
 * MODIFICATION HISTORY:	
 *
 *-------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------

	Queue data structures look something like this:

   mq->headAddress ---------------------------+
		  ____________________________V______________________
   mq->buf ----->|XXXXX|XXXXXXXXXXX|XXXXXXXXX|_______________________|
		   ^     ^           ^
		   |     |           |
		   |   +-+ +---------+
		  _|___|___|_____________________________
   mq->entries ->|000|111|222|___|___|___|___|___|___|___|
		   ^	       ^
   mq->tail -------+           |
   mq->head -------------------+

   'buf' is a circular buffer from which memory for the actual
    motion requests is allocated. The next available address is pointed
    to be 'headAddress'. The address and size of each request is stored
    as a entry in the 'entries' buffer, which is also circular but
    is composed of small fixed size records. When accessing the queue
    or testing it, one first consults the 'head' or 'tail' pointers,
    which are array indices into the 'entries' field.

    Insertion is done at the HEAD and removal is done at the TAIL.

 ------------------------------------------------------------------------- */

#include "../h/cdefs.h"
#include "../h/assignMem.h"

#include "../rccl/motionQueue.h"

init_queue(mq)
MOTION_QUEUE *mq;
{
	/* Called once at the beginning of each control session to
	   initialize the queue structures.
	 */

	mq->head = 0;
	mq->tail = 0;
	mq->spaceReserved = 0;
	mq->headAddress = mq->buf;
	mq->bufAddr = mq->buf;
}

check_queue (mq, space)
MOTION_QUEUE *mq;
int space;
{
	/* The motion queue is checked to see if there is the required
	   amount of space available for queueing a motion of the
	   indicated size.

	   The queued item is described by an entry in mq->entries,
	   which describes the starting address and amount of memory
	   associated with the queued item. Memory is allocated from 
	   mq->buf. Hence there must be room in both of these 
	   data structures to permit an item to be queued.
	   
	   A contiguous region of space is allocated from mq->buf.
	   Because allocation is circular, it may be necessary
	   soemtimes to leave dead spaec at the end of the buffer 
	   and reallocate from the beginning.
	 */	   

	int aspace, newHead, deadSpace, usedSpace, tail;
	char *headAddr, *tailAddr;

	tail = mq->tail;

	if (mq->spaceReserved || (aspace = ALIGN_SIZE(space)) < 0)
	 { return (0);
	 }

	/* first see if 'entries' field is full: */
	newHead = (mq->head+1) % MAX_MQ_ENTRIES;
	if (newHead == tail)
	 { return (0);
	 }

	/* get the buf address of last item in queue */
	if (tail == mq->head)
	 { tailAddr = mq->headAddress = mq->buf;
	 }
	else
	 { tailAddr = mq->entries[tail].addr;
	 }
	headAddr = mq->headAddress;

	/* see how much dead space at we will have at end of buffer */
	if (headAddr + aspace > MQ_BUF_SIZE + mq->buf)
	 { deadSpace = MQ_BUF_SIZE + mq->buf - headAddr;
	 }
	else
	 { deadSpace = 0;
	 }

	/* total of space + usedSpace + deadSpace must fit into buffer */
	usedSpace = headAddr - tailAddr;
	if (usedSpace < 0)
	 { usedSpace = MQ_BUF_SIZE + usedSpace;
	 }
	if (aspace + deadSpace + usedSpace >= MQ_BUF_SIZE)
	 { return (0);
	 }
	else
	 { return (1);
	 }
}

char *reserve_queue_insert (mq, space)
MOTION_QUEUE *mq;
int space;
{
	/* Do the actual allocation of 'space' bytes from the
	   motion queue. Do not queue the request, however. What
	   this routine does is return a pointer to the allocated area
	   and mark the queue as reserved until the request is
	   actually queued with confirm_queue_insert() */
	   
	int aspace, newHead, deadSpace, usedSpace, tail;
	char *headAddr, *tailAddr;

	tail = mq->tail;

	if (mq->spaceReserved || (aspace = ALIGN_SIZE(space)) < 0)
	 { return (NULL);
	 }
	newHead = (mq->head+1) % MAX_MQ_ENTRIES;
	if (newHead == tail)
	 { return (NULL);
	 }

	if (tail == mq->head)
	 { tailAddr = mq->headAddress = mq->buf;
	 }
	else
	 { tailAddr = mq->entries[tail].addr;
	 }
	headAddr = mq->headAddress;

	if (headAddr + aspace > MQ_BUF_SIZE + mq->buf)
	 { deadSpace = MQ_BUF_SIZE + mq->buf - headAddr;
	 }
	else
	 { deadSpace = 0;
	 }
	usedSpace = headAddr - tailAddr;
	if (usedSpace < 0)
	 { usedSpace = MQ_BUF_SIZE + usedSpace;
	 }
      
	if (aspace + deadSpace + usedSpace >= MQ_BUF_SIZE)
	 { return (NULL);
	 }

	mq->entries[mq->head].size = space;

	mq->spaceReserved = 1;
	if (deadSpace)
	 { mq->entries[mq->head].addr = mq->buf;
	   mq->headAddress = mq->buf + aspace;
	   return (mq->buf);
	 }
	else
	 { mq->entries[mq->head].addr = headAddr;
	   mq->headAddress = headAddr + aspace;
	   if (mq->headAddress == MQ_BUF_SIZE + mq->buf)
	    { mq->headAddress = mq->buf;
	    }
	   return (headAddr);
	 }
}

confirm_queue_insert (mq)
MOTION_QUEUE *mq;
{
	/* Actually increments the head pointer once the memory area allocated
	   by reserve_queue_insert() has been properly instantiated.
	*/

	if (!mq->spaceReserved)
	 { return (0);
	 }
	mq->spaceReserved = 0;
	mq->head = (mq->head+1) % MAX_MQ_ENTRIES;
}

motions_queued (mq)
MOTION_QUEUE *mq;
{
	/* Examines tail and head to determine the number of items on the
	   queue.
	*/

	int num;
	int tail = mq->tail;
	int head = mq->head;

	num = head - tail;
	if (num < 0)
	 { num = MAX_MQ_ENTRIES + num;
	 }
	return (num);
}

#ifdef TEST

#ifdef VXWORKS_CFG
#define main testMq_
#endif

/* Queue 'get' routines, defined here for the test software. The routines
   used by the system are contained in the file 'getQueue.c', where they
   are for the most part implemented as macros.
*/

#undef test_queue
#undef get_queue
#undef free_queue

test_queue (mq)
MOTION_QUEUE *mq;
{
	int head = mq->head;
	return (mq->tail == head ? 0 : 1);
}

char *get_queue (mq, size)
MOTION_QUEUE *mq;
int *size;
{
	int head = mq->head;

	if (mq->tail == head)
	 { return (NULL);
	 }
	if (size != NULL)
	 { *size = mq->entries[mq->tail].size;
	 }
	return (mq->entries[mq->tail].addr + (mq->buf - mq->bufAddr));
}

free_queue (mq)
MOTION_QUEUE *mq;
{
	mq->tail = (mq->tail+1) % MAX_MQ_ENTRIES;
	return (1);
}	

MOTION_QUEUE motionQueue;
MOTION_QUEUE *mq = &motionQueue;

main(argc, argv)
int argc;
char **argv;
{
	int cnt, i, j;

	int numinserts;
	int numgets;
	int size;
	int alignedSize;
	char *result;
	char *resultPred;
	int status;
	int statusPred;

	int usedSpace;
	int deadSpace;
	char *tailAddr;
	char *headAddr;

	int insertAttempts = 0;
	int entriesFull = 0;
	int getAttempts = 0;
	int insertSuccesses = 0;
	int getSuccesses = 0;

	if (argc > 1)
	 { sscanf (*++argv, "%d", &cnt);
	 }
	else
	 { cnt = 100;
	 }

	init_queue (mq);

	for (i=0; i<cnt; i++)
	 { numinserts = irandom (1,MAX_MQ_ENTRIES);
	   numgets = irandom (1,MAX_MQ_ENTRIES);

	   for (j=0; j<numinserts; j++)
	    { int k;

	      if (i%2)
	       { size = irandom (0, MQ_BUF_SIZE/numinserts);
	       }
	      else
	       { size = irandom (0,MQ_BUF_SIZE/2);
	       }
	      alignedSize = ALIGN_SIZE(size);

	      /* compute the free space on the queue */

	      if (mq->tail == mq->head)
	       { tailAddr = headAddr = mq->buf;
	       }
	      else
	       {
		 tailAddr = mq->entries[mq->tail].addr;
		 k = (mq->head-1);
		 if (k < 0)
		  { k = MAX_MQ_ENTRIES - 1;
		  }
		 headAddr = mq->entries[k].addr + 
			    ALIGN_SIZE(mq->entries[k].size);
		 if (headAddr == mq->buf + MQ_BUF_SIZE)
		  { headAddr = mq->buf;
		  }
	       }
	      usedSpace = headAddr - tailAddr;
	      if (usedSpace < 0)
	       { usedSpace = MQ_BUF_SIZE + usedSpace;
	       }
	      deadSpace = mq->buf + MQ_BUF_SIZE - headAddr;
	      if (deadSpace >= alignedSize)
	       { deadSpace = 0;
	       }

	      if (motions_queued(mq) == MAX_MQ_ENTRIES-1)
	       { resultPred = NULL;
		 entriesFull++;
	       }
	      else if (deadSpace + usedSpace + alignedSize >= MQ_BUF_SIZE)
	       { resultPred = NULL;
	       }
	      else
	       { if (deadSpace)
		  { resultPred = mq->buf;
		  }
	 	 else
		  { resultPred = headAddr;
		  }
	       }

	      insertAttempts++;
	      result = reserve_queue_insert(mq, size);
	      if (result != resultPred)
	       { printf ("ERROR/ reserve_queue() i = %d, j = %d\n", i, j);
		 printf ("***predicated:\n");
	         printf ("headAddr = 0x%x tailAddr = 0x%x\n",
			 headAddr, tailAddr);
		 printf ("result 0x%x usedSpace = %d DeadSpace = %d size = %d\n",
		         resultPred, usedSpace, deadSpace, size);
		 printf ("\n");
		 printf ("***real:\n");
		 printf ("result 0x%x\n", result);
		 printq(mq);

		 printf ("\n");
		 return;
	       }

	      if (result != NULL)
	       { for (k=0; k<size; k++)
	          { result[k] = k+1;
		  }
		 confirm_queue_insert(mq);
		 insertSuccesses++;
	       }
	    }

	   for (j=0; j<numgets; j++)
	    { int size, k;
	      statusPred = (motions_queued(mq) > 0) ? 1 : 0;

	      if (test_queue(mq) && !statusPred ||
		  !test_queue(mq) && statusPred)
	       { printf ("ERROR: queue_test(), i = %d, j = %d\n", i, j);
		 return;
	       }
	
	      getAttempts++;
	      result = get_queue(mq, &size);
	      if (result && !statusPred ||
		  !result && statusPred)
	       { printf ("ERROR: get_queue(), i = %d, j = %d\n", i, j);
		 return;
	       }

	      if (result)
	       { for (k=0; k<size; k++)
		  { if (result[k] != (char)(k+1))
		     { printf (
"ERROR: get_queue result, i=%d, j=%d result[%d]=%d vs. %d\n",
			       i, j, k, result[k], (char)k+1);
		     }
		  }
		 free_queue(mq);
		 getSuccesses++;
	       }
	    }
	 }
printf ("%6d successful insertions; %d failed; %d failed on max entries\n",
	insertSuccesses, insertAttempts-insertSuccesses, entriesFull);
printf ("%6d successful gets; %d failed\n", 
	getSuccesses, getAttempts-getSuccesses);
printf ("%6d items left on the queue\n", motions_queued(mq));

	printf ("\nROUTINES OK\n\n");
}

printq(mq)
MOTION_QUEUE *mq;
{
	int i;
	static int mark[MAX_MQ_ENTRIES];

	for (i=0; i<MAX_MQ_ENTRIES; i++)
	 { mark[i] = 0;
	 }
	for (i=mq->tail; i!=mq->head; i = (i+1)%MAX_MQ_ENTRIES)
	 { mark[i] = 1;
	 }

	for (i=0; i<MAX_MQ_ENTRIES; i++)
	 { printf (" %s 0x%x 0x%x   %d\n", mark[i] ? "*" : " ",
		   mq->entries[i].addr,
		   mq->entries[i].addr-mq->buf,
		   mq->entries[i].size);
	 }
}

#endif
