1 //-----------------------------------------------------------------------------
2 // Copyright (C) Proxmark3 contributors. See AUTHORS.md for details.
4 // This program is free software: you can redistribute it and/or modify
5 // it under the terms of the GNU General Public License as published by
6 // the Free Software Foundation, either version 3 of the License, or
7 // (at your option) any later version.
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU General Public License for more details.
14 // See LICENSE.txt for the text of the license.
15 //-----------------------------------------------------------------------------
16 // Utility functions used in many places, not specific to any piece of code.
17 //-----------------------------------------------------------------------------
18 #include "commonutil.h"
21 /* Similar to FpgaGatherVersion this formats stored version information
22 * into a string representation. It takes a pointer to the struct version_information_t,
23 * verifies the magic properties, then stores a formatted string, prefixed by
26 void FormatVersionInformation(char *dst
, int len
, const char *prefix
, const void *version_info
) {
27 const struct version_information_t
*v
= (const struct version_information_t
*)version_info
;
29 strncat(dst
, prefix
, len
- 1);
30 if (v
->magic
!= VERSION_INFORMATION_MAGIC
) {
31 strncat(dst
, "Missing/Invalid version information", len
- strlen(dst
) - 1);
34 if (v
->versionversion
!= 1) {
35 strncat(dst
, "Version information not understood", len
- strlen(dst
) - 1);
39 strncat(dst
, "Version information not available", len
- strlen(dst
) - 1);
43 strncat(dst
, v
->gitversion
, len
- strlen(dst
) - 1);
45 strncat(dst
, "-unclean", len
- strlen(dst
) - 1);
46 } else if (v
->clean
== 2) {
47 strncat(dst
, "-suspect", len
- strlen(dst
) - 1);
50 strncat(dst
, " ", len
- strlen(dst
) - 1);
51 strncat(dst
, v
->buildtime
, len
- strlen(dst
) - 1);
52 strncat(dst
, " ", len
- strlen(dst
) - 1);
53 strncat(dst
, v
->armsrc
, len
- strlen(dst
) - 1);
56 void format_version_information_short(char *dst
, int len
, const void *version_info
) {
57 const struct version_information_t
*v
= (const struct version_information_t
*)version_info
;
59 if (v
->magic
!= VERSION_INFORMATION_MAGIC
) {
60 strncat(dst
, "Missing/Invalid version information", len
- strlen(dst
) - 1);
63 if (v
->versionversion
!= 1) {
64 strncat(dst
, "Version information not understood", len
- strlen(dst
) - 1);
68 strncat(dst
, "Version information not available", len
- strlen(dst
) - 1);
72 strncat(dst
, v
->gitversion
, len
- strlen(dst
) - 1);
73 strncat(dst
, " ", len
- strlen(dst
) - 1);
74 strncat(dst
, v
->buildtime
, len
- strlen(dst
) - 1);
78 ref http://www.csm.ornl.gov/~dunigan/crc.html
79 Returns the value v with the bottom b [0,32] bits reflected.
80 Example: reflect(0x3e23L,3) == 0x3e26
82 uint32_t reflect(uint32_t v
, int b
) {
84 for (int i
= 0; i
< b
; ++i
) {
86 v
|= BITMASK((b
- 1) - i
);
88 v
&= ~BITMASK((b
- 1) - i
);
94 // https://graphics.stanford.edu/~seander/bithacks.html#BitReverseTable
96 // Reverse the bits in a byte with 3 operations (64-bit multiply and modulus division):
97 uint8_t reflect8(uint8_t b
) {
98 return (b
* 0x0202020202ULL
& 0x010884422010ULL
) % 1023;
101 uint16_t reflect16(uint16_t v
) {
102 v
= (reflect8(v
) << 8) | (reflect8(v
>> 8) & 0xFF);
106 uint32_t reflect32(uint32_t v
) {
107 // https://graphics.stanford.edu/~seander/bithacks.html#BitReverseTable
108 // swap odd and even bits
109 v
= ((v
>> 1) & 0x55555555) | ((v
& 0x55555555) << 1);
110 // swap consecutive pairs
111 v
= ((v
>> 2) & 0x33333333) | ((v
& 0x33333333) << 2);
113 v
= ((v
>> 4) & 0x0F0F0F0F) | ((v
& 0x0F0F0F0F) << 4);
115 v
= ((v
>> 8) & 0x00FF00FF) | ((v
& 0x00FF00FF) << 8);
116 // swap 2-byte long pairs
117 v
= (v
>> 16) | (v
<< 16);
121 uint64_t reflect48(uint64_t v
) {
122 uint64_t vhi
= reflect16(v
>> 32);
123 uint64_t vlo
= reflect32(v
);
124 v
= (vlo
<< 32) | (vhi
& 0xFFFF);
128 uint64_t reflect64(uint64_t v
) {
129 // https://graphics.stanford.edu/~seander/bithacks.html#BitReverseTable
130 // swap 4-byte long pairs
131 uint64_t v1
= reflect32(v
>> 32);
132 uint64_t v2
= reflect32(v
);
133 v
= (v2
<< 32) | (v1
& 0xFFFFFFFF);
137 void num_to_bytes(uint64_t n
, size_t len
, uint8_t *dest
) {
139 dest
[len
] = (uint8_t) n
;
144 uint64_t bytes_to_num(const uint8_t *src
, size_t len
) {
147 num
= (num
<< 8) | (*src
);
153 uint16_t MemLeToUint2byte(const uint8_t *data
) {
155 (((uint16_t)(data
[1])) << 8) +
156 (((uint16_t)(data
[0])) << 0)
160 uint32_t MemLeToUint3byte(const uint8_t *data
) {
162 (((uint32_t)(data
[2])) << 16) +
163 (((uint32_t)(data
[1])) << 8) +
164 (((uint32_t)(data
[0])) << 0)
168 uint32_t MemLeToUint4byte(const uint8_t *data
) {
170 (((uint32_t)(data
[3])) << 24) +
171 (((uint32_t)(data
[2])) << 16) +
172 (((uint32_t)(data
[1])) << 8) +
173 (((uint32_t)(data
[0])) << 0)
177 uint64_t MemLeToUint5byte(const uint8_t *data
) {
179 (((uint64_t)(data
[4])) << 32) +
180 (((uint64_t)(data
[3])) << 24) +
181 (((uint64_t)(data
[2])) << 16) +
182 (((uint64_t)(data
[1])) << 8) +
183 (((uint64_t)(data
[0])) << 0)
187 uint64_t MemLeToUint6byte(const uint8_t *data
) {
189 (((uint64_t)(data
[5])) << 40) +
190 (((uint64_t)(data
[4])) << 32) +
191 (((uint64_t)(data
[3])) << 24) +
192 (((uint64_t)(data
[2])) << 16) +
193 (((uint64_t)(data
[1])) << 8) +
194 (((uint64_t)(data
[0])) << 0)
198 uint64_t MemLeToUint7byte(const uint8_t *data
) {
200 (((uint64_t)(data
[6])) << 48) +
201 (((uint64_t)(data
[5])) << 40) +
202 (((uint64_t)(data
[4])) << 32) +
203 (((uint64_t)(data
[3])) << 24) +
204 (((uint64_t)(data
[2])) << 16) +
205 (((uint64_t)(data
[1])) << 8) +
206 (((uint64_t)(data
[0])) << 0)
210 uint64_t MemLeToUint8byte(const uint8_t *data
) {
212 (((uint64_t)(data
[7])) << 56) +
213 (((uint64_t)(data
[6])) << 48) +
214 (((uint64_t)(data
[5])) << 40) +
215 (((uint64_t)(data
[4])) << 32) +
216 (((uint64_t)(data
[3])) << 24) +
217 (((uint64_t)(data
[2])) << 16) +
218 (((uint64_t)(data
[1])) << 8) +
219 (((uint64_t)(data
[0])) << 0)
223 uint16_t MemBeToUint2byte(const uint8_t *data
) {
225 (((uint16_t)(data
[0])) << 8) +
226 (((uint16_t)(data
[1])) << 0)
230 uint32_t MemBeToUint3byte(const uint8_t *data
) {
232 (((uint32_t)(data
[0])) << 16) +
233 (((uint32_t)(data
[1])) << 8) +
234 (((uint32_t)(data
[2])) << 0)
238 uint32_t MemBeToUint4byte(const uint8_t *data
) {
240 (((uint32_t)(data
[0])) << 24) +
241 (((uint32_t)(data
[1])) << 16) +
242 (((uint32_t)(data
[2])) << 8) +
243 (((uint32_t)(data
[3])) << 0)
247 uint64_t MemBeToUint5byte(const uint8_t *data
) {
249 (((uint64_t)(data
[0])) << 32) +
250 (((uint64_t)(data
[1])) << 24) +
251 (((uint64_t)(data
[2])) << 16) +
252 (((uint64_t)(data
[3])) << 8) +
253 (((uint64_t)(data
[4])) << 0)
257 uint64_t MemBeToUint6byte(const uint8_t *data
) {
259 (((uint64_t)(data
[0])) << 40) +
260 (((uint64_t)(data
[1])) << 32) +
261 (((uint64_t)(data
[2])) << 24) +
262 (((uint64_t)(data
[3])) << 16) +
263 (((uint64_t)(data
[4])) << 8) +
264 (((uint64_t)(data
[5])) << 0)
268 uint64_t MemBeToUint7byte(const uint8_t *data
) {
270 (((uint64_t)(data
[0])) << 48) +
271 (((uint64_t)(data
[1])) << 40) +
272 (((uint64_t)(data
[2])) << 32) +
273 (((uint64_t)(data
[3])) << 24) +
274 (((uint64_t)(data
[4])) << 16) +
275 (((uint64_t)(data
[5])) << 8) +
276 (((uint64_t)(data
[6])) << 0)
280 uint64_t MemBeToUint8byte(const uint8_t *data
) {
282 (((uint64_t)(data
[0])) << 56) +
283 (((uint64_t)(data
[1])) << 48) +
284 (((uint64_t)(data
[2])) << 40) +
285 (((uint64_t)(data
[3])) << 32) +
286 (((uint64_t)(data
[4])) << 24) +
287 (((uint64_t)(data
[5])) << 16) +
288 (((uint64_t)(data
[6])) << 8) +
289 (((uint64_t)(data
[7])) << 0)
293 void Uint2byteToMemLe(uint8_t *data
, uint16_t value
) {
294 data
[0] = (uint8_t)((value
>> 0) & 0xffu
);
295 data
[1] = (uint8_t)((value
>> 8) & 0xffu
);
298 void Uint3byteToMemLe(uint8_t *data
, uint32_t value
) {
299 data
[0] = (uint8_t)((value
>> 0) & 0xffu
);
300 data
[1] = (uint8_t)((value
>> 8) & 0xffu
);
301 data
[2] = (uint8_t)((value
>> 16) & 0xffu
);
304 void Uint4byteToMemLe(uint8_t *data
, uint32_t value
) {
305 data
[0] = (uint8_t)((value
>> 0) & 0xffu
);
306 data
[1] = (uint8_t)((value
>> 8) & 0xffu
);
307 data
[2] = (uint8_t)((value
>> 16) & 0xffu
);
308 data
[3] = (uint8_t)((value
>> 24) & 0xffu
);
311 void Uint5byteToMemLe(uint8_t *data
, uint64_t value
) {
312 data
[0] = (uint8_t)((value
>> 0) & 0xffu
);
313 data
[1] = (uint8_t)((value
>> 8) & 0xffu
);
314 data
[2] = (uint8_t)((value
>> 16) & 0xffu
);
315 data
[3] = (uint8_t)((value
>> 24) & 0xffu
);
316 data
[4] = (uint8_t)((value
>> 32) & 0xffu
);
319 void Uint6byteToMemLe(uint8_t *data
, uint64_t value
) {
320 data
[0] = (uint8_t)((value
>> 0) & 0xffu
);
321 data
[1] = (uint8_t)((value
>> 8) & 0xffu
);
322 data
[2] = (uint8_t)((value
>> 16) & 0xffu
);
323 data
[3] = (uint8_t)((value
>> 24) & 0xffu
);
324 data
[4] = (uint8_t)((value
>> 32) & 0xffu
);
325 data
[5] = (uint8_t)((value
>> 40) & 0xffu
);
328 void Uint7byteToMemLe(uint8_t *data
, uint64_t value
) {
329 data
[0] = (uint8_t)((value
>> 0) & 0xffu
);
330 data
[1] = (uint8_t)((value
>> 8) & 0xffu
);
331 data
[2] = (uint8_t)((value
>> 16) & 0xffu
);
332 data
[3] = (uint8_t)((value
>> 24) & 0xffu
);
333 data
[4] = (uint8_t)((value
>> 32) & 0xffu
);
334 data
[5] = (uint8_t)((value
>> 40) & 0xffu
);
335 data
[6] = (uint8_t)((value
>> 48) & 0xffu
);
338 void Uint8byteToMemLe(uint8_t *data
, uint64_t value
) {
339 data
[0] = (uint8_t)((value
>> 0) & 0xffu
);
340 data
[1] = (uint8_t)((value
>> 8) & 0xffu
);
341 data
[2] = (uint8_t)((value
>> 16) & 0xffu
);
342 data
[3] = (uint8_t)((value
>> 24) & 0xffu
);
343 data
[4] = (uint8_t)((value
>> 32) & 0xffu
);
344 data
[5] = (uint8_t)((value
>> 40) & 0xffu
);
345 data
[6] = (uint8_t)((value
>> 48) & 0xffu
);
346 data
[7] = (uint8_t)((value
>> 56) & 0xffu
);
349 void Uint2byteToMemBe(uint8_t *data
, uint16_t value
) {
350 data
[0] = (uint8_t)((value
>> 8) & 0xffu
);
351 data
[1] = (uint8_t)((value
>> 0) & 0xffu
);
354 void Uint3byteToMemBe(uint8_t *data
, uint32_t value
) {
355 data
[0] = (uint8_t)((value
>> 16) & 0xffu
);
356 data
[1] = (uint8_t)((value
>> 8) & 0xffu
);
357 data
[2] = (uint8_t)((value
>> 0) & 0xffu
);
360 void Uint4byteToMemBe(uint8_t *data
, uint32_t value
) {
361 data
[0] = (uint8_t)((value
>> 24) & 0xffu
);
362 data
[1] = (uint8_t)((value
>> 16) & 0xffu
);
363 data
[2] = (uint8_t)((value
>> 8) & 0xffu
);
364 data
[3] = (uint8_t)((value
>> 0) & 0xffu
);
367 void Uint5byteToMemBe(uint8_t *data
, uint64_t value
) {
368 data
[0] = (uint8_t)((value
>> 32) & 0xffu
);
369 data
[1] = (uint8_t)((value
>> 24) & 0xffu
);
370 data
[2] = (uint8_t)((value
>> 16) & 0xffu
);
371 data
[3] = (uint8_t)((value
>> 8) & 0xffu
);
372 data
[4] = (uint8_t)((value
>> 0) & 0xffu
);
375 void Uint6byteToMemBe(uint8_t *data
, uint64_t value
) {
376 data
[0] = (uint8_t)((value
>> 40) & 0xffu
);
377 data
[1] = (uint8_t)((value
>> 32) & 0xffu
);
378 data
[2] = (uint8_t)((value
>> 24) & 0xffu
);
379 data
[3] = (uint8_t)((value
>> 16) & 0xffu
);
380 data
[4] = (uint8_t)((value
>> 8) & 0xffu
);
381 data
[5] = (uint8_t)((value
>> 0) & 0xffu
);
384 void Uint7byteToMemBe(uint8_t *data
, uint64_t value
) {
385 data
[0] = (uint8_t)((value
>> 48) & 0xffu
);
386 data
[1] = (uint8_t)((value
>> 40) & 0xffu
);
387 data
[2] = (uint8_t)((value
>> 32) & 0xffu
);
388 data
[3] = (uint8_t)((value
>> 24) & 0xffu
);
389 data
[4] = (uint8_t)((value
>> 16) & 0xffu
);
390 data
[5] = (uint8_t)((value
>> 8) & 0xffu
);
391 data
[6] = (uint8_t)((value
>> 0) & 0xffu
);
394 void Uint8byteToMemBe(uint8_t *data
, uint64_t value
) {
395 data
[0] = (uint8_t)((value
>> 56) & 0xffu
);
396 data
[1] = (uint8_t)((value
>> 48) & 0xffu
);
397 data
[2] = (uint8_t)((value
>> 40) & 0xffu
);
398 data
[3] = (uint8_t)((value
>> 32) & 0xffu
);
399 data
[4] = (uint8_t)((value
>> 24) & 0xffu
);
400 data
[5] = (uint8_t)((value
>> 16) & 0xffu
);
401 data
[6] = (uint8_t)((value
>> 8) & 0xffu
);
402 data
[7] = (uint8_t)((value
>> 0) & 0xffu
);
405 // Rotate Left - Ultralight, Desfire
406 void rol(uint8_t *data
, const size_t len
) {
407 uint8_t first
= data
[0];
408 for (size_t i
= 0; i
< len
- 1; i
++) {
409 data
[i
] = data
[i
+ 1];
411 data
[len
- 1] = first
;
414 // Rotate Right - Ultralight, Desfire
415 void ror(uint8_t *data
, const size_t len
) {
416 uint8_t last
= data
[len
- 1];
418 for (int i
= len
- 1; i
> 0; i
--) {
419 data
[i
] = data
[i
- 1];
426 void lsl(uint8_t *data
, size_t len
) {
427 for (size_t n
= 0; n
< len
- 1; n
++) {
428 data
[n
] = (data
[n
] << 1) | (data
[n
+ 1] >> 7);
434 // BSWAP24 of array[3]
435 uint32_t le24toh(const uint8_t data
[3]) {
436 return (data
[2] << 16) | (data
[1] << 8) | data
[0];
439 // BSWAP24, take u32, output array
440 void htole24(uint32_t val
, uint8_t data
[3]) {
441 data
[0] = (uint8_t) val
;
442 data
[1] = (uint8_t)(val
>> 8);
443 data
[2] = (uint8_t)(val
>> 16);
448 uint32_t rotl(uint32_t a
, uint8_t n
) {
450 return (a
<< n
) | (a
>> (32 - n
));
454 uint32_t rotr(uint32_t a
, uint8_t n
) {
456 return (a
>> n
) | (a
<< (32 - n
));
459 uint16_t get_sw(const uint8_t *d
, uint16_t n
) {
464 return (d
[n
] << 8 | d
[n
+ 1]);
467 // reverse same array
468 void reverse_array(uint8_t *d
, size_t n
) {
469 if (d
== NULL
|| n
< 2) {
473 for (int i
= 0, j
= n
- 1; i
< j
; ++i
, --j
) {
480 // reverse src array into dest array
481 void reverse_array_copy(const uint8_t *src
, int src_len
, uint8_t *dest
) {
482 if (src
== NULL
|| src_len
== 0 || dest
== NULL
) {
486 for (int i
= 0; i
< src_len
; i
++) {
487 dest
[i
] = src
[(src_len
- 1) - i
];
491 static int hexchar_to_dec(char ch
) {
492 if (ch
>= '0' && ch
<= '9') {
495 if (ch
>= 'a' && ch
<= 'f') {
496 return ch
- 'a' + 10;
498 if (ch
>= 'A' && ch
<= 'F') {
499 return ch
- 'A' + 10;
504 // no spaces allowed for input hex string
505 bool hexstr_to_byte_array(const char *hexstr
, uint8_t *d
, size_t *n
) {
507 size_t hexstr_len
= strlen(hexstr
);
508 if (hexstr_len
& 1) {
512 *n
= (hexstr_len
>> 1);
514 for (int i
= 0; i
< *n
; i
++) {
519 if (c1
== '\0' || c2
== '\0') {
523 int b
= (hexchar_to_dec(c1
) << 4) | hexchar_to_dec(c2
);
525 // Error: invalid hex character
533 void reverse_arraybytes(uint8_t *arr
, size_t len
) {
535 for (i
= 0; i
< len
; i
++) {
536 arr
[i
] = reflect8(arr
[i
]);
540 void reverse_arraybytes_copy(uint8_t *arr
, uint8_t *dest
, size_t len
) {
542 for (i
= 0; i
< len
; i
++) {
543 dest
[i
] = reflect8(arr
[i
]);
547 // TODO: Boost performance by copying in chunks of 1, 2, or 4 bytes when feasible.
548 size_t concatbits(uint8_t *dest
, int dest_offset
, const uint8_t *src
, int src_offset
, size_t nbits
) {
552 if ((src
- dest
) * 8 + src_offset
- dest_offset
> 0) {
562 for (; i
!= end
; i
+= step
) {
563 // equiv of dest_bits[dest_offset + i] = src_bits[src_offset + i]
564 CLEAR_BIT(dest
, dest_offset
+ i
);
565 if (TEST_BIT(src
, src_offset
+ i
)) SET_BIT(dest
, dest_offset
+ i
);
568 return dest_offset
+ nbits
;
571 int char2int(char c
) {
572 if (c
>= '0' && c
<= '9') return c
- '0';
573 if (c
>= 'A' && c
<= 'F') return c
- 'A' + 10;
574 if (c
>= 'a' && c
<= 'f') return c
- 'a' + 10;
575 return -1; // Invalid character for hex
578 // returns the number of bytes written
579 int hexstr2ByteArr(const char *hexstr
, unsigned char *array
, size_t asize
) {
581 while (hexstr
[n
] != '\0') {
585 // Check if the input is valid and fits in the output array
586 if (n
% 2 != 0 || asize
< n
>> 1) {
587 return -1; // Error: invalid length or insufficient byte array size
590 for (size_t i
= 0; i
< n
; i
+= 2) {
591 int high
= char2int(hexstr
[i
]);
592 int low
= char2int(hexstr
[i
+ 1]);
594 if (high
== -1 || low
== -1) {
595 return -1; // Error: invalid hex character
598 array
[i
>> 1] = (high
<< 4) | low
;