blob: 09c7bedd7ff55c427db1b537864a631aa55134ab (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
|
#include "interface.h"
#include "internals.h"
#include "id-errno.h"
#include "debug.h"
unsigned int extract_unsigned_pnum(struct id_cookie *cookie, int start_bit, int *err)
{
unsigned int value=0;
unsigned int bit_offset=0;
unsigned char bits;
unsigned char ch;
int oor;
*err = ID_EOK;
for (;;) {
ch = id_fetch_byte(cookie->mem_ptr, cookie->offset++, &oor);
if (oor != ID_EOK) {
*err = oor;
id_error("extract runs oor");
return 0;
}
if (ch & (1<<(start_bit-1))) {
/* more to go, accumulate bits */
bits = ch & ((1<<(start_bit - 1)) - 1);
value |= (bits << bit_offset);
bit_offset += start_bit-1;
start_bit = 8;
} else {
/* last byte of number */
bits = ch & ((1<<(start_bit - 1)) - 1);
value |= (bits << bit_offset);
break;
}
}
return value;
}
int extract_signed_pnum(struct id_cookie *cookie, int start_bit, int *err)
{
int value=0;
unsigned int bit_offset=0;
unsigned char bits;
unsigned char ch;
int oor;
*err = ID_EOK;
for (;;) {
ch = id_fetch_byte(cookie->mem_ptr, cookie->offset++, &oor);
if (oor != ID_EOK) {
*err = oor;
id_error("extract runs oor");
return 0;
}
if (ch & (1<<(start_bit-1))) {
/* more to go, accumulate bits */
bits = ch & ((1<<(start_bit - 1)) - 1);
value |= (bits << bit_offset);
bit_offset += start_bit-1;
start_bit = 8;
} else {
/* last byte of number */
bits = ch & ((1<<(start_bit - 2)) - 1);
value |= (bits << bit_offset);
if (ch & (1<<(start_bit - 2)))
value = -value;
break;
}
}
return value;
}
|