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  * SPDX-License-Identifier:     GPL-2.0+
   6  */
   7
   8 /*******************************************************/
   9 /* file: lenval.c                                      */
  10 /* abstract:  This file contains routines for using    */
  11 /*            the lenVal data structure.               */
  12 /*******************************************************/
  13
  14 #include <common.h>
  15 #include <asm/processor.h>
  16
  17 #include "lenval.h"
  18 #include "ports.h"
  19
  20
  21 /*****************************************************************************
  22  * Function:     value
  23  * Description:  Extract the long value from the lenval array.
  24  * Parameters:   plvValue    - ptr to lenval.
  25  * Returns:      long        - the extracted value.
  26  *****************************************************************************/
  27 long value( lenVal*     plvValue )
  28 {
  29         long    lValue;         /* result to hold the accumulated result */
  30         short   sIndex;
  31
  32         lValue  = 0;
  33         for ( sIndex = 0; sIndex < plvValue->len ; ++sIndex )
  34         {
  35                 lValue <<= 8;                       /* shift the accumulated result */
  36                 lValue |= plvValue->val[ sIndex];   /* get the last byte first */
  37         }
  38
  39         return( lValue );
  40 }
  41
  42 /*****************************************************************************
  43  * Function:     initLenVal
  44  * Description:  Initialize the lenval array with the given value.
  45  *               Assumes lValue is less than 256.
  46  * Parameters:   plv         - ptr to lenval.
  47  *               lValue      - the value to set.
  48  * Returns:      void.
  49  *****************************************************************************/
  50 void initLenVal( lenVal*    plv,
  51                  long       lValue )
  52 {
  53         plv->len    = 1;
  54         plv->val[0] = (unsigned char)lValue;
  55 }
  56
  57 /*****************************************************************************
  58  * Function:     EqualLenVal
  59  * Description:  Compare two lenval arrays with an optional mask.
  60  * Parameters:   plvTdoExpected  - ptr to lenval #1.
  61  *               plvTdoCaptured  - ptr to lenval #2.
  62  *               plvTdoMask      - optional ptr to mask (=0 if no mask).
  63  * Returns:      short   - 0 = mismatch; 1 = equal.
  64  *****************************************************************************/
  65 short EqualLenVal( lenVal*  plvTdoExpected,
  66                    lenVal*  plvTdoCaptured,
  67                    lenVal*  plvTdoMask )
  68 {
  69         short           sEqual;
  70         short           sIndex;
  71         unsigned char   ucByteVal1;
  72         unsigned char   ucByteVal2;
  73         unsigned char   ucByteMask;
  74
  75         sEqual  = 1;
  76         sIndex  = plvTdoExpected->len;
  77
  78         while ( sEqual && sIndex-- )
  79         {
  80                 ucByteVal1  = plvTdoExpected->val[ sIndex ];
  81                 ucByteVal2  = plvTdoCaptured->val[ sIndex ];
  82                 if ( plvTdoMask )
  83                 {
  84                         ucByteMask  = plvTdoMask->val[ sIndex ];
  85                         ucByteVal1  &= ucByteMask;
  86                         ucByteVal2  &= ucByteMask;
  87                 }
  88                 if ( ucByteVal1 != ucByteVal2 )
  89                 {
  90                         sEqual  = 0;
  91                 }
  92         }
  93
  94         return( sEqual );
  95 }
  96
  97
  98 /*****************************************************************************
  99  * Function:     RetBit
 100  * Description:  return the (byte, bit) of lv (reading from left to right).
 101  * Parameters:   plv     - ptr to lenval.
 102  *               iByte   - the byte to get the bit from.
 103  *               iBit    - the bit number (0=msb)
 104  * Returns:      short   - the bit value.
 105  *****************************************************************************/
 106 short RetBit( lenVal*   plv,
 107               int       iByte,
 108               int       iBit )
 109 {
 110         /* assert( ( iByte >= 0 ) && ( iByte < plv->len ) ); */
 111         /* assert( ( iBit >= 0 ) && ( iBit < 8 ) ); */
 112         return( (short)( ( plv->val[ iByte ] >> ( 7 - iBit ) ) & 0x1 ) );
 113 }
 114
 115 /*****************************************************************************
 116  * Function:     SetBit
 117  * Description:  set the (byte, bit) of lv equal to val
 118  * Example:      SetBit("00000000",byte, 1) equals "01000000".
 119  * Parameters:   plv     - ptr to lenval.
 120  *               iByte   - the byte to get the bit from.
 121  *               iBit    - the bit number (0=msb).
 122  *               sVal    - the bit value to set.
 123  * Returns:      void.
 124  *****************************************************************************/
 125 void SetBit( lenVal*    plv,
 126              int        iByte,
 127              int        iBit,
 128              short      sVal )
 129 {
 130         unsigned char   ucByteVal;
 131         unsigned char   ucBitMask;
 132
 133         ucBitMask   = (unsigned char)(1 << ( 7 - iBit ));
 134         ucByteVal   = (unsigned char)(plv->val[ iByte ] & (~ucBitMask));
 135
 136         if ( sVal )
 137         {
 138                 ucByteVal   |= ucBitMask;
 139         }
 140         plv->val[ iByte ]   = ucByteVal;
 141 }
 142
 143 /*****************************************************************************
 144  * Function:     AddVal
 145  * Description:  add val1 to val2 and store in resVal;
 146  *               assumes val1 and val2  are of equal length.
 147  * Parameters:   plvResVal   - ptr to result.
 148  *               plvVal1     - ptr of addendum.
 149  *               plvVal2     - ptr of addendum.
 150  * Returns:      void.
 151  *****************************************************************************/
 152 void addVal( lenVal*    plvResVal,
 153              lenVal*    plvVal1,
 154              lenVal*    plvVal2 )
 155 {
 156         unsigned char   ucCarry;
 157         unsigned short  usSum;
 158         unsigned short  usVal1;
 159         unsigned short  usVal2;
 160         short           sIndex;
 161
 162         plvResVal->len  = plvVal1->len;         /* set up length of result */
 163
 164         /* start at least significant bit and add bytes    */
 165         ucCarry = 0;
 166         sIndex  = plvVal1->len;
 167         while ( sIndex-- )
 168         {
 169                 usVal1  = plvVal1->val[ sIndex ];   /* i'th byte of val1 */
 170                 usVal2  = plvVal2->val[ sIndex ];   /* i'th byte of val2 */
 171
 172                 /* add the two bytes plus carry from previous addition */
 173                 usSum   = (unsigned short)( usVal1 + usVal2 + ucCarry );
 174
 175                 /* set up carry for next byte */
 176                 ucCarry = (unsigned char)( ( usSum > 255 ) ? 1 : 0 );
 177
 178                 /* set the i'th byte of the result */
 179                 plvResVal->val[ sIndex ]    = (unsigned char)usSum;
 180         }
 181 }
 182
 183 /*****************************************************************************
 184  * Function:     readVal
 185  * Description:  read from XSVF numBytes bytes of data into x.
 186  * Parameters:   plv         - ptr to lenval in which to put the bytes read.
 187  *               sNumBytes   - the number of bytes to read.
 188  * Returns:      void.
 189  *****************************************************************************/
 190 void readVal( lenVal*   plv,
 191               short     sNumBytes )
 192 {
 193         unsigned char*  pucVal;
 194
 195         plv->len    = sNumBytes;        /* set the length of the lenVal        */
 196         for ( pucVal = plv->val; sNumBytes; --sNumBytes, ++pucVal )
 197         {
 198                 /* read a byte of data into the lenVal */
 199                 readByte( pucVal );
 200         }
 201 }