diff --git a/lib/lfrfid/lfrfid_worker_modes.c b/lib/lfrfid/lfrfid_worker_modes.c index 33683589..16936cca 100644 --- a/lib/lfrfid/lfrfid_worker_modes.c +++ b/lib/lfrfid/lfrfid_worker_modes.c @@ -27,16 +27,16 @@ #define LFRFID_WORKER_READ_DROP_TIME_MS 50 #define LFRFID_WORKER_READ_STABILIZE_TIME_MS 450 -#define LFRFID_WORKER_READ_SWITCH_TIME_MS 1500 +#define LFRFID_WORKER_READ_SWITCH_TIME_MS 2000 -#define LFRFID_WORKER_WRITE_VERIFY_TIME_MS 1500 +#define LFRFID_WORKER_WRITE_VERIFY_TIME_MS 2000 #define LFRFID_WORKER_WRITE_DROP_TIME_MS 50 #define LFRFID_WORKER_WRITE_TOO_LONG_TIME_MS 10000 #define LFRFID_WORKER_WRITE_MAX_UNSUCCESSFUL_READS 5 #define LFRFID_WORKER_READ_BUFFER_SIZE 512 -#define LFRFID_WORKER_READ_BUFFER_COUNT 8 +#define LFRFID_WORKER_READ_BUFFER_COUNT 16 #define LFRFID_WORKER_EMULATE_BUFFER_SIZE 1024 @@ -132,6 +132,8 @@ static LFRFIDWorkerReadState lfrfid_worker_read_internal( #ifdef LFRFID_WORKER_READ_DEBUG_GPIO furi_hal_gpio_init_simple(LFRFID_WORKER_READ_DEBUG_GPIO_VALUE, GpioModeOutputPushPull); furi_hal_gpio_init_simple(LFRFID_WORKER_READ_DEBUG_GPIO_LOAD, GpioModeOutputPushPull); + furi_hal_gpio_write(LFRFID_WORKER_READ_DEBUG_GPIO_VALUE, false); + furi_hal_gpio_write(LFRFID_WORKER_READ_DEBUG_GPIO_LOAD, false); #endif LFRFIDWorkerReadContext ctx; @@ -171,10 +173,16 @@ static LFRFIDWorkerReadState lfrfid_worker_read_internal( if(buffer_stream_get_overrun_count(ctx.stream) > 0) { FURI_LOG_E(TAG, "Read overrun, recovering"); buffer_stream_reset(ctx.stream); +#ifdef LFRFID_WORKER_READ_DEBUG_GPIO + furi_hal_gpio_write(LFRFID_WORKER_READ_DEBUG_GPIO_LOAD, false); +#endif continue; } if(buffer == NULL) { +#ifdef LFRFID_WORKER_READ_DEBUG_GPIO + furi_hal_gpio_write(LFRFID_WORKER_READ_DEBUG_GPIO_LOAD, false); +#endif continue; } @@ -261,24 +269,26 @@ static LFRFIDWorkerReadState lfrfid_worker_read_internal( last_read_count = 0; } - string_t string_info; - string_init(string_info); - for(uint8_t i = 0; i < protocol_data_size; i++) { - if(i != 0) { - string_cat_printf(string_info, " "); + if(furi_log_get_level() >= FuriLogLevelDebug) { + string_t string_info; + string_init(string_info); + for(uint8_t i = 0; i < protocol_data_size; i++) { + if(i != 0) { + string_cat_printf(string_info, " "); + } + + string_cat_printf(string_info, "%02X", protocol_data[i]); } - string_cat_printf(string_info, "%02X", protocol_data[i]); + FURI_LOG_D( + TAG, + "%s, %d, [%s]", + protocol_dict_get_name(worker->protocols, protocol), + last_read_count, + string_get_cstr(string_info)); + string_clear(string_info); } - FURI_LOG_D( - TAG, - "%s, %d, [%s]", - protocol_dict_get_name(worker->protocols, protocol), - last_read_count, - string_get_cstr(string_info)); - string_clear(string_info); - protocol_dict_decoders_start(worker->protocols); } } @@ -321,6 +331,8 @@ static LFRFIDWorkerReadState lfrfid_worker_read_internal( free(last_data); #ifdef LFRFID_WORKER_READ_DEBUG_GPIO + furi_hal_gpio_write(LFRFID_WORKER_READ_DEBUG_GPIO_VALUE, false); + furi_hal_gpio_write(LFRFID_WORKER_READ_DEBUG_GPIO_LOAD, false); furi_hal_gpio_init_simple(LFRFID_WORKER_READ_DEBUG_GPIO_VALUE, GpioModeAnalog); furi_hal_gpio_init_simple(LFRFID_WORKER_READ_DEBUG_GPIO_LOAD, GpioModeAnalog); #endif diff --git a/lib/lfrfid/protocols/lfrfid_protocols.c b/lib/lfrfid/protocols/lfrfid_protocols.c index 30ba24e6..fc658610 100644 --- a/lib/lfrfid/protocols/lfrfid_protocols.c +++ b/lib/lfrfid/protocols/lfrfid_protocols.c @@ -8,6 +8,7 @@ #include "protocol_fdx_b.h" #include "protocol_hid_generic.h" #include "protocol_hid_ex_generic.h" +#include "protocol_pyramid.h" #include "protocol_viking.h" #include "protocol_jablotron.h" #include "protocol_paradox.h" @@ -23,6 +24,7 @@ const ProtocolBase* lfrfid_protocols[] = { [LFRFIDProtocolFDXB] = &protocol_fdx_b, [LFRFIDProtocolHidGeneric] = &protocol_hid_generic, [LFRFIDProtocolHidExGeneric] = &protocol_hid_ex_generic, + [LFRFIDProtocolPyramid] = &protocol_pyramid, [LFRFIDProtocolViking] = &protocol_viking, [LFRFIDProtocolJablotron] = &protocol_jablotron, [LFRFIDProtocolParadox] = &protocol_paradox, diff --git a/lib/lfrfid/protocols/lfrfid_protocols.h b/lib/lfrfid/protocols/lfrfid_protocols.h index 710b6e57..210ddd15 100644 --- a/lib/lfrfid/protocols/lfrfid_protocols.h +++ b/lib/lfrfid/protocols/lfrfid_protocols.h @@ -17,6 +17,7 @@ typedef enum { LFRFIDProtocolFDXB, LFRFIDProtocolHidGeneric, LFRFIDProtocolHidExGeneric, + LFRFIDProtocolPyramid, LFRFIDProtocolViking, LFRFIDProtocolJablotron, LFRFIDProtocolParadox, diff --git a/lib/lfrfid/protocols/protocol_awid.c b/lib/lfrfid/protocols/protocol_awid.c index 243b5ede..97c07d7b 100644 --- a/lib/lfrfid/protocols/protocol_awid.c +++ b/lib/lfrfid/protocols/protocol_awid.c @@ -53,7 +53,7 @@ void protocol_awid_decoder_start(ProtocolAwid* protocol) { memset(protocol->encoded_data, 0, AWID_ENCODED_DATA_SIZE); }; -static bool protocol_awid_can_be_decoded(const uint8_t* data) { +static bool protocol_awid_can_be_decoded(uint8_t* data) { bool result = false; // Index map @@ -77,6 +77,12 @@ static bool protocol_awid_can_be_decoded(const uint8_t* data) { bool parity_error = bit_lib_test_parity(data, 8, 88, BitLibParityOdd, 4); if(parity_error) break; + bit_lib_remove_bit_every_nth(data, 8, 88, 4); + + // Avoid detection for invalid formats + uint8_t len = bit_lib_get_bits(data, 8, 8); + if(len != 26 && len != 50 && len != 37 && len != 34) break; + result = true; } while(false); @@ -84,7 +90,6 @@ static bool protocol_awid_can_be_decoded(const uint8_t* data) { } static void protocol_awid_decode(uint8_t* encoded_data, uint8_t* decoded_data) { - bit_lib_remove_bit_every_nth(encoded_data, 8, 88, 4); bit_lib_copy_bits(decoded_data, 0, 66, encoded_data, 8); } diff --git a/lib/lfrfid/protocols/protocol_em4100.c b/lib/lfrfid/protocols/protocol_em4100.c index 92721fcd..17f57421 100644 --- a/lib/lfrfid/protocols/protocol_em4100.c +++ b/lib/lfrfid/protocols/protocol_em4100.c @@ -264,7 +264,7 @@ bool protocol_em4100_write_data(ProtocolEM4100* protocol, void* data) { void protocol_em4100_render_data(ProtocolEM4100* protocol, string_t result) { uint8_t* data = protocol->data; - string_printf(result, "ID: %03u,%05u", data[2], (uint16_t)((data[3] << 8) | (data[4]))); + string_printf(result, "FC: %03u, Card: %05u", data[2], (uint16_t)((data[3] << 8) | (data[4]))); }; const ProtocolBase protocol_em4100 = { diff --git a/lib/lfrfid/protocols/protocol_jablotron.c b/lib/lfrfid/protocols/protocol_jablotron.c index 64b55166..e00b1e59 100644 --- a/lib/lfrfid/protocols/protocol_jablotron.c +++ b/lib/lfrfid/protocols/protocol_jablotron.c @@ -162,12 +162,7 @@ LevelDuration protocol_jablotron_encoder_yield(ProtocolJablotron* protocol) { void protocol_jablotron_render_data(ProtocolJablotron* protocol, string_t result) { uint64_t id = protocol_jablotron_card_id(protocol->data); - string_printf(result, "ID: %llx\r\n", id); -}; - -void protocol_jablotron_render_brief_data(ProtocolJablotron* protocol, string_t result) { - uint64_t id = protocol_jablotron_card_id(protocol->data); - string_printf(result, "ID: %llx\r\n", id); + string_printf(result, "ID: %llX\r\n", id); }; bool protocol_jablotron_write_data(ProtocolJablotron* protocol, void* data) { @@ -207,6 +202,6 @@ const ProtocolBase protocol_jablotron = { .yield = (ProtocolEncoderYield)protocol_jablotron_encoder_yield, }, .render_data = (ProtocolRenderData)protocol_jablotron_render_data, - .render_brief_data = (ProtocolRenderData)protocol_jablotron_render_brief_data, + .render_brief_data = (ProtocolRenderData)protocol_jablotron_render_data, .write_data = (ProtocolWriteData)protocol_jablotron_write_data, }; \ No newline at end of file diff --git a/lib/lfrfid/protocols/protocol_paradox.c b/lib/lfrfid/protocols/protocol_paradox.c index 66399a60..be627600 100644 --- a/lib/lfrfid/protocols/protocol_paradox.c +++ b/lib/lfrfid/protocols/protocol_paradox.c @@ -155,7 +155,7 @@ void protocol_paradox_render_brief_data(ProtocolParadox* protocol, string_t resu uint8_t fc = bit_lib_get_bits(decoded_data, 10, 8); uint16_t card_id = bit_lib_get_bits_16(decoded_data, 18, 16); - string_cat_printf(result, "ID: %03u,%05u", fc, card_id); + string_cat_printf(result, "FC: %03u, Card: %05u", fc, card_id); }; bool protocol_paradox_write_data(ProtocolParadox* protocol, void* data) { diff --git a/lib/lfrfid/protocols/protocol_pyramid.c b/lib/lfrfid/protocols/protocol_pyramid.c new file mode 100644 index 00000000..a0404b48 --- /dev/null +++ b/lib/lfrfid/protocols/protocol_pyramid.c @@ -0,0 +1,277 @@ +#include +#include +#include +#include +#include "lfrfid_protocols.h" +#include + +#define JITTER_TIME (20) +#define MIN_TIME (64 - JITTER_TIME) +#define MAX_TIME (80 + JITTER_TIME) + +#define PYRAMID_DATA_SIZE 13 +#define PYRAMID_PREAMBLE_SIZE 3 + +#define PYRAMID_ENCODED_DATA_SIZE \ + (PYRAMID_PREAMBLE_SIZE + PYRAMID_DATA_SIZE + PYRAMID_PREAMBLE_SIZE) +#define PYRAMID_ENCODED_BIT_SIZE ((PYRAMID_PREAMBLE_SIZE + PYRAMID_DATA_SIZE) * 8) +#define PYRAMID_DECODED_DATA_SIZE (4) +#define PYRAMID_DECODED_BIT_SIZE ((PYRAMID_ENCODED_BIT_SIZE - PYRAMID_PREAMBLE_SIZE * 8) / 2) + +typedef struct { + FSKDemod* fsk_demod; +} ProtocolPyramidDecoder; + +typedef struct { + FSKOsc* fsk_osc; + uint8_t encoded_index; + uint32_t pulse; +} ProtocolPyramidEncoder; + +typedef struct { + ProtocolPyramidDecoder decoder; + ProtocolPyramidEncoder encoder; + uint8_t encoded_data[PYRAMID_ENCODED_DATA_SIZE]; + uint8_t data[PYRAMID_DECODED_DATA_SIZE]; +} ProtocolPyramid; + +ProtocolPyramid* protocol_pyramid_alloc(void) { + ProtocolPyramid* protocol = malloc(sizeof(ProtocolPyramid)); + protocol->decoder.fsk_demod = fsk_demod_alloc(MIN_TIME, 6, MAX_TIME, 5); + protocol->encoder.fsk_osc = fsk_osc_alloc(8, 10, 50); + + return protocol; +}; + +void protocol_pyramid_free(ProtocolPyramid* protocol) { + fsk_demod_free(protocol->decoder.fsk_demod); + fsk_osc_free(protocol->encoder.fsk_osc); + free(protocol); +}; + +uint8_t* protocol_pyramid_get_data(ProtocolPyramid* protocol) { + return protocol->data; +}; + +void protocol_pyramid_decoder_start(ProtocolPyramid* protocol) { + memset(protocol->encoded_data, 0, PYRAMID_ENCODED_DATA_SIZE); +}; + +static bool protocol_pyramid_can_be_decoded(uint8_t* data) { + // check preamble + if(bit_lib_get_bits_16(data, 0, 16) != 0b0000000000000001 || + bit_lib_get_bits(data, 16, 8) != 0b00000001) { + return false; + } + + if(bit_lib_get_bits_16(data, 128, 16) != 0b0000000000000001 || + bit_lib_get_bits(data, 136, 8) != 0b00000001) { + return false; + } + + uint8_t checksum = bit_lib_get_bits(data, 120, 8); + uint8_t checksum_data[13] = {0x00}; + for(uint8_t i = 0; i < 13; i++) { + checksum_data[i] = bit_lib_get_bits(data, 16 + (i * 8), 8); + } + + uint8_t calc_checksum = bit_lib_crc8(checksum_data, 13, 0x31, 0x00, true, true, 0x00); + if(checksum != calc_checksum) return false; + + // Remove parity + bit_lib_remove_bit_every_nth(data, 8, 15 * 8, 8); + + // Determine Startbit and format + int j; + for(j = 0; j < 105; ++j) { + if(bit_lib_get_bit(data, j)) break; + } + uint8_t fmt_len = 105 - j; + + // Only suppport 26bit format for now + if(fmt_len != 26) return false; + + return true; +} + +static void protocol_pyramid_decode(ProtocolPyramid* protocol) { + // Format + bit_lib_set_bits(protocol->data, 0, 26, 8); + + // Facility Code + bit_lib_copy_bits(protocol->data, 8, 8, protocol->encoded_data, 73 + 8); + + // Card Number + bit_lib_copy_bits(protocol->data, 16, 16, protocol->encoded_data, 81 + 8); +} + +bool protocol_pyramid_decoder_feed(ProtocolPyramid* protocol, bool level, uint32_t duration) { + bool value; + uint32_t count; + bool result = false; + + fsk_demod_feed(protocol->decoder.fsk_demod, level, duration, &value, &count); + if(count > 0) { + for(size_t i = 0; i < count; i++) { + bit_lib_push_bit(protocol->encoded_data, PYRAMID_ENCODED_DATA_SIZE, value); + if(protocol_pyramid_can_be_decoded(protocol->encoded_data)) { + protocol_pyramid_decode(protocol); + result = true; + } + } + } + + return result; +}; + +bool protocol_pyramid_get_parity(const uint8_t* bits, uint8_t type, int length) { + int x; + for(x = 0; length > 0; --length) x += bit_lib_get_bit(bits, length - 1); + x %= 2; + return x ^ type; +} + +void protocol_pyramid_add_wiegand_parity( + uint8_t* target, + uint8_t target_position, + uint8_t* source, + uint8_t length) { + bit_lib_set_bit( + target, target_position, protocol_pyramid_get_parity(source, 0 /* even */, length / 2)); + bit_lib_copy_bits(target, target_position + 1, length, source, 0); + bit_lib_set_bit( + target, + target_position + length + 1, + protocol_pyramid_get_parity(source + length / 2, 1 /* odd */, length / 2)); +} + +static void protocol_pyramid_encode(ProtocolPyramid* protocol) { + memset(protocol->encoded_data, 0, sizeof(protocol->encoded_data)); + + uint8_t pre[16]; + memset(pre, 0, sizeof(pre)); + + // Format start bit + bit_lib_set_bit(pre, 79, 1); + + uint8_t wiegand[3]; + memset(wiegand, 0, sizeof(wiegand)); + + // FC + bit_lib_copy_bits(wiegand, 0, 8, protocol->data, 8); + + // CardNum + bit_lib_copy_bits(wiegand, 8, 16, protocol->data, 16); + + // Wiegand parity + protocol_pyramid_add_wiegand_parity(pre, 80, wiegand, 24); + + bit_lib_add_parity(pre, 8, protocol->encoded_data, 8, 102, 8, 1); + + // Add checksum + uint8_t checksum_buffer[13]; + for(uint8_t i = 0; i < 13; i++) + checksum_buffer[i] = bit_lib_get_bits(protocol->encoded_data, 16 + (i * 8), 8); + + uint8_t crc = bit_lib_crc8(checksum_buffer, 13, 0x31, 0x00, true, true, 0x00); + bit_lib_set_bits(protocol->encoded_data, 120, crc, 8); +} + +bool protocol_pyramid_encoder_start(ProtocolPyramid* protocol) { + protocol->encoder.encoded_index = 0; + protocol->encoder.pulse = 0; + protocol_pyramid_encode(protocol); + + return true; +}; + +LevelDuration protocol_pyramid_encoder_yield(ProtocolPyramid* protocol) { + bool level = 0; + uint32_t duration = 0; + + // if pulse is zero, we need to output high, otherwise we need to output low + if(protocol->encoder.pulse == 0) { + // get bit + uint8_t bit = bit_lib_get_bit(protocol->encoded_data, protocol->encoder.encoded_index); + + // get pulse from oscillator + bool advance = fsk_osc_next(protocol->encoder.fsk_osc, bit, &duration); + + if(advance) { + bit_lib_increment_index(protocol->encoder.encoded_index, PYRAMID_ENCODED_BIT_SIZE); + } + + // duration diveded by 2 because we need to output high and low + duration = duration / 2; + protocol->encoder.pulse = duration; + level = true; + } else { + // output low half and reset pulse + duration = protocol->encoder.pulse; + protocol->encoder.pulse = 0; + level = false; + } + + return level_duration_make(level, duration); +}; + +bool protocol_pyramid_write_data(ProtocolPyramid* protocol, void* data) { + LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data; + bool result = false; + + protocol_pyramid_encoder_start(protocol); + + if(request->write_type == LFRFIDWriteTypeT5577) { + request->t5577.block[0] = LFRFID_T5577_MODULATION_FSK2a | LFRFID_T5577_BITRATE_RF_50 | + (4 << LFRFID_T5577_MAXBLOCK_SHIFT); + request->t5577.block[1] = bit_lib_get_bits_32(protocol->encoded_data, 0, 32); + request->t5577.block[2] = bit_lib_get_bits_32(protocol->encoded_data, 32, 32); + request->t5577.block[3] = bit_lib_get_bits_32(protocol->encoded_data, 64, 32); + request->t5577.block[4] = bit_lib_get_bits_32(protocol->encoded_data, 96, 32); + request->t5577.blocks_to_write = 5; + result = true; + } + return result; +}; + +void protocol_pyramid_render_data(ProtocolPyramid* protocol, string_t result) { + uint8_t* decoded_data = protocol->data; + uint8_t format_length = decoded_data[0]; + + string_cat_printf(result, "Format: 26\r\n", format_length); + if(format_length == 26) { + uint8_t facility; + bit_lib_copy_bits(&facility, 0, 8, decoded_data, 8); + + uint16_t card_id; + bit_lib_copy_bits((uint8_t*)&card_id, 8, 8, decoded_data, 16); + bit_lib_copy_bits((uint8_t*)&card_id, 0, 8, decoded_data, 24); + string_cat_printf(result, "FC: %03u, Card: %05u", facility, card_id); + } else { + string_cat_printf(result, "Data: unknown"); + } +}; + +const ProtocolBase protocol_pyramid = { + .name = "Pyramid", + .manufacturer = "Farpointe", + .data_size = PYRAMID_DECODED_DATA_SIZE, + .features = LFRFIDFeatureASK, + .validate_count = 3, + .alloc = (ProtocolAlloc)protocol_pyramid_alloc, + .free = (ProtocolFree)protocol_pyramid_free, + .get_data = (ProtocolGetData)protocol_pyramid_get_data, + .decoder = + { + .start = (ProtocolDecoderStart)protocol_pyramid_decoder_start, + .feed = (ProtocolDecoderFeed)protocol_pyramid_decoder_feed, + }, + .encoder = + { + .start = (ProtocolEncoderStart)protocol_pyramid_encoder_start, + .yield = (ProtocolEncoderYield)protocol_pyramid_encoder_yield, + }, + .render_data = (ProtocolRenderData)protocol_pyramid_render_data, + .render_brief_data = (ProtocolRenderData)protocol_pyramid_render_data, + .write_data = (ProtocolWriteData)protocol_pyramid_write_data, +}; diff --git a/lib/lfrfid/protocols/protocol_pyramid.h b/lib/lfrfid/protocols/protocol_pyramid.h new file mode 100644 index 00000000..940ecaec --- /dev/null +++ b/lib/lfrfid/protocols/protocol_pyramid.h @@ -0,0 +1,4 @@ +#pragma once +#include + +extern const ProtocolBase protocol_pyramid; diff --git a/lib/lfrfid/protocols/protocol_viking.c b/lib/lfrfid/protocols/protocol_viking.c index 7fd72bd9..6119b734 100644 --- a/lib/lfrfid/protocols/protocol_viking.c +++ b/lib/lfrfid/protocols/protocol_viking.c @@ -173,7 +173,7 @@ bool protocol_viking_write_data(ProtocolViking* protocol, void* data) { void protocol_viking_render_data(ProtocolViking* protocol, string_t result) { uint32_t id = bit_lib_get_bits_32(protocol->data, 0, 32); - string_printf(result, "ID: %08lx\r\n", id); + string_printf(result, "ID: %08lX\r\n", id); }; const ProtocolBase protocol_viking = { diff --git a/lib/lfrfid/tools/bit_lib.c b/lib/lfrfid/tools/bit_lib.c index 2df12707..c84f4b7e 100644 --- a/lib/lfrfid/tools/bit_lib.c +++ b/lib/lfrfid/tools/bit_lib.c @@ -129,6 +129,45 @@ bool bit_lib_test_parity( return result; } +size_t bit_lib_add_parity( + const uint8_t* data, + size_t position, + uint8_t* dest, + size_t dest_position, + uint8_t source_length, + uint8_t parity_length, + BitLibParity parity) { + uint32_t parity_word = 0; + size_t j = 0, bit_count = 0; + for(int word = 0; word < source_length; word += parity_length - 1) { + for(int bit = 0; bit < parity_length - 1; bit++) { + parity_word = (parity_word << 1) | bit_lib_get_bit(data, position + word + bit); + bit_lib_set_bit( + dest, dest_position + j++, bit_lib_get_bit(data, position + word + bit)); + } + // if parity fails then return 0 + switch(parity) { + case BitLibParityAlways0: + bit_lib_set_bit(dest, dest_position + j++, 0); + break; // marker bit which should be a 0 + case BitLibParityAlways1: + bit_lib_set_bit(dest, dest_position + j++, 1); + break; // marker bit which should be a 1 + default: + bit_lib_set_bit( + dest, + dest_position + j++, + (bit_lib_test_parity_32(parity_word, BitLibParityOdd) ^ parity) ^ 1); + break; + } + bit_count += parity_length; + parity_word = 0; + } + // if we got here then all the parities passed + // return bit count + return bit_count; +} + 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; @@ -269,6 +308,36 @@ uint8_t bit_lib_reverse_8_fast(uint8_t byte) { return byte; } +uint16_t bit_lib_crc8( + uint8_t const* data, + size_t data_size, + uint8_t polynom, + uint8_t init, + bool ref_in, + bool ref_out, + uint8_t xor_out) { + uint8_t crc = init; + + for(size_t i = 0; i < data_size; ++i) { + uint8_t byte = data[i]; + if(ref_in) bit_lib_reverse_bits(&byte, 0, 8); + crc ^= byte; + + for(size_t j = 8; j > 0; --j) { + if(crc & TOPBIT(8)) { + crc = (crc << 1) ^ polynom; + } else { + crc = (crc << 1); + } + } + } + + if(ref_out) bit_lib_reverse_bits(&crc, 0, 8); + crc ^= xor_out; + + return crc; +} + uint16_t bit_lib_crc16( uint8_t const* data, size_t data_size, diff --git a/lib/lfrfid/tools/bit_lib.h b/lib/lfrfid/tools/bit_lib.h index 482ae36b..1b048db3 100644 --- a/lib/lfrfid/tools/bit_lib.h +++ b/lib/lfrfid/tools/bit_lib.h @@ -7,6 +7,8 @@ extern "C" { #endif +#define TOPBIT(X) (1 << (X - 1)) + typedef enum { BitLibParityEven, BitLibParityOdd, @@ -114,6 +116,27 @@ bool bit_lib_test_parity( BitLibParity parity, uint8_t parity_length); +/** + * @brief Add parity to bit array + * + * @param data Source bit array + * @param position Start position + * @param dest Destination bit array + * @param dest_position Destination position + * @param source_length Source bit count + * @param parity_length Parity block length + * @param parity Parity to test against + * @return size_t + */ +size_t bit_lib_add_parity( + const uint8_t* data, + size_t position, + uint8_t* dest, + size_t dest_position, + uint8_t source_length, + uint8_t parity_length, + BitLibParity parity); + /** * @brief Remove bit every n in array and shift array left. Useful to remove parity. * @@ -202,6 +225,27 @@ uint16_t bit_lib_reverse_16_fast(uint16_t data); */ uint8_t bit_lib_reverse_8_fast(uint8_t byte); +/** + * @brief Slow, but generic CRC8 implementation + * + * @param data + * @param data_size + * @param polynom CRC polynom + * @param init init value + * @param ref_in true if the right bit is older + * @param ref_out true to reverse output + * @param xor_out xor output with this value + * @return uint8_t + */ +uint16_t bit_lib_crc8( + uint8_t const* data, + size_t data_size, + uint8_t polynom, + uint8_t init, + bool ref_in, + bool ref_out, + uint8_t xor_out); + /** * @brief Slow, but generic CRC16 implementation *