1 /** @file
2 Various utility functions for use by device drivers.
3
4 Copyright (C) 2015 Tommy Vestermark
5
6 This program is free software; you can redistribute it and/or modify
7 it under the terms of the GNU General Public License as published by
8 the Free Software Foundation; either version 2 of the License, or
9 (at your option) any later version.
10 */
11
12 #include "util.h"
13 #include <stdlib.h>
14 #include <stdio.h>
15 #include <string.h>
16
reverse8(uint8_t x)17 uint8_t reverse8(uint8_t x)
18 {
19 x = (x & 0xF0) >> 4 | (x & 0x0F) << 4;
20 x = (x & 0xCC) >> 2 | (x & 0x33) << 2;
21 x = (x & 0xAA) >> 1 | (x & 0x55) << 1;
22 return x;
23 }
24
reverse32(uint32_t x)25 uint32_t reverse32(uint32_t x)
26 {
27 uint32_t ret;
28 uint8_t* xp = (uint8_t*)&x;
29 ret = (uint32_t) reverse8(xp[0]) << 24 | reverse8(xp[1]) << 16 | reverse8(xp[2]) << 8 | reverse8(xp[3]);
30 return ret;
31 }
32
reflect_bytes(uint8_t message[],unsigned num_bytes)33 void reflect_bytes(uint8_t message[], unsigned num_bytes)
34 {
35 for (unsigned i = 0; i < num_bytes; ++i) {
36 message[i] = reverse8(message[i]);
37 }
38 }
39
reflect4(uint8_t x)40 uint8_t reflect4(uint8_t x)
41 {
42 x = (x & 0xCC) >> 2 | (x & 0x33) << 2;
43 x = (x & 0xAA) >> 1 | (x & 0x55) << 1;
44 return x;
45 }
46
reflect_nibbles(uint8_t message[],unsigned num_bytes)47 void reflect_nibbles(uint8_t message[], unsigned num_bytes)
48 {
49 for (unsigned i = 0; i < num_bytes; ++i) {
50 message[i] = reflect4(message[i]);
51 }
52 }
53
extract_nibbles_4b1s(uint8_t * message,unsigned offset_bits,unsigned num_bits,uint8_t * dst)54 unsigned extract_nibbles_4b1s(uint8_t *message, unsigned offset_bits, unsigned num_bits, uint8_t *dst)
55 {
56 unsigned ret = 0;
57
58 while (num_bits >= 5) {
59 uint16_t bits = (message[offset_bits / 8] << 8) | message[(offset_bits / 8) + 1];
60 bits >>= 11 - (offset_bits % 8); // align 5 bits to LSB
61 if ((bits & 1) != 1)
62 break; // stuff-bit error
63 *dst++ = (bits >> 1) & 0xf;
64 ret += 1;
65 offset_bits += 5;
66 num_bits -= 5;
67 }
68
69 return ret;
70 }
71
extract_bytes_uart(uint8_t * message,unsigned offset_bits,unsigned num_bits,uint8_t * dst)72 unsigned extract_bytes_uart(uint8_t *message, unsigned offset_bits, unsigned num_bits, uint8_t *dst)
73 {
74 unsigned ret = 0;
75
76 while (num_bits >= 10) {
77 int startb = message[offset_bits / 8] >> (7 - (offset_bits % 8));
78 offset_bits += 1;
79 int datab = message[offset_bits / 8];
80 if (offset_bits % 8) {
81 datab = (message[offset_bits / 8] << 8) | message[offset_bits / 8 + 1];
82 datab >>= 8 - (offset_bits % 8);
83 }
84 offset_bits += 8;
85 int stopb = message[offset_bits / 8] >> (7 - (offset_bits % 8));
86 offset_bits += 1;
87 if ((startb & 1) != 0)
88 break; // start-bit error
89 if ((stopb & 1) != 1)
90 break; // stop-bit error
91 *dst++ = reverse8(datab & 0xff);
92 ret += 1;
93 num_bits -= 10;
94 }
95
96 return ret;
97 }
98
crc4(uint8_t const message[],unsigned nBytes,uint8_t polynomial,uint8_t init)99 uint8_t crc4(uint8_t const message[], unsigned nBytes, uint8_t polynomial, uint8_t init)
100 {
101 unsigned remainder = init << 4; // LSBs are unused
102 unsigned poly = polynomial << 4;
103 unsigned bit;
104
105 while (nBytes--) {
106 remainder ^= *message++;
107 for (bit = 0; bit < 8; bit++) {
108 if (remainder & 0x80) {
109 remainder = (remainder << 1) ^ poly;
110 } else {
111 remainder = (remainder << 1);
112 }
113 }
114 }
115 return remainder >> 4 & 0x0f; // discard the LSBs
116 }
117
crc7(uint8_t const message[],unsigned nBytes,uint8_t polynomial,uint8_t init)118 uint8_t crc7(uint8_t const message[], unsigned nBytes, uint8_t polynomial, uint8_t init)
119 {
120 unsigned remainder = init << 1; // LSB is unused
121 unsigned poly = polynomial << 1;
122 unsigned byte, bit;
123
124 for (byte = 0; byte < nBytes; ++byte) {
125 remainder ^= message[byte];
126 for (bit = 0; bit < 8; ++bit) {
127 if (remainder & 0x80) {
128 remainder = (remainder << 1) ^ poly;
129 } else {
130 remainder = (remainder << 1);
131 }
132 }
133 }
134 return remainder >> 1 & 0x7f; // discard the LSB
135 }
136
crc8(uint8_t const message[],unsigned nBytes,uint8_t polynomial,uint8_t init)137 uint8_t crc8(uint8_t const message[], unsigned nBytes, uint8_t polynomial, uint8_t init)
138 {
139 uint8_t remainder = init;
140 unsigned byte, bit;
141
142 for (byte = 0; byte < nBytes; ++byte) {
143 remainder ^= message[byte];
144 for (bit = 0; bit < 8; ++bit) {
145 if (remainder & 0x80) {
146 remainder = (remainder << 1) ^ polynomial;
147 } else {
148 remainder = (remainder << 1);
149 }
150 }
151 }
152 return remainder;
153 }
154
crc8le(uint8_t const message[],unsigned nBytes,uint8_t polynomial,uint8_t init)155 uint8_t crc8le(uint8_t const message[], unsigned nBytes, uint8_t polynomial, uint8_t init)
156 {
157 uint8_t remainder = reverse8(init);
158 unsigned byte, bit;
159 polynomial = reverse8(polynomial);
160
161 for (byte = 0; byte < nBytes; ++byte) {
162 remainder ^= message[byte];
163 for (bit = 0; bit < 8; ++bit) {
164 if (remainder & 1) {
165 remainder = (remainder >> 1) ^ polynomial;
166 } else {
167 remainder = (remainder >> 1);
168 }
169 }
170 }
171 return remainder;
172 }
173
crc16lsb(uint8_t const message[],unsigned nBytes,uint16_t polynomial,uint16_t init)174 uint16_t crc16lsb(uint8_t const message[], unsigned nBytes, uint16_t polynomial, uint16_t init)
175 {
176 uint16_t remainder = init;
177 unsigned byte, bit;
178
179 for (byte = 0; byte < nBytes; ++byte) {
180 remainder ^= message[byte];
181 for (bit = 0; bit < 8; ++bit) {
182 if (remainder & 1) {
183 remainder = (remainder >> 1) ^ polynomial;
184 }
185 else {
186 remainder = (remainder >> 1);
187 }
188 }
189 }
190 return remainder;
191 }
192
crc16(uint8_t const message[],unsigned nBytes,uint16_t polynomial,uint16_t init)193 uint16_t crc16(uint8_t const message[], unsigned nBytes, uint16_t polynomial, uint16_t init)
194 {
195 uint16_t remainder = init;
196 unsigned byte, bit;
197
198 for (byte = 0; byte < nBytes; ++byte) {
199 remainder ^= message[byte] << 8;
200 for (bit = 0; bit < 8; ++bit) {
201 if (remainder & 0x8000) {
202 remainder = (remainder << 1) ^ polynomial;
203 }
204 else {
205 remainder = (remainder << 1);
206 }
207 }
208 }
209 return remainder;
210 }
211
lfsr_digest8(uint8_t const message[],unsigned bytes,uint8_t gen,uint8_t key)212 uint8_t lfsr_digest8(uint8_t const message[], unsigned bytes, uint8_t gen, uint8_t key)
213 {
214 uint8_t sum = 0;
215 for (unsigned k = 0; k < bytes; ++k) {
216 uint8_t data = message[k];
217 for (int i = 7; i >= 0; --i) {
218 // fprintf(stderr, "key is %02x\n", key);
219 // XOR key into sum if data bit is set
220 if ((data >> i) & 1)
221 sum ^= key;
222
223 // roll the key right (actually the lsb is dropped here)
224 // and apply the gen (needs to include the dropped lsb as msb)
225 if (key & 1)
226 key = (key >> 1) ^ gen;
227 else
228 key = (key >> 1);
229 }
230 }
231 return sum;
232 }
233
lfsr_digest8_reflect(uint8_t const message[],int bytes,uint8_t gen,uint8_t key)234 uint8_t lfsr_digest8_reflect(uint8_t const message[], int bytes, uint8_t gen, uint8_t key)
235 {
236 uint8_t sum = 0;
237 // Process message from last byte to first byte (reflected)
238 for (int k = bytes - 1; k >= 0; --k) {
239 uint8_t data = message[k];
240 // Process individual bits of each byte (reflected)
241 for (int i = 0; i < 8; ++i) {
242 // fprintf(stderr, "key is %02x\n", key);
243 // XOR key into sum if data bit is set
244 if ((data >> i) & 1) {
245 sum ^= key;
246 }
247
248 // roll the key left (actually the lsb is dropped here)
249 // and apply the gen (needs to include the dropped lsb as msb)
250 if (key & 0x80)
251 key = (key << 1) ^ gen;
252 else
253 key = (key << 1);
254 }
255 }
256 return sum;
257 }
258
lfsr_digest16(uint8_t const message[],unsigned bytes,uint16_t gen,uint16_t key)259 uint16_t lfsr_digest16(uint8_t const message[], unsigned bytes, uint16_t gen, uint16_t key)
260 {
261 uint16_t sum = 0;
262 for (unsigned k = 0; k < bytes; ++k) {
263 uint8_t data = message[k];
264 for (int i = 7; i >= 0; --i) {
265 // fprintf(stderr, "key at bit %d : %04x\n", i, key);
266 // if data bit is set then xor with key
267 if ((data >> i) & 1)
268 sum ^= key;
269
270 // roll the key right (actually the lsb is dropped here)
271 // and apply the gen (needs to include the dropped lsb as msb)
272 if (key & 1)
273 key = (key >> 1) ^ gen;
274 else
275 key = (key >> 1);
276 }
277 }
278 return sum;
279 }
280
281 /*
282 void lfsr_keys_fwd16(int rounds, uint16_t gen, uint16_t key)
283 {
284 for (int i = 0; i <= rounds; ++i) {
285 fprintf(stderr, "key at bit %d : %04x\n", i, key);
286
287 // roll the key right (actually the lsb is dropped here)
288 // and apply the gen (needs to include the dropped lsb as msb)
289 if (key & 1)
290 key = (key >> 1) ^ gen;
291 else
292 key = (key >> 1);
293 }
294 }
295
296 void lfsr_keys_rwd16(int rounds, uint16_t gen, uint16_t key)
297 {
298 for (int i = 0; i <= rounds; ++i) {
299 fprintf(stderr, "key at bit -%d : %04x\n", i, key);
300
301 // roll the key left (actually the msb is dropped here)
302 // and apply the gen (needs to include the dropped msb as lsb)
303 if (key & (1 << 15))
304 key = (key << 1) ^ gen;
305 else
306 key = (key << 1);
307 }
308 }
309 */
310
311 // we could use popcount intrinsic, but don't actually need the performance
parity8(uint8_t byte)312 int parity8(uint8_t byte)
313 {
314 byte ^= byte >> 4;
315 byte &= 0xf;
316 return (0x6996 >> byte) & 1;
317 }
318
parity_bytes(uint8_t const message[],unsigned num_bytes)319 int parity_bytes(uint8_t const message[], unsigned num_bytes)
320 {
321 int result = 0;
322 for (unsigned i = 0; i < num_bytes; ++i) {
323 result ^= parity8(message[i]);
324 }
325 return result;
326 }
327
xor_bytes(uint8_t const message[],unsigned num_bytes)328 uint8_t xor_bytes(uint8_t const message[], unsigned num_bytes)
329 {
330 uint8_t result = 0;
331 for (unsigned i = 0; i < num_bytes; ++i) {
332 result ^= message[i];
333 }
334 return result;
335 }
336
add_bytes(uint8_t const message[],unsigned num_bytes)337 int add_bytes(uint8_t const message[], unsigned num_bytes)
338 {
339 int result = 0;
340 for (unsigned i = 0; i < num_bytes; ++i) {
341 result += message[i];
342 }
343 return result;
344 }
345
add_nibbles(uint8_t const message[],unsigned num_bytes)346 int add_nibbles(uint8_t const message[], unsigned num_bytes)
347 {
348 int result = 0;
349 for (unsigned i = 0; i < num_bytes; ++i) {
350 result += (message[i] >> 4) + (message[i] & 0x0f);
351 }
352 return result;
353 }
354
355 // Unit testing
356 #ifdef _TEST
357 #define ASSERT_EQUALS(a, b) \
358 do { \
359 if ((a) == (b)) \
360 ++passed; \
361 else { \
362 ++failed; \
363 fprintf(stderr, "FAIL: %d <> %d\n", (a), (b)); \
364 } \
365 } while (0)
366
main(void)367 int main(void) {
368 unsigned passed = 0;
369 unsigned failed = 0;
370
371 fprintf(stderr, "util:: test\n");
372
373 uint8_t msg[] = {0x08, 0x0a, 0xe8, 0x80};
374
375 fprintf(stderr, "util::crc8(): odd parity\n");
376 ASSERT_EQUALS(crc8(msg, 3, 0x80, 0x00), 0x80);
377
378 fprintf(stderr, "util::crc8(): even parity\n");
379 ASSERT_EQUALS(crc8(msg, 4, 0x80, 0x00), 0x00);
380
381 // sync-word 0b0 0xff 0b1 0b0 0x33 0b1 (i.e. 0x7fd99, note that 0x33 is 0xcc "on the wire")
382 uint8_t uart[] = {0x7f, 0xd9, 0x90};
383 uint8_t bytes[6] = {0};
384
385 // y0 xff y1 y0 xcc y1 y0 x80 y1 y0 x40 y1 y0 xc0 y1
386 uint8_t uart123[] = {0x07, 0xfd, 0x99, 0x40, 0x48, 0x16, 0x04, 0x00};
387
388 fprintf(stderr, "util::extract_bytes_uart():\n");
389 ASSERT_EQUALS(extract_bytes_uart(uart, 0, 24, bytes), 2);
390 ASSERT_EQUALS(bytes[0], 0xff);
391 ASSERT_EQUALS(bytes[1], 0x33);
392
393 ASSERT_EQUALS(extract_bytes_uart(uart123, 4, 60, bytes), 5);
394 ASSERT_EQUALS(bytes[0], 0xff);
395 ASSERT_EQUALS(bytes[1], 0x33);
396 ASSERT_EQUALS(bytes[2], 0x01);
397 ASSERT_EQUALS(bytes[3], 0x02);
398 ASSERT_EQUALS(bytes[4], 0x03);
399
400 fprintf(stderr, "util:: test (%u/%u) passed, (%u) failed.\n", passed, passed + failed, failed);
401
402 return failed;
403 }
404 #endif /* _TEST */
405