1use num::{bigint, ToPrimitive};
2
3use can_dbc::Signal;
4
5pub struct Payload {
6 data: Vec<u8>,
7}
8
9pub struct CanData {
10 data: Vec<u8>
11}
12
13impl Payload {
14 pub fn new(data: Vec<u8>) -> Self {
15 return Payload { data: data }
16 }
17 pub fn decode_payload(&self, s: &Signal) -> f64 {
18 if s.signal_size == 1 {
19 return 1.0;
20 }
21 match s.value_type() {
22 can_dbc::ValueType::Signed => match s.byte_order() {
23 can_dbc::ByteOrder::BigEndian => {
24 let value =
25 self.signed_bits_big_endian(s.start_bit as u32, s.signal_size as u32);
26 return s.offset + value as f64 * s.factor;
27 }
28 can_dbc::ByteOrder::LittleEndian => {
29 let value =
30 self.signed_bits_little_endian(s.start_bit as u32, s.signal_size as u32);
31 return s.offset + value as f64 * s.factor;
32 }
33 },
34 can_dbc::ValueType::Unsigned => match s.byte_order() {
35 can_dbc::ByteOrder::BigEndian => {
36 let value =
37 self.unsigned_bits_big_endian(s.start_bit as u32, s.signal_size as u32);
38 return s.offset + value as f64 * s.factor;
39 }
40 can_dbc::ByteOrder::LittleEndian => {
41 let value =
42 self.unsigned_bits_little_endian(s.start_bit as u32, s.signal_size as u32);
43 return s.offset + value as f64 * s.factor;
44 }
45 },
46 }
47 }
48
49 fn signed_bits_big_endian(&self, start: u32, len: u32) -> i64 {
50 let unsigned = self.unsigned_bits_big_endian(start, len);
51 return Self::as_signed(unsigned, len as u16);
52 }
53
54 fn unsigned_bits_big_endian(&self, start: u32, len: u32) -> u64 {
55 let packed = self.pack_big_endian();
56 let msb_index = self.invert_endian(&(start as u16)) as u32;
57 let lsb_index = (msb_index + 1 - len) as u32;
58 let shifted = packed >> lsb_index;
59 let masked = &shifted & bigint::BigInt::from((1u128 << len) - 1);
60 return masked.to_u64().unwrap();
61 }
62
63 fn signed_bits_little_endian(&self, start: u32, len: u32) -> i64 {
64 let unsigned = self.unsigned_bits_little_endian(start, len);
65 return Self::as_signed(unsigned, len as u16);
66 }
67
68 fn unsigned_bits_little_endian(&self, start: u32, len: u32) -> u64 {
69 let packed = self.pack_little_endian();
70 let lsb_index = start as u8;
71 let shifted = &packed >> lsb_index;
72 let masked = &shifted & bigint::BigInt::from((1u128 << len) - 1);
73 return masked.to_u64().unwrap();
74 }
75
76 fn pack_big_endian(&self) -> bigint::BigInt {
77 let packed = bigint::BigInt::from_bytes_be(bigint::Sign::Plus, &self.data);
78 return packed;
79 }
80
81 fn pack_little_endian(&self) -> bigint::BigInt {
82 let packed = bigint::BigInt::from_bytes_be(bigint::Sign::Plus, &self.reverse());
83 return packed;
84 }
85
86 fn invert_endian(&self, i: &u16) -> u16 {
87 let row = i / 8;
88 let col = i % 8;
89 let opposite_row = (self.data.len() as u16 - row - 1) as u16;
90 let bit_index = (opposite_row * 8) + col;
91 return bit_index;
92 }
93
94 fn reverse(&self) -> Vec<u8> {
95 let mut reversed_vec: Vec<u8> = vec![0; self.data.len()];
96 for i in 0..self.data.len() {
97 reversed_vec.push(self.data[i]);
98 }
99 return reversed_vec;
100 }
101
102 fn as_signed(unsigned: u64, bits: u16) -> i64 {
103 match bits {
104 8 => {
105 return unsigned as u8 as i8 as i64;
106 }
107 16 => {
108 return unsigned as u16 as i16 as i64;
109 }
110 32 => {
111 return unsigned as u32 as i32 as i64;
112 }
113 64 => {
114 return unsigned as i64;
115 }
116 _ => {
117 let signed_bitmask: u64 = 1 << (bits - 1);
118 let is_negative = unsigned & signed_bitmask > 0;
119 if !is_negative {
120 return unsigned as i64;
121 }
122 let value_bitmask = signed_bitmask - 1;
123 let value = ((!unsigned) & value_bitmask) + 1;
124 return -1 * value as i64;
125 }
126 }
127 }
128}
129
130impl CanData {
131 pub fn new(data: Vec<u8>) -> Self {
132 return CanData { data: data };
133 }
134
135 pub fn len(&self) -> usize {
136 return self.data.len();
137 }
138
139 pub fn push(&mut self, value: u8) {
140 self.data.push(value)
141 }
142
143 pub fn decode(&self, s: &Signal) -> f64 {
144 if s.signal_size == 1 {
145 return 1.0;
146 }
147 match s.value_type() {
148 can_dbc::ValueType::Signed => match s.byte_order() {
149 can_dbc::ByteOrder::BigEndian => {
150 let value =
151 self.signed_bits_big_endian(s.start_bit as u8, s.signal_size as u8);
152 return s.offset + value as f64 * s.factor;
153 }
154 can_dbc::ByteOrder::LittleEndian => {
155 let value =
156 self.signed_bits_little_endian(s.start_bit as u8, s.signal_size as u8);
157 return s.offset + value as f64 * s.factor;
158 }
159 },
160 can_dbc::ValueType::Unsigned => match s.byte_order() {
161 can_dbc::ByteOrder::BigEndian => {
162 let value =
163 self.unsigned_bits_big_endian(s.start_bit as u8, s.signal_size as u8);
164 return s.offset + value as f64 * s.factor;
165 }
166 can_dbc::ByteOrder::LittleEndian => {
167 let value =
168 self.unsigned_bits_little_endian(s.start_bit as u8, s.signal_size as u8);
169 return s.offset + value as f64 * s.factor;
170 }
171 },
172 }
173 }
174
175 fn unsigned_bits_little_endian(&self, start: u8, len: u8) -> u64 {
176 let packed = self.pack_little_endian();
177 let lsb_index = start;
178 let shifted = packed >> lsb_index;
179 let masked = shifted & ((1 << len) - 1);
180 return masked;
181 }
182
183 fn unsigned_bits_big_endian(&self, start: u8, len: u8) -> u64 {
184 let packed = self.pack_big_endian();
185 let msb_index = Self::invert_endian(start);
186 let lsb_index = msb_index + 1 - len;
187 let shifted = packed >> lsb_index;
188 let masked = shifted & ((1 << len) - 1);
189 return masked;
190 }
191
192 fn signed_bits_little_endian(&self, start: u8, len: u8) -> i64 {
193 let unsigned = self.unsigned_bits_little_endian(start, len);
194 return Payload::as_signed(unsigned, len as u16);
195 }
196
197 fn signed_bits_big_endian(&self, start: u8, len: u8) -> i64 {
198 let unsigned = self.unsigned_bits_big_endian(start, len);
199 return Payload::as_signed(unsigned, len as u16);
200 }
201
202 fn pack_little_endian(&self) -> u64 {
203 let mut packed: u64 = 0;
204 packed = packed | u64::from(self.data[0]) << (0 * 8);
205 packed = packed | u64::from(self.data[1]) << (1 * 8);
206 packed = packed | u64::from(self.data[2]) << (2 * 8);
207 packed = packed | u64::from(self.data[3]) << (3 * 8);
208 packed = packed | u64::from(self.data[4]) << (4 * 8);
209 packed = packed | u64::from(self.data[5]) << (5 * 8);
210 packed = packed | u64::from(self.data[6]) << (6 * 8);
211 packed = packed | u64::from(self.data[7]) << (7 * 8);
212 return packed;
213 }
214
215 fn pack_big_endian(&self) -> u64 {
216 let mut packed: u64 = 0;
217 packed = packed | u64::from(self.data[0]) << (7 * 8);
218 packed = packed | u64::from(self.data[1]) << (6 * 8);
219 packed = packed | u64::from(self.data[2]) << (5 * 8);
220 packed = packed | u64::from(self.data[3]) << (4 * 8);
221 packed = packed | u64::from(self.data[4]) << (3 * 8);
222 packed = packed | u64::from(self.data[5]) << (2 * 8);
223 packed = packed | u64::from(self.data[6]) << (1 * 8);
224 packed = packed | u64::from(self.data[7]) << (0 * 8);
225 return packed;
226 }
227
228 fn invert_endian(i: u8) -> u8 {
229 let row = 1 / 8;
230 let col = i % 8;
231 let opposite_row = 7 - row;
232 let bit_index = (opposite_row * 8) + col;
233 return bit_index;
234 }
235}