unleashed-firmware/lib/lfrfid/tools/bit_lib.c
SG 9bfb641d3e
[FL-2529][FL-1628] New LF-RFID subsystem (#1601)
* Makefile: unit tests pack
* RFID: pulse joiner and its unit test
* Move pulse protocol helpers to appropriate place
* Drop pulse_joiner tests
* Generic protocol, protocols dictionary, unit test
* Protocol dict unit test
* iButton: protocols dictionary
* Lib: varint
* Lib: profiler
* Unit test: varint
* rfid: worker mockup
* LFRFID: em4100 unit test
* Storage: file_exist function
* rfid: fsk osc
* rfid: generic fsk demodulator
* rfid: protocol em4100
* rfid: protocol h10301
* rfid: protocol io prox xsf
* Unit test: rfid protocols
* rfid: new hal
* rfid: raw worker
* Unit test: fix error output
* rfid: worker
* rfid: plain c cli
* fw: migrate to scons
* lfrfid: full io prox support
* unit test: io prox protocol
* SubGHZ: move bit defines to source
* FSK oscillator: level duration compability
* libs: bit manipulation library
* lfrfid: ioprox protocol, use bit library and new level duration method of FSK ocillator
* bit lib: unit tests
* Bit lib: parity tests, remove every nth bit, copy bits
* Lfrfid: awid protocol
* bit lib: uint16 and uint32 getters, unit tests
* lfrfid: FDX-B read, draft version
* Minunit: better memeq assert
* bit lib: reverse, print, print regions
* Protocol dict: get protocol features, get protocol validate count
* lfrfid worker: improved read
* lfrfid raw worker: psk support
* Cli: rfid plain C cli
* protocol AWID: render
* protocol em4100: render
* protocol h10301: render
* protocol indala26: support every indala 26 scramble
* Protocol IO Prox: render
* Protocol FDX-B: advanced read
* lfrfid: remove unused test function
* lfrfid: fix os primitives
* bit lib: crc16 and unit tests
* FDX-B: save data
* lfrfid worker: increase stream size. Alloc raw worker only when needed.
* lfrfid: indala26 emulation
* lfrfid: prepare to write
* lfrfid: fdx-b emulation
* lfrfid: awid, ioprox write
* lfrfid: write t55xx w\o validation
* lfrfid: better t55xx block0 handling
* lfrfid: use new t5577 functions in worker
* lfrfid: improve protocol description
* lfrfid: write and verify
* lfrfid: delete cpp cli
* lfrfid: improve worker usage
* lfrfid-app: step to new worker
* lfrfid: old indala (I40134) load fallback
* lfrfid: indala26, recover wrong synced data
* lfrfid: remove old worker
* lfrfid app: dummy read screen
* lfrfid app: less dummy read screen
* lfrfid: generic 96-bit HID protocol (covers up to HID 37-bit)
* rename
* lfrfid: improve indala26 read
* lfrfid: generic 192-bit HID protocol (covers all HID extended)
* lfrfid: TODO about HID render
* lfrfid: new protocol FDX-A
* lfrfid-app: correct worker stop on exit
* misc fixes
* lfrfid: FDX-A and HID distinguishability has been fixed.
* lfrfid: decode HID size header and render it (#1612)
* lfrfid: rename HID96 and HID192 to HIDProx and HIDExt
* lfrfid: extra actions scene
* lfrfid: decode generic HID Proximity size lazily (#1618)
* lib: stream of data buffers concept
* lfrfid: raw file helper
* lfrfid: changed raw worker api
* lfrfid: packed varint pair
* lfrfid: read stream speedup
* lfrfid app: show read mode
* Documentation
* lfrfid app: raw read gui
* lfrfid app: storage check for raw read
* memleak fix
* review fixes
* lfrfid app: read blink color
* lfrfid app: reset key name after read
* review fixes
* lfrfid app: fix copypasted text
* review fixes
* lfrfid: disable debug gpio
* lfrfid: card detection events
* lfrfid: change validation color from magenta to green
* Update core_defines.
* lfrfid: prefix fdx-b id by zeroes
* lfrfid: parse up to 43-bit HID Proximity keys (#1640)
* Fbt: downgrade toolchain and fix PS1
* lfrfid: fix unit tests
* lfrfid app: remove printf
* lfrfid: indala26, use bit 55 as data
* lfrfid: indala26, better brief format
* lfrfid: indala26, loading fallback
* lfrfid: read timing tuning

Co-authored-by: James Ide <ide@users.noreply.github.com>
Co-authored-by: あく <alleteam@gmail.com>
2022-08-24 00:57:39 +09:00

291 lines
8.2 KiB
C

#include "bit_lib.h"
#include <core/check.h>
#include <stdio.h>
void bit_lib_push_bit(uint8_t* data, size_t data_size, bool bit) {
size_t last_index = data_size - 1;
for(size_t i = 0; i < last_index; ++i) {
data[i] = (data[i] << 1) | ((data[i + 1] >> 7) & 1);
}
data[last_index] = (data[last_index] << 1) | bit;
}
void bit_lib_set_bit(uint8_t* data, size_t position, bool bit) {
if(bit) {
data[position / 8] |= 1UL << (7 - (position % 8));
} else {
data[position / 8] &= ~(1UL << (7 - (position % 8)));
}
}
void bit_lib_set_bits(uint8_t* data, size_t position, uint8_t byte, uint8_t length) {
furi_check(length <= 8);
furi_check(length > 0);
for(uint8_t i = 0; i < length; ++i) {
uint8_t shift = (length - 1) - i;
bit_lib_set_bit(data, position + i, (byte >> shift) & 1);
}
}
bool bit_lib_get_bit(const uint8_t* data, size_t position) {
return (data[position / 8] >> (7 - (position % 8))) & 1;
}
uint8_t bit_lib_get_bits(const uint8_t* data, size_t position, uint8_t length) {
uint8_t shift = position % 8;
if(shift == 0) {
return data[position / 8] >> (8 - length);
} else {
// TODO fix read out of bounds
uint8_t value = (data[position / 8] << (shift));
value |= data[position / 8 + 1] >> (8 - shift);
value = value >> (8 - length);
return value;
}
}
uint16_t bit_lib_get_bits_16(const uint8_t* data, size_t position, uint8_t length) {
uint16_t value = 0;
if(length <= 8) {
value = bit_lib_get_bits(data, position, length);
} else {
value = bit_lib_get_bits(data, position, 8) << (length - 8);
value |= bit_lib_get_bits(data, position + 8, length - 8);
}
return value;
}
uint32_t bit_lib_get_bits_32(const uint8_t* data, size_t position, uint8_t length) {
uint32_t value = 0;
if(length <= 8) {
value = bit_lib_get_bits(data, position, length);
} else if(length <= 16) {
value = bit_lib_get_bits(data, position, 8) << (length - 8);
value |= bit_lib_get_bits(data, position + 8, length - 8);
} else if(length <= 24) {
value = bit_lib_get_bits(data, position, 8) << (length - 8);
value |= bit_lib_get_bits(data, position + 8, 8) << (length - 16);
value |= bit_lib_get_bits(data, position + 16, length - 16);
} else {
value = bit_lib_get_bits(data, position, 8) << (length - 8);
value |= bit_lib_get_bits(data, position + 8, 8) << (length - 16);
value |= bit_lib_get_bits(data, position + 16, 8) << (length - 24);
value |= bit_lib_get_bits(data, position + 24, length - 24);
}
return value;
}
bool bit_lib_test_parity_32(uint32_t bits, BitLibParity parity) {
#if !defined __GNUC__
#error Please, implement parity test for non-GCC compilers
#else
switch(parity) {
case BitLibParityEven:
return __builtin_parity(bits);
case BitLibParityOdd:
return !__builtin_parity(bits);
default:
furi_crash("Unknown parity");
}
#endif
}
bool bit_lib_test_parity(
const uint8_t* bits,
size_t position,
uint8_t length,
BitLibParity parity,
uint8_t parity_length) {
uint32_t parity_block;
bool result = true;
const size_t parity_blocks_count = length / parity_length;
for(size_t i = 0; i < parity_blocks_count; ++i) {
switch(parity) {
case BitLibParityEven:
case BitLibParityOdd:
parity_block = bit_lib_get_bits_32(bits, position + i * parity_length, parity_length);
if(!bit_lib_test_parity_32(parity_block, parity)) {
result = false;
}
break;
case BitLibParityAlways0:
if(bit_lib_get_bit(bits, position + i * parity_length + parity_length - 1)) {
result = false;
}
break;
case BitLibParityAlways1:
if(!bit_lib_get_bit(bits, position + i * parity_length + parity_length - 1)) {
result = false;
}
break;
}
if(!result) break;
}
return result;
}
size_t bit_lib_remove_bit_every_nth(uint8_t* data, size_t position, uint8_t length, uint8_t n) {
size_t counter = 0;
size_t result_counter = 0;
uint8_t bit_buffer = 0;
uint8_t bit_counter = 0;
while(counter < length) {
if((counter + 1) % n != 0) {
bit_buffer = (bit_buffer << 1) | bit_lib_get_bit(data, position + counter);
bit_counter++;
}
if(bit_counter == 8) {
bit_lib_set_bits(data, position + result_counter, bit_buffer, 8);
bit_counter = 0;
bit_buffer = 0;
result_counter += 8;
}
counter++;
}
if(bit_counter != 0) {
bit_lib_set_bits(data, position + result_counter, bit_buffer, bit_counter);
result_counter += bit_counter;
}
return result_counter;
}
void bit_lib_copy_bits(
uint8_t* data,
size_t position,
size_t length,
const uint8_t* source,
size_t source_position) {
for(size_t i = 0; i < length; ++i) {
bit_lib_set_bit(data, position + i, bit_lib_get_bit(source, source_position + i));
}
}
void bit_lib_reverse_bits(uint8_t* data, size_t position, uint8_t length) {
size_t i = 0;
size_t j = length - 1;
while(i < j) {
bool tmp = bit_lib_get_bit(data, position + i);
bit_lib_set_bit(data, position + i, bit_lib_get_bit(data, position + j));
bit_lib_set_bit(data, position + j, tmp);
i++;
j--;
}
}
uint8_t bit_lib_get_bit_count(uint32_t data) {
#if defined __GNUC__
return __builtin_popcountl(data);
#else
#error Please, implement popcount for non-GCC compilers
#endif
}
void bit_lib_print_bits(const uint8_t* data, size_t length) {
for(size_t i = 0; i < length; ++i) {
printf("%u", bit_lib_get_bit(data, i));
}
}
void bit_lib_print_regions(
const BitLibRegion* regions,
size_t region_count,
const uint8_t* data,
size_t length) {
// print data
bit_lib_print_bits(data, length);
printf("\r\n");
// print regions
for(size_t c = 0; c < length; ++c) {
bool print = false;
for(size_t i = 0; i < region_count; i++) {
if(regions[i].start <= c && c < regions[i].start + regions[i].length) {
print = true;
printf("%c", regions[i].mark);
break;
}
}
if(!print) {
printf(" ");
}
}
printf("\r\n");
// print regions data
for(size_t c = 0; c < length; ++c) {
bool print = false;
for(size_t i = 0; i < region_count; i++) {
if(regions[i].start <= c && c < regions[i].start + regions[i].length) {
print = true;
printf("%u", bit_lib_get_bit(data, c));
break;
}
}
if(!print) {
printf(" ");
}
}
printf("\r\n");
}
uint16_t bit_lib_reverse_16_fast(uint16_t data) {
uint16_t result = 0;
result |= (data & 0x8000) >> 15;
result |= (data & 0x4000) >> 13;
result |= (data & 0x2000) >> 11;
result |= (data & 0x1000) >> 9;
result |= (data & 0x0800) >> 7;
result |= (data & 0x0400) >> 5;
result |= (data & 0x0200) >> 3;
result |= (data & 0x0100) >> 1;
result |= (data & 0x0080) << 1;
result |= (data & 0x0040) << 3;
result |= (data & 0x0020) << 5;
result |= (data & 0x0010) << 7;
result |= (data & 0x0008) << 9;
result |= (data & 0x0004) << 11;
result |= (data & 0x0002) << 13;
result |= (data & 0x0001) << 15;
return result;
}
uint16_t bit_lib_crc16(
uint8_t const* data,
size_t data_size,
uint16_t polynom,
uint16_t init,
bool ref_in,
bool ref_out,
uint16_t xor_out) {
uint16_t crc = init;
for(size_t i = 0; i < data_size; ++i) {
uint8_t byte = data[i];
if(ref_in) byte = bit_lib_reverse_16_fast(byte) >> 8;
for(size_t j = 0; j < 8; ++j) {
bool c15 = (crc >> 15 & 1);
bool bit = (byte >> (7 - j) & 1);
crc <<= 1;
if(c15 ^ bit) crc ^= polynom;
}
}
if(ref_out) crc = bit_lib_reverse_16_fast(crc);
crc ^= xor_out;
return crc;
}