]>
Commit | Line | Data |
---|---|---|
9a2dd740 SR |
1 | /* |
2 | * (C) Copyright 2003 | |
3 | * Stefan Roese, esd gmbh germany, stefan.roese@esd-electronics.com | |
4 | * | |
1a459660 | 5 | * SPDX-License-Identifier: GPL-2.0+ |
9a2dd740 SR |
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, | |
42d1f039 | 51 | long lValue ) |
9a2dd740 SR |
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, | |
42d1f039 WD |
66 | lenVal* plvTdoCaptured, |
67 | lenVal* plvTdoMask ) | |
9a2dd740 SR |
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, | |
42d1f039 WD |
107 | int iByte, |
108 | int iBit ) | |
9a2dd740 SR |
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, | |
42d1f039 WD |
126 | int iByte, |
127 | int iBit, | |
128 | short sVal ) | |
9a2dd740 SR |
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, | |
42d1f039 WD |
153 | lenVal* plvVal1, |
154 | lenVal* plvVal2 ) | |
9a2dd740 SR |
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, | |
42d1f039 | 191 | short sNumBytes ) |
9a2dd740 SR |
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 | } |