board/esd/common/xilinx_jtag/lenval.c

   1 /*
   2  * (C) Copyright 2003
   3  * Stefan Roese, esd gmbh germany, stefan.roese@esd-electronics.com
   4  *
   5  * See file CREDITS for list of people who contributed to this
   6  * project.
   7  *
   8  * This program is free software; you can redistribute it and/or
   9  * modify it under the terms of the GNU General Public License as
  10  * published by the Free Software Foundation; either version 2 of
  11  * the License, or (at your option) any later version.
  12  *
  13  * This program is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  * GNU General Public License for more details.
  17  *
  18  * You should have received a copy of the GNU General Public License
  19  * along with this program; if not, write to the Free Software
  20  * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
  21  * MA 02111-1307 USA
  22  */
  23
  24 /*******************************************************/
  25 /* file: lenval.c                                      */
  26 /* abstract:  This file contains routines for using    */
  27 /*            the lenVal data structure.               */
  28 /*******************************************************/
  29
  30 #include <common.h>
  31 #include <asm/processor.h>
  32
  33 #include "lenval.h"
  34 #include "ports.h"
  35
  36
  37 /*****************************************************************************
  38  * Function:     value
  39  * Description:  Extract the long value from the lenval array.
  40  * Parameters:   plvValue    - ptr to lenval.
  41  * Returns:      long        - the extracted value.
  42  *****************************************************************************/
  43 long value( lenVal*     plvValue )
  44 {
  45         long    lValue;         /* result to hold the accumulated result */
  46         short   sIndex;
  47
  48         lValue  = 0;
  49         for ( sIndex = 0; sIndex < plvValue->len ; ++sIndex )
  50         {
  51                 lValue <<= 8;                       /* shift the accumulated result */
  52                 lValue |= plvValue->val[ sIndex];   /* get the last byte first */
  53         }
  54
  55         return( lValue );
  56 }
  57
  58 /*****************************************************************************
  59  * Function:     initLenVal
  60  * Description:  Initialize the lenval array with the given value.
  61  *               Assumes lValue is less than 256.
  62  * Parameters:   plv         - ptr to lenval.
  63  *               lValue      - the value to set.
  64  * Returns:      void.
  65  *****************************************************************************/
  66 void initLenVal( lenVal*    plv,
  67                  long       lValue )
  68 {
  69         plv->len    = 1;
  70         plv->val[0] = (unsigned char)lValue;
  71 }
  72
  73 /*****************************************************************************
  74  * Function:     EqualLenVal
  75  * Description:  Compare two lenval arrays with an optional mask.
  76  * Parameters:   plvTdoExpected  - ptr to lenval #1.
  77  *               plvTdoCaptured  - ptr to lenval #2.
  78  *               plvTdoMask      - optional ptr to mask (=0 if no mask).
  79  * Returns:      short   - 0 = mismatch; 1 = equal.
  80  *****************************************************************************/
  81 short EqualLenVal( lenVal*  plvTdoExpected,
  82                    lenVal*  plvTdoCaptured,
  83                    lenVal*  plvTdoMask )
  84 {
  85         short           sEqual;
  86         short           sIndex;
  87         unsigned char   ucByteVal1;
  88         unsigned char   ucByteVal2;
  89         unsigned char   ucByteMask;
  90
  91         sEqual  = 1;
  92         sIndex  = plvTdoExpected->len;
  93
  94         while ( sEqual && sIndex-- )
  95         {
  96                 ucByteVal1  = plvTdoExpected->val[ sIndex ];
  97                 ucByteVal2  = plvTdoCaptured->val[ sIndex ];
  98                 if ( plvTdoMask )
  99                 {
 100                         ucByteMask  = plvTdoMask->val[ sIndex ];
 101                         ucByteVal1  &= ucByteMask;
 102                         ucByteVal2  &= ucByteMask;
 103                 }
 104                 if ( ucByteVal1 != ucByteVal2 )
 105                 {
 106                         sEqual  = 0;
 107                 }
 108         }
 109
 110         return( sEqual );
 111 }
 112
 113
 114 /*****************************************************************************
 115  * Function:     RetBit
 116  * Description:  return the (byte, bit) of lv (reading from left to right).
 117  * Parameters:   plv     - ptr to lenval.
 118  *               iByte   - the byte to get the bit from.
 119  *               iBit    - the bit number (0=msb)
 120  * Returns:      short   - the bit value.
 121  *****************************************************************************/
 122 short RetBit( lenVal*   plv,
 123               int       iByte,
 124               int       iBit )
 125 {
 126         /* assert( ( iByte >= 0 ) && ( iByte < plv->len ) ); */
 127         /* assert( ( iBit >= 0 ) && ( iBit < 8 ) ); */
 128         return( (short)( ( plv->val[ iByte ] >> ( 7 - iBit ) ) & 0x1 ) );
 129 }
 130
 131 /*****************************************************************************
 132  * Function:     SetBit
 133  * Description:  set the (byte, bit) of lv equal to val
 134  * Example:      SetBit("00000000",byte, 1) equals "01000000".
 135  * Parameters:   plv     - ptr to lenval.
 136  *               iByte   - the byte to get the bit from.
 137  *               iBit    - the bit number (0=msb).
 138  *               sVal    - the bit value to set.
 139  * Returns:      void.
 140  *****************************************************************************/
 141 void SetBit( lenVal*    plv,
 142              int        iByte,
 143              int        iBit,
 144              short      sVal )
 145 {
 146         unsigned char   ucByteVal;
 147         unsigned char   ucBitMask;
 148
 149         ucBitMask   = (unsigned char)(1 << ( 7 - iBit ));
 150         ucByteVal   = (unsigned char)(plv->val[ iByte ] & (~ucBitMask));
 151
 152         if ( sVal )
 153         {
 154                 ucByteVal   |= ucBitMask;
 155         }
 156         plv->val[ iByte ]   = ucByteVal;
 157 }
 158
 159 /*****************************************************************************
 160  * Function:     AddVal
 161  * Description:  add val1 to val2 and store in resVal;
 162  *               assumes val1 and val2  are of equal length.
 163  * Parameters:   plvResVal   - ptr to result.
 164  *               plvVal1     - ptr of addendum.
 165  *               plvVal2     - ptr of addendum.
 166  * Returns:      void.
 167  *****************************************************************************/
 168 void addVal( lenVal*    plvResVal,
 169              lenVal*    plvVal1,
 170              lenVal*    plvVal2 )
 171 {
 172         unsigned char   ucCarry;
 173         unsigned short  usSum;
 174         unsigned short  usVal1;
 175         unsigned short  usVal2;
 176         short           sIndex;
 177
 178         plvResVal->len  = plvVal1->len;         /* set up length of result */
 179
 180         /* start at least significant bit and add bytes    */
 181         ucCarry = 0;
 182         sIndex  = plvVal1->len;
 183         while ( sIndex-- )
 184         {
 185                 usVal1  = plvVal1->val[ sIndex ];   /* i'th byte of val1 */
 186                 usVal2  = plvVal2->val[ sIndex ];   /* i'th byte of val2 */
 187
 188                 /* add the two bytes plus carry from previous addition */
 189                 usSum   = (unsigned short)( usVal1 + usVal2 + ucCarry );
 190
 191                 /* set up carry for next byte */
 192                 ucCarry = (unsigned char)( ( usSum > 255 ) ? 1 : 0 );
 193
 194                 /* set the i'th byte of the result */
 195                 plvResVal->val[ sIndex ]    = (unsigned char)usSum;
 196         }
 197 }
 198
 199 /*****************************************************************************
 200  * Function:     readVal
 201  * Description:  read from XSVF numBytes bytes of data into x.
 202  * Parameters:   plv         - ptr to lenval in which to put the bytes read.
 203  *               sNumBytes   - the number of bytes to read.
 204  * Returns:      void.
 205  *****************************************************************************/
 206 void readVal( lenVal*   plv,
 207               short     sNumBytes )
 208 {
 209         unsigned char*  pucVal;
 210
 211         plv->len    = sNumBytes;        /* set the length of the lenVal        */
 212         for ( pucVal = plv->val; sNumBytes; --sNumBytes, ++pucVal )
 213         {
 214                 /* read a byte of data into the lenVal */
 215                 readByte( pucVal );
 216         }
 217 }