FreeCalypso > hg > fc-selenite

comparison src/gpf/ccd/bitfun.c @ 5:1ea54a97e831

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
src/gpf: import of Magnetite src/gpf3
author Mychaela Falconia <falcon@freecalypso.org>
date Sun, 15 Jul 2018 08:11:07 +0000
parents
children
comparison
equal deleted inserted replaced
4:6e457872f745 5:1ea54a97e831
1 /*
2 +-----------------------------------------------------------------------------
3 | Project :
4 | Modul : bitfun.c
5 +-----------------------------------------------------------------------------
6 | Copyright 2002 Texas Instruments Berlin, AG
7 | All rights reserved.
8 |
9 | This file is confidential and a trade secret of Texas
10 | Instruments Berlin, AG
11 | The receipt of or possession of this file does not convey
12 | any rights to reproduce or disclose its contents or to
13 | manufacture, use, or sell anything it may describe, in
14 | whole, or in part, without the specific written consent of
15 | Texas Instruments Berlin, AG.
16 +-----------------------------------------------------------------------------
17 | Purpose : Definition of constants, types and global variables and bit
18 | stream manipulation functions for CCD
19 +-----------------------------------------------------------------------------
20 */
21
22 #define __BITFUN_C__
23
24
25
26 #ifdef DEBUG_CCD
27 #include <stdio.h>
28 #endif
29
30 #include "typedefs.h"
31 #include "header.h"
32
33 #include "string.h"
34 #include "ccd_globs.h"
35 #include "bitfun.h"
36 #include "ccd.h"
37
38
39 EXTERN int abs(int);
40
41 #ifndef RUN_FLASH
42 /*
43 * For each bitlength between 0 and 32 this table contains the
44 * valid masks for right-justificated values
45 */
46
47 const ULONG ccd_bitfun_mask[] =
48 {
49 0x00000000,
50 0x00000001, 0x00000003, 0x00000007, 0x0000000f,
51 0x0000001f, 0x0000003f, 0x0000007f, 0x000000ff,
52 0x000001ff, 0x000003ff, 0x000007ff, 0x00000fff,
53 0x00001fff, 0x00003fff, 0x00007fff, 0x0000ffff,
54 0x0001ffff, 0x0003ffff, 0x0007ffff, 0x000fffff,
55 0x001fffff, 0x003fffff, 0x007fffff, 0x00ffffff,
56 0x01ffffff, 0x03ffffff, 0x07ffffff, 0x0fffffff,
57 0x1fffffff, 0x3fffffff, 0x7fffffff, 0xffffffff
58 };
59
60 /*
61 * Table of one-bit values (2^X)
62 */
63
64 const UBYTE ccd_bitfun_shift[] =
65 {
66 128, 64, 32, 16, 8, 4, 2, 1
67 };
68 #else
69 extern const ULONG ccd_bitfun_mask[];
70 extern const UBYTE ccd_bitfun_shift[];
71 #endif
72
73 /*
74 * maschine-dependent implementation
75 * define M_INTEL or M_MOTOROLA dependend on the target system
76 */
77
78 #ifdef M_INTEL
79 #define MSB_POS 1
80 #define LSB_POS 0
81 #define MSW_POS 2
82 #define LSW_POS 0
83 #else
84 #ifdef M_MOTOROLA
85 #define MSB_POS 0
86 #define LSB_POS 1
87 #define MSW_POS 0
88 #define LSW_POS 2
89 #endif
90 #endif
91
92
93 typedef union
94 { /* Conversion structure */
95 UBYTE c[2]; /* 2 bytes <-> USHORT */
96 USHORT s;
97 }
98 t_conv16;
99
100 typedef union
101 { /* Conversion structure */
102 UBYTE c[4]; /* 4 Byte <-> ULONG */
103 ULONG l;
104 }
105 t_conv32;
106
107 #ifndef RUN_FLASH
108 #ifdef DEBUG_CCD
109 /*
110 +--------------------------------------------------------------------+
111 | PROJECT : CCD (6144) MODULE : BITFUN |
112 | STATE : code ROUTINE : ccd_BITIMAGE |
113 +--------------------------------------------------------------------+
114
115 PURPOSE : convert a ULONG value to a ascii-bitstring with the
116 length len.
117 */
118
119
120 char *ccd_BITIMAGE (ULONG val, ULONG len, T_CCD_Globs *globs)
121 {
122
123 int i;
124 for (i=31; i >= 0; i--)
125 {
126 globs->buf [i] = (char)(val & 1)+'0';
127 val >>= 1;
128 }
129 globs->buf [32] = '\0';
130
131 return globs->buf+(32-len);
132 }
133
134 #endif
135 #endif /* !RUN_FLASH */
136
137 #ifndef RUN_FLASH
138 /*
139 +--------------------------------------------------------------------+
140 | PROJECT : CCD (6144) MODULE : BITFUN |
141 | STATE : code ROUTINE : fmemcpy |
142 +--------------------------------------------------------------------+
143
144 PURPOSE : copies len bytes from source to dest
145 for FAR addresses.
146 */
147
148 void fmemcpy (UBYTE * d, UBYTE *s, USHORT len)
149 {
150 while (len--)
151 *d++ = *s++;
152 }
153 #endif /* !RUN_FLASH */
154
155 #ifndef RUN_FLASH
156 /*
157 +--------------------------------------------------------------------+
158 | PROJECT : CCD (6144) MODULE : BITFUN |
159 | STATE : code ROUTINE : bf_writeBitChunk |
160 +--------------------------------------------------------------------+
161
162 PURPOSE : The global pointer globs->pstruct+globs->pstructOffs references
163 a buffer struct containing the len, offset and content
164 of a bitbuffer. This funtions copies the bits to the
165 bitstream buffer at the actual position.
166 */
167
168 void bf_writeBitChunk (ULONG len, T_CCD_Globs *globs)
169 {
170 ULONG offs;
171 ULONG bytesToCopy=0;
172 signed char theShift;
173 U8 *MsgStruct = (U8*)(globs->pstruct + globs->pstructOffs);
174 U8 ByteOffs = globs->byteoffs;
175 U8 *MsgBuf = (U8*)(globs->bitbuf + globs->bytepos);
176
177 /*
178 * Read the length of the buffer (the l_buf component) and
179 * compare it with the parameter given.
180 */
181 len = MINIMUM (len, *((USHORT *)MsgStruct));
182 if (!len)
183 return;
184 bytesToCopy = (ULONG) ((len + ByteOffs + 7)>> 3);
185 offs = (ULONG)(*(USHORT *)(MsgStruct + 2));
186 theShift = (signed char) (offs - ByteOffs);
187
188 #ifdef DEBUG_CCD
189 TRACE_CCD (globs, "reading 2+2+%ld bytes from struct %08X, writing %d bits at byte %d.%d",
190 bytesToCopy, MsgStruct + 4, len, globs->bytepos, globs->byteoffs);
191 #endif
192
193 if ((abs(theShift) & 7) EQ 0)
194 {
195 MsgStruct += (4+(offs>>3));
196 /*
197 * the bits in the Buffer are in the right position
198 * -> quick memcopy.
199 */
200 /*
201 * write the first byte, clean the leading bits
202 */
203 *MsgBuf |= (*MsgStruct & (UBYTE) ccd_bitfun_mask[8 - (offs&7)]);
204 /*
205 * copy the remaining bytes
206 */
207 if (bytesToCopy)
208 {
209 memcpy ((UBYTE *) &MsgBuf[1],
210 MsgStruct + 1,
211 (size_t)bytesToCopy-1);
212 }
213 if ((ByteOffs+len)&7)
214 {
215 MsgBuf[bytesToCopy-1] &= ~ccd_bitfun_mask[8-((ByteOffs+len)&7)];
216 }
217 }
218 else
219 {
220 t_conv16 conv;
221 /*
222 * shift every single byte
223 */
224 MsgStruct += (4+(offs>>3));
225 if (bytesToCopy > 1)
226 {
227 --bytesToCopy;
228 }
229 if (theShift > 0)
230 {
231 /*
232 * shift all bytes leftwise
233 */
234 /*
235 * the first byte must be masked
236 */
237 theShift &= 7;
238 conv.c[MSB_POS] = *MsgStruct++ & (UBYTE) ccd_bitfun_mask[8-(offs&7)];
239 conv.c[LSB_POS] = *MsgStruct;
240 conv.s <<= theShift;
241 *MsgBuf++ |= conv.c[MSB_POS];
242 *MsgBuf = conv.c[LSB_POS];
243 --bytesToCopy;
244
245 while (bytesToCopy)
246 {
247 conv.c[MSB_POS] = *MsgStruct++;
248 conv.c[LSB_POS] = *MsgStruct;
249 conv.s <<= theShift;
250 *MsgBuf++ |= conv.c[MSB_POS];
251 *MsgBuf = conv.c[LSB_POS];
252 --bytesToCopy;
253 }
254
255 conv.c[MSB_POS] = *MsgStruct++;
256 conv.c[LSB_POS] = *MsgStruct;
257 conv.s <<= theShift;
258 *MsgBuf |= conv.c[MSB_POS];
259
260 if ((ByteOffs+len)&7)
261 {
262 *MsgBuf &= ~ccd_bitfun_mask[8-((ByteOffs+len)&7)];
263 }
264 }
265 else
266 {
267 /*
268 * shift all bytes rightwise
269 */
270 /*
271 * the first byte must be masked, we dont want to store the
272 * leading garbage before the valid bits
273 */
274 theShift = (-theShift & 7);
275 conv.c[MSB_POS] = *MsgStruct++ & (UBYTE) ccd_bitfun_mask[8-(offs&7)];
276 conv.c[LSB_POS] = *MsgStruct;
277 conv.s >>= theShift;
278 *MsgBuf++ |= conv.c[MSB_POS];
279 *MsgBuf = conv.c[LSB_POS];
280 --bytesToCopy;
281
282 while (bytesToCopy)
283 {
284 conv.c[MSB_POS] = *MsgStruct++;
285 conv.c[LSB_POS] = *MsgStruct;
286 conv.s >>= theShift;
287 *MsgBuf++ |= conv.c[MSB_POS];
288 *MsgBuf = conv.c[LSB_POS];
289 --bytesToCopy;
290 }
291
292 conv.c[MSB_POS] = *MsgStruct++;
293 conv.c[LSB_POS] = *MsgStruct;
294 conv.s >>= theShift;
295 *MsgBuf |= conv.c[MSB_POS];
296
297 if ((ByteOffs+len)&7)
298 {
299 *MsgBuf &= ~ccd_bitfun_mask[8-((ByteOffs+len)&7)];
300 }
301 }
302 }
303 bf_incBitpos (len, globs);
304 globs->pstructOffs = MsgStruct - globs->pstruct - 1;
305 }
306 #endif /* !RUN_FLASH */
307
308 #ifndef RUN_FLASH
309 /*
310 +--------------------------------------------------------------------+
311 | PROJECT : CCD (6144) MODULE : BITFUN |
312 | STATE : code ROUTINE : bf_writeBits |
313 +--------------------------------------------------------------------+
314
315 PURPOSE : The pointer globs->pstruct + globs->pstructOffs refers to
316 a var in the message C-structure. This function writes the
317 numeric content of the var into the bit stream referenced
318 by globs->bitbuf. According to the required lenght
319 (len) the type of the var is interpreted as BYTE/SHORT
320 or LONG.
321 */
322
323 void bf_writeBits (ULONG len, T_CCD_Globs *globs)
324 {
325 /*
326 * Bit field is given by its lenght, offset bit and buffer.
327 */
328 if (len > 32)
329 {
330 bf_writeBitChunk (len, globs);
331 return;
332 }
333 else
334 {
335 UBYTE *MsgStruct = (UBYTE*)(globs->pstruct + globs->pstructOffs);
336 U8 ByteOffs = globs->byteoffs;
337 UBYTE *MsgBuf = (UBYTE*)(globs->bitbuf + globs->bytepos);
338 U32 wBits;
339
340 wBits = len + ByteOffs;
341
342 if (len > 24)
343 {
344 t_conv32 conv32;
345 UBYTE FirstByte;
346
347 conv32.l = *((ULONG *) MsgStruct);
348 conv32.l &= (ULONG) ccd_bitfun_mask[len];
349 conv32.l <<= (32-len);
350
351 FirstByte = conv32.c[MSW_POS + MSB_POS];
352 FirstByte >>= ByteOffs;
353 FirstByte &= (UBYTE) ccd_bitfun_mask[8-ByteOffs];
354 *MsgBuf++ |= FirstByte;
355
356 conv32.l <<= (8-ByteOffs);
357
358 *MsgBuf++ = conv32.c[MSW_POS + MSB_POS] ;
359 *MsgBuf++ = conv32.c[MSW_POS + LSB_POS] ;
360 *MsgBuf++ = conv32.c[LSW_POS + MSB_POS] ;
361
362 if (wBits > 32)
363 *MsgBuf = conv32.c[LSW_POS + LSB_POS] ;
364 }
365 /*
366 * Bit field given in a C variable is shorter than a USHORT.
367 */
368 #ifdef DEBUG_CCD
369 TRACE_CCD (globs, "write %d bits at byte %d.%d", len, globs->bytepos, ByteOffs);
370 #endif
371 if (len > 8)
372 {
373 t_conv32 conv;
374
375 conv.l = (len > 16) ? *(ULONG *) MsgStruct
376 : (ULONG) * (USHORT *) MsgStruct;
377 conv.l &= ccd_bitfun_mask[len];
378 #ifdef DEBUG_CCD
379 TRACE_CCD (globs, " (.%s)", ccd_BITIMAGE (conv.l, len, globs));
380 #endif
381 conv.l <<= (32 - wBits);
382 *MsgBuf |= conv.c[MSW_POS + MSB_POS];
383 MsgBuf[1] |= conv.c[MSW_POS + LSB_POS];
384 if (wBits > 16)
385 {
386 MsgBuf[2] |= conv.c[LSW_POS + MSB_POS];
387 if (wBits > 24)
388 {
389 MsgBuf[3] |= conv.c[LSW_POS + LSB_POS];
390 }
391 }
392 }
393 else
394 {
395 t_conv16 conv;
396
397 conv.s = (USHORT) (*MsgStruct);
398 conv.s &= (USHORT) ccd_bitfun_mask[len];
399 #ifdef DEBUG_CCD
400 TRACE_CCD (globs, " (.%s)", ccd_BITIMAGE ((ULONG) conv.s, len, globs));
401 #endif
402 conv.s <<= (16 - wBits);
403 *MsgBuf |= conv.c[MSB_POS];
404
405 if (wBits > 8)
406 {
407 MsgBuf[1] |= conv.c[LSB_POS];
408 }
409 }
410 bf_incBitpos (len, globs);
411 }
412 }
413 #endif /* !RUN_FLASH */
414
415 #ifndef RUN_INT_RAM
416 /*
417 +--------------------------------------------------------------------+
418 | PROJECT : CCD (6144) MODULE : BITFUN |
419 | STATE : code ROUTINE : bf_writeBitStr_PER |
420 +--------------------------------------------------------------------+
421
422 PURPOSE : The pointer globs->pstruct + globs->pstructOffs refers to
423 a var in the message C-structure. This function writes the
424 numeric content of the var into the bit stream referenced
425 by globs->bitbuf.
426 */
427
428 void bf_writeBitStr_PER (USHORT len, T_CCD_Globs *globs)
429 {
430 USHORT bytesToCopy;
431 t_conv16 Last2Bytes;
432 U8 *MsgStruct = (U8*)(globs->pstruct + globs->pstructOffs);
433 U8 ByteOffs = globs->byteoffs;
434 U8 *MsgBuf;
435 MsgBuf = (U8*)(globs->bitbuf + globs->bytepos + ((ByteOffs + len) >> 3));
436 /*
437 * Store the data for a later compensation of buffer overwritting.
438 * bytepos is always greater than 1, since the first byte with bytepos=0
439 * is dedicated to message identifier.
440 */
441 Last2Bytes.c[MSB_POS] = MsgBuf[0];
442 Last2Bytes.c[LSB_POS] = MsgBuf[1];
443
444 MsgBuf -= (ByteOffs + len) >> 3;
445 bytesToCopy = (len+7) >> 3;
446
447 #ifdef DEBUG_CCD
448 TRACE_CCD (globs, "copying %d byte(s) from struct 0x%08X, writing %d bits at byte %d.%d",
449 bytesToCopy, MsgStruct, len, globs->bytepos, globs->byteoffs);
450 {
451 int i=0;
452 int j=0;
453 char s[64], c[4];
454 int leftBytes = bytesToCopy;
455
456 TRACE_CCD (globs, " Bit string to be encoded:");
457
458 s[0] = '\0';
459 for (i = 0; leftBytes > 0 ; i++)
460 {
461 for (j = 0; (j < 16 && (leftBytes-- > 0)) ; j++)
462 {
463 sprintf(c, " %02x", *(MsgStruct+(16*i)+j));
464 strcat (s, c);
465 }
466 TRACE_CCD (globs, "%s", s);
467 s[0] = '\0';
468 }
469 }
470 #endif
471
472 /*
473 * The bits will be appended to the bit buffer at byte boundary.
474 */
475 if (ByteOffs EQ 0)
476 {
477 /*
478 * CCD assumes that the bits to be copied are left adjusted
479 * int their place in the C-structure.
480 */
481 memcpy ((UBYTE *) MsgBuf,
482 MsgStruct,
483 (size_t)bytesToCopy);
484 MsgBuf += bytesToCopy;
485 }
486 else
487 {
488 t_conv16 conv;
489
490 /*
491 * Write byte for byte while compensating non-zero value of globs->byteoffs.
492 */
493 while (bytesToCopy--)
494 {
495 conv.c[MSB_POS] = *MsgStruct++;
496 conv.c[LSB_POS] = *MsgStruct;
497 conv.s >>= ByteOffs;
498 conv.s &= (USHORT) ccd_bitfun_mask[16-ByteOffs];
499 *MsgBuf++ |= conv.c[MSB_POS];
500 *MsgBuf |= conv.c[LSB_POS];
501 }
502 }
503
504 bf_incBitpos (len, globs);
505
506 /*
507 * Undo overwriting of bits which do not belong to the bit string.
508 */
509 MsgBuf = (U8*)(globs->bitbuf + globs->bytepos);
510 MsgBuf[0] &= ~ccd_bitfun_mask[8-globs->byteoffs];
511 Last2Bytes.c[MSB_POS] &= (UBYTE) ccd_bitfun_mask[8-globs->byteoffs];
512 MsgBuf[0] |= Last2Bytes.c[MSB_POS];
513 MsgBuf[1] = Last2Bytes.c[LSB_POS];
514
515 }
516 #endif /* !RUN_INT_RAM */
517
518 #ifndef RUN_FLASH
519 /*
520 +--------------------------------------------------------------------+
521 | PROJECT : CCD (6144) MODULE : BITFUN |
522 | STATE : code ROUTINE : bf_writePadBits |
523 +--------------------------------------------------------------------+
524
525 PURPOSE : The global pointer globs->bitbuf points to the bit stream
526 buffer. This function adds 0-7 zero bits to the bit stream
527 from the point globs->byteoffs refer to.
528 */
529
530 void bf_writePadBits (T_CCD_Globs *globs)
531 {
532 if (globs->byteoffs NEQ 0)
533 {
534 globs->bitbuf[globs->bytepos] &=
535 (UBYTE) ~ccd_bitfun_mask[8-globs->byteoffs];
536 #ifdef DEBUG_CCD
537 TRACE_CCD (globs, "writing %d pad bit(s) at byte %d.%d",
538 8-globs->byteoffs, globs->bytepos, globs->byteoffs);
539 #endif
540
541 bf_incBitpos (8-globs->byteoffs, globs);
542 }
543 }
544 #endif /* !RUN_FLASH */
545
546 #ifndef RUN_FLASH
547 /*
548 +--------------------------------------------------------------------+
549 | PROJECT : CCD (6144) MODULE : BITFUN |
550 | STATE : code ROUTINE : bf_readBit |
551 +--------------------------------------------------------------------+
552
553 PURPOSE : The global pointer globs->bitbuf points to the bit stream
554 buffer. This function reads the bit to which the global
555 positioning pointers are pointing to.
556 */
557
558 BOOL bf_readBit (T_CCD_Globs *globs)
559 {
560 UBYTE ret;
561
562 ret = globs->bitbuf[globs->bytepos] & ccd_bitfun_shift[globs->byteoffs];
563 bf_incBitpos (1, globs);
564 #ifdef DEBUG_CCD
565 TRACE_CCD (globs, "reading 1 bit (%d) at byte %d.%d", (ret>0)?1:0, globs->bytepos, globs->byteoffs);
566 #endif
567
568 return (ret > 0);
569 }
570 #endif /* !RUN_FLASH */
571
572 #ifndef RUN_FLASH
573 /*
574 +--------------------------------------------------------------------+
575 | PROJECT : CCD (6144) MODULE : BITFUN |
576 | STATE : code ROUTINE : bf_writeBit |
577 +--------------------------------------------------------------------+
578
579 PURPOSE : The global pointer globs->bitbuf points to the bit stream
580 buffer. This function writes a given value into the bit
581 to which the global positioning pointers are pointing to.
582 */
583
584 void bf_writeBit (BOOL Bit, T_CCD_Globs *globs)
585 {
586 globs->bitbuf[globs->bytepos] = Bit ? (globs->bitbuf[globs->bytepos] |
587 ccd_bitfun_shift[globs->byteoffs])
588 : (globs->bitbuf[globs->bytepos] & ~ccd_bitfun_shift[globs->byteoffs]);
589
590 #ifdef DEBUG_CCD
591 TRACE_CCD (globs, "writing 1 bit (%d) at byte %d.%d", Bit, globs->bytepos, globs->byteoffs);
592 #endif
593
594 bf_incBitpos (1, globs);
595 }
596 #endif /* !RUN_FLASH */
597
598 #ifndef RUN_FLASH
599 /*
600 +--------------------------------------------------------------------+
601 | PROJECT : CCD (6144) MODULE : BITFUN |
602 | STATE : code ROUTINE : bf_rShift8Bit |
603 +--------------------------------------------------------------------+
604
605 PURPOSE : shifts a bitfield in the bitstream by 8 bits rightwise.
606 The function is used only for octet aligned types.
607 Hence offset is not involved in calculation.
608 */
609 void bf_rShift8Bit
610 (
611 USHORT srcBitPos,
612 USHORT bitLen,
613 T_CCD_Globs *globs
614 )
615 {
616 register UBYTE bytesToCopy;
617 USHORT bytepos;
618
619 /*
620 * destination > source -> start with last byte
621 */
622 bytepos = srcBitPos >> 3;
623 bytesToCopy = bitLen >> 3;
624
625 #ifdef DEBUG_CCD
626 TRACE_CCD (globs, "shifting %d bits rightwise for 8 bits", bitLen);
627 #endif
628
629 while (bytesToCopy)
630 {
631 globs->bitbuf[bytepos+bytesToCopy] = globs->bitbuf[bytepos+bytesToCopy-1];
632 bytesToCopy--;
633 }
634 globs->bitbuf[bytepos] = 0;
635 }
636 #endif /* !RUN_FLASH */
637
638 #ifndef RUN_FLASH
639 /*
640 +--------------------------------------------------------------------+
641 | PROJECT : CCD (6144) MODULE : BITFUN |
642 | STATE : code ROUTINE : bf_readBitChunk |
643 +--------------------------------------------------------------------+
644
645 PURPOSE : reads (len) Bits from the Bitsream (globs->bitbuf) and
646 stores them to the T_MSGBUF struct, containig the len
647 the offset and a bitbuffer.
648 */
649
650 void bf_readBitChunk (ULONG len, T_CCD_Globs *globs)
651 {
652 ULONG bytesToCopy;
653 U8 *MsgStruct = (U8*)(globs->pstruct + globs->pstructOffs);
654 U8 ByteOffs = globs->byteoffs;
655 U8 *MsgBuf = (U8*)(globs->bitbuf + globs->bytepos);
656
657 *(U16*) (MsgStruct) = (U16) len;
658 MsgStruct += 2;
659 *(U16*) (MsgStruct) = (U16)ByteOffs;
660 MsgStruct += 2;
661 bytesToCopy = (ULONG) ((len >> 3)+(len&7?1:0)+(ByteOffs?1:0));
662
663 #ifdef DEBUG_CCD
664 TRACE_CCD (globs, "reading %ld bits from byte %d.%d, writing 2+2+%ld bytes to struct %08X",
665 len, globs->bytepos, globs->byteoffs, bytesToCopy, MsgStruct);
666 #endif
667 memcpy ((UBYTE *) MsgStruct,
668 MsgBuf,
669 (size_t)bytesToCopy);
670
671 #if 0
672 #ifdef DEBUG_CCD
673 {
674 int i;
675 for (i=0; i<bytesToCopy; i++)
676 {
677 TRACE_CCD (globs, "buf[%d] = 0x%02x", i, MsgStruct[i]);
678 }
679 }
680 #endif
681 #endif
682 /*
683 * cutoff the leading and trailing bits wich are obsolete
684 */
685 *MsgStruct &= ccd_bitfun_mask [8-ByteOffs];
686 if ((len+ByteOffs)&7)
687 {
688 MsgStruct += (bytesToCopy-1);
689 *MsgStruct &= ~ccd_bitfun_mask [8-((len+ByteOffs)&7)];
690 }
691
692 #if 0
693 #ifdef DEBUG_CCD
694 {
695 int i;
696 for (i=0; i<bytesToCopy; i++)
697 {
698 TRACE_CCD (globs, "buf[%d] = 0x%02x", i, MsgStruct[i]);
699 }
700 }
701 #endif
702 #endif
703 bf_incBitpos (len, globs);
704 }
705 #endif /* !RUN_FLASH */
706
707 #ifndef RUN_INT_RAM
708 /*
709 +--------------------------------------------------------------------+
710 | PROJECT : CCD (6144) MODULE : BITFUN |
711 | STATE : code ROUTINE : bf_readBitStr_PER |
712 +--------------------------------------------------------------------+
713
714 PURPOSE : The pointer globs->pstruct + globs->pstructOffs refers to
715 a var in the message C-structure. This function reads len
716 bits from the bit stream (globs->bitbuf) and stores them
717 left adjusted to the bytes in the message C-structure.
718 */
719
720 void bf_readBitStr_PER (USHORT len, T_CCD_Globs *globs)
721 {
722 USHORT bytesToCopy;
723 t_conv16 Last2Bytes;
724 U8 *MsgStruct = (U8*)(globs->pstruct + globs->pstructOffs);
725 U8 *MsgStructEnd = (U8*)(globs->pstruct + globs->pstructOffs + (len >> 3));
726 U8 ByteOffs = globs->byteoffs;
727 U8 *MsgBuf = (U8*)(globs->bitbuf + globs->bytepos);
728
729 bytesToCopy = (len+7) >> 3;
730
731 /*
732 * Store the data for a later compensation of buffer overwritting.
733 */
734 Last2Bytes.c[MSB_POS] = MsgStructEnd[0];
735 Last2Bytes.c[LSB_POS] = MsgStructEnd[1];
736
737 /*
738 * The bits will be read from the bit buffer at byte boundary.
739 */
740 if (ByteOffs EQ 0)
741 {
742
743 #ifdef DEBUG_CCD
744 TRACE_CCD (globs, "reading %d bits from byte %d.%d, copying %d bytes to struct at 0x%08X",
745 len, globs->bytepos, globs->byteoffs, bytesToCopy, MsgStruct);
746 #endif
747
748 /*
749 * CCD assumes that the caller function needs the bit string to be
750 * left adjusted in the C-structure.
751 */
752 memcpy ((UBYTE *) MsgStruct,//(globs->pstruct + globs->pstructOffs),
753 MsgBuf,
754 (size_t)bytesToCopy);
755 }
756 else
757 {
758 t_conv16 conv;
759
760 /*
761 * Read byte for byte while compensating the non-zero globs->byteoffs.
762 */
763 while (bytesToCopy--)
764 {
765 conv.c[MSB_POS] = *MsgBuf++;
766 conv.c[LSB_POS] = *MsgBuf;
767 conv.s <<= ByteOffs;
768 conv.s &= (USHORT) ~ccd_bitfun_mask[8-(len&7)];
769 *MsgStruct++ = conv.c[MSB_POS];
770 *MsgStruct = conv.c[LSB_POS];
771 }
772 }
773
774 /*
775 * Undo overwriting in C-Structure. This is specially necessary
776 * for later reading of valid flags for optional elments.
777 */
778 MsgStructEnd[0] &= (UBYTE) ~ccd_bitfun_mask[8-(len&7)];
779 Last2Bytes.c[MSB_POS] &= (UBYTE) ccd_bitfun_mask[8-(len&7)];
780 MsgStructEnd[0] |= Last2Bytes.c[MSB_POS];
781 MsgStructEnd[1] = Last2Bytes.c[LSB_POS];
782
783 #ifdef DEBUG_CCD
784 {
785 int i=0;
786 int j=0;
787 char s[64], c[4];
788 int leftBytes = (len+7) >> 3;;
789
790 MsgStruct = (U8*)(globs->pstruct + globs->pstructOffs);
791 TRACE_CCD (globs, " Decoded bit string:");
792
793 s[0] = '\0';
794 for (i = 0; leftBytes > 0 ; i++)
795 {
796 for (j = 0; (j < 16 && (leftBytes-- > 0)) ; j++)
797 {
798 sprintf(c, " %02x", *(MsgStruct+(16*i)+j));
799 strcat (s, c);
800 }
801 TRACE_CCD (globs, "%s", s);
802 s[0] = '\0';
803 }
804 }
805 #endif
806
807 globs->pstructOffs += ((len+7) >> 3);
808 bf_incBitpos (len, globs);
809 }
810 #endif /* !RUN_INT_RAM */
811
812 #ifndef RUN_FLASH
813 /*
814 +--------------------------------------------------------------------+
815 | PROJECT : CCD (6144) MODULE : BITFUN |
816 | STATE : code ROUTINE : bf_readBits |
817 +--------------------------------------------------------------------+
818
819 PURPOSE : reads (len) Bits from the Bitsream (globs->bitbuf) and
820 stores them typed (dependent on the length) to vars in
821 the C-struct referenced by the global var
822 globs->pstruct+globs->pstructOff.
823 */
824
825 void bf_readBits (ULONG len, T_CCD_Globs *globs)
826 {
827 /*
828 * Bit field is given by its lenght, offset bit and buffer.
829 */
830 if (len > 32)
831 {
832 bf_readBitChunk (len, globs);
833 return;
834 }
835 else
836 {
837 t_conv16 conv16;
838 t_conv32 conv32;
839 U8 *MsgStruct = (U8*)(globs->pstruct + globs->pstructOffs);
840 U8 ByteOffs = globs->byteoffs;
841 U8 *MsgBuf = (U8*)(globs->bitbuf + globs->bytepos);
842
843 #ifdef DEBUG_CCD
844 TRACE_CCD (globs, "reading %d bits form byte %d.%d,", len, globs->bytepos, globs->byteoffs);
845 #endif
846
847 /*
848 * Bit field is given in a C variable shorter than a USHORT.
849 */
850 if (len <= 8)
851 {
852 if (globs->lastbytepos16 != globs->bytepos)
853 {
854 conv16.c[MSB_POS] = *MsgBuf++;
855 conv16.c[LSB_POS] = *MsgBuf;
856 globs->lastbytepos16 = globs->bytepos;
857 globs->last16Bit = conv16.s;
858 }
859 else
860 conv16.s = globs->last16Bit;
861 conv16.s >>= (16 - (ByteOffs + len));
862 conv16.s &= (USHORT) ccd_bitfun_mask[len];
863 *MsgStruct = (UBYTE) conv16.s;
864 #ifdef DEBUG_CCD
865 TRACE_CCD (globs, "writing 1 bytes (%0X) to struct %08X", *MsgStruct, MsgStruct);
866 #endif
867 }
868 else if (len + ByteOffs <= 32)
869 {
870 if (globs->lastbytepos32 != globs->bytepos)
871 {
872 conv32.c[MSW_POS + MSB_POS] = *MsgBuf++;
873 conv32.c[MSW_POS + LSB_POS] = *MsgBuf++;
874 conv32.c[LSW_POS + MSB_POS] = *MsgBuf++;
875 conv32.c[LSW_POS + LSB_POS] = *MsgBuf;
876 globs->lastbytepos32 = globs->bytepos;
877 globs->last32Bit = conv32.l;
878 }
879 else
880 conv32.l = globs->last32Bit;
881 conv32.l >>= (32 - (ByteOffs + len));
882 conv32.l &= ccd_bitfun_mask[len];
883 if (len > 16)
884 {
885 *((ULONG *) MsgStruct) = conv32.l;
886 #ifdef DEBUG_CCD
887 TRACE_CCD (globs, "writing 4 bytes (%08X) to struct %08X",
888 conv32.l,
889 MsgStruct);
890 #endif
891 }
892 else
893 {
894 *((USHORT *) MsgStruct) = (USHORT) conv32.l;
895 #ifdef DEBUG_CCD
896 TRACE_CCD (globs, "writing 2 bytes (%04X) to struct %08X",
897 (USHORT) conv32.l,
898 MsgStruct);
899 #endif
900 }
901 }
902 else
903 {
904 UBYTE FirstByte;
905
906 FirstByte = *MsgBuf++;
907 FirstByte <<= ByteOffs;
908 FirstByte &= (UBYTE) ~ccd_bitfun_mask[ByteOffs];
909
910 if (globs->lastbytepos32 != globs->bytepos)
911 {
912 conv32.c[MSW_POS + MSB_POS] = *MsgBuf++;
913 conv32.c[MSW_POS + LSB_POS] = *MsgBuf++;
914 conv32.c[LSW_POS + MSB_POS] = *MsgBuf++;
915 conv32.c[LSW_POS + LSB_POS] = *MsgBuf;
916 globs->lastbytepos32 = globs->bytepos;
917 globs->last32Bit = conv32.l;
918 }
919 else
920 {
921 conv32.l = globs->last32Bit;
922 }
923 if (!ByteOffs)
924 {
925 conv32.l >>= 8;
926 }
927 else
928 {
929 conv32.l >>= (8-ByteOffs);
930 }
931 conv32.c[MSW_POS + MSB_POS] &= (UBYTE) ccd_bitfun_mask[ByteOffs];
932 conv32.c[MSW_POS + MSB_POS] |= FirstByte;
933 conv32.l >>= (32-len);
934 conv32.l &= (ULONG) ccd_bitfun_mask[len];
935
936 *((ULONG *) MsgStruct) = conv32.l;
937 #ifdef DEBUG_CCD
938 TRACE_CCD ( globs, "writing 4 bytes (%08X) to struct %08X",
939 conv32.l,
940 MsgStruct);
941 #endif
942 }
943 bf_incBitpos (len, globs); return;
944 }
945 }
946 #endif /* !RUN_FLASH */
947
948 #ifndef RUN_INT_RAM
949 /*
950 +--------------------------------------------------------------------+
951 | PROJECT : CCD (6144) MODULE : BITFUN |
952 | STATE : code ROUTINE : bf_writeVal |
953 +--------------------------------------------------------------------+
954
955 PURPOSE : writes value into the next (bSize) bits of the air message
956 buffer(globs->bitbuf). This function does not use the data
957 in the C-structure. This is done by the caller function
958 while calculating value.
959 */
960 void bf_writeVal (ULONG value, ULONG bSize, T_CCD_Globs *globs)
961 {
962 ULONG BitSum;
963 U8 *MsgBuf = (U8*)(globs->bitbuf + globs->bytepos);
964 U8 ByteOffs = globs->byteoffs;
965
966 #ifdef DEBUG_CCD
967 TRACE_CCD (globs, "writeVal %d bits at byte %d.%d", bSize, globs->bytepos, ByteOffs);
968 #endif
969 /*
970 * value will be written into a temporary buffer. This buffer will
971 * then be prepared for an ORing with the bit buffer of air message.
972 * BitSum is helpful to find out the right size for temporary buffer.
973 */
974 BitSum = bSize + (ULONG)ByteOffs;
975
976 /*
977 * Write in 1 to 8 bits (bSize is 1-8).
978 */
979 if (BitSum <= 8)
980 {
981 UBYTE TmpBuf;
982
983 #ifdef DEBUG_CCD
984 TRACE_CCD (globs, " (.%s)", ccd_BITIMAGE ((ULONG)(value & ccd_bitfun_mask[bSize]), bSize, globs));
985 #endif
986 TmpBuf = (UBYTE) value;
987 TmpBuf &= (UBYTE) ccd_bitfun_mask[bSize];
988 TmpBuf <<= (8 - BitSum);
989 *MsgBuf |= TmpBuf;
990 }
991 else
992 {
993 /*
994 * Write in 9 to 16 bits (bSize is 9-16).
995 */
996 if (BitSum <= 16)
997 {
998 t_conv16 conv;
999
1000 conv.s = (USHORT) value;
1001 conv.s &= (USHORT) ccd_bitfun_mask[bSize];
1002 #ifdef DEBUG_CCD
1003 TRACE_CCD (globs, " (.%s)", ccd_BITIMAGE ((ULONG) conv.s, bSize, globs));
1004 #endif
1005 conv.s <<= (16 - BitSum);
1006 MsgBuf[0] |= conv.c[MSB_POS];
1007 MsgBuf[1] |= conv.c[LSB_POS];
1008 }
1009
1010 /*
1011 * Write in 17 to 25 bits (bSize is 17-25).
1012 */
1013 else if (BitSum <= 32)
1014 {
1015 t_conv32 conv;
1016
1017 conv.l = value;
1018 conv.l &= ccd_bitfun_mask[bSize];
1019 #ifdef DEBUG_CCD
1020 TRACE_CCD (globs, " (.%s)", ccd_BITIMAGE ((ULONG)conv.l, bSize, globs));
1021 #endif
1022 conv.l <<= (32 - BitSum);
1023 MsgBuf[0] |= conv.c[MSW_POS + MSB_POS];
1024 MsgBuf[1] |= conv.c[MSW_POS + LSB_POS];
1025 MsgBuf[2] |= conv.c[LSW_POS + MSB_POS];
1026 if (BitSum > 24)
1027 {
1028 MsgBuf[3] |= conv.c[LSW_POS + LSB_POS];
1029 }
1030 }
1031 /*
1032 * Write in 25 to 32 bits (bSize is 25-32).
1033 */
1034 else if ( BitSum < 40)
1035 {
1036 UBYTE FirstByte;
1037 t_conv32 conv;
1038
1039 conv.l = value;
1040 conv.l <<= (32 - bSize);
1041 FirstByte = conv.c[MSW_POS + MSB_POS];
1042 FirstByte >>= ByteOffs;
1043 FirstByte &= (UBYTE) ccd_bitfun_mask[8-ByteOffs];
1044 MsgBuf[0] |= FirstByte;
1045
1046 conv.l <<= (8 - ByteOffs);
1047 MsgBuf[1] |= conv.c[MSW_POS + MSB_POS];
1048 MsgBuf[2] |= conv.c[MSW_POS + LSB_POS];
1049 MsgBuf[3] |= conv.c[LSW_POS + MSB_POS];
1050 MsgBuf[4] |= conv.c[LSW_POS + LSB_POS];
1051 #ifdef DEBUG_CCD
1052 conv.l &= (ULONG) ~ccd_bitfun_mask[24-ByteOffs];
1053 TRACE_CCD (globs, " (.%s)", ccd_BITIMAGE ((ULONG) FirstByte, (ULONG)(8-ByteOffs), globs));
1054 TRACE_CCD (globs, " (.%s)", ccd_BITIMAGE (conv.l, (ULONG)(24-ByteOffs), globs));
1055 #endif
1056 }
1057 /*
1058 * This case is currently not supported.
1059 * Integer values are written to and read from up to 32 bits.
1060 */
1061 else
1062 {
1063 return;
1064 }
1065 }
1066
1067 bf_incBitpos (bSize, globs);
1068 }
1069 #endif /* !RUN_INT_RAM */
1070
1071 #ifndef RUN_INT_RAM
1072 /*
1073 +--------------------------------------------------------------------+
1074 | PROJECT : CCD (6144) MODULE : BITFUN |
1075 | STATE : code ROUTINE : bf_getBits |
1076 +--------------------------------------------------------------------+
1077
1078 PURPOSE : reads len Bits from the Bitstream (globs->bitbuf) and
1079 stores them in a variable. The caller function can now
1080 interpret or process the content of the returned variable.
1081 */
1082
1083 ULONG bf_getBits (ULONG len, T_CCD_Globs *globs)
1084 {
1085 U8 ByteOffs = globs->byteoffs;
1086 U8 *MsgBuf = (U8*)(globs->bitbuf + globs->bytepos);
1087
1088 #ifdef DEBUG_CCD
1089 TRACE_CCD (globs, "reading %ld bits form byte %d.%d,", len, globs->bytepos, globs->byteoffs);
1090 #endif
1091
1092 /*
1093 * Read up to 8 bits from the air message buffer.
1094 */
1095 if (len <= 8)
1096 {
1097 t_conv16 conv16;
1098
1099 if (globs->lastbytepos16 != globs->bytepos)
1100 {
1101 conv16.c[MSB_POS] = *MsgBuf++;
1102 conv16.c[LSB_POS] = *MsgBuf;
1103 globs->lastbytepos16 = globs->bytepos;
1104 globs->last16Bit = conv16.s;
1105 }
1106 else
1107 {
1108 conv16.s = globs->last16Bit;
1109 }
1110 conv16.s >>= (16 - (ByteOffs + len));
1111 conv16.s &= (USHORT) ccd_bitfun_mask[len];
1112 #ifdef DEBUG_CCD
1113 TRACE_CCD (globs, "read value: %d", conv16.s);
1114 #endif
1115 bf_incBitpos (len, globs);
1116 return (ULONG) conv16.s;
1117 }
1118
1119 /*
1120 * Read between 8 and 24 bits from the air message buffer.
1121 */
1122 else if (len <= 24)
1123 {
1124 t_conv32 conv32;
1125
1126 if (globs->lastbytepos32 != globs->bytepos)
1127 {
1128 conv32.c[MSW_POS + MSB_POS] = *MsgBuf++;
1129 conv32.c[MSW_POS + LSB_POS] = *MsgBuf++;
1130 conv32.c[LSW_POS + MSB_POS] = *MsgBuf++;
1131 conv32.c[LSW_POS + LSB_POS] = *MsgBuf;
1132 globs->lastbytepos32 = globs->bytepos;
1133 globs->last32Bit = conv32.l;
1134 }
1135 else
1136 {
1137 conv32.l = globs->last32Bit;
1138 }
1139 conv32.l >>= (32 - (ByteOffs + len));
1140 conv32.l &= ccd_bitfun_mask[len];
1141 #ifdef DEBUG_CCD
1142 TRACE_CCD (globs, "read value: %ld", conv32.l);
1143 #endif
1144 bf_incBitpos (len, globs);
1145 return conv32.l;
1146 }
1147
1148 /*
1149 * Read between 24 and 32 bits from the air message buffer.
1150 */
1151 else if ( len <= 32)
1152 {
1153 UBYTE FirstByte;
1154 t_conv32 conv;
1155
1156 FirstByte = *MsgBuf++;
1157 FirstByte <<= ByteOffs;
1158 FirstByte &= (UBYTE) ~ccd_bitfun_mask[ByteOffs];
1159
1160 if (globs->lastbytepos32 != globs->bytepos)
1161 {
1162 conv.c[MSW_POS + MSB_POS] = *MsgBuf++;
1163 conv.c[MSW_POS + LSB_POS] = *MsgBuf++;
1164 conv.c[LSW_POS + MSB_POS] = *MsgBuf++;
1165 conv.c[LSW_POS + LSB_POS] = *MsgBuf;
1166 globs->lastbytepos32 = globs->bytepos;
1167 globs->last32Bit = conv.l;
1168 }
1169 else
1170 {
1171 conv.l = globs->last32Bit;
1172 }
1173 if (!ByteOffs)
1174 {
1175 conv.l >>= 8;
1176 }
1177 else
1178 {
1179 conv.l >>= (8-ByteOffs);
1180 }
1181 conv.c[MSW_POS + MSB_POS] &= (UBYTE) ccd_bitfun_mask[ByteOffs];
1182 conv.c[MSW_POS + MSB_POS] |= FirstByte;
1183 conv.l >>= (32-len);
1184 conv.l &= (ULONG) ccd_bitfun_mask[len];
1185
1186 #ifdef DEBUG_CCD
1187 TRACE_CCD (globs, "read value: %ld", conv.l);
1188 #endif
1189 bf_incBitpos (len, globs);
1190 return conv.l;
1191 }
1192
1193 /*
1194 * This case is currently not supported.
1195 * Integer values are written to and read from up to 32 bits.
1196 */
1197 else
1198 {
1199 return 0;
1200 }
1201 }
1202 #endif /* !RUN_INT_RAM */
1203
1204 #ifndef RUN_INT_RAM
1205 /*
1206 +--------------------------------------------------------------------+
1207 | PROJECT : CCD (6144) MODULE : BITFUN |
1208 | STATE : code ROUTINE : bf_decodeLongNumber |
1209 +--------------------------------------------------------------------+
1210
1211 PURPOSE : reads (len) Bits from the Bitsream (globs->bitbuf) and
1212 return this number as a 32 Bit number. The Position
1213 of the readpointer of the bitstream is incremented by
1214 the len.
1215 */
1216
1217 ULONG bf_decodeLongNumber (UBYTE len, T_CCD_Globs *globs)
1218 {
1219 U32 number;
1220 t_conv16 conv16;
1221 t_conv32 conv32;
1222 U8 ByteOffs = globs->byteoffs;
1223 U8 *MsgBuf = (U8*)(globs->bitbuf + globs->bytepos);
1224
1225 #ifdef DEBUG_CCD
1226 TRACE_CCD (globs, "reading %d bits form byte %d.%d,", len, globs->bytepos, globs->byteoffs);
1227 #endif
1228
1229 if (len <= 8)
1230 {
1231 if (globs->lastbytepos16 != globs->bytepos)
1232 {
1233 conv16.c[MSB_POS] = *MsgBuf++;
1234 conv16.c[LSB_POS] = *MsgBuf;
1235 globs->lastbytepos16 = globs->bytepos;
1236 globs->last16Bit = conv16.s;
1237 }
1238 else
1239 conv16.s = globs->last16Bit;
1240 conv16.s >>= (16 - (ByteOffs + len));
1241 conv16.s &= (USHORT) ccd_bitfun_mask[len];
1242 number = (ULONG) conv16.s;
1243 }
1244 else
1245 {
1246 if (globs->lastbytepos32 != globs->bytepos)
1247 {
1248 conv32.c[MSW_POS + MSB_POS] = *MsgBuf++;
1249 conv32.c[MSW_POS + LSB_POS] = *MsgBuf++;
1250 conv32.c[LSW_POS + MSB_POS] = *MsgBuf++;
1251 conv32.c[LSW_POS + LSB_POS] = *MsgBuf;
1252 globs->lastbytepos32 = globs->bytepos;
1253 globs->last32Bit = conv32.l;
1254 }
1255 else
1256 conv32.l = globs->last32Bit;
1257 conv32.l >>= (32 - (ByteOffs + len));
1258 conv32.l &= ccd_bitfun_mask[len];
1259 number = conv32.l;
1260 }
1261
1262 bf_incBitpos (len, globs);
1263
1264 #ifdef DEBUG_CCD
1265 TRACE_CCD (globs, " (%08X)", number);
1266 #endif
1267
1268 return number;
1269 }
1270 #endif /* !RUN_INT_RAM */
1271
1272 #ifndef RUN_FLASH
1273 /*
1274 +--------------------------------------------------------------------+
1275 | PROJECT : CCD (6144) MODULE : BITFUN |
1276 | STATE : code ROUTINE : bf_decodeShortNumber|
1277 +--------------------------------------------------------------------+
1278
1279 PURPOSE : reads (len) Bits from the Bitsream (globs->bitbuf) and
1280 returns the resulting value as an ULONG.
1281 */
1282
1283 ULONG bf_decodeShortNumber (const ULONG len, T_CCD_Globs *globs)
1284 {
1285 UBYTE *p;
1286 t_conv32 conv32;
1287
1288 p = globs->bitbuf + globs->bytepos;
1289
1290 conv32.c[MSW_POS + MSB_POS] = *p++;
1291 conv32.c[MSW_POS + LSB_POS] = *p++;
1292 conv32.c[LSW_POS + MSB_POS] = *p++;
1293 conv32.c[LSW_POS + LSB_POS] = *p;
1294 conv32.l >>= (32 - (globs->byteoffs + len));
1295 conv32.l &= ccd_bitfun_mask[len];
1296 bf_incBitpos (len, globs);
1297 #ifdef DEBUG_CCD
1298 TRACE_CCD (globs, "reading %d bits as LONG (%d) at byte %d.%d", len, (ULONG) conv32.l, globs->bytepos, globs->byteoffs);
1299 #endif
1300 return (ULONG) conv32.l;
1301 }
1302 #endif /* !RUN_FLASH */
1303
1304 #ifndef RUN_FLASH
1305 /*
1306 +--------------------------------------------------------------------+
1307 | PROJECT : CCD (6144) MODULE : BITFUN |
1308 | STATE : code ROUTINE : bf_decodeByteNumber |
1309 +--------------------------------------------------------------------+
1310
1311 PURPOSE : reads (len) Bits from the Bitsream (globs->bitbuf) and
1312 returns the resulting value as an UBYTE.
1313 */
1314
1315 UBYTE bf_decodeByteNumber (const ULONG len, T_CCD_Globs *globs)
1316 {
1317 UBYTE *p;
1318 t_conv16 conv16;
1319
1320 p = globs->bitbuf + globs->bytepos;
1321
1322 conv16.c[MSB_POS] = *p++;
1323 conv16.c[LSB_POS] = *p;
1324 conv16.s >>= (16 - (globs->byteoffs + len));
1325 conv16.s &= (USHORT) ccd_bitfun_mask[len];
1326 bf_incBitpos (len, globs);
1327 #ifdef DEBUG_CCD
1328 TRACE_CCD (globs, "reading %d bits as BYTE (%d) at byte %d.%d", len, (UBYTE) conv16.s, globs->bytepos, globs->byteoffs);
1329 #endif
1330 return (UBYTE) conv16.s;
1331 }
1332 #endif /* !RUN_FLASH */
1333
1334 #ifndef RUN_INT_RAM
1335 /*
1336 +--------------------------------------------------------------------+
1337 | PROJECT : CCD (6144) MODULE : BITFUN |
1338 | STATE : code ROUTINE : bf_codeShortNumber |
1339 +--------------------------------------------------------------------+
1340
1341 PURPOSE : Converts the value (val) into a MSB/LSB-Bitstring and
1342 writes it to the aktual position into the bitstream
1343 globs->bitbuf. The maximum value of (len) is 16.
1344 If the value of (val) is greater then (2^len)-1 it
1345 will be truncated.
1346 */
1347
1348 void bf_codeShortNumber (UBYTE len, USHORT val, T_CCD_Globs *globs)
1349 {
1350 UBYTE *p;
1351 t_conv32 conv32;
1352
1353 #ifdef DEBUG_CCD
1354 TRACE_CCD (globs, "codeShortNumber: writing %d bits (.%s) at byte %d.%d",
1355 len, ccd_BITIMAGE (val, (ULONG) len, globs), globs->bytepos, globs->byteoffs);
1356 #endif
1357 p = globs->bitbuf + globs->bytepos;
1358 conv32.l = (ULONG) val;
1359 conv32.l <<= (32 - len - globs->byteoffs);
1360 *p++ |= conv32.c[MSW_POS + MSB_POS];
1361 *p = conv32.c[MSW_POS + LSB_POS];
1362 if ((globs->byteoffs + len) > 16)
1363 *++p = conv32.c[LSW_POS + MSB_POS];
1364 bf_incBitpos (len, globs);
1365 }
1366 #endif /* !RUN_INT_RAM */
1367
1368 #ifndef RUN_FLASH
1369 /*
1370 +--------------------------------------------------------------------+
1371 | PROJECT : CCD (6144) MODULE : BITFUN |
1372 | STATE : code ROUTINE : bf_recodeShortNumber|
1373 +--------------------------------------------------------------------+
1374
1375 PURPOSE: Converts the value (val) into a MSB/LSB-Bitstring and
1376 writes it at the position (pos) into the bitstream
1377 globs->bitbuf. The rest of the bitstream and the actual
1378 position will not changed. The maximum value of (len)
1379 is 16.
1380 If the value of (val) is greater then (2^len)-1
1381 it will be truncated.
1382 */
1383
1384 void bf_recodeShortNumber (USHORT pos, UBYTE len, USHORT val, T_CCD_Globs *globs)
1385 {
1386 UBYTE *p;
1387 USHORT oldbitpos;
1388 t_conv32 conv32;
1389 USHORT wBits;
1390
1391 oldbitpos = globs->bitpos;
1392 bf_setBitpos (pos, globs);
1393 #ifdef DEBUG_CCD
1394 TRACE_CCD (globs, "bf_recodeShortNumber:rewriting %d bits (.%s) at byte %d.%d",
1395 len, ccd_BITIMAGE ((ULONG) val, len, globs), globs->bytepos, globs->byteoffs);
1396 #endif
1397 wBits = len + globs->byteoffs;
1398 p = globs->bitbuf + globs->bytepos;
1399 conv32.l = (ULONG) val;
1400 conv32.l <<= (32 - wBits);
1401
1402 /*
1403 * Since the bits to write are cleared (memclr) in the bitstream,
1404 * it is ok to perform an OR operation on them.
1405 */
1406 *p++ |= conv32.c[MSW_POS + MSB_POS];
1407 if (wBits > 8)
1408 {
1409 *p++ |= conv32.c[MSW_POS + LSB_POS];
1410 if (wBits > 16)
1411 {
1412 *p++ |= conv32.c[LSW_POS + MSB_POS];
1413 if (wBits > 24)
1414 *p |= conv32.c[LSW_POS + LSB_POS];
1415 }
1416 }
1417
1418 bf_setBitpos (oldbitpos, globs);
1419 }
1420 #endif /* !RUN_FLASH */
1421
1422 #ifndef RUN_FLASH
1423 /*
1424 +--------------------------------------------------------------------+
1425 | PROJECT : CCD (6144) MODULE : BITFUN |
1426 | STATE : code ROUTINE : bf_recodeByteNumber |
1427 +--------------------------------------------------------------------+
1428
1429 PURPOSE: Converts the value (val) into a MSB/LSB-Bitstring and
1430 writes it at the position (pos) into the bitstream
1431 globs->bitbuf. The rest of the bitstream and the actual
1432 position will not changed. The maximum value of (len)
1433 is 8.
1434 If the value of (val) is greater then (2^len)-1
1435 it will be truncated.
1436 */
1437
1438 void bf_recodeByteNumber (USHORT pos, UBYTE len, UBYTE val, T_CCD_Globs *globs)
1439 {
1440 UBYTE *p;
1441 USHORT oldbitpos;
1442 t_conv16 conv16;
1443
1444 oldbitpos = globs->bitpos;
1445 bf_setBitpos (pos, globs);
1446 #ifdef DEBUG_CCD
1447 TRACE_CCD (globs, "bf_recodeByteNumber:rewriting %d bits (.%s) at byte %d.%d",
1448 len, ccd_BITIMAGE ((ULONG) val, len, globs), globs->bytepos, globs->byteoffs);
1449 #endif
1450 p = globs->bitbuf + globs->bytepos;
1451 conv16.s = (USHORT) val;
1452 conv16.s <<= (16 - len - globs->byteoffs);
1453 /*
1454 * if the bits to write are cleared (memclr) in the bitstream
1455 * we can perform an OR operation on it
1456 */
1457 *p++ |= conv16.c[MSB_POS];
1458 if ((len + globs->byteoffs) > 8)
1459 *p |= conv16.c[LSB_POS];
1460
1461 bf_setBitpos (oldbitpos, globs);
1462 }
1463 #endif /* !RUN_FLASH */
1464
1465 #ifndef RUN_FLASH
1466 /*
1467 +--------------------------------------------------------------------+
1468 | PROJECT : CCD (6144) MODULE : BITFUN |
1469 | STATE : code ROUTINE : bf_recodeBit |
1470 +--------------------------------------------------------------------+
1471
1472 PURPOSE: Writes the value of (Bit) at the position (pos)
1473 into the bitstream globs->bitbuf. The rest of the bitstream
1474 and the actual position will not changed.
1475 */
1476
1477 void bf_recodeBit (USHORT pos, UBYTE Bit, T_CCD_Globs *globs)
1478 {
1479 U16 oldbitpos = globs->bitpos;;
1480
1481 bf_setBitpos (pos, globs);
1482 globs->bitbuf[globs->bytepos] = Bit ? (globs->bitbuf[globs->bytepos] |
1483 ccd_bitfun_shift[globs->byteoffs])
1484 : (globs->bitbuf[globs->bytepos] &
1485 ~ccd_bitfun_shift[globs->byteoffs]);
1486 bf_setBitpos (oldbitpos, globs);
1487 #ifdef DEBUG_CCD
1488 TRACE_CCD (globs, "recode 1 bit (.%d) at bitpos %d", Bit, pos);
1489 #endif
1490 }
1491 #endif /* !RUN_FLASH */
1492
1493 #ifndef RUN_FLASH
1494 /*
1495 +--------------------------------------------------------------------+
1496 | PROJECT : CCD (6144) MODULE : BITFUN |
1497 | STATE : code ROUTINE : bf_codeByteNumber |
1498 +--------------------------------------------------------------------+
1499
1500 PURPOSE: Converts the value (val) into a Bitstring with the
1501 length (len) and writes it at the actual position
1502 into the bitstream globs->bitbuf. The maximum value of
1503 (len) is 8.
1504 If the value is greater then (2^len)-1 it will be
1505 truncated.
1506
1507 */
1508
1509 void bf_codeByteNumber (UBYTE len, UBYTE val, T_CCD_Globs *globs)
1510 {
1511 UBYTE *p;
1512 t_conv16 conv16;
1513
1514 #ifdef DEBUG_CCD
1515 TRACE_CCD (globs, "codeByteNumber:writing %d bits (.%s) at byte %d.%d",
1516 len, ccd_BITIMAGE (val, (ULONG) len, globs), globs->bytepos, globs->byteoffs);
1517 #endif
1518 p = globs->bitbuf + globs->bytepos;
1519
1520 conv16.s = (USHORT) val;
1521 conv16.s <<= (16 - len - globs->byteoffs);
1522 *p++ |= conv16.c[MSB_POS];
1523 if ((globs->byteoffs + len) > 8)
1524 *p |= conv16.c[LSB_POS];
1525 bf_incBitpos (len, globs);
1526 }
1527 #endif /* !RUN_FLASH */
1528
1529 #ifndef RUN_FLASH
1530 /*
1531 +--------------------------------------------------------------------+
1532 | PROJECT : CCD (6144) MODULE : BITFUN |
1533 | STATE : code ROUTINE : bf_codeLongNumber |
1534 +--------------------------------------------------------------------+
1535
1536 PURPOSE : This funtion writes the numeric content of
1537 the var to the aktual position into the bitstream
1538 referenced by globs->bitbuf.
1539 */
1540
1541 void bf_codeLongNumber (UBYTE len, ULONG val, T_CCD_Globs *globs)
1542 {
1543 UBYTE wBits = len + globs->byteoffs;
1544 U8 *MsgBuf = (U8*)(globs->bitbuf + globs->bytepos);
1545
1546 #ifdef DEBUG_CCD
1547 TRACE_CCD (globs, "codeLongNumber: writing %d bits (.%s) at byte %d.%d",
1548 len, ccd_BITIMAGE (val, (ULONG) len, globs), globs->bytepos, globs->byteoffs);
1549 #endif
1550
1551 if (len > 8)
1552 {
1553 t_conv32 conv;
1554
1555 conv.l = val;
1556 conv.l &= ccd_bitfun_mask[len];
1557 conv.l <<= (32 - wBits);
1558 MsgBuf[0] |= conv.c[MSW_POS + MSB_POS];
1559 MsgBuf[1] = conv.c[MSW_POS + LSB_POS];
1560 if (wBits > 16)
1561 {
1562 MsgBuf[2] = conv.c[LSW_POS + MSB_POS];
1563 if (wBits > 24)
1564 {
1565 MsgBuf[3] = conv.c[LSW_POS + LSB_POS];
1566 }
1567 }
1568 }
1569 else
1570 {
1571 t_conv16 conv;
1572
1573 conv.s = (USHORT) val;
1574 conv.s &= (USHORT) ccd_bitfun_mask[len];
1575 conv.s <<= (16 - wBits);
1576 MsgBuf[0] |= conv.c[MSB_POS];
1577 if (wBits > 8)
1578 {
1579 MsgBuf[1] = conv.c[LSB_POS];
1580 }
1581 }
1582
1583 bf_incBitpos (len, globs);
1584 }
1585 #endif /* !RUN_FLASH */
1586
1587 #if 0 /* not used - maybe for the future */
1588 #ifndef RUN_FLASH
1589 /*
1590 +--------------------------------------------------------------------+
1591 | PROJECT : CCD (6144) MODULE : BITFUN |
1592 | STATE : code ROUTINE : bf_swapBits |
1593 +--------------------------------------------------------------------+
1594
1595 PURPOSE : reads (len) Bits from the Bitsream (1-8) and swap
1596 this part with the next (len) bits in the bitstream.
1597 The read/write pointer of the bitstream left unchanged.
1598 This function is used for swapping the nibbles in some
1599 ugly coded GSM messages.
1600 */
1601
1602 void bf_swapBits (ULONG len, T_CCD_Globs *globs)
1603 {
1604 UBYTE s1, s2;
1605 USHORT s21;
1606 USHORT startpos = globs->bitpos;
1607 UBYTE *p;
1608 t_conv32 conv32;
1609
1610 if (len > 0 AND len <= 8)
1611 {
1612 #ifdef DEBUG_CCD
1613 TRACE_CCD (globs, "swapping %d bits", len);
1614 #endif
1615
1616 /*
1617 * read bitstring#1
1618 */
1619 s1 = bf_decodeByteNumber (len, globs);
1620 /*
1621 * read bitstring#2
1622 */
1623 s2 = bf_decodeByteNumber (len, globs);
1624 /*
1625 * concat bitstring#2 and bitstring#1
1626 */
1627 s21 = (USHORT) s2 <<len;
1628 s21 |= s1;
1629
1630 /*
1631 * recode it into the bitstream
1632 */
1633 bf_setBitpos (startpos, globs);
1634 p = globs->bitbuf + globs->bytepos;
1635 conv32.l = (ULONG) s21;
1636 conv32.l <<= (32 - len) - globs->byteoffs;
1637 *p++ |= conv32.c[MSW_POS + MSB_POS];
1638 *p++ |= conv32.c[MSW_POS + LSB_POS];
1639 *p++ |= conv32.c[LSW_POS + MSB_POS];
1640 *p |= conv32.c[LSW_POS + LSB_POS];
1641
1642 bf_setBitpos (startpos, globs);
1643 bf_recodeShortNumber (startpos, (UBYTE) (len<<1), s21, globs);
1644 bf_setBitpos (startpos, globs);
1645 }
1646 }
1647
1648 #endif /* !RUN_FLASH */
1649 #endif