diff --git a/lib/lfrfid/protocols/lfrfid_protocols.c b/lib/lfrfid/protocols/lfrfid_protocols.c index 8014c917..c8d8d58c 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_paradox.h" #include "protocol_pac_stanley.h" const ProtocolBase* lfrfid_protocols[] = { @@ -20,5 +21,6 @@ const ProtocolBase* lfrfid_protocols[] = { [LFRFIDProtocolFDXB] = &protocol_fdx_b, [LFRFIDProtocolHidGeneric] = &protocol_hid_generic, [LFRFIDProtocolHidExGeneric] = &protocol_hid_ex_generic, + [LFRFIDProtocolParadox] = &protocol_paradox, [LFRFIDProtocolPACStanley] = &protocol_pac_stanley, }; \ No newline at end of file diff --git a/lib/lfrfid/protocols/lfrfid_protocols.h b/lib/lfrfid/protocols/lfrfid_protocols.h index 2a216412..4f529e65 100644 --- a/lib/lfrfid/protocols/lfrfid_protocols.h +++ b/lib/lfrfid/protocols/lfrfid_protocols.h @@ -17,6 +17,7 @@ typedef enum { LFRFIDProtocolFDXB, LFRFIDProtocolHidGeneric, LFRFIDProtocolHidExGeneric, + LFRFIDProtocolParadox, LFRFIDProtocolPACStanley, LFRFIDProtocolMax, diff --git a/lib/lfrfid/protocols/protocol_paradox.c b/lib/lfrfid/protocols/protocol_paradox.c new file mode 100644 index 00000000..66399a60 --- /dev/null +++ b/lib/lfrfid/protocols/protocol_paradox.c @@ -0,0 +1,201 @@ +#include +#include +#include +#include +#include +#include "lfrfid_protocols.h" + +#define JITTER_TIME (20) +#define MIN_TIME (64 - JITTER_TIME) +#define MAX_TIME (80 + JITTER_TIME) + +#define PARADOX_DECODED_DATA_SIZE (6) + +#define PARADOX_PREAMBLE_LENGTH (8) +#define PARADOX_ENCODED_BIT_SIZE (96) +#define PARADOX_ENCODED_DATA_SIZE (((PARADOX_ENCODED_BIT_SIZE) / 8) + 1) +#define PARADOX_ENCODED_DATA_LAST (PARADOX_ENCODED_DATA_SIZE - 1) + +typedef struct { + FSKDemod* fsk_demod; +} ProtocolParadoxDecoder; + +typedef struct { + FSKOsc* fsk_osc; + uint8_t encoded_index; +} ProtocolParadoxEncoder; + +typedef struct { + ProtocolParadoxDecoder decoder; + ProtocolParadoxEncoder encoder; + uint8_t encoded_data[PARADOX_ENCODED_DATA_SIZE]; + uint8_t data[PARADOX_DECODED_DATA_SIZE]; +} ProtocolParadox; + +ProtocolParadox* protocol_paradox_alloc(void) { + ProtocolParadox* protocol = malloc(sizeof(ProtocolParadox)); + 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_paradox_free(ProtocolParadox* protocol) { + fsk_demod_free(protocol->decoder.fsk_demod); + fsk_osc_free(protocol->encoder.fsk_osc); + free(protocol); +}; + +uint8_t* protocol_paradox_get_data(ProtocolParadox* protocol) { + return protocol->data; +}; + +void protocol_paradox_decoder_start(ProtocolParadox* protocol) { + memset(protocol->encoded_data, 0, PARADOX_ENCODED_DATA_SIZE); +}; + +static bool protocol_paradox_can_be_decoded(ProtocolParadox* protocol) { + // check preamble + if(protocol->encoded_data[0] != 0b00001111 || + protocol->encoded_data[PARADOX_ENCODED_DATA_LAST] != 0b00001111) + return false; + + for(uint32_t i = PARADOX_PREAMBLE_LENGTH; i < 96; i += 2) { + if(bit_lib_get_bit(protocol->encoded_data, i) == + bit_lib_get_bit(protocol->encoded_data, i + 1)) { + return false; + } + } + + return true; +} + +static void protocol_paradox_decode(uint8_t* encoded_data, uint8_t* decoded_data) { + for(uint32_t i = PARADOX_PREAMBLE_LENGTH; i < 96; i += 2) { + if(bit_lib_get_bits(encoded_data, i, 2) == 0b01) { + bit_lib_push_bit(decoded_data, PARADOX_DECODED_DATA_SIZE, 0); + } else if(bit_lib_get_bits(encoded_data, i, 2) == 0b10) { + bit_lib_push_bit(decoded_data, PARADOX_DECODED_DATA_SIZE, 1); + } + } + bit_lib_push_bit(decoded_data, PARADOX_DECODED_DATA_SIZE, 0); + bit_lib_push_bit(decoded_data, PARADOX_DECODED_DATA_SIZE, 0); + bit_lib_push_bit(decoded_data, PARADOX_DECODED_DATA_SIZE, 0); + bit_lib_push_bit(decoded_data, PARADOX_DECODED_DATA_SIZE, 0); +} + +bool protocol_paradox_decoder_feed(ProtocolParadox* protocol, bool level, uint32_t duration) { + bool value; + uint32_t count; + + 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, PARADOX_ENCODED_DATA_SIZE, value); + if(protocol_paradox_can_be_decoded(protocol)) { + protocol_paradox_decode(protocol->encoded_data, protocol->data); + + return true; + } + } + } + + return false; +}; + +static void protocol_paradox_encode(const uint8_t* decoded_data, uint8_t* encoded_data) { + // preamble + bit_lib_set_bits(encoded_data, 0, 0b00001111, 8); + + for(size_t i = 0; i < 44; i++) { + if(bit_lib_get_bit(decoded_data, i)) { + bit_lib_set_bits(encoded_data, PARADOX_PREAMBLE_LENGTH + i * 2, 0b10, 2); + } else { + bit_lib_set_bits(encoded_data, PARADOX_PREAMBLE_LENGTH + i * 2, 0b01, 2); + } + } +}; + +bool protocol_paradox_encoder_start(ProtocolParadox* protocol) { + protocol_paradox_encode(protocol->data, (uint8_t*)protocol->encoded_data); + protocol->encoder.encoded_index = 0; + fsk_osc_reset(protocol->encoder.fsk_osc); + return true; +}; + +LevelDuration protocol_paradox_encoder_yield(ProtocolParadox* protocol) { + bool level; + uint32_t duration; + + bool bit = bit_lib_get_bit(protocol->encoded_data, protocol->encoder.encoded_index); + bool advance = fsk_osc_next_half(protocol->encoder.fsk_osc, bit, &level, &duration); + + if(advance) { + bit_lib_increment_index(protocol->encoder.encoded_index, PARADOX_ENCODED_BIT_SIZE); + } + return level_duration_make(level, duration); +}; + +void protocol_paradox_render_data(ProtocolParadox* protocol, string_t result) { + uint8_t* decoded_data = protocol->data; + 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, "Facility: %u\r\n", fc); + string_cat_printf(result, "Card: %lu\r\n", card_id); + string_cat_printf(result, "Data: "); + for(size_t i = 0; i < PARADOX_DECODED_DATA_SIZE; i++) { + string_cat_printf(result, "%02X", decoded_data[i]); + } +}; + +void protocol_paradox_render_brief_data(ProtocolParadox* protocol, string_t result) { + uint8_t* decoded_data = protocol->data; + + 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); +}; + +bool protocol_paradox_write_data(ProtocolParadox* protocol, void* data) { + LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data; + bool result = false; + + protocol_paradox_encode(protocol->data, (uint8_t*)protocol->encoded_data); + + if(request->write_type == LFRFIDWriteTypeT5577) { + request->t5577.block[0] = LFRFID_T5577_MODULATION_FSK2a | LFRFID_T5577_BITRATE_RF_50 | + (3 << 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.blocks_to_write = 4; + result = true; + } + return result; +}; + +const ProtocolBase protocol_paradox = { + .name = "Paradox", + .manufacturer = "Paradox", + .data_size = PARADOX_DECODED_DATA_SIZE, + .features = LFRFIDFeatureASK, + .validate_count = 3, + .alloc = (ProtocolAlloc)protocol_paradox_alloc, + .free = (ProtocolFree)protocol_paradox_free, + .get_data = (ProtocolGetData)protocol_paradox_get_data, + .decoder = + { + .start = (ProtocolDecoderStart)protocol_paradox_decoder_start, + .feed = (ProtocolDecoderFeed)protocol_paradox_decoder_feed, + }, + .encoder = + { + .start = (ProtocolEncoderStart)protocol_paradox_encoder_start, + .yield = (ProtocolEncoderYield)protocol_paradox_encoder_yield, + }, + .render_data = (ProtocolRenderData)protocol_paradox_render_data, + .render_brief_data = (ProtocolRenderData)protocol_paradox_render_brief_data, + .write_data = (ProtocolWriteData)protocol_paradox_write_data, +}; \ No newline at end of file diff --git a/lib/lfrfid/protocols/protocol_paradox.h b/lib/lfrfid/protocols/protocol_paradox.h new file mode 100644 index 00000000..a80696c3 --- /dev/null +++ b/lib/lfrfid/protocols/protocol_paradox.h @@ -0,0 +1,4 @@ +#pragma once +#include + +extern const ProtocolBase protocol_paradox; \ No newline at end of file