cxqadd.c

CMEX source code RTOS for atmel atmega128

/*********************************************************

Copyright (c) CMX Company. 1999. All rights reserved

*********************************************************/
/* version 5.30 */

#define CMXMODULE 1

#include <cxfuncs.h>	/* get cmx include header file */
#include <cxextern.h>	/* get cmx include header file */

#ifdef CMXTRACKER
#include <cmxtrack.h>	/* get cmx include header file */
#endif

/************************************************************
	CMX general add to queue function.
	called by K_Que_Add_Top and K_Que_Add_Bottom functions.

	test: if testing for less then 0, then pointer = num_slots - 1
			if testing for equal to num_slots then pointer = 0

	when adding to top 
	add to where head points
	increment head and test.

	when adding to bottom.
	decrement tail and test
	add to where tail points

	when removing from top
	decrement head and test
	remove from where head points

	when removing from bottom
	remove from where tail points
	increment tail and test

*************************************************************/
/*
top = 0: add to bottom
top = 1: add to top
*/
byte K_I_Que_Add_Common(byte queuenum,void *byteptr,sign_word16 top)
{
	QUEHDR *queue_ptr;

	queue_ptr = &queue[queuenum];	/* get address of proper queue handler. */
	if (queuenum >= MAX_QUEUES || (!queue_ptr->size_slot))
		{
#ifdef CMXTRACKER
		if (CMXTRACKER_ON)
			{
			cmxtracker_in2(CXQADD_K_ERROR,queuenum);
			}
#endif
		return(K_ERROR);
		}

	K_I_Disable_Sched();	/* set task block */

	/* see if queue is full already */
	if (queue_ptr->queue_cnt == queue_ptr->num_slots)
		{
#ifdef CMXTRACKER
		if (CMXTRACKER_ON)
			{
			cmxtracker_in2(CXQADD_FULL,queuenum);
			}
#endif
		K_I_Func_Return();	/* release task block. */
		return(K_ERROR);	/* return proper status. */
		}
	if (top)	/* see if adding to top of queue */
		{
		top = queue_ptr->head;	/* top will now be our index into array. */

		/* see if head equals the max number slots, and
			if so, set pointer to bottom of queue */
		if (++queue_ptr->head == queue_ptr->num_slots)
			queue_ptr->head = 0;
		}
	else
		{
		/* were adding to bottom of queue, use tail pointer */
		if (--queue_ptr->tail < 0)	/* decrement tail pointer and test */
			queue_ptr->tail = (queue_ptr->num_slots - 1);

		top = queue_ptr->tail;	/* top will now be our index into array. */
		}
		/* K_I_Copy = dest,source,count */
		K_I_Copy((&(queue_ptr->base_ptr[top *
			queue_ptr->size_slot])),byteptr,queue_ptr->size_slot); 

#ifdef CMXTRACKER
		if (CMXTRACKER_ON)
			{
			cmxtracker_in2(CXQADD_K_OK,queuenum);
			}
#endif
	/* see if queue now full */
	if (++queue_ptr->queue_cnt == queue_ptr->num_slots)
		{
		K_I_Func_Return();
		return(K_QUE_FULL);	/* return status indicating queue now full. */
		}
	K_I_Func_Return();	/* release task block */
	return(K_OK);	/* return good status */
}