source: pacpusframework/trunk/include/Pacpus/PacpusTools/BinaryDecoder.h@ 310

Last change on this file since 310 was 305, checked in by phudelai, 11 years ago

BinaryDecoder.h corrected
  • Property svn:executable set to *
File size: 97.1 KB
RevLine 
[305]1<<<<<<< .mine
2// %pacpus:license{
3// This file is part of the PACPUS framework distributed under the
4// CECILL-C License, Version 1.0.
5// %pacpus:license}
6/// @file
7/// @author Gerald Dherbomez <firstname.surname@utc.fr>
8/// @date July, 2008
9/// @version $Id: BinaryDecoder.h 76 2013-01-10 17:05:10Z kurdejma $
10/// @copyright Copyright (c) UTC/CNRS Heudiasyc 2006 - 2013. All rights reserved.
11/// @brief Extracts data of specified type in a string.
12///
13/// Purpose: Extract data of specified type in a string (char *)
14/// Usage:
15/// bool xDecodeToXXX(T* result, const char * data, const unsigned int startBit, const unsigned int length)
16/// => x = format (i:Intel, Little Endian)
17/// (m:Motorola, Big Endian)
18/// => XXX = type of return value
19/// (Bool = bool)
20/// (I8 = char - 8 bits)
21/// (UI8 = unsigned char - 8 bits)
22/// (I16 = short - 16 bits)
23/// (UI16 = unsigned short - 16 bits)
24/// (I32 = long - 32 bits)
25/// (UI32 = unsigned long - 32 bits)
26/// (I64 = long long - 64 bits)
27/// (UI16 = unisgned long long - 64 bits)
28///
29/// ex: inline bool mDecodeToUI16(r, str, 8, 12);
30
31#ifndef BINARYDECODER_H
32#define BINARYDECODER_H
33
34#ifdef __cplusplus
35extern "C"
36{
37#endif // __cplusplus
38
39#include <stdio.h>
40
41//////////////////////////////////////////////////////////////////////////
42// print the data in the hexadecimal format in the console
43inline void displayData(const unsigned char * data, const unsigned long length, const int id)
44{
45 printf("\ndata = ");
46 for (unsigned int i = 0;i< length;++i )
47 printf("%02x ",data[i]);
48 printf(" dlc = %ld ID = 0x%x\n", length, id);
49}
50
51//////////////////////////////////////////////////////////////////////////
52// for Big Endian - Motorola coding
53//------------------------------------------------------------------------
54// return true if the signal is spread on more than 1 byte
55// ie if length is greater than ((startBit%8) + 1)
56// example:
57// 7654 3210
58// 0 .... ..||
59// 1 |||| ....
60// startBit is 1 and the length is 8: result gives (1%8)+1 = 2 < length(=6)
61inline bool mSpreadOnMoreThan1Byte(const unsigned int startBit, const unsigned int length)
62{
63 if ( (length > (startBit & 0x07) + 1) ) {
64 return true;
65 } else {
66 return false;
67 }
68}
69
70//////////////////////////////////////////////////////////////////////////
71// for Little Endian - Intel coding
72//------------------------------------------------------------------------
73// return true if the signal is spread on more than 1 byte
74// ie if length is greater than 8-(startBit%8)
75// example:
76// 7654 3210
77// 0 ||.. ....
78// 1 .... ||||
79// startBit is 6 and the length is 6: result gives 8-((6%8)) = 2 < length(=6)
80inline bool iSpreadOnMoreThan1Byte(const unsigned int startBit, const unsigned int length)
81{
82 if ( length > 8 - (startBit & 0x07) ) {
83 return true;
84 } else {
85 return false;
86 }
87}
88
89//////////////////////////////////////////////////////////////////////////
90// for Big Endian - Motorola coding
91//------------------------------------------------------------------------
92// return true if the signal is spread on more than 2 bytes
93// ie if (length - ((startBit%8) + 1) is greater than 8
94// example:
95// 7654 3210
96// 0 .... ...|
97// 1 |||| ||||
98// 2 ||.. ....
99// the start bit is 0 and the length is 11: result gives 11 - (0%8+1) = 10 (>8)
100//////////////////////////////////////////////////////////////////////////
101inline bool mSpreadOnMoreThan2Bytes(const unsigned int startBit, const unsigned int length)
102{
103 if ( length - ((startBit & 0x07) + 1) > 8 )
104 return true;
105 else
106 return false;
107}
108
109//////////////////////////////////////////////////////////////////////////
110// for Little Endian - Intel coding
111//------------------------------------------------------------------------
112// return true if the signal is spread on more than 2 bytes
113// ie if (length - (8 - startBit)) is greater than 8
114// example:
115// 7654 3210
116// 0 |... ....
117// 1 |||| ||||
118// 2 .... ..||
119// the start bit is 7 and the length is 11: result gives 11 - (8 - 7) = 10 (>8)
120//////////////////////////////////////////////////////////////////////////
121inline bool iSpreadOnMoreThan2Bytes(const unsigned int startBit, const unsigned int length)
122{
123 if ( length - (8 - (startBit & 0x07)) > 8 )
124 return true;
125 else
126 return false;
127}
128
129//////////////////////////////////////////////////////////////////////////
130// for Big Endian - Motorola coding
131//------------------------------------------------------------------------
132// return true if the signal is spread on more than 4 bytes
133// ie if length is greater than ((startBit%8) + 1)
134// example:
135// 7654 3210
136// 0 .... ..||
137// 1 |||| ||||
138// 2 |||| ||||
139// 3 |||| ||||
140// 4 |||. ....
141// startBit is 1 and the length is 29: result gives 29 - ((1%8)+1) = 27 (> 24)
142inline bool mSpreadOnMoreThan4Bytes(const unsigned int startBit, const unsigned int length)
143{
144 return (3*8) < (length - ((startBit & 0x07) + 1));
145}
146
147//////////////////////////////////////////////////////////////////////////
148// for Little Endian - Intel coding
149//------------------------------------------------------------------------
150// return true if the signal is spread on more than 4 bytes
151// ie if length is greater than 8-(startBit%8)
152// example:
153// 7654 3210
154// 0 ||.. ....
155// 1 |||| ||||
156// 2 |||| ||||
157// 3 |||| ||||
158// 4 .... ..||
159// startBit is 6 and the length is 28: result gives 28 - (8-((6%8))) = 26 (>24)
160inline bool iSpreadOnMoreThan4Bytes(const unsigned int startBit, const unsigned int length)
161{
162 return (3*8) < (length - (8 - (startBit & 0x07)));
163}
164
165//////////////////////////////////////////////////////////////////////////
166// return a 8-bits shifted-left mask corresponding to the length parameter
167// ex: shift = 2 and length = 3
168// mask will be 0b00011100
169inline unsigned char getShiftMask8(const unsigned int shift, const unsigned int length)
170{
171 unsigned char mask;
172 switch (length)
173 {
174 case 1: mask = 0x01; break;
175 case 2: mask = 0x03; break;
176 case 3: mask = 0x07; break;
177 case 4: mask = 0x0F; break;
178 case 5: mask = 0x1F; break;
179 case 6: mask = 0x3F; break;
180 case 7: mask = 0x7F; break;
181 case 8: mask = 0xFF; break;
182 default: mask = 0; break;
183 }
184 return mask << shift;
185}
186
187//////////////////////////////////////////////////////////////////////////
188// return a 8-bits mask corresponding to the length parameter with bit order decreasing
189// ex: startBit = 2 and length = 3
190// mask will be 0b00000111
191inline unsigned char getMask8( const unsigned int length )
192{
193 unsigned char mask;
194 switch (length) {
195 case 1: mask = 0x01; break;
196 case 2: mask = 0x03; break;
197 case 3: mask = 0x07; break;
198 case 4: mask = 0x0F; break;
199 case 5: mask = 0x1F; break;
200 case 6: mask = 0x3F; break;
201 case 7: mask = 0x7F; break;
202 case 8: mask = 0xFF; break;
203 default: mask = 0; break;
204 }
205 return mask;
206}
207
208//////////////////////////////////////////////////////////////////////////
209// return a 16-bits shifted-left mask corresponding to the length parameter
210inline unsigned short getShiftMask16(const unsigned int shift, const unsigned int length)
211{
212 unsigned short mask;
213 switch (length) {
214 case 1: mask = 0x0001; break;
215 case 2: mask = 0x0003; break;
216 case 3: mask = 0x0007; break;
217 case 4: mask = 0x000F; break;
218 case 5: mask = 0x001F; break;
219 case 6: mask = 0x003F; break;
220 case 7: mask = 0x007F; break;
221 case 8: mask = 0x00FF; break;
222 case 9: mask = 0x01FF; break;
223 case 10: mask = 0x03FF; break;
224 case 11: mask = 0x07FF; break;
225 case 12: mask = 0x0FFF; break;
226 case 13: mask = 0x1FFF; break;
227 case 14: mask = 0x3FFF; break;
228 case 15: mask = 0x7FFF; break;
229 case 16: mask = 0xFFFF; break;
230 default: mask = 0; break;
231 }
232 return mask << shift;
233}
234
235//////////////////////////////////////////////////////////////////////////
236// return a 16-bits mask corresponding to the length parameter with bit order decreasing
237inline unsigned short getMask16( const unsigned int length )
238{
239 unsigned short mask;
240 switch (length) {
241 case 1: mask = 0x0001; break;
242 case 2: mask = 0x0003; break;
243 case 3: mask = 0x0007; break;
244 case 4: mask = 0x000F; break;
245 case 5: mask = 0x001F; break;
246 case 6: mask = 0x003F; break;
247 case 7: mask = 0x007F; break;
248 case 8: mask = 0x00FF; break;
249 case 9: mask = 0x01FF; break;
250 case 10: mask = 0x03FF; break;
251 case 11: mask = 0x07FF; break;
252 case 12: mask = 0x0FFF; break;
253 case 13: mask = 0x1FFF; break;
254 case 14: mask = 0x3FFF; break;
255 case 15: mask = 0x7FFF; break;
256 case 16: mask = 0xFFFF; break;
257 default: mask = 0; break;
258 }
259 return mask;
260}
261
262//////////////////////////////////////////////////////////////////////////
263// return a 32-bits shifted-left mask corresponding to the length parameter
264inline unsigned long getShiftMask32(const unsigned int shift, const unsigned int length)
265{
266 unsigned long mask;
267 switch (length) {
268 case 1: mask = 0x00000001; break;
269 case 2: mask = 0x00000003; break;
270 case 3: mask = 0x00000007; break;
271 case 4: mask = 0x0000000F; break;
272 case 5: mask = 0x0000001F; break;
273 case 6: mask = 0x0000003F; break;
274 case 7: mask = 0x0000007F; break;
275 case 8: mask = 0x000000FF; break;
276 case 9: mask = 0x000001FF; break;
277 case 10: mask = 0x000003FF; break;
278 case 11: mask = 0x000007FF; break;
279 case 12: mask = 0x00000FFF; break;
280 case 13: mask = 0x00001FFF; break;
281 case 14: mask = 0x00003FFF; break;
282 case 15: mask = 0x00007FFF; break;
283 case 16: mask = 0x0000FFFF; break;
284 case 17: mask = 0x0001FFFF; break;
285 case 18: mask = 0x0003FFFF; break;
286 case 19: mask = 0x0007FFFF; break;
287 case 20: mask = 0x000FFFFF; break;
288 case 21: mask = 0x001FFFFF; break;
289 case 22: mask = 0x003FFFFF; break;
290 case 23: mask = 0x007FFFFF; break;
291 case 24: mask = 0x00FFFFFF; break;
292 case 25: mask = 0x01FFFFFF; break;
293 case 26: mask = 0x03FFFFFF; break;
294 case 27: mask = 0x07FFFFFF; break;
295 case 28: mask = 0x0FFFFFFF; break;
296 case 29: mask = 0x1FFFFFFF; break;
297 case 30: mask = 0x3FFFFFFF; break;
298 case 31: mask = 0x7FFFFFFF; break;
299 case 32: mask = 0xFFFFFFFF; break;
300 default: mask = 0; break;
301 }
302 return mask << shift;
303}
304
305//////////////////////////////////////////////////////////////////////////
306// return a 32-bits mask corresponding to the length parameter with bit order decreasing
307inline unsigned long getMask32( const unsigned int length )
308{
309 unsigned long mask;
310 switch (length) {
311 case 1: mask = 0x00000001; break;
312 case 2: mask = 0x00000003; break;
313 case 3: mask = 0x00000007; break;
314 case 4: mask = 0x0000000F; break;
315 case 5: mask = 0x0000001F; break;
316 case 6: mask = 0x0000003F; break;
317 case 7: mask = 0x0000007F; break;
318 case 8: mask = 0x000000FF; break;
319 case 9: mask = 0x000001FF; break;
320 case 10: mask = 0x000003FF; break;
321 case 11: mask = 0x000007FF; break;
322 case 12: mask = 0x00000FFF; break;
323 case 13: mask = 0x00001FFF; break;
324 case 14: mask = 0x00003FFF; break;
325 case 15: mask = 0x00007FFF; break;
326 case 16: mask = 0x0000FFFF; break;
327 case 17: mask = 0x0001FFFF; break;
328 case 18: mask = 0x0003FFFF; break;
329 case 19: mask = 0x0007FFFF; break;
330 case 20: mask = 0x000FFFFF; break;
331 case 21: mask = 0x001FFFFF; break;
332 case 22: mask = 0x003FFFFF; break;
333 case 23: mask = 0x007FFFFF; break;
334 case 24: mask = 0x00FFFFFF; break;
335 case 25: mask = 0x01FFFFFF; break;
336 case 26: mask = 0x03FFFFFF; break;
337 case 27: mask = 0x07FFFFFF; break;
338 case 28: mask = 0x0FFFFFFF; break;
339 case 29: mask = 0x1FFFFFFF; break;
340 case 30: mask = 0x3FFFFFFF; break;
341 case 31: mask = 0x7FFFFFFF; break;
342 case 32: mask = 0xFFFFFFFF; break;
343 default: mask = 0; break;
344 }
345 return mask;
346}
347
348//////////////////////////////////////////////////////////////////////////
349// return a 64-bits mask corresponding to the length parameter with bit order decreasing
350inline unsigned long long getMask64( const unsigned int length )
351{
352 unsigned long long mask;
353 switch (length) {
354 case 1: mask = 0x0000000000000001ULL; break;
355 case 2: mask = 0x0000000000000003ULL; break;
356 case 3: mask = 0x0000000000000007ULL; break;
357 case 4: mask = 0x000000000000000FULL; break;
358 case 5: mask = 0x000000000000001FULL; break;
359 case 6: mask = 0x000000000000003FULL; break;
360 case 7: mask = 0x000000000000007FULL; break;
361 case 8: mask = 0x00000000000000FFULL; break;
362 case 9: mask = 0x00000000000001FFULL; break;
363 case 10: mask = 0x00000000000003FFULL; break;
364 case 11: mask = 0x00000000000007FFULL; break;
365 case 12: mask = 0x0000000000000FFFULL; break;
366 case 13: mask = 0x0000000000001FFFULL; break;
367 case 14: mask = 0x0000000000003FFFULL; break;
368 case 15: mask = 0x0000000000007FFFULL; break;
369 case 16: mask = 0x000000000000FFFFULL; break;
370 case 17: mask = 0x000000000001FFFFULL; break;
371 case 18: mask = 0x000000000003FFFFULL; break;
372 case 19: mask = 0x000000000007FFFFULL; break;
373 case 20: mask = 0x00000000000FFFFFULL; break;
374 case 21: mask = 0x00000000001FFFFFULL; break;
375 case 22: mask = 0x00000000003FFFFFULL; break;
376 case 23: mask = 0x00000000007FFFFFULL; break;
377 case 24: mask = 0x0000000000FFFFFFULL; break;
378 case 25: mask = 0x0000000001FFFFFFULL; break;
379 case 26: mask = 0x0000000003FFFFFFULL; break;
380 case 27: mask = 0x0000000007FFFFFFULL; break;
381 case 28: mask = 0x000000000FFFFFFFULL; break;
382 case 29: mask = 0x000000001FFFFFFFULL; break;
383 case 30: mask = 0x000000003FFFFFFFULL; break;
384 case 31: mask = 0x000000007FFFFFFFULL; break;
385 case 32: mask = 0x00000000FFFFFFFFULL; break;
386 case 33: mask = 0x00000001FFFFFFFFULL; break;
387 case 34: mask = 0x00000003FFFFFFFFULL; break;
388 case 35: mask = 0x00000007FFFFFFFFULL; break;
389 case 36: mask = 0x0000000FFFFFFFFFULL; break;
390 case 37: mask = 0x0000001FFFFFFFFFULL; break;
391 case 38: mask = 0x0000003FFFFFFFFFULL; break;
392 case 39: mask = 0x0000007FFFFFFFFFULL; break;
393 case 40: mask = 0x000000FFFFFFFFFFULL; break;
394 case 41: mask = 0x000001FFFFFFFFFFULL; break;
395 case 42: mask = 0x000003FFFFFFFFFFULL; break;
396 case 43: mask = 0x000007FFFFFFFFFFULL; break;
397 case 44: mask = 0x00000FFFFFFFFFFFULL; break;
398 case 45: mask = 0x00001FFFFFFFFFFFULL; break;
399 case 46: mask = 0x00003FFFFFFFFFFFULL; break;
400 case 47: mask = 0x00007FFFFFFFFFFFULL; break;
401 case 48: mask = 0x0000FFFFFFFFFFFFULL; break;
402 case 49: mask = 0x0001FFFFFFFFFFFFULL; break;
403 case 50: mask = 0x0003FFFFFFFFFFFFULL; break;
404 case 51: mask = 0x0007FFFFFFFFFFFFULL; break;
405 case 52: mask = 0x000FFFFFFFFFFFFFULL; break;
406 case 53: mask = 0x001FFFFFFFFFFFFFULL; break;
407 case 54: mask = 0x003FFFFFFFFFFFFFULL; break;
408 case 55: mask = 0x007FFFFFFFFFFFFFULL; break;
409 case 56: mask = 0x00FFFFFFFFFFFFFFULL; break;
410 case 57: mask = 0x01FFFFFFFFFFFFFFULL; break;
411 case 58: mask = 0x03FFFFFFFFFFFFFFULL; break;
412 case 59: mask = 0x07FFFFFFFFFFFFFFULL; break;
413 case 60: mask = 0x0FFFFFFFFFFFFFFFULL; break;
414 case 61: mask = 0x1FFFFFFFFFFFFFFFULL; break;
415 case 62: mask = 0x3FFFFFFFFFFFFFFFULL; break;
416 case 63: mask = 0x7FFFFFFFFFFFFFFFULL; break;
417 case 64: mask = 0xFFFFFFFFFFFFFFFFULL; break;
418 default: mask = 0; break;
419 }
420 return mask;
421}
422
423//////////////////////////////////////////////////////////////////////////
424/// Returns a 64-bits shifted-left mask corresponding to the length parameter
425inline unsigned long long getShiftMask64(const unsigned int shift, const unsigned int length)
426{
427 unsigned long long mask;
428 mask = getMask64( length );
429 mask <<= shift;
430 return mask;
431}
432
433//////////////////////////////////////////////////////////////////////////
434/// Big Endian - Motorola coding
435inline bool mDecodeToBool(const unsigned char * data, const unsigned int startBit)
436{
437 return (data[startBit>>3] & getShiftMask8(startBit&0x07,1)) != 0;
438}
439
440//////////////////////////////////////////////////////////////////////////
441// Big Endian - Motorola coding
442// to test ?
443inline bool mDecodeToUI64(unsigned long long * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
444{
445 if (length > 64) {
446 *result = 0;
447 return false;
448 }
449
450 if (length == 64) {
451 // only work if data are correctly byte-aligned
452 unsigned char c[8]; // c[0] = MSB
453 for (int i = 0 ; i < 8 ; i++)
454 c[i] = data[(startBit>>3) + i];
455
456 unsigned long long temp = 0;
457 for (int i = 0 ; i < 8 ; i++)
458 temp += (c[i] << ( 56 - (8*i) ) );
459 *result = temp;
460 return true;
461 } else {
462 unsigned char c[8], mask[8], nbBits[8]; // MSB = c[0]
463 nbBits[0] = (startBit & 0x07)+1;
464 mask[0] = getMask8( nbBits[0] );
465 c[0] = data[startBit>>3] & mask[0];
466 unsigned short nbBitsIncrement = nbBits[0];
467 for (int i = 1 ; i < 8 ; i++) {
468 nbBits[i] = static_cast<unsigned char>( (length - nbBitsIncrement) < 8 ? length - nbBitsIncrement : 8 );
469 nbBitsIncrement += nbBits[i];
470 mask[i] = getShiftMask8(8 - nbBits[i],nbBits[i]);
471 c[i] = data[(startBit>>3) + i] & mask[i];
472 }
473
474 unsigned long long temp = 0;
475 for (int i = 0 ; i < 8 ; i++)
476 temp += (c[i] << ( 56 - (8*i) ) );
477 *result = temp >> (56 + (startBit&0x07) + 1 - length);
478 return true;
479 }
480}
481
482//////////////////////////////////////////////////////////////////////////
483// Big Endian - Motorola coding
484// to test ?
485inline bool mDecodeToI64(long long *result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
486{
487 if (length>64) {
488 *result = 0;
489 return false;
490 }
491
492 if (length == 64) {
493 // only work if data are correctly byte-aligned
494 unsigned char c[8]; // c[0] = MSB
495 for (int i = 0 ; i < 8 ; i++)
496 c[i] = data[(startBit>>3) + i];
497
498 for (int i = 0 ; i < 8 ; i++)
499 *result += (c[i] << ( 56 - (8*i) ) );
500
501 // need to be signed ??
502 return true;
503 } else {
504 unsigned char c[8], mask[8], nbBits[8]; // MSB = c[0]
505 nbBits[0] = (startBit & 0x07)+1;
506 mask[0] = getMask8( nbBits[0] );
507 c[0] = data[startBit>>3] & mask[0];
508 unsigned short nbBitsIncrement = nbBits[0];
509 for (int i = 1 ; i < 8 ; i++) {
510 nbBits[i] = static_cast<unsigned char>( (length - nbBitsIncrement) < 8 ? (length - nbBitsIncrement) : 8 );
511 nbBitsIncrement += nbBits[i];
512 mask[i] = getShiftMask8(8 - nbBits[i],nbBits[i]);
513 c[i] = data[(startBit>>3) + i] & mask[i];
514 }
515 unsigned long long temp = 0;
516 for (int i = 0 ; i < 8 ; i++)
517 temp += (c[i] << ( 56 - (8*i) ) );
518 temp >>= (56 + (startBit&0x07) + 1 - length);
519
520 if ( temp & getShiftMask64(length-1,1) )
521 // do the two's complement to get the signed value if the msb=1
522 *result = -1 * ( ( (~temp) + 1 ) & getMask64(length) );
523 else
524 *result = temp;
525
526 return true;
527 }
528}
529
530//////////////////////////////////////////////////////////////////////////
531// Big Endian - Motorola coding
532// ok
533inline bool mDecodeToUI32(unsigned long *result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
534{
535 if (length>32) {
536 *result = 0;
537 return false;
538 }
539
540 // verify that the frame is not spread in 5 bytes
541 if (mSpreadOnMoreThan4Bytes(startBit, length)) {
542 // decode in a 64-bits integer
543 unsigned long long temp;
544 if (mDecodeToUI64(&temp,data,startBit,length))
545 {
546 *result = static_cast<unsigned long> (temp);
547 return true;
548 } else {
549 *result = 0;
550 return false;
551 }
552 } else {
553 if (length == 32) {
554 // only work if data are correctly byte-aligned
555 unsigned char c1, c2, c3, c4;
556 c1 = data[startBit>>3];
557 c2 = data[(startBit>>3) + 1];
558 c3 = data[(startBit>>3) + 2];
559 c4 = data[(startBit>>3) + 3];
560
561 *result = (c1 << 24) + (c2 << 16) + (c3 << 8) + c4;
562 return true;
563 } else {
564 unsigned char c[4], mask[4], nbBits[4]; // MSB = c[0]
565 nbBits[0] = (startBit & 0x07)+1;
566 nbBits[1] = static_cast<unsigned char>( (length - nbBits[0]) < 8 ? length - nbBits[0] : 8 );
567 nbBits[2] = static_cast<unsigned char>( (length - nbBits[0] - nbBits[1]) < 8 ? (length - nbBits[0] - nbBits[1]) : 8 );
568 nbBits[3] = static_cast<unsigned char>( (length - nbBits[0] - nbBits[1] - nbBits[2]) < 8 ? (length - nbBits[0] - nbBits[1] - nbBits[2]) : 8 );
569 mask[0] = getMask8( nbBits[0] );
570 mask[1] = getShiftMask8(8 - nbBits[1],nbBits[1]);
571 mask[2] = getShiftMask8(8 - nbBits[2],nbBits[2]);
572 mask[3] = getShiftMask8(8 - nbBits[3],nbBits[3]);
573 c[0] = data[startBit>>3] & mask[0];
574 c[1] = data[(startBit>>3) + 1] & mask[1];
575 c[2] = data[(startBit>>3) + 2] & mask[2];
576 c[3] = data[(startBit>>3) + 3] & mask[3];
577 *result = ( (c[0]<<24) + (c[1]<<16) + (c[2]<<8) + c[3]) >> (24 + (startBit&0x07) + 1 - length);
578 return true;
579 }
580 }
581}
582
583//////////////////////////////////////////////////////////////////////////
584// Big Endian - Motorola coding
585// ok
586inline bool mDecodeToI32(long *result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
587{
588 if (length>32) {
589 *result = 0;
590 return false;
591 }
592
593 // verify that the frame is not spread in 5 bytes
594 if (mSpreadOnMoreThan4Bytes(startBit, length)) {
595 // decode in a 64-bits integer
596 long long temp;
597 if (mDecodeToI64(&temp,data,startBit,length))
598 {
599 *result = static_cast<long>(temp);
600 return true;
601 } else {
602 *result = 0;
603 return false;
604 }
605 } else {
606 if (length == 32)
607 {
608 // only work if data are correctly byte-aligned
609 unsigned char c1, c2, c3, c4;
610 c1 = data[startBit>>3];
611 c2 = data[(startBit>>3) + 1];
612 c3 = data[(startBit>>3) + 2];
613 c4 = data[(startBit>>3) + 3];
614
615 *result = (c1 << 24) + (c2 << 16) + (c3 << 8) + c4;
616 return true;
617 } else {
618 unsigned char c[4], mask[4], nbBits[4]; // MSB = c[0]
619 nbBits[0] = (startBit & 0x07) + 1;
620 nbBits[1] = static_cast<unsigned char>( (length - nbBits[0]) < 8 ? length - nbBits[0] : 8 );
621 nbBits[2] = static_cast<unsigned char>( (length - nbBits[0] - nbBits[1]) < 8 ? (length - nbBits[0] - nbBits[1]) : 8 );
622 nbBits[3] = static_cast<unsigned char>( (length - nbBits[0] - nbBits[1] - nbBits[2]) < 8 ? length - nbBits[0] - nbBits[1] - nbBits[2] : 8 );
623 mask[0] = getMask8( nbBits[0] );
624 mask[1] = getShiftMask8(8 - nbBits[1],nbBits[1]);
625 mask[2] = getShiftMask8(8 - nbBits[2],nbBits[2]);
626 mask[3] = getShiftMask8(8 - nbBits[3],nbBits[3]);
627 c[0] = data[startBit>>3] & mask[0];
628 c[1] = data[(startBit>>3) + 1] & mask[1];
629 c[2] = data[(startBit>>3) + 2] & mask[2];
630 c[3] = data[(startBit>>3) + 3] & mask[3];
631
632 unsigned long temp = ( (c[0]<<24) + (c[1]<<16) + (c[2]<<8) + c[3]) >> (24 + (startBit&0x07) + 1 - length);
633 if (temp & getShiftMask32(length-1, 1))
634 // do the two's complement to get the signed value if the msb=1
635 *result = -1 * ( ( (~temp) + 1 ) & getMask32(length) );
636 else
637 *result = temp;
638 return true;
639 }
640 }
641}
642
643
644
645//////////////////////////////////////////////////////////////////////////
646// Big Endian - Motorola coding
647// ok
648inline bool mDecodeToUI16(unsigned short * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
649{
650 if (length > 16) {
651 *result = 0;
652 return false;
653 }
654
655 // verify that the frame is not spread in 3 bytes
656 if (mSpreadOnMoreThan2Bytes(startBit, length)) {
657 // decode in a 32-bits integer
658 unsigned long temp;
659 if (mDecodeToUI32(&temp,data,startBit,length)) {
660 *result = static_cast<unsigned short>(temp);
661 return true;
662 } else {
663 *result = 0;
664 return false;
665 }
666 } else {
667 if (length == 16) {
668 // only work if data are correctly byte-aligned
669 unsigned char LSB, MSB;
670 MSB = data[startBit>>3];
671 LSB = data[(startBit>>3) + 1];
672
673 *result = ( (MSB << 8) + LSB );
674 return true;
675 } else {
676 unsigned char LSB, MSB, maskMSB, maskLSB, nbBitsMSB, nbBitsLSB;
677 nbBitsMSB = (startBit & 0x07) + 1; // the number of bits contained in the MSB
678 nbBitsLSB = static_cast<unsigned char>(length - nbBitsMSB);
679 maskMSB = getMask8( nbBitsMSB );
680 maskLSB = getShiftMask8(8 - nbBitsLSB , nbBitsLSB);
681 MSB = data[startBit>>3] & maskMSB;
682 LSB = data[(startBit>>3) + 1] & maskLSB;
683
684 *result = ( (MSB << 8) + LSB ) >> (8 + (startBit&0x07) + 1 - length);
685 return true;
686 }
687 }
688}
689
690//////////////////////////////////////////////////////////////////////////
691// Big Endian - Motorola coding
692// ok
693inline bool mDecodeToI16(short *result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
694{
695 if (length > 16) {
696 *result = 0;
697 return false;
698 }
699
700 // verify that the frame is not spread in 3 bytes
701 if (mSpreadOnMoreThan2Bytes(startBit, length)) {
702 // decode in a 32-bits integer
703 long temp;
704 if (mDecodeToI32(&temp, data, startBit, length)) {
705 *result = (short)temp;
706 return true;
707 } else {
708 *result = 0;
709 return false;
710 }
711 } else {
712 // ok data are stored at most in 2 bytes
713 if (length == 16) {
714 // only work if data are correctly byte-aligned
715 unsigned char LSB, MSB;
716 MSB = data[startBit>>3];
717 LSB = data[(startBit>>3) + 1];
718 short temp = (MSB << 8) + LSB;
719
720 // Attention : a-t on besoin d'appliquer le signe ?
721 // n'est-il pas deja inclu dans la donn�e comme elle est correctement align�e sur l'octet ?
722 if (temp & 0x8000) {
723 // do the two's complement to get the signed value
724 *result = - ( ( (~temp) + 1 ) & 0xFFFF );
725 } else {
726 *result = temp;
727 }
728 return true;
729 } else {
730 unsigned char LSB, MSB, maskMSB, maskLSB, nbBitsMSB, nbBitsLSB;
731 nbBitsMSB = (startBit & 0x07) + 1; // the number of bits contained in the MSB
732 nbBitsLSB = static_cast<unsigned char>(length - nbBitsMSB);
733 maskMSB = getMask8( nbBitsMSB );
734 maskLSB = getShiftMask8(8 - nbBitsLSB , nbBitsLSB);
735 MSB = data[startBit>>3] & maskMSB;
736 LSB = data[(startBit>>3) + 1] & maskLSB;
737 // assign the MSB and LSB char in the short integer value and right-shift
738 // to place the lsb to the bit 0
739 unsigned short temp = ( (MSB << 8) + LSB ) >> (8 + (startBit&0x07) + 1 - length);
740 if (temp & getShiftMask16(length-1,1))
741 // do the two's complement to get the signed value if the msb=1
742 *result = -1 * ( ( (~temp) + 1 ) & getMask16(length) );
743 else
744 *result = temp;
745 return true;
746 }
747 }
748}
749
750//////////////////////////////////////////////////////////////////////////
751// Big Endian - Motorola coding
752// OK
753inline bool mDecodeToUI8(unsigned char * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
754{
755 if (length > 8) {
756 *result = 0;
757 return false;
758 }
759
760 // verify that the frame is not spread in 2 bytes
761 if (mSpreadOnMoreThan1Byte(startBit, length)) {
762 // decode in a 16-bit integer
763 unsigned short temp;
764 if ( mDecodeToUI16(&temp, data, startBit, length) ) {
765 *result = static_cast<unsigned char>(temp);
766 return true;
767 } else {
768 *result = 0;
769 return false;
770 }
771 } else {
772 // ok data is stored at most in 1 byte
773 unsigned char c;
774
775 c = data[startBit>>3]; // >>3 <=> div 8
776 //c >>= (8-((startBit & 0x07)+1)); // &0x07 <=> modulo 8
777 c >>= (startBit & 0x07) - (length - 1); // &0x07 <=> modulo 8
778 *result = c & getMask8( length );
779
780 return true;
781 }
782}
783
784//////////////////////////////////////////////////////////////////////////
785// Big Endian - Motorola coding
786// OK
787inline bool mDecodeToI8(char * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
788{
789 if (length > 8) {
790 *result = 0;
791 return false;
792 }
793
794 // verify that the frame is not spread in 2 bytes
795 if (mSpreadOnMoreThan1Byte(startBit, length)) {
796 // decode in a 16-bit integer
797 short temp;
798 if ( mDecodeToI16(&temp, data, startBit, length) ) {
799 *result = static_cast<char>(temp);
800 return true;
801 } else {
802 *result = 0;
803 return false;
804 }
805 }
806 else {
807 // ok data is stored at most in 1 byte
808 char c;
809 c = data[startBit>>3];
810 c >>= (8-((startBit & 0x07)+1));
811 c = c & getMask8( length );
812
813 // a-t-on besoin d'appliquer le signe dans le cas ou length = 8
814 // la donnee est correctement alignee sur l'octet donc le signe est peut-etre compris
815 if (c & getShiftMask8(length-1,1))
816 // do the two's complement to get the signed value if the msb=1
817 *result = - ( ( (~c) + 1 ) & getMask8(length) );
818 else
819 *result = c;
820 return true;
821 }
822}
823
824//////////////////////////////////////////////////////////////////////////
825// Little Endian - Intel coding
826// ok
827inline bool iDecodeToBool(const unsigned char * data, const unsigned int startBit)
828{
829 mDecodeToBool(data, startBit);
830}
831
832//////////////////////////////////////////////////////////////////////////
833// Little Endian - Intel coding
834// pas de depassement possible - la trame CAN fait 64 bits max
835// gerer le signe
836inline bool iDecodeToUI64(unsigned long long * /*result*/, const unsigned char * /*data*/, const unsigned int /*startBit*/, const unsigned int /*length*/)
837{
838 return true;
839}
840
841//////////////////////////////////////////////////////////////////////////
842// Little Endian - Intel coding
843// pas de depassement possible - la trame CAN fait 64 bits max
844// gerer le signe
845inline bool iDecodeToI64(long long * /*result*/, const unsigned char * /*data*/, const unsigned int /*startBit*/, const unsigned int /*length*/)
846{
847 return true;
848}
849
850//////////////////////////////////////////////////////////////////////////
851// Little Endian - Intel coding
852// gerer le depassement sur plus de 4 octets : necessite la fonction de decodage 64 bits
853// verifier le depassement avant cette ligne : if (length == 32)
854inline bool iDecodeToUI32(unsigned long * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
855{
856 if (length>32) {
857 *result = 0;
858 return false;
859 }
860
861 if (length == 32) {
862 // only work if length == 32
863 unsigned char c1, c2, c3, c4;
864 c4 = data[startBit>>3]; // LSB
865 c3 = data[(startBit>>3) + 1];
866 c2 = data[(startBit>>3) + 2];
867 c1 = data[(startBit>>3) + 3]; // MSB
868
869 *result = (c1 << 24) + (c2 << 16) + (c3 << 8) + c4;
870 return true;
871 } else {
872 // todo
873 *result = 0;
874 return false;
875 }
876}
877
878//////////////////////////////////////////////////////////////////////////
879// Little Endian - Intel coding
880// gerer le signe
881// gerer le depassement sur plus de 4 octets : necessite la fonction de decodage 64 bits
882// verifier le depassement avant cette ligne : if (length == 32)
883inline bool iDecodeToI32(long * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
884{
885 if (length>32) {
886 *result = 0;
887 return false;
888 }
889
890 if (length == 32) {
891 // only work if length == 32
892 unsigned char c1, c2, c3, c4;
893 c4 = data[startBit>>3]; // LSB
894 c3 = data[(startBit>>3) + 1];
895 c2 = data[(startBit>>3) + 2];
896 c1 = data[(startBit>>3) + 3]; // MSB
897
898 *result = (c1 << 24) + (c2 << 16) + (c3 << 8) + c4;
899 return true;
900 } else {
901 // todo
902 *result = 0;
903 return false;
904 }
905}
906
907//////////////////////////////////////////////////////////////////////////
908// Little Endian - Intel coding
909// gerer le depassement sur plus de 2 octets
910// verifier le depassement avant cette ligne : if (length == 16)
911inline bool iDecodeToUI16(unsigned short * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
912{
913 if (length > 16) {
914 *result = 0;
915 return false;
916 }
917
918 if (length == 16) {
919 // only work if length == 16
920 unsigned char LSB, MSB;
921 LSB = data[startBit>>3];
922 MSB = data[(startBit>>3) + 1];
923
924 *result = (MSB << 8) + LSB;
925 return true;
926 } else {
927 // TODO
928 *result = 0;
929 return false;
930 }
931}
932
933//////////////////////////////////////////////////////////////////////////
934// Little Endian - Intel coding
935// gerer le signe
936// gerer le depassement sur plus de 2 octets
937// verifier le depassement avant cette ligne : if (length == 16)
938// manque le decalage
939inline bool iDecodeToI16(short * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
940{
941 if (length > 16) {
942 *result = 0;
943 return false;
944 }
945
946 if (length == 16) {
947 // only work if length == 16
948 unsigned char LSB, MSB;
949 LSB = data[startBit>>3];
950 MSB = data[(startBit>>3) + 1];
951
952 *result = (MSB << 8) + LSB;
953 return true;
954 } else {
955 // todo
956 *result = 0;
957 return false;
958 }
959}
960
961//////////////////////////////////////////////////////////////////////////
962// Little Endian - Intel coding
963// OK
964inline bool iDecodeToUI8(unsigned char * result,const unsigned char * data, const unsigned int startBit, const unsigned int length)
965{
966 if (length > 8) {
967 *result = 0;
968 return false;
969 }
970
971 // verify that the frame is not spread in 2 bytes
972 if (iSpreadOnMoreThan1Byte(startBit, length)) {
973 // decode in a 16-bit integer
974 unsigned short temp;
975 if ( iDecodeToUI16(&temp, data, startBit, length) ) {
976 // and cast in an 8 bit integer
977 *result = (unsigned char) temp;
978 return true;
979 } else {
980 *result = 0;
981 return true;
982 }
983 } else {
984 // ok data is stored at most in 1 byte
985 unsigned char c;
986 c = data[startBit>>3];
987 c >>= (startBit & 0x07);
988 *result = c & getMask8( length );
989
990 return true;
991 }
992}
993
994//////////////////////////////////////////////////////////////////////////
995// Little Endian - Intel coding
996// OK
997inline bool iDecodeToI8(char * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
998{
999 if (length > 8) {
1000 *result = 0;
1001 return false;
1002 }
1003
1004 // verify that the frame is not spread in 2 bytes
1005 if (iSpreadOnMoreThan1Byte(startBit, length)) {
1006 // decode in a 16-bit integer
1007 short temp;
1008 if ( iDecodeToI16(&temp, data, startBit, length) ) {
1009 // and cast in an 8 bit integer
1010 *result = static_cast<char>(temp);
1011 return true;
1012 } else {
1013 *result = 0;
1014 return true;
1015 }
1016 } else {
1017 // ok data is stored at most in 1 byte
1018 char c;
1019 c = data[startBit>>3]; // >>3 <=> div 8
1020 c >>= (startBit & 0x07); // &0x07 <=> modulo 8
1021 c = c & getMask8( length );
1022 if (c & getShiftMask8(length-1,1))
1023 // do the two's complement to get the signed value if the msb=1
1024 *result = - ( ( (~c) + 1 ) & getMask8(length) );
1025 else
1026 *result = c;
1027
1028 return true;
1029 }
1030}
1031
1032//////////////////////////////////////////////////////////////////////////
1033// Big Endian - Motorola coding
1034// ok
1035inline bool mobileyemDecodeToUI16(unsigned short * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
1036{
1037 if (length>16){ *result = 0; return false; }
1038
1039 // verify that the frame is not spread in 3 bytes
1040 /* if (mSpreadOnMoreThan2Bytes(startBit, length))
1041 {
1042 // decode in a 32-bits integer
1043 unsigned long temp;
1044 if (mDecodeToUI32(&temp,data,startBit,length))
1045 {
1046 *result = static_cast<unsigned short>(temp);
1047 return true;
1048 } else {
1049 *result = 0;
1050 return false;
1051 }
1052 }
1053 else
1054 {*/
1055 if (length == 16) {
1056 // only work if data are correctly byte-aligned
1057 unsigned char LSB, MSB;
1058 MSB = data[startBit>>3];
1059 LSB = data[(startBit>>3) + 1];
1060
1061 *result = ( (MSB << 8) + LSB );
1062 return true;
1063 } else {
1064 unsigned char LSB, MSB, maskMSB, maskLSB, nbBitsMSB, nbBitsLSB;
1065 nbBitsMSB = (startBit & 0x07) + 1; // the number of bits contained in the MSB
1066 nbBitsLSB = static_cast<unsigned char>(length - nbBitsMSB);
1067 maskMSB = getMask8( nbBitsMSB );
1068 maskLSB = getShiftMask8(8 - nbBitsLSB , nbBitsLSB);
1069 MSB = data[startBit>>3] & maskMSB;
1070 LSB = data[(startBit>>3) + 1] & maskLSB;
1071
1072 *result = ( (MSB << 8) + LSB ) >> (8 + (startBit&0x07) + 1 - length);
1073 return true;
1074 }
1075 //}
1076}
1077
1078//////////////////////////////////////////////////////////////////////////
1079// Big Endian - Motorola coding
1080// ok
1081inline bool mobileyemDecodeToI16(short *result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
1082{
1083 if (length>16) { *result = 0; return false; }
1084
1085 // verify that the frame is not spread in 3 bytes
1086 /* if (mSpreadOnMoreThan2Bytes(startBit, length))
1087 {
1088 // decode in a 32-bits integer
1089 long temp;
1090 if (mDecodeToI32(&temp, data, startBit, length)) {
1091 *result = (short)temp;
1092 return true;
1093 } else {
1094 *result = 0;
1095 return false;
1096 }
1097 }
1098 else // ok data are stored at most in 2 bytes
1099 {*/
1100 if (length == 16)
1101 {
1102 // only work if data are correctly byte-aligned
1103 unsigned char LSB, MSB;
1104 MSB = data[startBit>>3];
1105 LSB = data[(startBit>>3) + 1];
1106 short temp = (MSB << 8) + LSB;
1107
1108 // Attention : a-t on besoin d'appliquer le signe ?
1109 // n'est-il pas deja inclu dans la donn�e comme elle est correctement align�e sur l'octet ?
1110 if (temp & 0x8000) {
1111 // do the two's complement to get the signed value
1112 *result = - ( ( (~temp) + 1 ) & 0xFFFF );
1113 } else {
1114 *result = temp;
1115 }
1116 return true;
1117 } else {
1118 unsigned char LSB, MSB, maskMSB, maskLSB, nbBitsMSB, nbBitsLSB;
1119 nbBitsMSB = (startBit & 0x07)+1; // the number of bits contained in the MSB
1120 nbBitsLSB = static_cast<unsigned char>(length - nbBitsMSB);
1121 maskMSB = getMask8( nbBitsMSB );
1122 maskLSB = getShiftMask8(8 - nbBitsLSB , nbBitsLSB);
1123 MSB = data[startBit>>3] & maskMSB;
1124 LSB = data[(startBit>>3) + 1] & maskLSB;
1125 // assign the MSB and LSB char in the short integer value and right-shift
1126 // to place the lsb to the bit 0
1127 unsigned short temp = ( (MSB << 8) + LSB ) >> (8 + (startBit&0x07) + 1 - length);
1128 if (temp & getShiftMask16(length-1,1))
1129 // do the two's complement to get the signed value if the msb=1
1130 *result = -1 * ( ( (~temp) + 1 ) & getMask16(length) );
1131 else
1132 *result = temp;
1133 return true;
1134 }
1135 //}
1136}
1137
1138//////////////////////////////////////////////////////////////////////////
1139// Big Endian - Motorola coding
1140// OK
1141inline bool mobileyemDecodeToUI8(unsigned char * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
1142{
1143 if (length>8) { *result = 0; return false; }
1144
1145 // verify that the frame is not spread in 2 bytes
1146 if (mSpreadOnMoreThan1Byte(startBit, length))
1147 {
1148 // decode in a 16-bit integer
1149 unsigned short temp;
1150 if ( mDecodeToUI16(&temp, data, startBit, length) ) {
1151 *result = static_cast<unsigned char>(temp);
1152 return true;
1153 } else {
1154 *result = 0;
1155 return true;
1156 }
1157 }
1158 else // ok data is stored at most in 1 byte
1159 {
1160 unsigned char c;
1161 c = data[startBit>>3]; // >>3 <=> div 8
1162 c >>= (8-((startBit & 0x07)+1)); // &0x07 <=> modulo 8
1163 *result = c & getMask8( length );
1164
1165 return true;
1166 }
1167}
1168
1169//////////////////////////////////////////////////////////////////////////
1170// Big Endian - Motorola coding
1171// OK
1172inline bool mobileyemDecodeToI8(char * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
1173{
1174 if (length>8) { *result = 0; return false; }
1175
1176 // verify that the frame is not spread in 2 bytes
1177 if (mSpreadOnMoreThan1Byte(startBit, length))
1178 {
1179 // decode in a 16-bit integer
1180 short temp;
1181 if ( mDecodeToI16(&temp, data, startBit, length) ) {
1182 *result = static_cast<char>(temp);
1183 return true;
1184 } else {
1185 *result = 0;
1186 return true;
1187 }
1188 }
1189 else // ok data is stored at most in 1 byte
1190 {
1191 char c;
1192 c = data[startBit>>3];
1193 c >>= (8-((startBit & 0x07)+1));
1194 c = c & getMask8( length );
1195
1196 // a-t-on besoin d'appliquer le signe dans le cas ou length = 8
1197 // la donnee est correctement alignee sur l'octet donc le signe est peut-etre compris
1198 if (c & getShiftMask8(length-1,1))
1199 // do the two's complement to get the signed value if the msb=1
1200 *result = - ( ( (~c) + 1 ) & getMask8(length) );
1201 else
1202 *result = c;
1203 return true;
1204 }
1205}
1206
1207//////////////////////////////////////////////////////////////////////////
1208// Little Endian - Intel coding
1209// OK, mais gerer le depassement sur plus de 2 octets
1210inline bool mobileyeiDecodeToUI16(unsigned short * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
1211{
1212 if (length>16) {
1213 *result = 0;
1214 return false;
1215 }
1216
1217 if (length == 16) {
1218 // only work if length == 16
1219 unsigned char LSB, MSB;
1220 LSB = data[startBit>>3];
1221 MSB = data[(startBit>>3) + 1];
1222
1223 *result = (unsigned short)((MSB << 8) + LSB);
1224 return true;
1225 } else {
1226 unsigned char LSB, MSB, maskMSB, maskLSB, nbBitsMSB, nbBitsLSB;
1227
1228 nbBitsLSB = static_cast<unsigned char>(8*((startBit>>3) + 1) - startBit);
1229 nbBitsMSB = static_cast<unsigned char>(length - nbBitsLSB);
1230 maskLSB = getShiftMask8(8 - nbBitsLSB , nbBitsLSB);
1231 maskMSB = getMask8( nbBitsMSB );
1232 LSB = data[startBit>>3] & maskLSB;
1233 MSB = data[(startBit>>3) + 1] & maskMSB;
1234
1235 *result = (unsigned short)((MSB<<8) + (LSB>>(startBit-8*(startBit>>3))));
1236
1237 return true;
1238 }
1239}
1240
1241//////////////////////////////////////////////////////////////////////////
1242// Little Endian - Intel coding
1243// gerer le signe
1244// gerer le depassement sur plus de 2 octets
1245// verifier le depassement avant cette ligne : if (length == 16)
1246// manque le decalage
1247inline bool mobileyeiDecodeToI16(short * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
1248{
1249 if (length > 16) {
1250 *result = 0;
1251 return false;
1252 }
1253
1254 if (length == 16) {
1255 // only work if length == 16
1256 unsigned char LSB, MSB;
1257 LSB = data[startBit>>3];
1258 MSB = data[(startBit>>3) + 1];
1259
1260 short temp = (MSB << 8) + LSB;
1261
1262 if (temp & 0x8000) {
1263 // do the two's complement to get the signed value
1264 *result = - ( ( (~temp) + 1 ) & 0xFFFF );
1265 } else {
1266 *result = temp;
1267 }
1268 return true;
1269 } else {
1270 unsigned char LSB, MSB, maskMSB, maskLSB, nbBitsMSB, nbBitsLSB;
1271
1272 nbBitsLSB = static_cast<unsigned char>(8*((startBit>>3) + 1) - startBit);
1273 nbBitsMSB = static_cast<unsigned char>(length - nbBitsLSB);
1274 maskLSB = getShiftMask8(8 - nbBitsLSB , nbBitsLSB);
1275 maskMSB = getMask8( nbBitsMSB );
1276 LSB = data[startBit>>3] & maskLSB;
1277 MSB = data[(startBit>>3) + 1] & maskMSB;
1278
1279 unsigned short temp = (unsigned short)((MSB<<8) + (LSB>>(startBit-8*(startBit>>3))));
1280
1281 if (temp & getShiftMask16(length-1,1))
1282 // do the two's complement to get the signed value if the msb=1
1283 *result = -1 * ( ( (~temp) + 1 ) & getMask16(length) );
1284 else
1285 *result = temp;
1286 return true;
1287 }
1288}
1289
1290//////////////////////////////////////////////////////////////////////////
1291// Little Endian - Intel coding
1292// OK
1293inline bool mobileyeiDecodeToUI8(unsigned char * result,const unsigned char * data, const unsigned int startBit, const unsigned int length)
1294{
1295 if (length>8) { *result = 0; return false; }
1296
1297 // verify that the frame is not spread in 2 bytes
1298 if (iSpreadOnMoreThan1Byte(startBit, length))
1299 {
1300 // decode in a 16-bit integer
1301 unsigned short temp;
1302 if ( iDecodeToUI16(&temp, data, startBit, length) )
1303 {
1304 // and cast in an 8 bit integer
1305 *result = (unsigned char) temp;
1306 return true;
1307 }
1308 else
1309 {
1310 *result = 0;
1311 return true;
1312 }
1313 }
1314 else // ok data is stored at most in 1 byte
1315 {
1316 unsigned char c;
1317 c = data[startBit>>3];
1318 c >>= (startBit & 0x07);
1319 *result = c & getMask8( length );
1320
1321 return true;
1322 }
1323
1324}
1325
1326//////////////////////////////////////////////////////////////////////////
1327// Little Endian - Intel coding
1328// OK
1329inline bool mobileyeiDecodeToI8(char * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
1330{
1331 if (length > 8) {
1332 *result = 0;
1333 return false;
1334 }
1335
1336 // verify that the frame is not spread in 2 bytes
1337 if (iSpreadOnMoreThan1Byte(startBit, length)) {
1338 // decode in a 16-bit integer
1339 short temp;
1340 if ( iDecodeToI16(&temp, data, startBit, length) ) {
1341 // and cast in an 8 bit integer
1342 *result = (char)(temp);
1343 return true;
1344 } else {
1345 *result = 0;
1346 return true;
1347 }
1348 } else {
1349 // ok data is stored at most in 1 byte
1350 char c;
1351 c = data[startBit>>3]; // >>3 <=> div 8
1352 c >>= (startBit & 0x07); // &0x07 <=> modulo 8
1353 c = c & getMask8( length );
1354 if (c & getShiftMask8(length-1,1))
1355 // do the two's complement to get the signed value if the msb=1
1356 *result = - ( ( (~c) + 1 ) & getMask8(length) );
1357 else
1358 *result = c;
1359
1360 return true;
1361 }
1362}
1363
1364inline int mobileyeDecodeCAN(unsigned char *data,unsigned char bigEndian,unsigned int startBit,unsigned int length,unsigned char Signed)
1365{
1366 int value;
1367 unsigned short ustemp16 = 0;
1368 unsigned char uctemp8 = 0;
1369 short stemp16 = 0;
1370 char ctemp8 = 0;
1371
1372 if(bigEndian) {
1373 if(length>8){
1374 if(Signed){mobileyemDecodeToI16(&stemp16,data, startBit, length);value = stemp16;}
1375 else {mobileyemDecodeToUI16(&ustemp16,data, startBit, length);value = ustemp16;}}
1376 else if(Signed) {mobileyemDecodeToI8(&ctemp8,data, startBit, length);value = ctemp8;}
1377 else {mobileyemDecodeToUI8(&uctemp8,data, startBit, length);value = uctemp8;}
1378 } else {
1379 if(length>8){
1380 if(Signed) {mobileyeiDecodeToI16(&stemp16,data, startBit, length);value = stemp16;}
1381 else {mobileyeiDecodeToUI16(&ustemp16,data, startBit, length);value = ustemp16;}}
1382 else if(Signed) {mobileyeiDecodeToI8(&ctemp8,data, startBit, length);value = ctemp8;}
1383 else {mobileyeiDecodeToUI8(&uctemp8,data, startBit, length);value = uctemp8;}
1384 }
1385
1386 return value;
1387}
1388
1389#ifdef __cplusplus
1390}
1391#endif // __cplusplus
1392
1393#endif // BINARYDECODER_H
1394=======
[295]1395// %pacpus:license{
1396// This file is part of the PACPUS framework distributed under the
1397// CECILL-C License, Version 1.0.
1398// %pacpus:license}
1399/// @file
1400/// @author Gerald Dherbomez <firstname.surname@utc.fr>
1401/// @date July, 2008
1402/// @version $Id: BinaryDecoder.h 76 2013-01-10 17:05:10Z kurdejma $
1403/// @copyright Copyright (c) UTC/CNRS Heudiasyc 2006 - 2013. All rights reserved.
1404/// @brief Extracts data of specified type in a string.
1405///
1406/// Purpose: Extract data of specified type in a string (char *)
1407/// Usage:
1408/// bool xDecodeToXXX(T* result, const char * data, const unsigned int startBit, const unsigned int length)
1409/// => x = format (i:Intel, Little Endian)
1410/// (m:Motorola, Big Endian)
1411/// => XXX = type of return value
1412/// (Bool = bool)
1413/// (I8 = char - 8 bits)
1414/// (UI8 = unsigned char - 8 bits)
1415/// (I16 = short - 16 bits)
1416/// (UI16 = unsigned short - 16 bits)
1417/// (I32 = long - 32 bits)
1418/// (UI32 = unsigned long - 32 bits)
1419/// (I64 = long long - 64 bits)
1420/// (UI16 = unisgned long long - 64 bits)
1421///
1422/// ex: inline bool mDecodeToUI16(r, str, 8, 12);
1423
1424#ifndef BINARYDECODER_H
1425#define BINARYDECODER_H
1426
1427#ifdef __cplusplus
1428extern "C"
1429{
1430#endif // __cplusplus
1431
1432#include "PacpusToolsConfig.h"
1433
1434#include <stdio.h>
1435
1436//////////////////////////////////////////////////////////////////////////
1437// print the data in the hexadecimal format in the console
1438inline void displayData(const unsigned char * data, const unsigned long length, const int id)
1439{
1440 printf("\ndata = ");
1441 for (unsigned int i = 0;i< length;++i )
1442 printf("%02x ",data[i]);
1443 printf(" dlc = %ld ID = 0x%x\n", length, id);
1444}
1445
1446//////////////////////////////////////////////////////////////////////////
1447// for Big Endian - Motorola coding
1448//------------------------------------------------------------------------
1449// return true if the signal is spread on more than 1 byte
1450// ie if length is greater than ((startBit%8) + 1)
1451// example:
1452// 7654 3210
1453// 0 .... ..||
1454// 1 |||| ....
1455// startBit is 1 and the length is 8: result gives (1%8)+1 = 2 < length(=6)
1456inline bool mSpreadOnMoreThan1Byte(const unsigned int startBit, const unsigned int length)
1457{
1458 if ( (length > (startBit & 0x07) + 1) ) {
1459 return true;
1460 } else {
1461 return false;
1462 }
1463}
1464
1465//////////////////////////////////////////////////////////////////////////
1466// for Little Endian - Intel coding
1467//------------------------------------------------------------------------
1468// return true if the signal is spread on more than 1 byte
1469// ie if length is greater than 8-(startBit%8)
1470// example:
1471// 7654 3210
1472// 0 ||.. ....
1473// 1 .... ||||
1474// startBit is 6 and the length is 6: result gives 8-((6%8)) = 2 < length(=6)
1475inline bool iSpreadOnMoreThan1Byte(const unsigned int startBit, const unsigned int length)
1476{
1477 if ( length > 8 - (startBit & 0x07) ) {
1478 return true;
1479 } else {
1480 return false;
1481 }
1482}
1483
1484//////////////////////////////////////////////////////////////////////////
1485// for Big Endian - Motorola coding
1486//------------------------------------------------------------------------
1487// return true if the signal is spread on more than 2 bytes
1488// ie if (length - ((startBit%8) + 1) is greater than 8
1489// example:
1490// 7654 3210
1491// 0 .... ...|
1492// 1 |||| ||||
1493// 2 ||.. ....
1494// the start bit is 0 and the length is 11: result gives 11 - (0%8+1) = 10 (>8)
1495//////////////////////////////////////////////////////////////////////////
1496inline bool mSpreadOnMoreThan2Bytes(const unsigned int startBit, const unsigned int length)
1497{
1498 if ( length - ((startBit & 0x07) + 1) > 8 )
1499 return true;
1500 else
1501 return false;
1502}
1503
1504//////////////////////////////////////////////////////////////////////////
1505// for Little Endian - Intel coding
1506//------------------------------------------------------------------------
1507// return true if the signal is spread on more than 2 bytes
1508// ie if (length - (8 - startBit)) is greater than 8
1509// example:
1510// 7654 3210
1511// 0 |... ....
1512// 1 |||| ||||
1513// 2 .... ..||
1514// the start bit is 7 and the length is 11: result gives 11 - (8 - 7) = 10 (>8)
1515//////////////////////////////////////////////////////////////////////////
1516inline bool iSpreadOnMoreThan2Bytes(const unsigned int startBit, const unsigned int length)
1517{
1518 if ( length - (8 - (startBit & 0x07)) > 8 )
1519 return true;
1520 else
1521 return false;
1522}
1523
1524//////////////////////////////////////////////////////////////////////////
1525// for Big Endian - Motorola coding
1526//------------------------------------------------------------------------
1527// return true if the signal is spread on more than 4 bytes
1528// ie if length is greater than ((startBit%8) + 1)
1529// example:
1530// 7654 3210
1531// 0 .... ..||
1532// 1 |||| ||||
1533// 2 |||| ||||
1534// 3 |||| ||||
1535// 4 |||. ....
1536// startBit is 1 and the length is 29: result gives 29 - ((1%8)+1) = 27 (> 24)
1537inline bool mSpreadOnMoreThan4Bytes(const unsigned int startBit, const unsigned int length)
1538{
1539 return (3*8) < (length - ((startBit & 0x07) + 1));
1540}
1541
1542//////////////////////////////////////////////////////////////////////////
1543// for Little Endian - Intel coding
1544//------------------------------------------------------------------------
1545// return true if the signal is spread on more than 4 bytes
1546// ie if length is greater than 8-(startBit%8)
1547// example:
1548// 7654 3210
1549// 0 ||.. ....
1550// 1 |||| ||||
1551// 2 |||| ||||
1552// 3 |||| ||||
1553// 4 .... ..||
1554// startBit is 6 and the length is 28: result gives 28 - (8-((6%8))) = 26 (>24)
1555inline bool iSpreadOnMoreThan4Bytes(const unsigned int startBit, const unsigned int length)
1556{
1557 return (3*8) < (length - (8 - (startBit & 0x07)));
1558}
1559
1560//////////////////////////////////////////////////////////////////////////
1561// return a 8-bits shifted-left mask corresponding to the length parameter
1562// ex: shift = 2 and length = 3
1563// mask will be 0b00011100
1564inline unsigned char getShiftMask8(const unsigned int shift, const unsigned int length)
1565{
1566 unsigned char mask;
1567 switch (length)
1568 {
1569 case 1: mask = 0x01; break;
1570 case 2: mask = 0x03; break;
1571 case 3: mask = 0x07; break;
1572 case 4: mask = 0x0F; break;
1573 case 5: mask = 0x1F; break;
1574 case 6: mask = 0x3F; break;
1575 case 7: mask = 0x7F; break;
1576 case 8: mask = 0xFF; break;
1577 default: mask = 0; break;
1578 }
1579 return mask << shift;
1580}
1581
1582//////////////////////////////////////////////////////////////////////////
1583// return a 8-bits mask corresponding to the length parameter with bit order decreasing
1584// ex: startBit = 2 and length = 3
1585// mask will be 0b00000111
1586inline unsigned char getMask8( const unsigned int length )
1587{
1588 unsigned char mask;
1589 switch (length) {
1590 case 1: mask = 0x01; break;
1591 case 2: mask = 0x03; break;
1592 case 3: mask = 0x07; break;
1593 case 4: mask = 0x0F; break;
1594 case 5: mask = 0x1F; break;
1595 case 6: mask = 0x3F; break;
1596 case 7: mask = 0x7F; break;
1597 case 8: mask = 0xFF; break;
1598 default: mask = 0; break;
1599 }
1600 return mask;
1601}
1602
1603//////////////////////////////////////////////////////////////////////////
1604// return a 16-bits shifted-left mask corresponding to the length parameter
1605inline unsigned short getShiftMask16(const unsigned int shift, const unsigned int length)
1606{
1607 unsigned short mask;
1608 switch (length) {
1609 case 1: mask = 0x0001; break;
1610 case 2: mask = 0x0003; break;
1611 case 3: mask = 0x0007; break;
1612 case 4: mask = 0x000F; break;
1613 case 5: mask = 0x001F; break;
1614 case 6: mask = 0x003F; break;
1615 case 7: mask = 0x007F; break;
1616 case 8: mask = 0x00FF; break;
1617 case 9: mask = 0x01FF; break;
1618 case 10: mask = 0x03FF; break;
1619 case 11: mask = 0x07FF; break;
1620 case 12: mask = 0x0FFF; break;
1621 case 13: mask = 0x1FFF; break;
1622 case 14: mask = 0x3FFF; break;
1623 case 15: mask = 0x7FFF; break;
1624 case 16: mask = 0xFFFF; break;
1625 default: mask = 0; break;
1626 }
1627 return mask << shift;
1628}
1629
1630//////////////////////////////////////////////////////////////////////////
1631// return a 16-bits mask corresponding to the length parameter with bit order decreasing
1632inline unsigned short getMask16( const unsigned int length )
1633{
1634 unsigned short mask;
1635 switch (length) {
1636 case 1: mask = 0x0001; break;
1637 case 2: mask = 0x0003; break;
1638 case 3: mask = 0x0007; break;
1639 case 4: mask = 0x000F; break;
1640 case 5: mask = 0x001F; break;
1641 case 6: mask = 0x003F; break;
1642 case 7: mask = 0x007F; break;
1643 case 8: mask = 0x00FF; break;
1644 case 9: mask = 0x01FF; break;
1645 case 10: mask = 0x03FF; break;
1646 case 11: mask = 0x07FF; break;
1647 case 12: mask = 0x0FFF; break;
1648 case 13: mask = 0x1FFF; break;
1649 case 14: mask = 0x3FFF; break;
1650 case 15: mask = 0x7FFF; break;
1651 case 16: mask = 0xFFFF; break;
1652 default: mask = 0; break;
1653 }
1654 return mask;
1655}
1656
1657//////////////////////////////////////////////////////////////////////////
1658// return a 32-bits shifted-left mask corresponding to the length parameter
1659inline unsigned long getShiftMask32(const unsigned int shift, const unsigned int length)
1660{
1661 unsigned long mask;
1662 switch (length) {
1663 case 1: mask = 0x00000001; break;
1664 case 2: mask = 0x00000003; break;
1665 case 3: mask = 0x00000007; break;
1666 case 4: mask = 0x0000000F; break;
1667 case 5: mask = 0x0000001F; break;
1668 case 6: mask = 0x0000003F; break;
1669 case 7: mask = 0x0000007F; break;
1670 case 8: mask = 0x000000FF; break;
1671 case 9: mask = 0x000001FF; break;
1672 case 10: mask = 0x000003FF; break;
1673 case 11: mask = 0x000007FF; break;
1674 case 12: mask = 0x00000FFF; break;
1675 case 13: mask = 0x00001FFF; break;
1676 case 14: mask = 0x00003FFF; break;
1677 case 15: mask = 0x00007FFF; break;
1678 case 16: mask = 0x0000FFFF; break;
1679 case 17: mask = 0x0001FFFF; break;
1680 case 18: mask = 0x0003FFFF; break;
1681 case 19: mask = 0x0007FFFF; break;
1682 case 20: mask = 0x000FFFFF; break;
1683 case 21: mask = 0x001FFFFF; break;
1684 case 22: mask = 0x003FFFFF; break;
1685 case 23: mask = 0x007FFFFF; break;
1686 case 24: mask = 0x00FFFFFF; break;
1687 case 25: mask = 0x01FFFFFF; break;
1688 case 26: mask = 0x03FFFFFF; break;
1689 case 27: mask = 0x07FFFFFF; break;
1690 case 28: mask = 0x0FFFFFFF; break;
1691 case 29: mask = 0x1FFFFFFF; break;
1692 case 30: mask = 0x3FFFFFFF; break;
1693 case 31: mask = 0x7FFFFFFF; break;
1694 case 32: mask = 0xFFFFFFFF; break;
1695 default: mask = 0; break;
1696 }
1697 return mask << shift;
1698}
1699
1700//////////////////////////////////////////////////////////////////////////
1701// return a 32-bits mask corresponding to the length parameter with bit order decreasing
1702inline unsigned long getMask32( const unsigned int length )
1703{
1704 unsigned long mask;
1705 switch (length) {
1706 case 1: mask = 0x00000001; break;
1707 case 2: mask = 0x00000003; break;
1708 case 3: mask = 0x00000007; break;
1709 case 4: mask = 0x0000000F; break;
1710 case 5: mask = 0x0000001F; break;
1711 case 6: mask = 0x0000003F; break;
1712 case 7: mask = 0x0000007F; break;
1713 case 8: mask = 0x000000FF; break;
1714 case 9: mask = 0x000001FF; break;
1715 case 10: mask = 0x000003FF; break;
1716 case 11: mask = 0x000007FF; break;
1717 case 12: mask = 0x00000FFF; break;
1718 case 13: mask = 0x00001FFF; break;
1719 case 14: mask = 0x00003FFF; break;
1720 case 15: mask = 0x00007FFF; break;
1721 case 16: mask = 0x0000FFFF; break;
1722 case 17: mask = 0x0001FFFF; break;
1723 case 18: mask = 0x0003FFFF; break;
1724 case 19: mask = 0x0007FFFF; break;
1725 case 20: mask = 0x000FFFFF; break;
1726 case 21: mask = 0x001FFFFF; break;
1727 case 22: mask = 0x003FFFFF; break;
1728 case 23: mask = 0x007FFFFF; break;
1729 case 24: mask = 0x00FFFFFF; break;
1730 case 25: mask = 0x01FFFFFF; break;
1731 case 26: mask = 0x03FFFFFF; break;
1732 case 27: mask = 0x07FFFFFF; break;
1733 case 28: mask = 0x0FFFFFFF; break;
1734 case 29: mask = 0x1FFFFFFF; break;
1735 case 30: mask = 0x3FFFFFFF; break;
1736 case 31: mask = 0x7FFFFFFF; break;
1737 case 32: mask = 0xFFFFFFFF; break;
1738 default: mask = 0; break;
1739 }
1740 return mask;
1741}
1742
1743//////////////////////////////////////////////////////////////////////////
1744// return a 64-bits mask corresponding to the length parameter with bit order decreasing
1745inline unsigned long long getMask64( const unsigned int length )
1746{
1747 unsigned long long mask;
1748 switch (length) {
1749 case 1: mask = 0x0000000000000001ULL; break;
1750 case 2: mask = 0x0000000000000003ULL; break;
1751 case 3: mask = 0x0000000000000007ULL; break;
1752 case 4: mask = 0x000000000000000FULL; break;
1753 case 5: mask = 0x000000000000001FULL; break;
1754 case 6: mask = 0x000000000000003FULL; break;
1755 case 7: mask = 0x000000000000007FULL; break;
1756 case 8: mask = 0x00000000000000FFULL; break;
1757 case 9: mask = 0x00000000000001FFULL; break;
1758 case 10: mask = 0x00000000000003FFULL; break;
1759 case 11: mask = 0x00000000000007FFULL; break;
1760 case 12: mask = 0x0000000000000FFFULL; break;
1761 case 13: mask = 0x0000000000001FFFULL; break;
1762 case 14: mask = 0x0000000000003FFFULL; break;
1763 case 15: mask = 0x0000000000007FFFULL; break;
1764 case 16: mask = 0x000000000000FFFFULL; break;
1765 case 17: mask = 0x000000000001FFFFULL; break;
1766 case 18: mask = 0x000000000003FFFFULL; break;
1767 case 19: mask = 0x000000000007FFFFULL; break;
1768 case 20: mask = 0x00000000000FFFFFULL; break;
1769 case 21: mask = 0x00000000001FFFFFULL; break;
1770 case 22: mask = 0x00000000003FFFFFULL; break;
1771 case 23: mask = 0x00000000007FFFFFULL; break;
1772 case 24: mask = 0x0000000000FFFFFFULL; break;
1773 case 25: mask = 0x0000000001FFFFFFULL; break;
1774 case 26: mask = 0x0000000003FFFFFFULL; break;
1775 case 27: mask = 0x0000000007FFFFFFULL; break;
1776 case 28: mask = 0x000000000FFFFFFFULL; break;
1777 case 29: mask = 0x000000001FFFFFFFULL; break;
1778 case 30: mask = 0x000000003FFFFFFFULL; break;
1779 case 31: mask = 0x000000007FFFFFFFULL; break;
1780 case 32: mask = 0x00000000FFFFFFFFULL; break;
1781 case 33: mask = 0x00000001FFFFFFFFULL; break;
1782 case 34: mask = 0x00000003FFFFFFFFULL; break;
1783 case 35: mask = 0x00000007FFFFFFFFULL; break;
1784 case 36: mask = 0x0000000FFFFFFFFFULL; break;
1785 case 37: mask = 0x0000001FFFFFFFFFULL; break;
1786 case 38: mask = 0x0000003FFFFFFFFFULL; break;
1787 case 39: mask = 0x0000007FFFFFFFFFULL; break;
1788 case 40: mask = 0x000000FFFFFFFFFFULL; break;
1789 case 41: mask = 0x000001FFFFFFFFFFULL; break;
1790 case 42: mask = 0x000003FFFFFFFFFFULL; break;
1791 case 43: mask = 0x000007FFFFFFFFFFULL; break;
1792 case 44: mask = 0x00000FFFFFFFFFFFULL; break;
1793 case 45: mask = 0x00001FFFFFFFFFFFULL; break;
1794 case 46: mask = 0x00003FFFFFFFFFFFULL; break;
1795 case 47: mask = 0x00007FFFFFFFFFFFULL; break;
1796 case 48: mask = 0x0000FFFFFFFFFFFFULL; break;
1797 case 49: mask = 0x0001FFFFFFFFFFFFULL; break;
1798 case 50: mask = 0x0003FFFFFFFFFFFFULL; break;
1799 case 51: mask = 0x0007FFFFFFFFFFFFULL; break;
1800 case 52: mask = 0x000FFFFFFFFFFFFFULL; break;
1801 case 53: mask = 0x001FFFFFFFFFFFFFULL; break;
1802 case 54: mask = 0x003FFFFFFFFFFFFFULL; break;
1803 case 55: mask = 0x007FFFFFFFFFFFFFULL; break;
1804 case 56: mask = 0x00FFFFFFFFFFFFFFULL; break;
1805 case 57: mask = 0x01FFFFFFFFFFFFFFULL; break;
1806 case 58: mask = 0x03FFFFFFFFFFFFFFULL; break;
1807 case 59: mask = 0x07FFFFFFFFFFFFFFULL; break;
1808 case 60: mask = 0x0FFFFFFFFFFFFFFFULL; break;
1809 case 61: mask = 0x1FFFFFFFFFFFFFFFULL; break;
1810 case 62: mask = 0x3FFFFFFFFFFFFFFFULL; break;
1811 case 63: mask = 0x7FFFFFFFFFFFFFFFULL; break;
1812 case 64: mask = 0xFFFFFFFFFFFFFFFFULL; break;
1813 default: mask = 0; break;
1814 }
1815 return mask;
1816}
1817
1818//////////////////////////////////////////////////////////////////////////
1819/// Returns a 64-bits shifted-left mask corresponding to the length parameter
1820inline unsigned long long getShiftMask64(const unsigned int shift, const unsigned int length)
1821{
1822 unsigned long long mask;
1823 mask = getMask64( length );
1824 mask <<= shift;
1825 return mask;
1826}
1827
1828//////////////////////////////////////////////////////////////////////////
1829/// Big Endian - Motorola coding
1830inline bool mDecodeToBool(const unsigned char * data, const unsigned int startBit)
1831{
1832 return (data[startBit>>3] & getShiftMask8(startBit&0x07,1)) != 0;
1833}
1834
1835//////////////////////////////////////////////////////////////////////////
1836// Big Endian - Motorola coding
1837// to test ?
1838inline bool mDecodeToUI64(unsigned long long * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
1839{
1840 if (length > 64) {
1841 *result = 0;
1842 return false;
1843 }
1844
1845 if (length == 64) {
1846 // only work if data are correctly byte-aligned
1847 unsigned char c[8]; // c[0] = MSB
1848 for (int i = 0 ; i < 8 ; i++)
1849 c[i] = data[(startBit>>3) + i];
1850
1851 unsigned long long temp = 0;
1852 for (int i = 0 ; i < 8 ; i++)
1853 temp += (c[i] << ( 56 - (8*i) ) );
1854 *result = temp;
1855 return true;
1856 } else {
1857 unsigned char c[8], mask[8], nbBits[8]; // MSB = c[0]
1858 nbBits[0] = (startBit & 0x07)+1;
1859 mask[0] = getMask8( nbBits[0] );
1860 c[0] = data[startBit>>3] & mask[0];
1861 unsigned short nbBitsIncrement = nbBits[0];
1862 for (int i = 1 ; i < 8 ; i++) {
1863 nbBits[i] = static_cast<unsigned char>( (length - nbBitsIncrement) < 8 ? length - nbBitsIncrement : 8 );
1864 nbBitsIncrement += nbBits[i];
1865 mask[i] = getShiftMask8(8 - nbBits[i],nbBits[i]);
1866 c[i] = data[(startBit>>3) + i] & mask[i];
1867 }
1868
1869 unsigned long long temp = 0;
1870 for (int i = 0 ; i < 8 ; i++)
1871 temp += (c[i] << ( 56 - (8*i) ) );
1872 *result = temp >> (56 + (startBit&0x07) + 1 - length);
1873 return true;
1874 }
1875}
1876
1877//////////////////////////////////////////////////////////////////////////
1878// Big Endian - Motorola coding
1879// to test ?
1880inline bool mDecodeToI64(long long *result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
1881{
1882 if (length>64) {
1883 *result = 0;
1884 return false;
1885 }
1886
1887 if (length == 64) {
1888 // only work if data are correctly byte-aligned
1889 unsigned char c[8]; // c[0] = MSB
1890 for (int i = 0 ; i < 8 ; i++)
1891 c[i] = data[(startBit>>3) + i];
1892
1893 for (int i = 0 ; i < 8 ; i++)
1894 *result += (c[i] << ( 56 - (8*i) ) );
1895
1896 // need to be signed ??
1897 return true;
1898 } else {
1899 unsigned char c[8], mask[8], nbBits[8]; // MSB = c[0]
1900 nbBits[0] = (startBit & 0x07)+1;
1901 mask[0] = getMask8( nbBits[0] );
1902 c[0] = data[startBit>>3] & mask[0];
1903 unsigned short nbBitsIncrement = nbBits[0];
1904 for (int i = 1 ; i < 8 ; i++) {
1905 nbBits[i] = static_cast<unsigned char>( (length - nbBitsIncrement) < 8 ? (length - nbBitsIncrement) : 8 );
1906 nbBitsIncrement += nbBits[i];
1907 mask[i] = getShiftMask8(8 - nbBits[i],nbBits[i]);
1908 c[i] = data[(startBit>>3) + i] & mask[i];
1909 }
1910 unsigned long long temp = 0;
1911 for (int i = 0 ; i < 8 ; i++)
1912 temp += (c[i] << ( 56 - (8*i) ) );
1913 temp >>= (56 + (startBit&0x07) + 1 - length);
1914
1915 if ( temp & getShiftMask64(length-1,1) )
1916 // do the two's complement to get the signed value if the msb=1
1917 *result = -1 * ( ( (~temp) + 1 ) & getMask64(length) );
1918 else
1919 *result = temp;
1920
1921 return true;
1922 }
1923}
1924
1925//////////////////////////////////////////////////////////////////////////
1926// Big Endian - Motorola coding
1927// ok
1928inline bool mDecodeToUI32(unsigned long *result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
1929{
1930 if (length>32) {
1931 *result = 0;
1932 return false;
1933 }
1934
1935 // verify that the frame is not spread in 5 bytes
1936 if (mSpreadOnMoreThan4Bytes(startBit, length)) {
1937 // decode in a 64-bits integer
1938 unsigned long long temp;
1939 if (mDecodeToUI64(&temp,data,startBit,length))
1940 {
1941 *result = static_cast<unsigned long> (temp);
1942 return true;
1943 } else {
1944 *result = 0;
1945 return false;
1946 }
1947 } else {
1948 if (length == 32) {
1949 // only work if data are correctly byte-aligned
1950 unsigned char c1, c2, c3, c4;
1951 c1 = data[startBit>>3];
1952 c2 = data[(startBit>>3) + 1];
1953 c3 = data[(startBit>>3) + 2];
1954 c4 = data[(startBit>>3) + 3];
1955
1956 *result = (c1 << 24) + (c2 << 16) + (c3 << 8) + c4;
1957 return true;
1958 } else {
1959 unsigned char c[4], mask[4], nbBits[4]; // MSB = c[0]
1960 nbBits[0] = (startBit & 0x07)+1;
1961 nbBits[1] = static_cast<unsigned char>( (length - nbBits[0]) < 8 ? length - nbBits[0] : 8 );
1962 nbBits[2] = static_cast<unsigned char>( (length - nbBits[0] - nbBits[1]) < 8 ? (length - nbBits[0] - nbBits[1]) : 8 );
1963 nbBits[3] = static_cast<unsigned char>( (length - nbBits[0] - nbBits[1] - nbBits[2]) < 8 ? (length - nbBits[0] - nbBits[1] - nbBits[2]) : 8 );
1964 mask[0] = getMask8( nbBits[0] );
1965 mask[1] = getShiftMask8(8 - nbBits[1],nbBits[1]);
1966 mask[2] = getShiftMask8(8 - nbBits[2],nbBits[2]);
1967 mask[3] = getShiftMask8(8 - nbBits[3],nbBits[3]);
1968 c[0] = data[startBit>>3] & mask[0];
1969 c[1] = data[(startBit>>3) + 1] & mask[1];
1970 c[2] = data[(startBit>>3) + 2] & mask[2];
1971 c[3] = data[(startBit>>3) + 3] & mask[3];
1972 *result = ( (c[0]<<24) + (c[1]<<16) + (c[2]<<8) + c[3]) >> (24 + (startBit&0x07) + 1 - length);
1973 return true;
1974 }
1975 }
1976}
1977
1978//////////////////////////////////////////////////////////////////////////
1979// Big Endian - Motorola coding
1980// ok
1981inline bool mDecodeToI32(long *result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
1982{
1983 if (length>32) {
1984 *result = 0;
1985 return false;
1986 }
1987
1988 // verify that the frame is not spread in 5 bytes
1989 if (mSpreadOnMoreThan4Bytes(startBit, length)) {
1990 // decode in a 64-bits integer
1991 long long temp;
1992 if (mDecodeToI64(&temp,data,startBit,length))
1993 {
1994 *result = static_cast<long>(temp);
1995 return true;
1996 } else {
1997 *result = 0;
1998 return false;
1999 }
2000 } else {
2001 if (length == 32)
2002 {
2003 // only work if data are correctly byte-aligned
2004 unsigned char c1, c2, c3, c4;
2005 c1 = data[startBit>>3];
2006 c2 = data[(startBit>>3) + 1];
2007 c3 = data[(startBit>>3) + 2];
2008 c4 = data[(startBit>>3) + 3];
2009
2010 *result = (c1 << 24) + (c2 << 16) + (c3 << 8) + c4;
2011 return true;
2012 } else {
2013 unsigned char c[4], mask[4], nbBits[4]; // MSB = c[0]
2014 nbBits[0] = (startBit & 0x07) + 1;
2015 nbBits[1] = static_cast<unsigned char>( (length - nbBits[0]) < 8 ? length - nbBits[0] : 8 );
2016 nbBits[2] = static_cast<unsigned char>( (length - nbBits[0] - nbBits[1]) < 8 ? (length - nbBits[0] - nbBits[1]) : 8 );
2017 nbBits[3] = static_cast<unsigned char>( (length - nbBits[0] - nbBits[1] - nbBits[2]) < 8 ? length - nbBits[0] - nbBits[1] - nbBits[2] : 8 );
2018 mask[0] = getMask8( nbBits[0] );
2019 mask[1] = getShiftMask8(8 - nbBits[1],nbBits[1]);
2020 mask[2] = getShiftMask8(8 - nbBits[2],nbBits[2]);
2021 mask[3] = getShiftMask8(8 - nbBits[3],nbBits[3]);
2022 c[0] = data[startBit>>3] & mask[0];
2023 c[1] = data[(startBit>>3) + 1] & mask[1];
2024 c[2] = data[(startBit>>3) + 2] & mask[2];
2025 c[3] = data[(startBit>>3) + 3] & mask[3];
2026
2027 unsigned long temp = ( (c[0]<<24) + (c[1]<<16) + (c[2]<<8) + c[3]) >> (24 + (startBit&0x07) + 1 - length);
2028 if (temp & getShiftMask32(length-1, 1))
2029 // do the two's complement to get the signed value if the msb=1
2030 *result = -1 * ( ( (~temp) + 1 ) & getMask32(length) );
2031 else
2032 *result = temp;
2033 return true;
2034 }
2035 }
2036}
2037
2038
2039
2040//////////////////////////////////////////////////////////////////////////
2041// Big Endian - Motorola coding
2042// ok
2043inline bool mDecodeToUI16(unsigned short * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2044{
2045 if (length > 16) {
2046 *result = 0;
2047 return false;
2048 }
2049
2050 // verify that the frame is not spread in 3 bytes
2051 if (mSpreadOnMoreThan2Bytes(startBit, length)) {
2052 // decode in a 32-bits integer
2053 unsigned long temp;
2054 if (mDecodeToUI32(&temp,data,startBit,length)) {
2055 *result = static_cast<unsigned short>(temp);
2056 return true;
2057 } else {
2058 *result = 0;
2059 return false;
2060 }
2061 } else {
2062 if (length == 16) {
2063 // only work if data are correctly byte-aligned
2064 unsigned char LSB, MSB;
2065 MSB = data[startBit>>3];
2066 LSB = data[(startBit>>3) + 1];
2067
2068 *result = ( (MSB << 8) + LSB );
2069 return true;
2070 } else {
2071 unsigned char LSB, MSB, maskMSB, maskLSB, nbBitsMSB, nbBitsLSB;
2072 nbBitsMSB = (startBit & 0x07) + 1; // the number of bits contained in the MSB
2073 nbBitsLSB = static_cast<unsigned char>(length - nbBitsMSB);
2074 maskMSB = getMask8( nbBitsMSB );
2075 maskLSB = getShiftMask8(8 - nbBitsLSB , nbBitsLSB);
2076 MSB = data[startBit>>3] & maskMSB;
2077 LSB = data[(startBit>>3) + 1] & maskLSB;
2078
2079 *result = ( (MSB << 8) + LSB ) >> (8 + (startBit&0x07) + 1 - length);
2080 return true;
2081 }
2082 }
2083}
2084
2085//////////////////////////////////////////////////////////////////////////
2086// Big Endian - Motorola coding
2087// ok
2088inline bool mDecodeToI16(short *result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2089{
2090 if (length > 16) {
2091 *result = 0;
2092 return false;
2093 }
2094
2095 // verify that the frame is not spread in 3 bytes
2096 if (mSpreadOnMoreThan2Bytes(startBit, length)) {
2097 // decode in a 32-bits integer
2098 long temp;
2099 if (mDecodeToI32(&temp, data, startBit, length)) {
2100 *result = (short)temp;
2101 return true;
2102 } else {
2103 *result = 0;
2104 return false;
2105 }
2106 } else {
2107 // ok data are stored at most in 2 bytes
2108 if (length == 16) {
2109 // only work if data are correctly byte-aligned
2110 unsigned char LSB, MSB;
2111 MSB = data[startBit>>3];
2112 LSB = data[(startBit>>3) + 1];
2113 short temp = (MSB << 8) + LSB;
2114
2115 // Attention : a-t on besoin d'appliquer le signe ?
2116 // n'est-il pas deja inclu dans la donn�e comme elle est correctement align�e sur l'octet ?
2117 if (temp & 0x8000) {
2118 // do the two's complement to get the signed value
2119 *result = - ( ( (~temp) + 1 ) & 0xFFFF );
2120 } else {
2121 *result = temp;
2122 }
2123 return true;
2124 } else {
2125 unsigned char LSB, MSB, maskMSB, maskLSB, nbBitsMSB, nbBitsLSB;
2126 nbBitsMSB = (startBit & 0x07) + 1; // the number of bits contained in the MSB
2127 nbBitsLSB = static_cast<unsigned char>(length - nbBitsMSB);
2128 maskMSB = getMask8( nbBitsMSB );
2129 maskLSB = getShiftMask8(8 - nbBitsLSB , nbBitsLSB);
2130 MSB = data[startBit>>3] & maskMSB;
2131 LSB = data[(startBit>>3) + 1] & maskLSB;
2132 // assign the MSB and LSB char in the short integer value and right-shift
2133 // to place the lsb to the bit 0
2134 unsigned short temp = ( (MSB << 8) + LSB ) >> (8 + (startBit&0x07) + 1 - length);
2135 if (temp & getShiftMask16(length-1,1))
2136 // do the two's complement to get the signed value if the msb=1
2137 *result = -1 * ( ( (~temp) + 1 ) & getMask16(length) );
2138 else
2139 *result = temp;
2140 return true;
2141 }
2142 }
2143}
2144
2145//////////////////////////////////////////////////////////////////////////
2146// Big Endian - Motorola coding
2147// OK
2148inline bool mDecodeToUI8(unsigned char * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2149{
2150 if (length > 8) {
2151 *result = 0;
2152 return false;
2153 }
2154
2155 // verify that the frame is not spread in 2 bytes
2156 if (mSpreadOnMoreThan1Byte(startBit, length)) {
2157 // decode in a 16-bit integer
2158 unsigned short temp;
2159 if ( mDecodeToUI16(&temp, data, startBit, length) ) {
2160 *result = static_cast<unsigned char>(temp);
2161 return true;
2162 } else {
2163 *result = 0;
2164 return false;
2165 }
2166 } else {
2167 // ok data is stored at most in 1 byte
2168 unsigned char c;
2169
2170 c = data[startBit>>3]; // >>3 <=> div 8
2171 //c >>= (8-((startBit & 0x07)+1)); // &0x07 <=> modulo 8
2172 c >>= startBit & 0x07 - (length - 1); // &0x07 <=> modulo 8
2173 *result = c & getMask8( length );
2174
2175 return true;
2176 }
2177}
2178
2179//////////////////////////////////////////////////////////////////////////
2180// Big Endian - Motorola coding
2181// OK
2182inline bool mDecodeToI8(char * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2183{
2184 if (length > 8) {
2185 *result = 0;
2186 return false;
2187 }
2188
2189 // verify that the frame is not spread in 2 bytes
2190 if (mSpreadOnMoreThan1Byte(startBit, length)) {
2191 // decode in a 16-bit integer
2192 short temp;
2193 if ( mDecodeToI16(&temp, data, startBit, length) ) {
2194 *result = static_cast<char>(temp);
2195 return true;
2196 } else {
2197 *result = 0;
2198 return false;
2199 }
2200 }
2201 else {
2202 // ok data is stored at most in 1 byte
2203 char c;
2204 c = data[startBit>>3];
2205 c >>= (8-((startBit & 0x07)+1));
2206 c = c & getMask8( length );
2207
2208 // a-t-on besoin d'appliquer le signe dans le cas ou length = 8
2209 // la donnee est correctement alignee sur l'octet donc le signe est peut-etre compris
2210 if (c & getShiftMask8(length-1,1))
2211 // do the two's complement to get the signed value if the msb=1
2212 *result = - ( ( (~c) + 1 ) & getMask8(length) );
2213 else
2214 *result = c;
2215 return true;
2216 }
2217}
2218
2219//////////////////////////////////////////////////////////////////////////
2220// Little Endian - Intel coding
2221// ok
2222inline bool iDecodeToBool(const unsigned char * data, const unsigned int startBit)
2223{
2224 mDecodeToBool(data, startBit);
2225}
2226
2227//////////////////////////////////////////////////////////////////////////
2228// Little Endian - Intel coding
2229// pas de depassement possible - la trame CAN fait 64 bits max
2230// gerer le signe
2231inline bool iDecodeToUI64(unsigned long long * /*result*/, const unsigned char * /*data*/, const unsigned int /*startBit*/, const unsigned int /*length*/)
2232{
2233 return true;
2234}
2235
2236//////////////////////////////////////////////////////////////////////////
2237// Little Endian - Intel coding
2238// pas de depassement possible - la trame CAN fait 64 bits max
2239// gerer le signe
2240inline bool iDecodeToI64(long long * /*result*/, const unsigned char * /*data*/, const unsigned int /*startBit*/, const unsigned int /*length*/)
2241{
2242 return true;
2243}
2244
2245//////////////////////////////////////////////////////////////////////////
2246// Little Endian - Intel coding
2247// gerer le depassement sur plus de 4 octets : necessite la fonction de decodage 64 bits
2248// verifier le depassement avant cette ligne : if (length == 32)
2249inline bool iDecodeToUI32(unsigned long * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2250{
2251 if (length>32) {
2252 *result = 0;
2253 return false;
2254 }
2255
2256 if (length == 32) {
2257 // only work if length == 32
2258 unsigned char c1, c2, c3, c4;
2259 c4 = data[startBit>>3]; // LSB
2260 c3 = data[(startBit>>3) + 1];
2261 c2 = data[(startBit>>3) + 2];
2262 c1 = data[(startBit>>3) + 3]; // MSB
2263
2264 *result = (c1 << 24) + (c2 << 16) + (c3 << 8) + c4;
2265 return true;
2266 } else {
2267 // todo
2268 *result = 0;
2269 return false;
2270 }
2271}
2272
2273//////////////////////////////////////////////////////////////////////////
2274// Little Endian - Intel coding
2275// gerer le signe
2276// gerer le depassement sur plus de 4 octets : necessite la fonction de decodage 64 bits
2277// verifier le depassement avant cette ligne : if (length == 32)
2278inline bool iDecodeToI32(long * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2279{
2280 if (length>32) {
2281 *result = 0;
2282 return false;
2283 }
2284
2285 if (length == 32) {
2286 // only work if length == 32
2287 unsigned char c1, c2, c3, c4;
2288 c4 = data[startBit>>3]; // LSB
2289 c3 = data[(startBit>>3) + 1];
2290 c2 = data[(startBit>>3) + 2];
2291 c1 = data[(startBit>>3) + 3]; // MSB
2292
2293 *result = (c1 << 24) + (c2 << 16) + (c3 << 8) + c4;
2294 return true;
2295 } else {
2296 // todo
2297 *result = 0;
2298 return false;
2299 }
2300}
2301
2302//////////////////////////////////////////////////////////////////////////
2303// Little Endian - Intel coding
2304// gerer le depassement sur plus de 2 octets
2305// verifier le depassement avant cette ligne : if (length == 16)
2306inline bool iDecodeToUI16(unsigned short * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2307{
2308 if (length > 16) {
2309 *result = 0;
2310 return false;
2311 }
2312
2313 if (length == 16) {
2314 // only work if length == 16
2315 unsigned char LSB, MSB;
2316 LSB = data[startBit>>3];
2317 MSB = data[(startBit>>3) + 1];
2318
2319 *result = (MSB << 8) + LSB;
2320 return true;
2321 } else {
2322 // TODO
2323 *result = 0;
2324 return false;
2325 }
2326}
2327
2328//////////////////////////////////////////////////////////////////////////
2329// Little Endian - Intel coding
2330// gerer le signe
2331// gerer le depassement sur plus de 2 octets
2332// verifier le depassement avant cette ligne : if (length == 16)
2333// manque le decalage
2334inline bool iDecodeToI16(short * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2335{
2336 if (length > 16) {
2337 *result = 0;
2338 return false;
2339 }
2340
2341 if (length == 16) {
2342 // only work if length == 16
2343 unsigned char LSB, MSB;
2344 LSB = data[startBit>>3];
2345 MSB = data[(startBit>>3) + 1];
2346
2347 *result = (MSB << 8) + LSB;
2348 return true;
2349 } else {
2350 // todo
2351 *result = 0;
2352 return false;
2353 }
2354}
2355
2356//////////////////////////////////////////////////////////////////////////
2357// Little Endian - Intel coding
2358// OK
2359inline bool iDecodeToUI8(unsigned char * result,const unsigned char * data, const unsigned int startBit, const unsigned int length)
2360{
2361 if (length > 8) {
2362 *result = 0;
2363 return false;
2364 }
2365
2366 // verify that the frame is not spread in 2 bytes
2367 if (iSpreadOnMoreThan1Byte(startBit, length)) {
2368 // decode in a 16-bit integer
2369 unsigned short temp;
2370 if ( iDecodeToUI16(&temp, data, startBit, length) ) {
2371 // and cast in an 8 bit integer
2372 *result = (unsigned char) temp;
2373 return true;
2374 } else {
2375 *result = 0;
2376 return true;
2377 }
2378 } else {
2379 // ok data is stored at most in 1 byte
2380 unsigned char c;
2381 c = data[startBit>>3];
2382 c >>= (startBit & 0x07);
2383 *result = c & getMask8( length );
2384
2385 return true;
2386 }
2387}
2388
2389//////////////////////////////////////////////////////////////////////////
2390// Little Endian - Intel coding
2391// OK
2392inline bool iDecodeToI8(char * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2393{
2394 if (length > 8) {
2395 *result = 0;
2396 return false;
2397 }
2398
2399 // verify that the frame is not spread in 2 bytes
2400 if (iSpreadOnMoreThan1Byte(startBit, length)) {
2401 // decode in a 16-bit integer
2402 short temp;
2403 if ( iDecodeToI16(&temp, data, startBit, length) ) {
2404 // and cast in an 8 bit integer
2405 *result = static_cast<char>(temp);
2406 return true;
2407 } else {
2408 *result = 0;
2409 return true;
2410 }
2411 } else {
2412 // ok data is stored at most in 1 byte
2413 char c;
2414 c = data[startBit>>3]; // >>3 <=> div 8
2415 c >>= (startBit & 0x07); // &0x07 <=> modulo 8
2416 c = c & getMask8( length );
2417 if (c & getShiftMask8(length-1,1))
2418 // do the two's complement to get the signed value if the msb=1
2419 *result = - ( ( (~c) + 1 ) & getMask8(length) );
2420 else
2421 *result = c;
2422
2423 return true;
2424 }
2425}
2426
2427//////////////////////////////////////////////////////////////////////////
2428// Big Endian - Motorola coding
2429// ok
2430inline bool mobileyemDecodeToUI16(unsigned short * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2431{
2432 if (length>16){ *result = 0; return false; }
2433
2434 // verify that the frame is not spread in 3 bytes
2435 /* if (mSpreadOnMoreThan2Bytes(startBit, length))
2436 {
2437 // decode in a 32-bits integer
2438 unsigned long temp;
2439 if (mDecodeToUI32(&temp,data,startBit,length))
2440 {
2441 *result = static_cast<unsigned short>(temp);
2442 return true;
2443 } else {
2444 *result = 0;
2445 return false;
2446 }
2447 }
2448 else
2449 {*/
2450 if (length == 16) {
2451 // only work if data are correctly byte-aligned
2452 unsigned char LSB, MSB;
2453 MSB = data[startBit>>3];
2454 LSB = data[(startBit>>3) + 1];
2455
2456 *result = ( (MSB << 8) + LSB );
2457 return true;
2458 } else {
2459 unsigned char LSB, MSB, maskMSB, maskLSB, nbBitsMSB, nbBitsLSB;
2460 nbBitsMSB = (startBit & 0x07) + 1; // the number of bits contained in the MSB
2461 nbBitsLSB = static_cast<unsigned char>(length - nbBitsMSB);
2462 maskMSB = getMask8( nbBitsMSB );
2463 maskLSB = getShiftMask8(8 - nbBitsLSB , nbBitsLSB);
2464 MSB = data[startBit>>3] & maskMSB;
2465 LSB = data[(startBit>>3) + 1] & maskLSB;
2466
2467 *result = ( (MSB << 8) + LSB ) >> (8 + (startBit&0x07) + 1 - length);
2468 return true;
2469 }
2470 //}
2471}
2472
2473//////////////////////////////////////////////////////////////////////////
2474// Big Endian - Motorola coding
2475// ok
2476inline bool mobileyemDecodeToI16(short *result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2477{
2478 if (length>16) { *result = 0; return false; }
2479
2480 // verify that the frame is not spread in 3 bytes
2481 /* if (mSpreadOnMoreThan2Bytes(startBit, length))
2482 {
2483 // decode in a 32-bits integer
2484 long temp;
2485 if (mDecodeToI32(&temp, data, startBit, length)) {
2486 *result = (short)temp;
2487 return true;
2488 } else {
2489 *result = 0;
2490 return false;
2491 }
2492 }
2493 else // ok data are stored at most in 2 bytes
2494 {*/
2495 if (length == 16)
2496 {
2497 // only work if data are correctly byte-aligned
2498 unsigned char LSB, MSB;
2499 MSB = data[startBit>>3];
2500 LSB = data[(startBit>>3) + 1];
2501 short temp = (MSB << 8) + LSB;
2502
2503 // Attention : a-t on besoin d'appliquer le signe ?
2504 // n'est-il pas deja inclu dans la donn�e comme elle est correctement align�e sur l'octet ?
2505 if (temp & 0x8000) {
2506 // do the two's complement to get the signed value
2507 *result = - ( ( (~temp) + 1 ) & 0xFFFF );
2508 } else {
2509 *result = temp;
2510 }
2511 return true;
2512 } else {
2513 unsigned char LSB, MSB, maskMSB, maskLSB, nbBitsMSB, nbBitsLSB;
2514 nbBitsMSB = (startBit & 0x07)+1; // the number of bits contained in the MSB
2515 nbBitsLSB = static_cast<unsigned char>(length - nbBitsMSB);
2516 maskMSB = getMask8( nbBitsMSB );
2517 maskLSB = getShiftMask8(8 - nbBitsLSB , nbBitsLSB);
2518 MSB = data[startBit>>3] & maskMSB;
2519 LSB = data[(startBit>>3) + 1] & maskLSB;
2520 // assign the MSB and LSB char in the short integer value and right-shift
2521 // to place the lsb to the bit 0
2522 unsigned short temp = ( (MSB << 8) + LSB ) >> (8 + (startBit&0x07) + 1 - length);
2523 if (temp & getShiftMask16(length-1,1))
2524 // do the two's complement to get the signed value if the msb=1
2525 *result = -1 * ( ( (~temp) + 1 ) & getMask16(length) );
2526 else
2527 *result = temp;
2528 return true;
2529 }
2530 //}
2531}
2532
2533//////////////////////////////////////////////////////////////////////////
2534// Big Endian - Motorola coding
2535// OK
2536inline bool mobileyemDecodeToUI8(unsigned char * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2537{
2538 if (length>8) { *result = 0; return false; }
2539
2540 // verify that the frame is not spread in 2 bytes
2541 if (mSpreadOnMoreThan1Byte(startBit, length))
2542 {
2543 // decode in a 16-bit integer
2544 unsigned short temp;
2545 if ( mDecodeToUI16(&temp, data, startBit, length) ) {
2546 *result = static_cast<unsigned char>(temp);
2547 return true;
2548 } else {
2549 *result = 0;
2550 return true;
2551 }
2552 }
2553 else // ok data is stored at most in 1 byte
2554 {
2555 unsigned char c;
2556 c = data[startBit>>3]; // >>3 <=> div 8
2557 c >>= (8-((startBit & 0x07)+1)); // &0x07 <=> modulo 8
2558 *result = c & getMask8( length );
2559
2560 return true;
2561 }
2562}
2563
2564//////////////////////////////////////////////////////////////////////////
2565// Big Endian - Motorola coding
2566// OK
2567inline bool mobileyemDecodeToI8(char * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2568{
2569 if (length>8) { *result = 0; return false; }
2570
2571 // verify that the frame is not spread in 2 bytes
2572 if (mSpreadOnMoreThan1Byte(startBit, length))
2573 {
2574 // decode in a 16-bit integer
2575 short temp;
2576 if ( mDecodeToI16(&temp, data, startBit, length) ) {
2577 *result = static_cast<char>(temp);
2578 return true;
2579 } else {
2580 *result = 0;
2581 return true;
2582 }
2583 }
2584 else // ok data is stored at most in 1 byte
2585 {
2586 char c;
2587 c = data[startBit>>3];
2588 c >>= (8-((startBit & 0x07)+1));
2589 c = c & getMask8( length );
2590
2591 // a-t-on besoin d'appliquer le signe dans le cas ou length = 8
2592 // la donnee est correctement alignee sur l'octet donc le signe est peut-etre compris
2593 if (c & getShiftMask8(length-1,1))
2594 // do the two's complement to get the signed value if the msb=1
2595 *result = - ( ( (~c) + 1 ) & getMask8(length) );
2596 else
2597 *result = c;
2598 return true;
2599 }
2600}
2601
2602//////////////////////////////////////////////////////////////////////////
2603// Little Endian - Intel coding
2604// OK, mais gerer le depassement sur plus de 2 octets
2605inline bool mobileyeiDecodeToUI16(unsigned short * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2606{
2607 if (length>16) {
2608 *result = 0;
2609 return false;
2610 }
2611
2612 if (length == 16) {
2613 // only work if length == 16
2614 unsigned char LSB, MSB;
2615 LSB = data[startBit>>3];
2616 MSB = data[(startBit>>3) + 1];
2617
2618 *result = (unsigned short)((MSB << 8) + LSB);
2619 return true;
2620 } else {
2621 unsigned char LSB, MSB, maskMSB, maskLSB, nbBitsMSB, nbBitsLSB;
2622
2623 nbBitsLSB = static_cast<unsigned char>(8*((startBit>>3) + 1) - startBit);
2624 nbBitsMSB = static_cast<unsigned char>(length - nbBitsLSB);
2625 maskLSB = getShiftMask8(8 - nbBitsLSB , nbBitsLSB);
2626 maskMSB = getMask8( nbBitsMSB );
2627 LSB = data[startBit>>3] & maskLSB;
2628 MSB = data[(startBit>>3) + 1] & maskMSB;
2629
2630 *result = (unsigned short)((MSB<<8) + (LSB>>(startBit-8*(startBit>>3))));
2631
2632 return true;
2633 }
2634}
2635
2636//////////////////////////////////////////////////////////////////////////
2637// Little Endian - Intel coding
2638// gerer le signe
2639// gerer le depassement sur plus de 2 octets
2640// verifier le depassement avant cette ligne : if (length == 16)
2641// manque le decalage
2642inline bool mobileyeiDecodeToI16(short * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2643{
2644 if (length > 16) {
2645 *result = 0;
2646 return false;
2647 }
2648
2649 if (length == 16) {
2650 // only work if length == 16
2651 unsigned char LSB, MSB;
2652 LSB = data[startBit>>3];
2653 MSB = data[(startBit>>3) + 1];
2654
2655 short temp = (MSB << 8) + LSB;
2656
2657 if (temp & 0x8000) {
2658 // do the two's complement to get the signed value
2659 *result = - ( ( (~temp) + 1 ) & 0xFFFF );
2660 } else {
2661 *result = temp;
2662 }
2663 return true;
2664 } else {
2665 unsigned char LSB, MSB, maskMSB, maskLSB, nbBitsMSB, nbBitsLSB;
2666
2667 nbBitsLSB = static_cast<unsigned char>(8*((startBit>>3) + 1) - startBit);
2668 nbBitsMSB = static_cast<unsigned char>(length - nbBitsLSB);
2669 maskLSB = getShiftMask8(8 - nbBitsLSB , nbBitsLSB);
2670 maskMSB = getMask8( nbBitsMSB );
2671 LSB = data[startBit>>3] & maskLSB;
2672 MSB = data[(startBit>>3) + 1] & maskMSB;
2673
2674 unsigned short temp = (unsigned short)((MSB<<8) + (LSB>>(startBit-8*(startBit>>3))));
2675
2676 if (temp & getShiftMask16(length-1,1))
2677 // do the two's complement to get the signed value if the msb=1
2678 *result = -1 * ( ( (~temp) + 1 ) & getMask16(length) );
2679 else
2680 *result = temp;
2681 return true;
2682 }
2683}
2684
2685//////////////////////////////////////////////////////////////////////////
2686// Little Endian - Intel coding
2687// OK
2688inline bool mobileyeiDecodeToUI8(unsigned char * result,const unsigned char * data, const unsigned int startBit, const unsigned int length)
2689{
2690 if (length>8) { *result = 0; return false; }
2691
2692 // verify that the frame is not spread in 2 bytes
2693 if (iSpreadOnMoreThan1Byte(startBit, length))
2694 {
2695 // decode in a 16-bit integer
2696 unsigned short temp;
2697 if ( iDecodeToUI16(&temp, data, startBit, length) )
2698 {
2699 // and cast in an 8 bit integer
2700 *result = (unsigned char) temp;
2701 return true;
2702 }
2703 else
2704 {
2705 *result = 0;
2706 return true;
2707 }
2708 }
2709 else // ok data is stored at most in 1 byte
2710 {
2711 unsigned char c;
2712 c = data[startBit>>3];
2713 c >>= (startBit & 0x07);
2714 *result = c & getMask8( length );
2715
2716 return true;
2717 }
2718
2719}
2720
2721//////////////////////////////////////////////////////////////////////////
2722// Little Endian - Intel coding
2723// OK
2724inline bool mobileyeiDecodeToI8(char * result, const unsigned char * data, const unsigned int startBit, const unsigned int length)
2725{
2726 if (length > 8) {
2727 *result = 0;
2728 return false;
2729 }
2730
2731 // verify that the frame is not spread in 2 bytes
2732 if (iSpreadOnMoreThan1Byte(startBit, length)) {
2733 // decode in a 16-bit integer
2734 short temp;
2735 if ( iDecodeToI16(&temp, data, startBit, length) ) {
2736 // and cast in an 8 bit integer
2737 *result = (char)(temp);
2738 return true;
2739 } else {
2740 *result = 0;
2741 return true;
2742 }
2743 } else {
2744 // ok data is stored at most in 1 byte
2745 char c;
2746 c = data[startBit>>3]; // >>3 <=> div 8
2747 c >>= (startBit & 0x07); // &0x07 <=> modulo 8
2748 c = c & getMask8( length );
2749 if (c & getShiftMask8(length-1,1))
2750 // do the two's complement to get the signed value if the msb=1
2751 *result = - ( ( (~c) + 1 ) & getMask8(length) );
2752 else
2753 *result = c;
2754
2755 return true;
2756 }
2757}
2758
2759inline int mobileyeDecodeCAN(unsigned char *data,unsigned char bigEndian,unsigned int startBit,unsigned int length,unsigned char Signed)
2760{
2761 int value;
2762 unsigned short ustemp16 = 0;
2763 unsigned char uctemp8 = 0;
2764 short stemp16 = 0;
2765 char ctemp8 = 0;
2766
2767 if(bigEndian) {
2768 if(length>8){
2769 if(Signed){mobileyemDecodeToI16(&stemp16,data, startBit, length);value = stemp16;}
2770 else {mobileyemDecodeToUI16(&ustemp16,data, startBit, length);value = ustemp16;}}
2771 else if(Signed) {mobileyemDecodeToI8(&ctemp8,data, startBit, length);value = ctemp8;}
2772 else {mobileyemDecodeToUI8(&uctemp8,data, startBit, length);value = uctemp8;}
2773 } else {
2774 if(length>8){
2775 if(Signed) {mobileyeiDecodeToI16(&stemp16,data, startBit, length);value = stemp16;}
2776 else {mobileyeiDecodeToUI16(&ustemp16,data, startBit, length);value = ustemp16;}}
2777 else if(Signed) {mobileyeiDecodeToI8(&ctemp8,data, startBit, length);value = ctemp8;}
2778 else {mobileyeiDecodeToUI8(&uctemp8,data, startBit, length);value = uctemp8;}
2779 }
2780
2781 return value;
2782}
2783
2784#ifdef __cplusplus
2785}
2786#endif // __cplusplus
2787
2788#endif // BINARYDECODER_H
[305]2789>>>>>>> .r303
Note: See TracBrowser for help on using the repository browser.