board/esd/common/xilinx_jtag/lenval.c

/*
 * (C) Copyright 2003
 * Stefan Roese, esd gmbh germany, stefan.roese@esd-electronics.com
 *
 * See file CREDITS for list of people who contributed to this
 * project.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 * MA 02111-1307 USA
 */

/*******************************************************/
/* file: lenval.c                                      */
/* abstract:  This file contains routines for using    */
/*            the lenVal data structure.               */
/*******************************************************/

#include <common.h>
#include <asm/processor.h>

#include "lenval.h"
#include "ports.h"


/*****************************************************************************
 * Function:     value
 * Description:  Extract the long value from the lenval array.
 * Parameters:   plvValue    - ptr to lenval.
 * Returns:      long        - the extracted value.
 *****************************************************************************/
long value( lenVal*     plvValue )
{
	long    lValue;         /* result to hold the accumulated result */
	short   sIndex;

	lValue  = 0;
	for ( sIndex = 0; sIndex < plvValue->len ; ++sIndex )
	{
		lValue <<= 8;                       /* shift the accumulated result */
		lValue |= plvValue->val[ sIndex];   /* get the last byte first */
	}

	return( lValue );
}

/*****************************************************************************
 * Function:     initLenVal
 * Description:  Initialize the lenval array with the given value.
 *               Assumes lValue is less than 256.
 * Parameters:   plv         - ptr to lenval.
 *               lValue      - the value to set.
 * Returns:      void.
 *****************************************************************************/
void initLenVal( lenVal*    plv,
		 long       lValue )
{
	plv->len    = 1;
	plv->val[0] = (unsigned char)lValue;
}

/*****************************************************************************
 * Function:     EqualLenVal
 * Description:  Compare two lenval arrays with an optional mask.
 * Parameters:   plvTdoExpected  - ptr to lenval #1.
 *               plvTdoCaptured  - ptr to lenval #2.
 *               plvTdoMask      - optional ptr to mask (=0 if no mask).
 * Returns:      short   - 0 = mismatch; 1 = equal.
 *****************************************************************************/
short EqualLenVal( lenVal*  plvTdoExpected,
		   lenVal*  plvTdoCaptured,
		   lenVal*  plvTdoMask )
{
	short           sEqual;
	short           sIndex;
	unsigned char   ucByteVal1;
	unsigned char   ucByteVal2;
	unsigned char   ucByteMask;

	sEqual  = 1;
	sIndex  = plvTdoExpected->len;

	while ( sEqual && sIndex-- )
	{
		ucByteVal1  = plvTdoExpected->val[ sIndex ];
		ucByteVal2  = plvTdoCaptured->val[ sIndex ];
		if ( plvTdoMask )
		{
			ucByteMask  = plvTdoMask->val[ sIndex ];
			ucByteVal1  &= ucByteMask;
			ucByteVal2  &= ucByteMask;
		}
		if ( ucByteVal1 != ucByteVal2 )
		{
			sEqual  = 0;
		}
	}

	return( sEqual );
}


/*****************************************************************************
 * Function:     RetBit
 * Description:  return the (byte, bit) of lv (reading from left to right).
 * Parameters:   plv     - ptr to lenval.
 *               iByte   - the byte to get the bit from.
 *               iBit    - the bit number (0=msb)
 * Returns:      short   - the bit value.
 *****************************************************************************/
short RetBit( lenVal*   plv,
	      int       iByte,
	      int       iBit )
{
	/* assert( ( iByte >= 0 ) && ( iByte < plv->len ) ); */
	/* assert( ( iBit >= 0 ) && ( iBit < 8 ) ); */
	return( (short)( ( plv->val[ iByte ] >> ( 7 - iBit ) ) & 0x1 ) );
}

/*****************************************************************************
 * Function:     SetBit
 * Description:  set the (byte, bit) of lv equal to val
 * Example:      SetBit("00000000",byte, 1) equals "01000000".
 * Parameters:   plv     - ptr to lenval.
 *               iByte   - the byte to get the bit from.
 *               iBit    - the bit number (0=msb).
 *               sVal    - the bit value to set.
 * Returns:      void.
 *****************************************************************************/
void SetBit( lenVal*    plv,
	     int        iByte,
	     int        iBit,
	     short      sVal )
{
	unsigned char   ucByteVal;
	unsigned char   ucBitMask;

	ucBitMask   = (unsigned char)(1 << ( 7 - iBit ));
	ucByteVal   = (unsigned char)(plv->val[ iByte ] & (~ucBitMask));

	if ( sVal )
	{
		ucByteVal   |= ucBitMask;
	}
	plv->val[ iByte ]   = ucByteVal;
}

/*****************************************************************************
 * Function:     AddVal
 * Description:  add val1 to val2 and store in resVal;
 *               assumes val1 and val2  are of equal length.
 * Parameters:   plvResVal   - ptr to result.
 *               plvVal1     - ptr of addendum.
 *               plvVal2     - ptr of addendum.
 * Returns:      void.
 *****************************************************************************/
void addVal( lenVal*    plvResVal,
	     lenVal*    plvVal1,
	     lenVal*    plvVal2 )
{
	unsigned char   ucCarry;
	unsigned short  usSum;
	unsigned short  usVal1;
	unsigned short  usVal2;
	short           sIndex;

	plvResVal->len  = plvVal1->len;         /* set up length of result */

	/* start at least significant bit and add bytes    */
	ucCarry = 0;
	sIndex  = plvVal1->len;
	while ( sIndex-- )
	{
		usVal1  = plvVal1->val[ sIndex ];   /* i'th byte of val1 */
		usVal2  = plvVal2->val[ sIndex ];   /* i'th byte of val2 */

		/* add the two bytes plus carry from previous addition */
		usSum   = (unsigned short)( usVal1 + usVal2 + ucCarry );

		/* set up carry for next byte */
		ucCarry = (unsigned char)( ( usSum > 255 ) ? 1 : 0 );

		/* set the i'th byte of the result */
		plvResVal->val[ sIndex ]    = (unsigned char)usSum;
	}
}

/*****************************************************************************
 * Function:     readVal
 * Description:  read from XSVF numBytes bytes of data into x.
 * Parameters:   plv         - ptr to lenval in which to put the bytes read.
 *               sNumBytes   - the number of bytes to read.
 * Returns:      void.
 *****************************************************************************/
void readVal( lenVal*   plv,
	      short     sNumBytes )
{
	unsigned char*  pucVal;

	plv->len    = sNumBytes;        /* set the length of the lenVal        */
	for ( pucVal = plv->val; sNumBytes; --sNumBytes, ++pucVal )
	{
		/* read a byte of data into the lenVal */
		readByte( pucVal );
	}
}