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 uint8_t reverse_mask[] =
{ 0x55, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe, 0xff };
static const uint8_t reverse_mask_xor[] =
{ 0xff, 0x7f, 0x3f, 0x1f, 0x0f, 0x07, 0x03, 0x01, 0x00 };
bool copy_bits(const uint8_t* source_origin, const uint16_t source_length,
const uint16_t source_offset, uint16_t bit_count,
uint8_t* destination_origin, const uint16_t destination_length,
const uint16_t 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;
}
uint16_t bits_to_bytes(uint32_t bits) {
uint8_t 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.
*/
uint8_t find_end_bit(const uint16_t numBits) {
int endBit = numBits % CHAR_BIT;
return endBit == 0 ? CHAR_BIT : endBit;
}
bool copy_bits_right_aligned(const uint8_t source[], const uint16_t source_length,
const uint16_t offset, const uint16_t bit_count,
uint8_t* destination, const uint16_t 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 uint16_t source_length,
const uint16_t offset, const uint16_t byte_count,
uint8_t* destination, const uint16_t destination_length) {
return copy_bits_right_aligned(source, source_length, offset * CHAR_BIT,
byte_count * CHAR_BIT, destination, destination_length);
}
|
