1use crate::model::ByteOrder;
28
29#[must_use]
36pub fn extract_raw(start_bit: u16, bit_length: u8, byte_order: ByteOrder, data: &[u8]) -> u64 {
37 match byte_order {
38 ByteOrder::Intel => {
39 let start = usize::from(start_bit);
40 let mut value = 0_u64;
41 for i in 0..usize::from(bit_length) {
42 let pos = start + i;
43 let bit = u64::from((data.get(pos / 8).copied().unwrap_or(0) >> (pos % 8)) & 1);
44 value |= bit << i;
45 }
46 value
47 }
48 ByteOrder::Motorola => {
49 let mut pos = usize::from(start_bit);
50 let mut value = 0_u64;
51 for _ in 0..usize::from(bit_length) {
52 let bit = u64::from((data.get(pos / 8).copied().unwrap_or(0) >> (pos % 8)) & 1);
53 value = (value << 1) | bit;
54 pos = if pos % 8 == 0 { pos + 15 } else { pos - 1 };
57 }
58 value
59 }
60 }
61}
62
63pub fn insert_raw(
71 start_bit: u16,
72 bit_length: u8,
73 byte_order: ByteOrder,
74 data: &mut [u8],
75 value: u64,
76) -> Result<(), crate::DbcError> {
77 let need = max_bit_position(start_bit, bit_length, byte_order) / 8 + 1;
78 if data.len() < need {
79 return Err(crate::DbcError::BufferTooShort {
80 need,
81 have: data.len(),
82 });
83 }
84 match byte_order {
85 ByteOrder::Intel => {
86 let start = usize::from(start_bit);
87 for i in 0..usize::from(bit_length) {
88 let pos = start + i;
89 write_bit(data, pos, (value >> i) & 1 == 1);
90 }
91 }
92 ByteOrder::Motorola => {
93 let mut pos = usize::from(start_bit);
94 for i in (0..usize::from(bit_length)).rev() {
95 write_bit(data, pos, (value >> i) & 1 == 1);
96 pos = if pos % 8 == 0 { pos + 15 } else { pos - 1 };
97 }
98 }
99 }
100 Ok(())
101}
102
103#[must_use]
105pub fn max_bit_position(start_bit: u16, bit_length: u8, byte_order: ByteOrder) -> usize {
106 if bit_length == 0 {
107 return usize::from(start_bit);
108 }
109 match byte_order {
110 ByteOrder::Intel => usize::from(start_bit) + usize::from(bit_length) - 1,
111 ByteOrder::Motorola => {
112 let mut pos = usize::from(start_bit);
113 let mut max = pos;
114 for _ in 1..usize::from(bit_length) {
115 pos = if pos % 8 == 0 { pos + 15 } else { pos - 1 };
116 max = max.max(pos);
117 }
118 max
119 }
120 }
121}
122
123fn write_bit(data: &mut [u8], pos: usize, set: bool) {
125 if let Some(byte) = data.get_mut(pos / 8) {
126 let mask = 1_u8 << (pos % 8);
127 if set {
128 *byte |= mask;
129 } else {
130 *byte &= !mask;
131 }
132 }
133}
134
135#[must_use]
138pub fn sign_extend(raw: u64, bit_length: u8) -> i64 {
139 #[allow(clippy::cast_possible_wrap)]
141 if bit_length == 0 || bit_length >= 64 {
142 return raw as i64;
143 }
144 let bits = u32::from(bit_length);
145 let sign_bit = 1_u64 << (bits - 1);
146 #[allow(clippy::cast_possible_wrap)]
147 if raw & sign_bit != 0 {
148 (raw | (!0_u64 << bits)) as i64
150 } else {
151 raw as i64
152 }
153}
154
155#[cfg(test)]
156mod tests {
157 #![allow(clippy::unwrap_used, clippy::expect_used, clippy::indexing_slicing)]
160 use super::*;
161
162 #[test]
164 fn motorola_big_endian_across_bytes() {
165 let data = [0x00, 0x00, 0x00, 0x00, 0xAB, 0xCD, 0x00, 0x00];
166 assert_eq!(extract_raw(39, 16, ByteOrder::Motorola, &data), 0xABCD);
167 }
168
169 #[test]
171 fn intel_little_endian_across_bytes() {
172 let data = [0x00, 0x00, 0xAB, 0xCD, 0x00, 0x00, 0x00, 0x00];
173 assert_eq!(extract_raw(16, 16, ByteOrder::Intel, &data), 0xCDAB);
174 }
175
176 #[test]
179 fn motorola_sawtooth_jump() {
180 let mut data = [0u8; 8];
182 insert_raw(0, 12, ByteOrder::Motorola, &mut data, 0b1101_0100_1001).unwrap();
183 assert_eq!(data[0], 0b0000_0001);
187 assert_eq!(data[1], 0b1010_1001, "byte 1 holds the 8 mid bits");
188 assert_eq!(data[2] >> 5, 0b001, "top 3 bits of byte 2 carry the tail");
189 assert_eq!(
191 extract_raw(0, 12, ByteOrder::Motorola, &data),
192 0b1101_0100_1001
193 );
194 }
195
196 #[test]
198 fn extract_is_total() {
199 let data = [0xFF_u8];
200 assert_eq!(extract_raw(0, 16, ByteOrder::Intel, &data), 0x00FF);
202 assert_eq!(extract_raw(7, 16, ByteOrder::Motorola, &data), 0xFF00);
204 assert_eq!(extract_raw(8, 8, ByteOrder::Intel, &data), 0);
206 }
207
208 #[test]
210 fn insert_bounds_checked() {
211 let mut data = [0u8; 2];
212 assert_eq!(
213 insert_raw(16, 1, ByteOrder::Intel, &mut data, 1),
214 Err(crate::DbcError::BufferTooShort { need: 3, have: 2 })
215 );
216 let mut four = [0u8; 3];
218 assert!(insert_raw(0, 12, ByteOrder::Motorola, &mut four, 0xFFF).is_ok());
219 let mut two = [0u8; 2];
220 assert_eq!(
221 insert_raw(0, 12, ByteOrder::Motorola, &mut two, 0xFFF),
222 Err(crate::DbcError::BufferTooShort { need: 3, have: 2 })
223 );
224 }
225
226 #[test]
228 fn insert_extract_inverse() {
229 for &(start, len) in &[
230 (0_u16, 1_u8),
231 (7, 8),
232 (2, 6),
233 (39, 16),
234 (0, 12),
235 (15, 20),
236 (7, 64),
237 ] {
238 for order in [ByteOrder::Motorola, ByteOrder::Intel] {
239 let mut data = [0_u8; 9];
240 let value: u64 = 0x0123_4567_89AB_CDEF & (u64::MAX >> (64 - len).min(64));
241 insert_raw(start, len, order, &mut data, value).unwrap();
242 assert_eq!(
243 extract_raw(start, len, order, &data),
244 value,
245 "round-trip failed at start={start} len={len} order={order:?}"
246 );
247 }
248 }
249 }
250
251 #[test]
253 fn sign_extension() {
254 assert_eq!(sign_extend(0x7F, 8), 127);
255 assert_eq!(sign_extend(0xFF, 8), -1);
256 assert_eq!(sign_extend(0x800, 12), -2048);
257 assert_eq!(sign_extend(0x7FF, 12), 2047);
258 assert_eq!(sign_extend(1, 1), -1);
259 assert_eq!(sign_extend(0, 1), 0);
260 }
261
262 #[test]
264 fn max_position() {
265 assert_eq!(max_bit_position(16, 16, ByteOrder::Intel), 31);
266 assert_eq!(max_bit_position(39, 16, ByteOrder::Motorola), 47);
267 assert_eq!(max_bit_position(0, 12, ByteOrder::Motorola), 23);
268 assert_eq!(max_bit_position(5, 0, ByteOrder::Intel), 5);
269 }
270}