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
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
|
#include <bitfield/bitfield.h>
#include <stddef.h>
#include <limits.h>
#include <string.h>
#define PREPARE_FIRST_COPY() \
do { \
if (bit_count >= (CHAR_BIT - destination_offset_modulo)) { \
*destination &= reverse_mask[destination_offset_modulo]; \
bit_count -= CHAR_BIT - destination_offset_modulo; \
} else { \
*destination &= reverse_mask[destination_offset_modulo] \
| reverse_mask_xor[destination_offset_modulo + bit_count + 1];\
c &= reverse_mask[destination_offset_modulo + bit_count ];\
bit_count = 0; \
} } while (0)
static const unsigned int reverse_mask[] =
{ 0x55, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe, 0xff };
static const unsigned int reverse_mask_xor[] =
{ 0xff, 0x7f, 0x3f, 0x1f, 0x0f, 0x07, 0x03, 0x01, 0x00 };
bool copy_bits(const uint8_t* source_origin, const unsigned int source_length,
const unsigned int source_offset, unsigned int bit_count,
uint8_t* destination_origin, const unsigned int destination_length,
const unsigned int destination_offset) {
if(bit_count < 1) {
return false;
}
if(source_offset + bit_count > source_length * CHAR_BIT ||
destination_offset + bit_count > destination_length * CHAR_BIT ) {
return false;
}
const uint8_t* source = source_origin + (source_offset / CHAR_BIT);
uint8_t* destination = destination_origin + (destination_offset / CHAR_BIT);
int source_offset_modulo = source_offset % CHAR_BIT;
int destination_offset_modulo = destination_offset % CHAR_BIT;
if(source_offset_modulo == destination_offset_modulo) {
if(source_offset_modulo > 0) {
uint8_t c = reverse_mask_xor[destination_offset_modulo] & *source++;
PREPARE_FIRST_COPY();
*destination++ |= c;
}
int byte_len = bit_count / CHAR_BIT;
int bit_count_modulo = bit_count % CHAR_BIT;
if(byte_len > 0) {
memcpy(destination, source, byte_len);
source += byte_len;
destination += byte_len;
}
if(bit_count_modulo > 0) {
*destination &= reverse_mask_xor[bit_count_modulo];
*destination |= reverse_mask[bit_count_modulo] & *source;
}
} else {
int bit_diff_left_shift;
int bit_diff_right_shift;
uint8_t c;
/*
* Begin: Line things up on destination.
*/
if(source_offset_modulo > destination_offset_modulo) {
bit_diff_left_shift = source_offset_modulo - destination_offset_modulo;
bit_diff_right_shift = CHAR_BIT - bit_diff_left_shift;
c = *source++ << bit_diff_left_shift;
c |= *source >> bit_diff_right_shift;
c &= reverse_mask_xor[destination_offset_modulo];
} else {
bit_diff_right_shift = destination_offset_modulo - source_offset_modulo;
bit_diff_left_shift = CHAR_BIT - bit_diff_right_shift;
c = *source >> bit_diff_right_shift &
reverse_mask_xor[destination_offset_modulo];
}
PREPARE_FIRST_COPY();
*destination++ |= c;
/*
* Middle: copy with only shifting the source.
*/
int byte_len = bit_count / CHAR_BIT;
while(--byte_len >= 0) {
c = *source++ << bit_diff_left_shift;
c |= *source >> bit_diff_right_shift;
*destination++ = c;
}
/*
* End: copy the remaing bits;
*/
int bit_count_modulo = bit_count % CHAR_BIT;
if(bit_count_modulo > 0) {
c = *source++ << bit_diff_left_shift;
c |= *source >> bit_diff_right_shift;
c &= reverse_mask[bit_count_modulo];
*destination &= reverse_mask_xor[bit_count_modulo];
*destination |= c;
}
}
return true;
}
unsigned int bits_to_bytes(uint32_t bits) {
unsigned int byte_count = bits / CHAR_BIT;
if(bits % CHAR_BIT != 0) {
++byte_count;
}
return byte_count;
}
/**
* Find the ending bit of a bitfield within the final byte.
*
* Returns: a bit position from 0 to 7.
*/
unsigned int find_end_bit(const unsigned int numBits) {
int endBit = numBits % CHAR_BIT;
return endBit == 0 ? CHAR_BIT : endBit;
}
bool copy_bits_right_aligned(const uint8_t source[], const unsigned int source_length,
const unsigned int offset, const unsigned int bit_count,
uint8_t* destination, const unsigned int destination_length) {
return copy_bits(source, source_length, offset, bit_count, destination,
destination_length,
// provide a proper destination offset so the result is right
// aligned
(destination_length - bits_to_bytes(bit_count)) * CHAR_BIT +
CHAR_BIT - find_end_bit(bit_count));
}
bool copy_bytes_right_aligned(const uint8_t source[], const unsigned int source_length,
const unsigned int offset, const unsigned int byte_count,
uint8_t* destination, const unsigned int destination_length) {
return copy_bits_right_aligned(source, source_length, offset * CHAR_BIT,
byte_count * CHAR_BIT, destination, destination_length);
}
|
