[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>
This commit is contained in:
SG
GitHub
22
lib/lfrfid/protocols/lfrfid_protocols.c
Normal file
22
lib/lfrfid/protocols/lfrfid_protocols.c
Normal file
@@ -0,0 +1,22 @@
|
||||
#include "lfrfid_protocols.h"
|
||||
#include "protocol_em4100.h"
|
||||
#include "protocol_h10301.h"
|
||||
#include "protocol_indala26.h"
|
||||
#include "protocol_io_prox_xsf.h"
|
||||
#include "protocol_awid.h"
|
||||
#include "protocol_fdx_a.h"
|
||||
#include "protocol_fdx_b.h"
|
||||
#include "protocol_hid_generic.h"
|
||||
#include "protocol_hid_ex_generic.h"
|
||||
|
||||
const ProtocolBase* lfrfid_protocols[] = {
|
||||
[LFRFIDProtocolEM4100] = &protocol_em4100,
|
||||
[LFRFIDProtocolH10301] = &protocol_h10301,
|
||||
[LFRFIDProtocolIndala26] = &protocol_indala26,
|
||||
[LFRFIDProtocolIOProxXSF] = &protocol_io_prox_xsf,
|
||||
[LFRFIDProtocolAwid] = &protocol_awid,
|
||||
[LFRFIDProtocolFDXA] = &protocol_fdx_a,
|
||||
[LFRFIDProtocolFDXB] = &protocol_fdx_b,
|
||||
[LFRFIDProtocolHidGeneric] = &protocol_hid_generic,
|
||||
[LFRFIDProtocolHidExGeneric] = &protocol_hid_ex_generic,
|
||||
};
|
||||
35
lib/lfrfid/protocols/lfrfid_protocols.h
Normal file
35
lib/lfrfid/protocols/lfrfid_protocols.h
Normal file
@@ -0,0 +1,35 @@
|
||||
#pragma once
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
#include "../tools/t5577.h"
|
||||
|
||||
typedef enum {
|
||||
LFRFIDFeatureASK = 1 << 0, /** ASK Demodulation */
|
||||
LFRFIDFeaturePSK = 1 << 1, /** PSK Demodulation */
|
||||
} LFRFIDFeature;
|
||||
|
||||
typedef enum {
|
||||
LFRFIDProtocolEM4100,
|
||||
LFRFIDProtocolH10301,
|
||||
LFRFIDProtocolIndala26,
|
||||
LFRFIDProtocolIOProxXSF,
|
||||
LFRFIDProtocolAwid,
|
||||
LFRFIDProtocolFDXA,
|
||||
LFRFIDProtocolFDXB,
|
||||
LFRFIDProtocolHidGeneric,
|
||||
LFRFIDProtocolHidExGeneric,
|
||||
|
||||
LFRFIDProtocolMax,
|
||||
} LFRFIDProtocol;
|
||||
|
||||
extern const ProtocolBase* lfrfid_protocols[];
|
||||
|
||||
typedef enum {
|
||||
LFRFIDWriteTypeT5577,
|
||||
} LFRFIDWriteType;
|
||||
|
||||
typedef struct {
|
||||
LFRFIDWriteType write_type;
|
||||
union {
|
||||
LFRFIDT5577 t5577;
|
||||
};
|
||||
} LFRFIDWriteRequest;
|
||||
239
lib/lfrfid/protocols/protocol_awid.c
Normal file
239
lib/lfrfid/protocols/protocol_awid.c
Normal file
@@ -0,0 +1,239 @@
|
||||
#include <furi.h>
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
#include <lfrfid/tools/fsk_demod.h>
|
||||
#include <lfrfid/tools/fsk_osc.h>
|
||||
#include <lfrfid/tools/bit_lib.h>
|
||||
#include "lfrfid_protocols.h"
|
||||
|
||||
#define JITTER_TIME (20)
|
||||
#define MIN_TIME (64 - JITTER_TIME)
|
||||
#define MAX_TIME (80 + JITTER_TIME)
|
||||
|
||||
#define AWID_DECODED_DATA_SIZE (9)
|
||||
|
||||
#define AWID_ENCODED_BIT_SIZE (96)
|
||||
#define AWID_ENCODED_DATA_SIZE (((AWID_ENCODED_BIT_SIZE) / 8) + 1)
|
||||
#define AWID_ENCODED_DATA_LAST (AWID_ENCODED_DATA_SIZE - 1)
|
||||
|
||||
typedef struct {
|
||||
FSKDemod* fsk_demod;
|
||||
} ProtocolAwidDecoder;
|
||||
|
||||
typedef struct {
|
||||
FSKOsc* fsk_osc;
|
||||
uint8_t encoded_index;
|
||||
} ProtocolAwidEncoder;
|
||||
|
||||
typedef struct {
|
||||
ProtocolAwidDecoder decoder;
|
||||
ProtocolAwidEncoder encoder;
|
||||
uint8_t encoded_data[AWID_ENCODED_DATA_SIZE];
|
||||
uint8_t data[AWID_DECODED_DATA_SIZE];
|
||||
} ProtocolAwid;
|
||||
|
||||
ProtocolAwid* protocol_awid_alloc(void) {
|
||||
ProtocolAwid* protocol = malloc(sizeof(ProtocolAwid));
|
||||
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_awid_free(ProtocolAwid* protocol) {
|
||||
fsk_demod_free(protocol->decoder.fsk_demod);
|
||||
fsk_osc_free(protocol->encoder.fsk_osc);
|
||||
free(protocol);
|
||||
};
|
||||
|
||||
uint8_t* protocol_awid_get_data(ProtocolAwid* protocol) {
|
||||
return protocol->data;
|
||||
};
|
||||
|
||||
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) {
|
||||
bool result = false;
|
||||
|
||||
// Index map
|
||||
// 0 10 20 30 40 50 60
|
||||
// | | | | | | |
|
||||
// 01234567 890 1 234 5 678 9 012 3 456 7 890 1 234 5 678 9 012 3 456 7 890 1 234 5 678 9 012 3 - to 96
|
||||
// -----------------------------------------------------------------------------
|
||||
// 00000001 000 1 110 1 101 1 011 1 101 1 010 0 000 1 000 1 010 0 001 0 110 1 100 0 000 1 000 1
|
||||
// preamble bbb o bbb o bbw o fff o fff o ffc o ccc o ccc o ccc o ccc o ccc o wxx o xxx o xxx o - to 96
|
||||
// |---26 bit---| |-----117----||-------------142-------------|
|
||||
// b = format bit len, o = odd parity of last 3 bits
|
||||
// f = facility code, c = card number
|
||||
// w = wiegand parity
|
||||
// (26 bit format shown)
|
||||
|
||||
do {
|
||||
// check preamble and spacing
|
||||
if(data[0] != 0b00000001 || data[AWID_ENCODED_DATA_LAST] != 0b00000001) break;
|
||||
|
||||
// check odd parity for every 4 bits starting from the second byte
|
||||
bool parity_error = bit_lib_test_parity(data, 8, 88, BitLibParityOdd, 4);
|
||||
if(parity_error) break;
|
||||
|
||||
result = true;
|
||||
} while(false);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
bool protocol_awid_decoder_feed(ProtocolAwid* 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, AWID_ENCODED_DATA_SIZE, value);
|
||||
if(protocol_awid_can_be_decoded(protocol->encoded_data)) {
|
||||
protocol_awid_decode(protocol->encoded_data, protocol->data);
|
||||
|
||||
result = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
};
|
||||
|
||||
static void protocol_awid_encode(const uint8_t* decoded_data, uint8_t* encoded_data) {
|
||||
memset(encoded_data, 0, AWID_ENCODED_DATA_SIZE);
|
||||
|
||||
// preamble
|
||||
bit_lib_set_bits(encoded_data, 0, 0b00000001, 8);
|
||||
|
||||
for(size_t i = 0; i < 88 / 4; i++) {
|
||||
uint8_t value = bit_lib_get_bits(decoded_data, i * 3, 3) << 1;
|
||||
value |= bit_lib_test_parity_32(value, BitLibParityOdd);
|
||||
bit_lib_set_bits(encoded_data, 8 + i * 4, value, 4);
|
||||
}
|
||||
};
|
||||
|
||||
bool protocol_awid_encoder_start(ProtocolAwid* protocol) {
|
||||
protocol_awid_encode(protocol->data, (uint8_t*)protocol->encoded_data);
|
||||
protocol->encoder.encoded_index = 0;
|
||||
fsk_osc_reset(protocol->encoder.fsk_osc);
|
||||
return true;
|
||||
};
|
||||
|
||||
LevelDuration protocol_awid_encoder_yield(ProtocolAwid* 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, AWID_ENCODED_BIT_SIZE);
|
||||
}
|
||||
return level_duration_make(level, duration);
|
||||
};
|
||||
|
||||
void protocol_awid_render_data(ProtocolAwid* protocol, string_t result) {
|
||||
// Index map
|
||||
// 0 10 20 30 40 50 60
|
||||
// | | | | | | |
|
||||
// 01234567 8 90123456 7890123456789012 3 456789012345678901234567890123456
|
||||
// ------------------------------------------------------------------------
|
||||
// 00011010 1 01110101 0000000010001110 1 000000000000000000000000000000000
|
||||
// bbbbbbbb w ffffffff cccccccccccccccc w xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
|
||||
// |26 bit| |-117--| |-----142------|
|
||||
// b = format bit len, o = odd parity of last 3 bits
|
||||
// f = facility code, c = card number
|
||||
// w = wiegand parity
|
||||
// (26 bit format shown)
|
||||
|
||||
uint8_t* decoded_data = protocol->data;
|
||||
uint8_t format_length = decoded_data[0];
|
||||
|
||||
string_cat_printf(result, "Format: %d\r\n", format_length);
|
||||
if(format_length == 26) {
|
||||
uint8_t facility;
|
||||
bit_lib_copy_bits(&facility, 0, 8, decoded_data, 9);
|
||||
|
||||
uint16_t card_id;
|
||||
bit_lib_copy_bits((uint8_t*)&card_id, 8, 8, decoded_data, 17);
|
||||
bit_lib_copy_bits((uint8_t*)&card_id, 0, 8, decoded_data, 25);
|
||||
string_cat_printf(result, "Facility: %d\r\n", facility);
|
||||
string_cat_printf(result, "Card: %d", card_id);
|
||||
} else {
|
||||
// print 66 bits as hex
|
||||
string_cat_printf(result, "Data: ");
|
||||
for(size_t i = 0; i < AWID_DECODED_DATA_SIZE; i++) {
|
||||
string_cat_printf(result, "%02X", decoded_data[i]);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
void protocol_awid_render_brief_data(ProtocolAwid* protocol, string_t result) {
|
||||
uint8_t* decoded_data = protocol->data;
|
||||
uint8_t format_length = decoded_data[0];
|
||||
|
||||
string_cat_printf(result, "Format: %d\r\n", format_length);
|
||||
if(format_length == 26) {
|
||||
uint8_t facility;
|
||||
bit_lib_copy_bits(&facility, 0, 8, decoded_data, 9);
|
||||
|
||||
uint16_t card_id;
|
||||
bit_lib_copy_bits((uint8_t*)&card_id, 8, 8, decoded_data, 17);
|
||||
bit_lib_copy_bits((uint8_t*)&card_id, 0, 8, decoded_data, 25);
|
||||
string_cat_printf(result, "ID: %03u,%05u", facility, card_id);
|
||||
} else {
|
||||
string_cat_printf(result, "Data: unknown");
|
||||
}
|
||||
};
|
||||
|
||||
bool protocol_awid_write_data(ProtocolAwid* protocol, void* data) {
|
||||
LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data;
|
||||
bool result = false;
|
||||
|
||||
protocol_awid_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_awid = {
|
||||
.name = "AWID",
|
||||
.manufacturer = "AWIG",
|
||||
.data_size = AWID_DECODED_DATA_SIZE,
|
||||
.features = LFRFIDFeatureASK,
|
||||
.validate_count = 3,
|
||||
.alloc = (ProtocolAlloc)protocol_awid_alloc,
|
||||
.free = (ProtocolFree)protocol_awid_free,
|
||||
.get_data = (ProtocolGetData)protocol_awid_get_data,
|
||||
.decoder =
|
||||
{
|
||||
.start = (ProtocolDecoderStart)protocol_awid_decoder_start,
|
||||
.feed = (ProtocolDecoderFeed)protocol_awid_decoder_feed,
|
||||
},
|
||||
.encoder =
|
||||
{
|
||||
.start = (ProtocolEncoderStart)protocol_awid_encoder_start,
|
||||
.yield = (ProtocolEncoderYield)protocol_awid_encoder_yield,
|
||||
},
|
||||
.render_data = (ProtocolRenderData)protocol_awid_render_data,
|
||||
.render_brief_data = (ProtocolRenderData)protocol_awid_render_brief_data,
|
||||
.write_data = (ProtocolWriteData)protocol_awid_write_data,
|
||||
};
|
||||
4
lib/lfrfid/protocols/protocol_awid.h
Normal file
4
lib/lfrfid/protocols/protocol_awid.h
Normal file
@@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
|
||||
extern const ProtocolBase protocol_awid;
|
||||
292
lib/lfrfid/protocols/protocol_em4100.c
Normal file
292
lib/lfrfid/protocols/protocol_em4100.c
Normal file
@@ -0,0 +1,292 @@
|
||||
#include <furi.h>
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
#include <toolbox/manchester_decoder.h>
|
||||
#include "lfrfid_protocols.h"
|
||||
|
||||
typedef uint64_t EM4100DecodedData;
|
||||
|
||||
#define EM_HEADER_POS (55)
|
||||
#define EM_HEADER_MASK (0x1FFLLU << EM_HEADER_POS)
|
||||
|
||||
#define EM_FIRST_ROW_POS (50)
|
||||
|
||||
#define EM_ROW_COUNT (10)
|
||||
#define EM_COLUMN_COUNT (4)
|
||||
#define EM_BITS_PER_ROW_COUNT (EM_COLUMN_COUNT + 1)
|
||||
|
||||
#define EM_COLUMN_POS (4)
|
||||
#define EM_STOP_POS (0)
|
||||
#define EM_STOP_MASK (0x1LLU << EM_STOP_POS)
|
||||
|
||||
#define EM_HEADER_AND_STOP_MASK (EM_HEADER_MASK | EM_STOP_MASK)
|
||||
#define EM_HEADER_AND_STOP_DATA (EM_HEADER_MASK)
|
||||
|
||||
#define EM4100_DECODED_DATA_SIZE (5)
|
||||
#define EM4100_ENCODED_DATA_SIZE (sizeof(EM4100DecodedData))
|
||||
|
||||
#define EM4100_CLOCK_PER_BIT (64)
|
||||
|
||||
#define EM_READ_SHORT_TIME (256)
|
||||
#define EM_READ_LONG_TIME (512)
|
||||
#define EM_READ_JITTER_TIME (100)
|
||||
|
||||
#define EM_READ_SHORT_TIME_LOW (EM_READ_SHORT_TIME - EM_READ_JITTER_TIME)
|
||||
#define EM_READ_SHORT_TIME_HIGH (EM_READ_SHORT_TIME + EM_READ_JITTER_TIME)
|
||||
#define EM_READ_LONG_TIME_LOW (EM_READ_LONG_TIME - EM_READ_JITTER_TIME)
|
||||
#define EM_READ_LONG_TIME_HIGH (EM_READ_LONG_TIME + EM_READ_JITTER_TIME)
|
||||
|
||||
typedef struct {
|
||||
uint8_t data[EM4100_DECODED_DATA_SIZE];
|
||||
|
||||
EM4100DecodedData encoded_data;
|
||||
uint8_t encoded_data_index;
|
||||
bool encoded_polarity;
|
||||
|
||||
ManchesterState decoder_manchester_state;
|
||||
} ProtocolEM4100;
|
||||
|
||||
ProtocolEM4100* protocol_em4100_alloc(void) {
|
||||
ProtocolEM4100* proto = malloc(sizeof(ProtocolEM4100));
|
||||
return (void*)proto;
|
||||
};
|
||||
|
||||
void protocol_em4100_free(ProtocolEM4100* proto) {
|
||||
free(proto);
|
||||
};
|
||||
|
||||
uint8_t* protocol_em4100_get_data(ProtocolEM4100* proto) {
|
||||
return proto->data;
|
||||
};
|
||||
|
||||
static void em4100_decode(
|
||||
const uint8_t* encoded_data,
|
||||
const uint8_t encoded_data_size,
|
||||
uint8_t* decoded_data,
|
||||
const uint8_t decoded_data_size) {
|
||||
furi_check(decoded_data_size >= EM4100_DECODED_DATA_SIZE);
|
||||
furi_check(encoded_data_size >= EM4100_ENCODED_DATA_SIZE);
|
||||
|
||||
uint8_t decoded_data_index = 0;
|
||||
EM4100DecodedData card_data = *((EM4100DecodedData*)(encoded_data));
|
||||
|
||||
// clean result
|
||||
memset(decoded_data, 0, decoded_data_size);
|
||||
|
||||
// header
|
||||
for(uint8_t i = 0; i < 9; i++) {
|
||||
card_data = card_data << 1;
|
||||
}
|
||||
|
||||
// nibbles
|
||||
uint8_t value = 0;
|
||||
for(uint8_t r = 0; r < EM_ROW_COUNT; r++) {
|
||||
uint8_t nibble = 0;
|
||||
for(uint8_t i = 0; i < 5; i++) {
|
||||
if(i < 4) nibble = (nibble << 1) | (card_data & (1LLU << 63) ? 1 : 0);
|
||||
card_data = card_data << 1;
|
||||
}
|
||||
value = (value << 4) | nibble;
|
||||
if(r % 2) {
|
||||
decoded_data[decoded_data_index] |= value;
|
||||
decoded_data_index++;
|
||||
value = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool em4100_can_be_decoded(const uint8_t* encoded_data, const uint8_t encoded_data_size) {
|
||||
furi_check(encoded_data_size >= EM4100_ENCODED_DATA_SIZE);
|
||||
const EM4100DecodedData* card_data = (EM4100DecodedData*)encoded_data;
|
||||
|
||||
// check header and stop bit
|
||||
if((*card_data & EM_HEADER_AND_STOP_MASK) != EM_HEADER_AND_STOP_DATA) return false;
|
||||
|
||||
// check row parity
|
||||
for(uint8_t i = 0; i < EM_ROW_COUNT; i++) {
|
||||
uint8_t parity_sum = 0;
|
||||
|
||||
for(uint8_t j = 0; j < EM_BITS_PER_ROW_COUNT; j++) {
|
||||
parity_sum += (*card_data >> (EM_FIRST_ROW_POS - i * EM_BITS_PER_ROW_COUNT + j)) & 1;
|
||||
}
|
||||
|
||||
if((parity_sum % 2)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// check columns parity
|
||||
for(uint8_t i = 0; i < EM_COLUMN_COUNT; i++) {
|
||||
uint8_t parity_sum = 0;
|
||||
|
||||
for(uint8_t j = 0; j < EM_ROW_COUNT + 1; j++) {
|
||||
parity_sum += (*card_data >> (EM_COLUMN_POS - i + j * EM_BITS_PER_ROW_COUNT)) & 1;
|
||||
}
|
||||
|
||||
if((parity_sum % 2)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void protocol_em4100_decoder_start(ProtocolEM4100* proto) {
|
||||
memset(proto->data, 0, EM4100_DECODED_DATA_SIZE);
|
||||
proto->encoded_data = 0;
|
||||
manchester_advance(
|
||||
proto->decoder_manchester_state,
|
||||
ManchesterEventReset,
|
||||
&proto->decoder_manchester_state,
|
||||
NULL);
|
||||
};
|
||||
|
||||
bool protocol_em4100_decoder_feed(ProtocolEM4100* proto, bool level, uint32_t duration) {
|
||||
bool result = false;
|
||||
|
||||
ManchesterEvent event = ManchesterEventReset;
|
||||
|
||||
if(duration > EM_READ_SHORT_TIME_LOW && duration < EM_READ_SHORT_TIME_HIGH) {
|
||||
if(!level) {
|
||||
event = ManchesterEventShortHigh;
|
||||
} else {
|
||||
event = ManchesterEventShortLow;
|
||||
}
|
||||
} else if(duration > EM_READ_LONG_TIME_LOW && duration < EM_READ_LONG_TIME_HIGH) {
|
||||
if(!level) {
|
||||
event = ManchesterEventLongHigh;
|
||||
} else {
|
||||
event = ManchesterEventLongLow;
|
||||
}
|
||||
}
|
||||
|
||||
if(event != ManchesterEventReset) {
|
||||
bool data;
|
||||
bool data_ok = manchester_advance(
|
||||
proto->decoder_manchester_state, event, &proto->decoder_manchester_state, &data);
|
||||
|
||||
if(data_ok) {
|
||||
proto->encoded_data = (proto->encoded_data << 1) | data;
|
||||
|
||||
if(em4100_can_be_decoded((uint8_t*)&proto->encoded_data, sizeof(EM4100DecodedData))) {
|
||||
em4100_decode(
|
||||
(uint8_t*)&proto->encoded_data,
|
||||
sizeof(EM4100DecodedData),
|
||||
proto->data,
|
||||
EM4100_DECODED_DATA_SIZE);
|
||||
result = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
};
|
||||
|
||||
static void em4100_write_nibble(bool low_nibble, uint8_t data, EM4100DecodedData* encoded_data) {
|
||||
uint8_t parity_sum = 0;
|
||||
uint8_t start = 0;
|
||||
if(!low_nibble) start = 4;
|
||||
|
||||
for(int8_t i = (start + 3); i >= start; i--) {
|
||||
parity_sum += (data >> i) & 1;
|
||||
*encoded_data = (*encoded_data << 1) | ((data >> i) & 1);
|
||||
}
|
||||
|
||||
*encoded_data = (*encoded_data << 1) | ((parity_sum % 2) & 1);
|
||||
}
|
||||
|
||||
bool protocol_em4100_encoder_start(ProtocolEM4100* proto) {
|
||||
// header
|
||||
proto->encoded_data = 0b111111111;
|
||||
|
||||
// data
|
||||
for(uint8_t i = 0; i < EM4100_DECODED_DATA_SIZE; i++) {
|
||||
em4100_write_nibble(false, proto->data[i], &proto->encoded_data);
|
||||
em4100_write_nibble(true, proto->data[i], &proto->encoded_data);
|
||||
}
|
||||
|
||||
// column parity and stop bit
|
||||
uint8_t parity_sum;
|
||||
|
||||
for(uint8_t c = 0; c < EM_COLUMN_COUNT; c++) {
|
||||
parity_sum = 0;
|
||||
for(uint8_t i = 1; i <= EM_ROW_COUNT; i++) {
|
||||
uint8_t parity_bit = (proto->encoded_data >> (i * EM_BITS_PER_ROW_COUNT - 1)) & 1;
|
||||
parity_sum += parity_bit;
|
||||
}
|
||||
proto->encoded_data = (proto->encoded_data << 1) | ((parity_sum % 2) & 1);
|
||||
}
|
||||
|
||||
// stop bit
|
||||
proto->encoded_data = (proto->encoded_data << 1) | 0;
|
||||
|
||||
proto->encoded_data_index = 0;
|
||||
proto->encoded_polarity = true;
|
||||
|
||||
return true;
|
||||
};
|
||||
|
||||
LevelDuration protocol_em4100_encoder_yield(ProtocolEM4100* proto) {
|
||||
bool level = (proto->encoded_data >> (63 - proto->encoded_data_index)) & 1;
|
||||
uint32_t duration = EM4100_CLOCK_PER_BIT / 2;
|
||||
|
||||
if(proto->encoded_polarity) {
|
||||
proto->encoded_polarity = false;
|
||||
} else {
|
||||
level = !level;
|
||||
|
||||
proto->encoded_polarity = true;
|
||||
proto->encoded_data_index++;
|
||||
if(proto->encoded_data_index >= 64) {
|
||||
proto->encoded_data_index = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return level_duration_make(level, duration);
|
||||
};
|
||||
|
||||
bool protocol_em4100_write_data(ProtocolEM4100* protocol, void* data) {
|
||||
LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data;
|
||||
bool result = false;
|
||||
|
||||
protocol_em4100_encoder_start(protocol);
|
||||
|
||||
if(request->write_type == LFRFIDWriteTypeT5577) {
|
||||
request->t5577.block[0] =
|
||||
(LFRFID_T5577_MODULATION_MANCHESTER | LFRFID_T5577_BITRATE_RF_64 |
|
||||
(2 << LFRFID_T5577_MAXBLOCK_SHIFT));
|
||||
request->t5577.block[1] = protocol->encoded_data;
|
||||
request->t5577.block[2] = protocol->encoded_data >> 32;
|
||||
request->t5577.blocks_to_write = 3;
|
||||
result = true;
|
||||
}
|
||||
return result;
|
||||
};
|
||||
|
||||
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])));
|
||||
};
|
||||
|
||||
const ProtocolBase protocol_em4100 = {
|
||||
.name = "EM4100",
|
||||
.manufacturer = "EM-Micro",
|
||||
.data_size = EM4100_DECODED_DATA_SIZE,
|
||||
.features = LFRFIDFeatureASK | LFRFIDFeaturePSK,
|
||||
.validate_count = 3,
|
||||
.alloc = (ProtocolAlloc)protocol_em4100_alloc,
|
||||
.free = (ProtocolFree)protocol_em4100_free,
|
||||
.get_data = (ProtocolGetData)protocol_em4100_get_data,
|
||||
.decoder =
|
||||
{
|
||||
.start = (ProtocolDecoderStart)protocol_em4100_decoder_start,
|
||||
.feed = (ProtocolDecoderFeed)protocol_em4100_decoder_feed,
|
||||
},
|
||||
.encoder =
|
||||
{
|
||||
.start = (ProtocolEncoderStart)protocol_em4100_encoder_start,
|
||||
.yield = (ProtocolEncoderYield)protocol_em4100_encoder_yield,
|
||||
},
|
||||
.render_data = (ProtocolRenderData)protocol_em4100_render_data,
|
||||
.render_brief_data = (ProtocolRenderData)protocol_em4100_render_data,
|
||||
.write_data = (ProtocolWriteData)protocol_em4100_write_data,
|
||||
};
|
||||
4
lib/lfrfid/protocols/protocol_em4100.h
Normal file
4
lib/lfrfid/protocols/protocol_em4100.h
Normal file
@@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
|
||||
extern const ProtocolBase protocol_em4100;
|
||||
239
lib/lfrfid/protocols/protocol_fdx_a.c
Normal file
239
lib/lfrfid/protocols/protocol_fdx_a.c
Normal file
@@ -0,0 +1,239 @@
|
||||
#include <furi.h>
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
#include <lfrfid/tools/fsk_demod.h>
|
||||
#include <lfrfid/tools/fsk_osc.h>
|
||||
#include "lfrfid_protocols.h"
|
||||
#include <lfrfid/tools/bit_lib.h>
|
||||
|
||||
#define JITTER_TIME (20)
|
||||
#define MIN_TIME (64 - JITTER_TIME)
|
||||
#define MAX_TIME (80 + JITTER_TIME)
|
||||
|
||||
#define FDXA_DATA_SIZE 10
|
||||
#define FDXA_PREAMBLE_SIZE 2
|
||||
|
||||
#define FDXA_ENCODED_DATA_SIZE (FDXA_PREAMBLE_SIZE + FDXA_DATA_SIZE + FDXA_PREAMBLE_SIZE)
|
||||
#define FDXA_ENCODED_BIT_SIZE ((FDXA_PREAMBLE_SIZE + FDXA_DATA_SIZE) * 8)
|
||||
#define FDXA_DECODED_DATA_SIZE (5)
|
||||
#define FDXA_DECODED_BIT_SIZE ((FDXA_ENCODED_BIT_SIZE - FDXA_PREAMBLE_SIZE * 8) / 2)
|
||||
|
||||
#define FDXA_PREAMBLE_0 0x55
|
||||
#define FDXA_PREAMBLE_1 0x1D
|
||||
|
||||
typedef struct {
|
||||
FSKDemod* fsk_demod;
|
||||
} ProtocolFDXADecoder;
|
||||
|
||||
typedef struct {
|
||||
FSKOsc* fsk_osc;
|
||||
uint8_t encoded_index;
|
||||
uint32_t pulse;
|
||||
} ProtocolFDXAEncoder;
|
||||
|
||||
typedef struct {
|
||||
ProtocolFDXADecoder decoder;
|
||||
ProtocolFDXAEncoder encoder;
|
||||
uint8_t encoded_data[FDXA_ENCODED_DATA_SIZE];
|
||||
uint8_t data[FDXA_DECODED_DATA_SIZE];
|
||||
size_t protocol_size;
|
||||
} ProtocolFDXA;
|
||||
|
||||
ProtocolFDXA* protocol_fdx_a_alloc(void) {
|
||||
ProtocolFDXA* protocol = malloc(sizeof(ProtocolFDXA));
|
||||
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_fdx_a_free(ProtocolFDXA* protocol) {
|
||||
fsk_demod_free(protocol->decoder.fsk_demod);
|
||||
fsk_osc_free(protocol->encoder.fsk_osc);
|
||||
free(protocol);
|
||||
};
|
||||
|
||||
uint8_t* protocol_fdx_a_get_data(ProtocolFDXA* protocol) {
|
||||
return protocol->data;
|
||||
};
|
||||
|
||||
void protocol_fdx_a_decoder_start(ProtocolFDXA* protocol) {
|
||||
memset(protocol->encoded_data, 0, FDXA_ENCODED_DATA_SIZE);
|
||||
};
|
||||
|
||||
static bool protocol_fdx_a_decode(const uint8_t* from, uint8_t* to) {
|
||||
size_t bit_index = 0;
|
||||
for(size_t i = FDXA_PREAMBLE_SIZE; i < (FDXA_PREAMBLE_SIZE + FDXA_DATA_SIZE); i++) {
|
||||
for(size_t n = 0; n < 4; n++) {
|
||||
uint8_t bit_pair = (from[i] >> (6 - (n * 2))) & 0b11;
|
||||
if(bit_pair == 0b01) {
|
||||
bit_lib_set_bit(to, bit_index, 0);
|
||||
} else if(bit_pair == 0b10) {
|
||||
bit_lib_set_bit(to, bit_index, 1);
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
bit_index++;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool protocol_fdx_a_can_be_decoded(const uint8_t* data) {
|
||||
// check preamble
|
||||
if(data[0] != FDXA_PREAMBLE_0 || data[1] != FDXA_PREAMBLE_1 || data[12] != FDXA_PREAMBLE_0 ||
|
||||
data[13] != FDXA_PREAMBLE_1) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// check for manchester encoding
|
||||
uint8_t decoded_data[FDXA_DECODED_DATA_SIZE];
|
||||
if(!protocol_fdx_a_decode(data, decoded_data)) return false;
|
||||
|
||||
uint8_t parity_sum = 0;
|
||||
for(size_t i = 0; i < FDXA_DECODED_DATA_SIZE; i++) {
|
||||
parity_sum += bit_lib_test_parity_32(decoded_data[i], BitLibParityOdd);
|
||||
decoded_data[i] &= 0x7F;
|
||||
}
|
||||
|
||||
return (parity_sum == 0);
|
||||
}
|
||||
|
||||
bool protocol_fdx_a_decoder_feed(ProtocolFDXA* 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, FDXA_ENCODED_DATA_SIZE, value);
|
||||
if(protocol_fdx_a_can_be_decoded(protocol->encoded_data)) {
|
||||
protocol_fdx_a_decode(protocol->encoded_data, protocol->data);
|
||||
result = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
};
|
||||
|
||||
static void protocol_fdx_a_encode(ProtocolFDXA* protocol) {
|
||||
protocol->encoded_data[0] = FDXA_PREAMBLE_0;
|
||||
protocol->encoded_data[1] = FDXA_PREAMBLE_1;
|
||||
|
||||
size_t bit_index = 0;
|
||||
for(size_t i = 0; i < FDXA_DECODED_BIT_SIZE; i++) {
|
||||
bool bit = bit_lib_get_bit(protocol->data, i);
|
||||
if(bit) {
|
||||
bit_lib_set_bit(protocol->encoded_data, 16 + bit_index, 1);
|
||||
bit_lib_set_bit(protocol->encoded_data, 16 + bit_index + 1, 0);
|
||||
} else {
|
||||
bit_lib_set_bit(protocol->encoded_data, 16 + bit_index, 0);
|
||||
bit_lib_set_bit(protocol->encoded_data, 16 + bit_index + 1, 1);
|
||||
}
|
||||
bit_index += 2;
|
||||
}
|
||||
}
|
||||
|
||||
bool protocol_fdx_a_encoder_start(ProtocolFDXA* protocol) {
|
||||
protocol->encoder.encoded_index = 0;
|
||||
protocol->encoder.pulse = 0;
|
||||
protocol_fdx_a_encode(protocol);
|
||||
|
||||
return true;
|
||||
};
|
||||
|
||||
LevelDuration protocol_fdx_a_encoder_yield(ProtocolFDXA* 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, FDXA_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_fdx_a_write_data(ProtocolFDXA* protocol, void* data) {
|
||||
LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data;
|
||||
bool result = false;
|
||||
|
||||
protocol_fdx_a_encoder_start(protocol);
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
void protocol_fdx_a_render_data(ProtocolFDXA* protocol, string_t result) {
|
||||
uint8_t data[FDXA_DECODED_DATA_SIZE];
|
||||
memcpy(data, protocol->data, FDXA_DECODED_DATA_SIZE);
|
||||
|
||||
uint8_t parity_sum = 0;
|
||||
for(size_t i = 0; i < FDXA_DECODED_DATA_SIZE; i++) {
|
||||
parity_sum += bit_lib_test_parity_32(data[i], BitLibParityOdd);
|
||||
data[i] &= 0x7F;
|
||||
}
|
||||
|
||||
string_printf(
|
||||
result,
|
||||
"ID: %02X%02X%02X%02X%02X\r\n"
|
||||
"Parity: %s",
|
||||
data[0],
|
||||
data[1],
|
||||
data[2],
|
||||
data[3],
|
||||
data[4],
|
||||
parity_sum == 0 ? "+" : "-");
|
||||
};
|
||||
|
||||
const ProtocolBase protocol_fdx_a = {
|
||||
.name = "FDX-A",
|
||||
.manufacturer = "FECAVA",
|
||||
.data_size = FDXA_DECODED_DATA_SIZE,
|
||||
.features = LFRFIDFeatureASK,
|
||||
.validate_count = 3,
|
||||
.alloc = (ProtocolAlloc)protocol_fdx_a_alloc,
|
||||
.free = (ProtocolFree)protocol_fdx_a_free,
|
||||
.get_data = (ProtocolGetData)protocol_fdx_a_get_data,
|
||||
.decoder =
|
||||
{
|
||||
.start = (ProtocolDecoderStart)protocol_fdx_a_decoder_start,
|
||||
.feed = (ProtocolDecoderFeed)protocol_fdx_a_decoder_feed,
|
||||
},
|
||||
.encoder =
|
||||
{
|
||||
.start = (ProtocolEncoderStart)protocol_fdx_a_encoder_start,
|
||||
.yield = (ProtocolEncoderYield)protocol_fdx_a_encoder_yield,
|
||||
},
|
||||
.render_data = (ProtocolRenderData)protocol_fdx_a_render_data,
|
||||
.render_brief_data = (ProtocolRenderData)protocol_fdx_a_render_data,
|
||||
.write_data = (ProtocolWriteData)protocol_fdx_a_write_data,
|
||||
};
|
||||
4
lib/lfrfid/protocols/protocol_fdx_a.h
Normal file
4
lib/lfrfid/protocols/protocol_fdx_a.h
Normal file
@@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
|
||||
extern const ProtocolBase protocol_fdx_a;
|
||||
374
lib/lfrfid/protocols/protocol_fdx_b.c
Normal file
374
lib/lfrfid/protocols/protocol_fdx_b.c
Normal file
@@ -0,0 +1,374 @@
|
||||
#include <furi.h>
|
||||
#include "toolbox/level_duration.h"
|
||||
#include "protocol_fdx_b.h"
|
||||
#include <toolbox/manchester_decoder.h>
|
||||
#include <lfrfid/tools/bit_lib.h>
|
||||
#include "lfrfid_protocols.h"
|
||||
|
||||
#define FDX_B_ENCODED_BIT_SIZE (128)
|
||||
#define FDX_B_ENCODED_BYTE_SIZE (((FDX_B_ENCODED_BIT_SIZE) / 8))
|
||||
#define FDX_B_PREAMBLE_BIT_SIZE (11)
|
||||
#define FDX_B_PREAMBLE_BYTE_SIZE (2)
|
||||
#define FDX_B_ENCODED_BYTE_FULL_SIZE (FDX_B_ENCODED_BYTE_SIZE + FDX_B_PREAMBLE_BYTE_SIZE)
|
||||
|
||||
#define FDXB_DECODED_DATA_SIZE (11)
|
||||
|
||||
#define FDX_B_SHORT_TIME (128)
|
||||
#define FDX_B_LONG_TIME (256)
|
||||
#define FDX_B_JITTER_TIME (60)
|
||||
|
||||
#define FDX_B_SHORT_TIME_LOW (FDX_B_SHORT_TIME - FDX_B_JITTER_TIME)
|
||||
#define FDX_B_SHORT_TIME_HIGH (FDX_B_SHORT_TIME + FDX_B_JITTER_TIME)
|
||||
#define FDX_B_LONG_TIME_LOW (FDX_B_LONG_TIME - FDX_B_JITTER_TIME)
|
||||
#define FDX_B_LONG_TIME_HIGH (FDX_B_LONG_TIME + FDX_B_JITTER_TIME)
|
||||
|
||||
typedef struct {
|
||||
bool last_short;
|
||||
bool last_level;
|
||||
size_t encoded_index;
|
||||
uint8_t encoded_data[FDX_B_ENCODED_BYTE_FULL_SIZE];
|
||||
uint8_t data[FDXB_DECODED_DATA_SIZE];
|
||||
} ProtocolFDXB;
|
||||
|
||||
ProtocolFDXB* protocol_fdx_b_alloc(void) {
|
||||
ProtocolFDXB* protocol = malloc(sizeof(ProtocolFDXB));
|
||||
return protocol;
|
||||
};
|
||||
|
||||
void protocol_fdx_b_free(ProtocolFDXB* protocol) {
|
||||
free(protocol);
|
||||
};
|
||||
|
||||
uint8_t* protocol_fdx_b_get_data(ProtocolFDXB* proto) {
|
||||
return proto->data;
|
||||
};
|
||||
|
||||
void protocol_fdx_b_decoder_start(ProtocolFDXB* protocol) {
|
||||
memset(protocol->encoded_data, 0, FDX_B_ENCODED_BYTE_FULL_SIZE);
|
||||
protocol->last_short = false;
|
||||
};
|
||||
|
||||
static bool protocol_fdx_b_can_be_decoded(ProtocolFDXB* protocol) {
|
||||
bool result = false;
|
||||
|
||||
/*
|
||||
msb lsb
|
||||
0 10000000000 Header pattern. 11 bits.
|
||||
11 1nnnnnnnn
|
||||
20 1nnnnnnnn 38 bit (12 digit) National code.
|
||||
29 1nnnnnnnn eg. 000000001008 (decimal).
|
||||
38 1nnnnnnnn
|
||||
47 1nnnnnncc 10 bit (3 digit) Country code.
|
||||
56 1cccccccc eg. 999 (decimal).
|
||||
65 1s------- 1 bit data block status flag.
|
||||
74 1-------a 1 bit animal application indicator.
|
||||
83 1xxxxxxxx 16 bit checksum.
|
||||
92 1xxxxxxxx
|
||||
101 1eeeeeeee 24 bits of extra data if present.
|
||||
110 1eeeeeeee eg. $123456.
|
||||
119 1eeeeeeee
|
||||
*/
|
||||
|
||||
do {
|
||||
// check 11 bits preamble
|
||||
if(bit_lib_get_bits_16(protocol->encoded_data, 0, 11) != 0b10000000000) break;
|
||||
// check next 11 bits preamble
|
||||
if(bit_lib_get_bits_16(protocol->encoded_data, 128, 11) != 0b10000000000) break;
|
||||
// check control bits
|
||||
if(!bit_lib_test_parity(protocol->encoded_data, 3, 13 * 9, BitLibParityAlways1, 9)) break;
|
||||
|
||||
// compute checksum
|
||||
uint8_t crc_data[8];
|
||||
for(size_t i = 0; i < 8; i++) {
|
||||
bit_lib_copy_bits(crc_data, i * 8, 8, protocol->encoded_data, 12 + 9 * i);
|
||||
}
|
||||
uint16_t crc_res = bit_lib_crc16(crc_data, 8, 0x1021, 0x0000, false, false, 0x0000);
|
||||
|
||||
// read checksum
|
||||
uint16_t crc_ex = 0;
|
||||
bit_lib_copy_bits((uint8_t*)&crc_ex, 8, 8, protocol->encoded_data, 84);
|
||||
bit_lib_copy_bits((uint8_t*)&crc_ex, 0, 8, protocol->encoded_data, 93);
|
||||
|
||||
// compare checksum
|
||||
if(crc_res != crc_ex) break;
|
||||
|
||||
result = true;
|
||||
} while(false);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void protocol_fdx_b_decode(ProtocolFDXB* protocol) {
|
||||
// remove parity
|
||||
bit_lib_remove_bit_every_nth(protocol->encoded_data, 3, 13 * 9, 9);
|
||||
|
||||
// remove header pattern
|
||||
for(size_t i = 0; i < 11; i++)
|
||||
bit_lib_push_bit(protocol->encoded_data, FDX_B_ENCODED_BYTE_FULL_SIZE, 0);
|
||||
|
||||
// 0 nnnnnnnn
|
||||
// 8 nnnnnnnn 38 bit (12 digit) National code.
|
||||
// 16 nnnnnnnn eg. 000000001008 (decimal).
|
||||
// 24 nnnnnnnn
|
||||
// 32 nnnnnncc 10 bit (3 digit) Country code.
|
||||
// 40 cccccccc eg. 999 (decimal).
|
||||
// 48 s------- 1 bit data block status flag.
|
||||
// 56 -------a 1 bit animal application indicator.
|
||||
// 64 xxxxxxxx 16 bit checksum.
|
||||
// 72 xxxxxxxx
|
||||
// 80 eeeeeeee 24 bits of extra data if present.
|
||||
// 88 eeeeeeee eg. $123456.
|
||||
// 92 eeeeeeee
|
||||
|
||||
// copy data without checksum
|
||||
bit_lib_copy_bits(protocol->data, 0, 64, protocol->encoded_data, 0);
|
||||
bit_lib_copy_bits(protocol->data, 64, 24, protocol->encoded_data, 80);
|
||||
|
||||
// const BitLibRegion regions_encoded[] = {
|
||||
// {'n', 0, 38},
|
||||
// {'c', 38, 10},
|
||||
// {'b', 48, 16},
|
||||
// {'x', 64, 16},
|
||||
// {'e', 80, 24},
|
||||
// };
|
||||
|
||||
// bit_lib_print_regions(regions_encoded, 5, protocol->encoded_data, FDX_B_ENCODED_BIT_SIZE);
|
||||
|
||||
// const BitLibRegion regions_decoded[] = {
|
||||
// {'n', 0, 38},
|
||||
// {'c', 38, 10},
|
||||
// {'b', 48, 16},
|
||||
// {'e', 64, 24},
|
||||
// };
|
||||
|
||||
// bit_lib_print_regions(regions_decoded, 4, protocol->data, FDXB_DECODED_DATA_SIZE * 8);
|
||||
}
|
||||
|
||||
bool protocol_fdx_b_decoder_feed(ProtocolFDXB* protocol, bool level, uint32_t duration) {
|
||||
bool result = false;
|
||||
UNUSED(level);
|
||||
|
||||
bool pushed = false;
|
||||
|
||||
// Bi-Phase Manchester decoding
|
||||
if(duration >= FDX_B_SHORT_TIME_LOW && duration <= FDX_B_SHORT_TIME_HIGH) {
|
||||
if(protocol->last_short == false) {
|
||||
protocol->last_short = true;
|
||||
} else {
|
||||
pushed = true;
|
||||
bit_lib_push_bit(protocol->encoded_data, FDX_B_ENCODED_BYTE_FULL_SIZE, false);
|
||||
protocol->last_short = false;
|
||||
}
|
||||
} else if(duration >= FDX_B_LONG_TIME_LOW && duration <= FDX_B_LONG_TIME_HIGH) {
|
||||
if(protocol->last_short == false) {
|
||||
pushed = true;
|
||||
bit_lib_push_bit(protocol->encoded_data, FDX_B_ENCODED_BYTE_FULL_SIZE, true);
|
||||
} else {
|
||||
// reset
|
||||
protocol->last_short = false;
|
||||
}
|
||||
} else {
|
||||
// reset
|
||||
protocol->last_short = false;
|
||||
}
|
||||
|
||||
if(pushed && protocol_fdx_b_can_be_decoded(protocol)) {
|
||||
protocol_fdx_b_decode(protocol);
|
||||
result = true;
|
||||
}
|
||||
|
||||
return result;
|
||||
};
|
||||
|
||||
bool protocol_fdx_b_encoder_start(ProtocolFDXB* protocol) {
|
||||
memset(protocol->encoded_data, 0, FDX_B_ENCODED_BYTE_FULL_SIZE);
|
||||
bit_lib_set_bit(protocol->encoded_data, 0, 1);
|
||||
for(size_t i = 0; i < 13; i++) {
|
||||
bit_lib_set_bit(protocol->encoded_data, 11 + 9 * i, 1);
|
||||
if(i == 8 || i == 9) continue;
|
||||
|
||||
if(i < 8) {
|
||||
bit_lib_copy_bits(protocol->encoded_data, 12 + 9 * i, 8, protocol->data, i * 8);
|
||||
} else {
|
||||
bit_lib_copy_bits(protocol->encoded_data, 12 + 9 * i, 8, protocol->data, (i - 2) * 8);
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t crc_res = bit_lib_crc16(protocol->data, 8, 0x1021, 0x0000, false, false, 0x0000);
|
||||
bit_lib_copy_bits(protocol->encoded_data, 84, 8, (uint8_t*)&crc_res, 8);
|
||||
bit_lib_copy_bits(protocol->encoded_data, 93, 8, (uint8_t*)&crc_res, 0);
|
||||
|
||||
protocol->encoded_index = 0;
|
||||
protocol->last_short = false;
|
||||
protocol->last_level = false;
|
||||
return true;
|
||||
};
|
||||
|
||||
LevelDuration protocol_fdx_b_encoder_yield(ProtocolFDXB* protocol) {
|
||||
uint32_t duration;
|
||||
protocol->last_level = !protocol->last_level;
|
||||
|
||||
bool bit = bit_lib_get_bit(protocol->encoded_data, protocol->encoded_index);
|
||||
|
||||
// Bi-Phase Manchester encoder
|
||||
if(bit) {
|
||||
// one long pulse for 1
|
||||
duration = FDX_B_LONG_TIME / 8;
|
||||
bit_lib_increment_index(protocol->encoded_index, FDX_B_ENCODED_BIT_SIZE);
|
||||
} else {
|
||||
// two short pulses for 0
|
||||
duration = FDX_B_SHORT_TIME / 8;
|
||||
if(protocol->last_short) {
|
||||
bit_lib_increment_index(protocol->encoded_index, FDX_B_ENCODED_BIT_SIZE);
|
||||
protocol->last_short = false;
|
||||
} else {
|
||||
protocol->last_short = true;
|
||||
}
|
||||
}
|
||||
|
||||
return level_duration_make(protocol->last_level, duration);
|
||||
};
|
||||
|
||||
// 0 nnnnnnnn
|
||||
// 8 nnnnnnnn 38 bit (12 digit) National code.
|
||||
// 16 nnnnnnnn eg. 000000001008 (decimal).
|
||||
// 24 nnnnnnnn
|
||||
// 32 nnnnnnnn 10 bit (3 digit) Country code.
|
||||
// 40 cccccccc eg. 999 (decimal).
|
||||
// 48 s------- 1 bit data block status flag.
|
||||
// 56 -------a 1 bit animal application indicator.
|
||||
// 64 eeeeeeee 24 bits of extra data if present.
|
||||
// 72 eeeeeeee eg. $123456.
|
||||
// 80 eeeeeeee
|
||||
|
||||
static uint64_t protocol_fdx_b_get_national_code(const uint8_t* data) {
|
||||
uint64_t national_code = bit_lib_get_bits_32(data, 0, 32);
|
||||
national_code = national_code << 32;
|
||||
national_code |= bit_lib_get_bits_32(data, 32, 6) << (32 - 6);
|
||||
bit_lib_reverse_bits((uint8_t*)&national_code, 0, 64);
|
||||
return national_code;
|
||||
}
|
||||
|
||||
static uint16_t protocol_fdx_b_get_country_code(const uint8_t* data) {
|
||||
uint16_t country_code = bit_lib_get_bits_16(data, 38, 10) << 6;
|
||||
bit_lib_reverse_bits((uint8_t*)&country_code, 0, 16);
|
||||
return country_code;
|
||||
}
|
||||
|
||||
static bool protocol_fdx_b_get_temp(const uint8_t* data, float* temp) {
|
||||
uint32_t extended = bit_lib_get_bits_32(data, 64, 24) << 8;
|
||||
bit_lib_reverse_bits((uint8_t*)&extended, 0, 32);
|
||||
|
||||
uint8_t ex_parity = (extended & 0x100) >> 8;
|
||||
uint8_t ex_temperature = extended & 0xff;
|
||||
uint8_t ex_calc_parity = bit_lib_test_parity_32(ex_temperature, BitLibParityOdd);
|
||||
bool ex_temperature_present = (ex_calc_parity == ex_parity) && !(extended & 0xe00);
|
||||
|
||||
if(ex_temperature_present) {
|
||||
float temperature_f = 74 + ex_temperature * 0.2;
|
||||
*temp = temperature_f;
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void protocol_fdx_b_render_data(ProtocolFDXB* protocol, string_t result) {
|
||||
// 38 bits of national code
|
||||
uint64_t national_code = protocol_fdx_b_get_national_code(protocol->data);
|
||||
|
||||
// 10 bit of country code
|
||||
uint16_t country_code = protocol_fdx_b_get_country_code(protocol->data);
|
||||
|
||||
bool block_status = bit_lib_get_bit(protocol->data, 48);
|
||||
bool rudi_bit = bit_lib_get_bit(protocol->data, 49);
|
||||
uint8_t reserved = bit_lib_get_bits(protocol->data, 50, 5);
|
||||
uint8_t user_info = bit_lib_get_bits(protocol->data, 55, 5);
|
||||
uint8_t replacement_number = bit_lib_get_bits(protocol->data, 60, 3);
|
||||
bool animal_flag = bit_lib_get_bit(protocol->data, 63);
|
||||
|
||||
string_printf(result, "ID: %03u-%012llu\r\n", country_code, national_code);
|
||||
string_cat_printf(result, "Animal: %s, ", animal_flag ? "Yes" : "No");
|
||||
|
||||
float temperature;
|
||||
if(protocol_fdx_b_get_temp(protocol->data, &temperature)) {
|
||||
float temperature_c = (temperature - 32) / 1.8;
|
||||
string_cat_printf(
|
||||
result, "T: %.2fF, %.2fC\r\n", (double)temperature, (double)temperature_c);
|
||||
} else {
|
||||
string_cat_printf(result, "T: ---\r\n");
|
||||
}
|
||||
|
||||
string_cat_printf(
|
||||
result,
|
||||
"Bits: %X-%X-%X-%X-%X",
|
||||
block_status,
|
||||
rudi_bit,
|
||||
reserved,
|
||||
user_info,
|
||||
replacement_number);
|
||||
};
|
||||
|
||||
void protocol_fdx_b_render_brief_data(ProtocolFDXB* protocol, string_t result) {
|
||||
// 38 bits of national code
|
||||
uint64_t national_code = protocol_fdx_b_get_national_code(protocol->data);
|
||||
|
||||
// 10 bit of country code
|
||||
uint16_t country_code = protocol_fdx_b_get_country_code(protocol->data);
|
||||
|
||||
bool animal_flag = bit_lib_get_bit(protocol->data, 63);
|
||||
|
||||
string_printf(result, "ID: %03u-%012llu\r\n", country_code, national_code);
|
||||
string_cat_printf(result, "Animal: %s, ", animal_flag ? "Yes" : "No");
|
||||
|
||||
float temperature;
|
||||
if(protocol_fdx_b_get_temp(protocol->data, &temperature)) {
|
||||
float temperature_c = (temperature - 32) / 1.8;
|
||||
string_cat_printf(result, "T: %.2fC", (double)temperature_c);
|
||||
} else {
|
||||
string_cat_printf(result, "T: ---");
|
||||
}
|
||||
};
|
||||
|
||||
bool protocol_fdx_b_write_data(ProtocolFDXB* protocol, void* data) {
|
||||
LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data;
|
||||
bool result = false;
|
||||
|
||||
protocol_fdx_b_encoder_start(protocol);
|
||||
|
||||
if(request->write_type == LFRFIDWriteTypeT5577) {
|
||||
request->t5577.block[0] = LFRFID_T5577_MODULATION_DIPHASE | LFRFID_T5577_BITRATE_RF_32 |
|
||||
(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;
|
||||
};
|
||||
|
||||
const ProtocolBase protocol_fdx_b = {
|
||||
.name = "FDX-B",
|
||||
.manufacturer = "ISO",
|
||||
.data_size = FDXB_DECODED_DATA_SIZE,
|
||||
.features = LFRFIDFeatureASK,
|
||||
.validate_count = 3,
|
||||
.alloc = (ProtocolAlloc)protocol_fdx_b_alloc,
|
||||
.free = (ProtocolFree)protocol_fdx_b_free,
|
||||
.get_data = (ProtocolGetData)protocol_fdx_b_get_data,
|
||||
.decoder =
|
||||
{
|
||||
.start = (ProtocolDecoderStart)protocol_fdx_b_decoder_start,
|
||||
.feed = (ProtocolDecoderFeed)protocol_fdx_b_decoder_feed,
|
||||
},
|
||||
.encoder =
|
||||
{
|
||||
.start = (ProtocolEncoderStart)protocol_fdx_b_encoder_start,
|
||||
.yield = (ProtocolEncoderYield)protocol_fdx_b_encoder_yield,
|
||||
},
|
||||
.render_data = (ProtocolRenderData)protocol_fdx_b_render_data,
|
||||
.render_brief_data = (ProtocolRenderData)protocol_fdx_b_render_brief_data,
|
||||
.write_data = (ProtocolWriteData)protocol_fdx_b_write_data,
|
||||
};
|
||||
4
lib/lfrfid/protocols/protocol_fdx_b.h
Normal file
4
lib/lfrfid/protocols/protocol_fdx_b.h
Normal file
@@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
|
||||
extern const ProtocolBase protocol_fdx_b;
|
||||
386
lib/lfrfid/protocols/protocol_h10301.c
Normal file
386
lib/lfrfid/protocols/protocol_h10301.c
Normal file
@@ -0,0 +1,386 @@
|
||||
#include <furi.h>
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
#include <lfrfid/tools/fsk_demod.h>
|
||||
#include <lfrfid/tools/fsk_osc.h>
|
||||
#include "lfrfid_protocols.h"
|
||||
|
||||
#define JITTER_TIME (20)
|
||||
#define MIN_TIME (64 - JITTER_TIME)
|
||||
#define MAX_TIME (80 + JITTER_TIME)
|
||||
|
||||
#define H10301_DECODED_DATA_SIZE (3)
|
||||
#define H10301_ENCODED_DATA_SIZE_U32 (3)
|
||||
#define H10301_ENCODED_DATA_SIZE (sizeof(uint32_t) * H10301_ENCODED_DATA_SIZE_U32)
|
||||
|
||||
#define H10301_BIT_SIZE (sizeof(uint32_t) * 8)
|
||||
#define H10301_BIT_MAX_SIZE (H10301_BIT_SIZE * H10301_DECODED_DATA_SIZE)
|
||||
|
||||
typedef struct {
|
||||
FSKDemod* fsk_demod;
|
||||
} ProtocolH10301Decoder;
|
||||
|
||||
typedef struct {
|
||||
FSKOsc* fsk_osc;
|
||||
uint8_t encoded_index;
|
||||
uint32_t pulse;
|
||||
} ProtocolH10301Encoder;
|
||||
|
||||
typedef struct {
|
||||
ProtocolH10301Decoder decoder;
|
||||
ProtocolH10301Encoder encoder;
|
||||
uint32_t encoded_data[H10301_ENCODED_DATA_SIZE_U32];
|
||||
uint8_t data[H10301_DECODED_DATA_SIZE];
|
||||
} ProtocolH10301;
|
||||
|
||||
ProtocolH10301* protocol_h10301_alloc(void) {
|
||||
ProtocolH10301* protocol = malloc(sizeof(ProtocolH10301));
|
||||
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_h10301_free(ProtocolH10301* protocol) {
|
||||
fsk_demod_free(protocol->decoder.fsk_demod);
|
||||
fsk_osc_free(protocol->encoder.fsk_osc);
|
||||
free(protocol);
|
||||
};
|
||||
|
||||
uint8_t* protocol_h10301_get_data(ProtocolH10301* protocol) {
|
||||
return protocol->data;
|
||||
};
|
||||
|
||||
void protocol_h10301_decoder_start(ProtocolH10301* protocol) {
|
||||
memset(protocol->encoded_data, 0, sizeof(uint32_t) * 3);
|
||||
};
|
||||
|
||||
static void protocol_h10301_decoder_store_data(ProtocolH10301* protocol, bool data) {
|
||||
protocol->encoded_data[0] = (protocol->encoded_data[0] << 1) |
|
||||
((protocol->encoded_data[1] >> 31) & 1);
|
||||
protocol->encoded_data[1] = (protocol->encoded_data[1] << 1) |
|
||||
((protocol->encoded_data[2] >> 31) & 1);
|
||||
protocol->encoded_data[2] = (protocol->encoded_data[2] << 1) | data;
|
||||
}
|
||||
|
||||
static bool protocol_h10301_can_be_decoded(const uint32_t* card_data) {
|
||||
const uint8_t* encoded_data = (const uint8_t*)card_data;
|
||||
|
||||
// packet preamble
|
||||
// raw data
|
||||
if(*(encoded_data + 3) != 0x1D) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// encoded company/oem
|
||||
// coded with 01 = 0, 10 = 1 transitions
|
||||
// stored in word 0
|
||||
if((*card_data >> 10 & 0x3FFF) != 0x1556) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// encoded format/length
|
||||
// coded with 01 = 0, 10 = 1 transitions
|
||||
// stored in word 0 and word 1
|
||||
if((((*card_data & 0x3FF) << 12) | ((*(card_data + 1) >> 20) & 0xFFF)) != 0x155556) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// data decoding
|
||||
uint32_t result = 0;
|
||||
|
||||
// decode from word 1
|
||||
// coded with 01 = 0, 10 = 1 transitions
|
||||
for(int8_t i = 9; i >= 0; i--) {
|
||||
switch((*(card_data + 1) >> (2 * i)) & 0b11) {
|
||||
case 0b01:
|
||||
result = (result << 1) | 0;
|
||||
break;
|
||||
case 0b10:
|
||||
result = (result << 1) | 1;
|
||||
break;
|
||||
default:
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// decode from word 2
|
||||
// coded with 01 = 0, 10 = 1 transitions
|
||||
for(int8_t i = 15; i >= 0; i--) {
|
||||
switch((*(card_data + 2) >> (2 * i)) & 0b11) {
|
||||
case 0b01:
|
||||
result = (result << 1) | 0;
|
||||
break;
|
||||
case 0b10:
|
||||
result = (result << 1) | 1;
|
||||
break;
|
||||
default:
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// trailing parity (odd) test
|
||||
uint8_t parity_sum = 0;
|
||||
for(int8_t i = 0; i < 13; i++) {
|
||||
if(((result >> i) & 1) == 1) {
|
||||
parity_sum++;
|
||||
}
|
||||
}
|
||||
|
||||
if((parity_sum % 2) != 1) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// leading parity (even) test
|
||||
parity_sum = 0;
|
||||
for(int8_t i = 13; i < 26; i++) {
|
||||
if(((result >> i) & 1) == 1) {
|
||||
parity_sum++;
|
||||
}
|
||||
}
|
||||
|
||||
if((parity_sum % 2) == 1) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void protocol_h10301_decode(const uint32_t* card_data, uint8_t* decoded_data) {
|
||||
// data decoding
|
||||
uint32_t result = 0;
|
||||
|
||||
// decode from word 1
|
||||
// coded with 01 = 0, 10 = 1 transitions
|
||||
for(int8_t i = 9; i >= 0; i--) {
|
||||
switch((*(card_data + 1) >> (2 * i)) & 0b11) {
|
||||
case 0b01:
|
||||
result = (result << 1) | 0;
|
||||
break;
|
||||
case 0b10:
|
||||
result = (result << 1) | 1;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// decode from word 2
|
||||
// coded with 01 = 0, 10 = 1 transitions
|
||||
for(int8_t i = 15; i >= 0; i--) {
|
||||
switch((*(card_data + 2) >> (2 * i)) & 0b11) {
|
||||
case 0b01:
|
||||
result = (result << 1) | 0;
|
||||
break;
|
||||
case 0b10:
|
||||
result = (result << 1) | 1;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t data[H10301_DECODED_DATA_SIZE] = {
|
||||
(uint8_t)(result >> 17), (uint8_t)(result >> 9), (uint8_t)(result >> 1)};
|
||||
|
||||
memcpy(decoded_data, &data, H10301_DECODED_DATA_SIZE);
|
||||
}
|
||||
|
||||
bool protocol_h10301_decoder_feed(ProtocolH10301* 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++) {
|
||||
protocol_h10301_decoder_store_data(protocol, value);
|
||||
if(protocol_h10301_can_be_decoded(protocol->encoded_data)) {
|
||||
protocol_h10301_decode(protocol->encoded_data, protocol->data);
|
||||
result = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
};
|
||||
|
||||
static void protocol_h10301_write_raw_bit(bool bit, uint8_t position, uint32_t* card_data) {
|
||||
if(bit) {
|
||||
card_data[position / H10301_BIT_SIZE] |=
|
||||
1UL << (H10301_BIT_SIZE - (position % H10301_BIT_SIZE) - 1);
|
||||
} else {
|
||||
card_data[position / H10301_BIT_SIZE] &=
|
||||
~(1UL << (H10301_BIT_SIZE - (position % H10301_BIT_SIZE) - 1));
|
||||
}
|
||||
}
|
||||
|
||||
static void protocol_h10301_write_bit(bool bit, uint8_t position, uint32_t* card_data) {
|
||||
protocol_h10301_write_raw_bit(bit, position + 0, card_data);
|
||||
protocol_h10301_write_raw_bit(!bit, position + 1, card_data);
|
||||
}
|
||||
|
||||
void protocol_h10301_encode(const uint8_t* decoded_data, uint8_t* encoded_data) {
|
||||
uint32_t card_data[H10301_DECODED_DATA_SIZE] = {0, 0, 0};
|
||||
|
||||
uint32_t fc_cn = (decoded_data[0] << 16) | (decoded_data[1] << 8) | decoded_data[2];
|
||||
|
||||
// even parity sum calculation (high 12 bits of data)
|
||||
uint8_t even_parity_sum = 0;
|
||||
for(int8_t i = 12; i < 24; i++) {
|
||||
if(((fc_cn >> i) & 1) == 1) {
|
||||
even_parity_sum++;
|
||||
}
|
||||
}
|
||||
|
||||
// odd parity sum calculation (low 12 bits of data)
|
||||
uint8_t odd_parity_sum = 1;
|
||||
for(int8_t i = 0; i < 12; i++) {
|
||||
if(((fc_cn >> i) & 1) == 1) {
|
||||
odd_parity_sum++;
|
||||
}
|
||||
}
|
||||
|
||||
// 0x1D preamble
|
||||
protocol_h10301_write_raw_bit(0, 0, card_data);
|
||||
protocol_h10301_write_raw_bit(0, 1, card_data);
|
||||
protocol_h10301_write_raw_bit(0, 2, card_data);
|
||||
protocol_h10301_write_raw_bit(1, 3, card_data);
|
||||
protocol_h10301_write_raw_bit(1, 4, card_data);
|
||||
protocol_h10301_write_raw_bit(1, 5, card_data);
|
||||
protocol_h10301_write_raw_bit(0, 6, card_data);
|
||||
protocol_h10301_write_raw_bit(1, 7, card_data);
|
||||
|
||||
// company / OEM code 1
|
||||
protocol_h10301_write_bit(0, 8, card_data);
|
||||
protocol_h10301_write_bit(0, 10, card_data);
|
||||
protocol_h10301_write_bit(0, 12, card_data);
|
||||
protocol_h10301_write_bit(0, 14, card_data);
|
||||
protocol_h10301_write_bit(0, 16, card_data);
|
||||
protocol_h10301_write_bit(0, 18, card_data);
|
||||
protocol_h10301_write_bit(1, 20, card_data);
|
||||
|
||||
// card format / length 1
|
||||
protocol_h10301_write_bit(0, 22, card_data);
|
||||
protocol_h10301_write_bit(0, 24, card_data);
|
||||
protocol_h10301_write_bit(0, 26, card_data);
|
||||
protocol_h10301_write_bit(0, 28, card_data);
|
||||
protocol_h10301_write_bit(0, 30, card_data);
|
||||
protocol_h10301_write_bit(0, 32, card_data);
|
||||
protocol_h10301_write_bit(0, 34, card_data);
|
||||
protocol_h10301_write_bit(0, 36, card_data);
|
||||
protocol_h10301_write_bit(0, 38, card_data);
|
||||
protocol_h10301_write_bit(0, 40, card_data);
|
||||
protocol_h10301_write_bit(1, 42, card_data);
|
||||
|
||||
// even parity bit
|
||||
protocol_h10301_write_bit((even_parity_sum % 2), 44, card_data);
|
||||
|
||||
// data
|
||||
for(uint8_t i = 0; i < 24; i++) {
|
||||
protocol_h10301_write_bit((fc_cn >> (23 - i)) & 1, 46 + (i * 2), card_data);
|
||||
}
|
||||
|
||||
// odd parity bit
|
||||
protocol_h10301_write_bit((odd_parity_sum % 2), 94, card_data);
|
||||
|
||||
memcpy(encoded_data, &card_data, H10301_ENCODED_DATA_SIZE);
|
||||
}
|
||||
|
||||
bool protocol_h10301_encoder_start(ProtocolH10301* protocol) {
|
||||
protocol_h10301_encode(protocol->data, (uint8_t*)protocol->encoded_data);
|
||||
protocol->encoder.encoded_index = 0;
|
||||
protocol->encoder.pulse = 0;
|
||||
|
||||
return true;
|
||||
};
|
||||
|
||||
LevelDuration protocol_h10301_encoder_yield(ProtocolH10301* 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 =
|
||||
(protocol->encoded_data[protocol->encoder.encoded_index / H10301_BIT_SIZE] >>
|
||||
((H10301_BIT_SIZE - 1) - (protocol->encoder.encoded_index % H10301_BIT_SIZE))) &
|
||||
1;
|
||||
|
||||
// get pulse from oscillator
|
||||
bool advance = fsk_osc_next(protocol->encoder.fsk_osc, bit, &duration);
|
||||
|
||||
if(advance) {
|
||||
protocol->encoder.encoded_index++;
|
||||
if(protocol->encoder.encoded_index >= (H10301_BIT_MAX_SIZE)) {
|
||||
protocol->encoder.encoded_index = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// 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_h10301_write_data(ProtocolH10301* protocol, void* data) {
|
||||
LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data;
|
||||
bool result = false;
|
||||
|
||||
protocol_h10301_encoder_start(protocol);
|
||||
|
||||
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] = protocol->encoded_data[0];
|
||||
request->t5577.block[2] = protocol->encoded_data[1];
|
||||
request->t5577.block[3] = protocol->encoded_data[2];
|
||||
request->t5577.blocks_to_write = 4;
|
||||
result = true;
|
||||
}
|
||||
return result;
|
||||
};
|
||||
|
||||
void protocol_h10301_render_data(ProtocolH10301* protocol, string_t result) {
|
||||
uint8_t* data = protocol->data;
|
||||
string_printf(
|
||||
result,
|
||||
"FC: %u\r\n"
|
||||
"Card: %u",
|
||||
data[0],
|
||||
(uint16_t)((data[1] << 8) | (data[2])));
|
||||
};
|
||||
|
||||
const ProtocolBase protocol_h10301 = {
|
||||
.name = "H10301",
|
||||
.manufacturer = "HID",
|
||||
.data_size = H10301_DECODED_DATA_SIZE,
|
||||
.features = LFRFIDFeatureASK,
|
||||
.validate_count = 3,
|
||||
.alloc = (ProtocolAlloc)protocol_h10301_alloc,
|
||||
.free = (ProtocolFree)protocol_h10301_free,
|
||||
.get_data = (ProtocolGetData)protocol_h10301_get_data,
|
||||
.decoder =
|
||||
{
|
||||
.start = (ProtocolDecoderStart)protocol_h10301_decoder_start,
|
||||
.feed = (ProtocolDecoderFeed)protocol_h10301_decoder_feed,
|
||||
},
|
||||
.encoder =
|
||||
{
|
||||
.start = (ProtocolEncoderStart)protocol_h10301_encoder_start,
|
||||
.yield = (ProtocolEncoderYield)protocol_h10301_encoder_yield,
|
||||
},
|
||||
.render_data = (ProtocolRenderData)protocol_h10301_render_data,
|
||||
.render_brief_data = (ProtocolRenderData)protocol_h10301_render_data,
|
||||
.write_data = (ProtocolWriteData)protocol_h10301_write_data,
|
||||
};
|
||||
4
lib/lfrfid/protocols/protocol_h10301.h
Normal file
4
lib/lfrfid/protocols/protocol_h10301.h
Normal file
@@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
|
||||
extern const ProtocolBase protocol_h10301;
|
||||
219
lib/lfrfid/protocols/protocol_hid_ex_generic.c
Normal file
219
lib/lfrfid/protocols/protocol_hid_ex_generic.c
Normal file
@@ -0,0 +1,219 @@
|
||||
#include <furi.h>
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
#include <lfrfid/tools/fsk_demod.h>
|
||||
#include <lfrfid/tools/fsk_osc.h>
|
||||
#include "lfrfid_protocols.h"
|
||||
#include <lfrfid/tools/bit_lib.h>
|
||||
|
||||
#define JITTER_TIME (20)
|
||||
#define MIN_TIME (64 - JITTER_TIME)
|
||||
#define MAX_TIME (80 + JITTER_TIME)
|
||||
|
||||
#define HID_DATA_SIZE 23
|
||||
#define HID_PREAMBLE_SIZE 1
|
||||
|
||||
#define HID_ENCODED_DATA_SIZE (HID_PREAMBLE_SIZE + HID_DATA_SIZE + HID_PREAMBLE_SIZE)
|
||||
#define HID_ENCODED_BIT_SIZE ((HID_PREAMBLE_SIZE + HID_DATA_SIZE) * 8)
|
||||
#define HID_DECODED_DATA_SIZE (12)
|
||||
#define HID_DECODED_BIT_SIZE ((HID_ENCODED_BIT_SIZE - HID_PREAMBLE_SIZE * 8) / 2)
|
||||
|
||||
#define HID_PREAMBLE 0x1D
|
||||
|
||||
typedef struct {
|
||||
FSKDemod* fsk_demod;
|
||||
} ProtocolHIDExDecoder;
|
||||
|
||||
typedef struct {
|
||||
FSKOsc* fsk_osc;
|
||||
uint8_t encoded_index;
|
||||
uint32_t pulse;
|
||||
} ProtocolHIDExEncoder;
|
||||
|
||||
typedef struct {
|
||||
ProtocolHIDExDecoder decoder;
|
||||
ProtocolHIDExEncoder encoder;
|
||||
uint8_t encoded_data[HID_ENCODED_DATA_SIZE];
|
||||
uint8_t data[HID_DECODED_DATA_SIZE];
|
||||
size_t protocol_size;
|
||||
} ProtocolHIDEx;
|
||||
|
||||
ProtocolHIDEx* protocol_hid_ex_generic_alloc(void) {
|
||||
ProtocolHIDEx* protocol = malloc(sizeof(ProtocolHIDEx));
|
||||
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_hid_ex_generic_free(ProtocolHIDEx* protocol) {
|
||||
fsk_demod_free(protocol->decoder.fsk_demod);
|
||||
fsk_osc_free(protocol->encoder.fsk_osc);
|
||||
free(protocol);
|
||||
};
|
||||
|
||||
uint8_t* protocol_hid_ex_generic_get_data(ProtocolHIDEx* protocol) {
|
||||
return protocol->data;
|
||||
};
|
||||
|
||||
void protocol_hid_ex_generic_decoder_start(ProtocolHIDEx* protocol) {
|
||||
memset(protocol->encoded_data, 0, HID_ENCODED_DATA_SIZE);
|
||||
};
|
||||
|
||||
static bool protocol_hid_ex_generic_can_be_decoded(const uint8_t* data) {
|
||||
// check preamble
|
||||
if(data[0] != HID_PREAMBLE || data[HID_PREAMBLE_SIZE + HID_DATA_SIZE] != HID_PREAMBLE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// check for manchester encoding
|
||||
for(size_t i = HID_PREAMBLE_SIZE; i < (HID_PREAMBLE_SIZE + HID_DATA_SIZE); i++) {
|
||||
for(size_t n = 0; n < 4; n++) {
|
||||
uint8_t bit_pair = (data[i] >> (n * 2)) & 0b11;
|
||||
if(bit_pair == 0b11 || bit_pair == 0b00) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void protocol_hid_ex_generic_decode(const uint8_t* from, uint8_t* to) {
|
||||
size_t bit_index = 0;
|
||||
for(size_t i = HID_PREAMBLE_SIZE; i < (HID_PREAMBLE_SIZE + HID_DATA_SIZE); i++) {
|
||||
for(size_t n = 0; n < 4; n++) {
|
||||
uint8_t bit_pair = (from[i] >> (6 - (n * 2))) & 0b11;
|
||||
if(bit_pair == 0b01) {
|
||||
bit_lib_set_bit(to, bit_index, 0);
|
||||
} else if(bit_pair == 0b10) {
|
||||
bit_lib_set_bit(to, bit_index, 1);
|
||||
}
|
||||
bit_index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool protocol_hid_ex_generic_decoder_feed(ProtocolHIDEx* 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, HID_ENCODED_DATA_SIZE, value);
|
||||
if(protocol_hid_ex_generic_can_be_decoded(protocol->encoded_data)) {
|
||||
protocol_hid_ex_generic_decode(protocol->encoded_data, protocol->data);
|
||||
result = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
};
|
||||
|
||||
static void protocol_hid_ex_generic_encode(ProtocolHIDEx* protocol) {
|
||||
protocol->encoded_data[0] = HID_PREAMBLE;
|
||||
|
||||
size_t bit_index = 0;
|
||||
for(size_t i = 0; i < HID_DECODED_BIT_SIZE; i++) {
|
||||
bool bit = bit_lib_get_bit(protocol->data, i);
|
||||
if(bit) {
|
||||
bit_lib_set_bit(protocol->encoded_data, 8 + bit_index, 1);
|
||||
bit_lib_set_bit(protocol->encoded_data, 8 + bit_index + 1, 0);
|
||||
} else {
|
||||
bit_lib_set_bit(protocol->encoded_data, 8 + bit_index, 0);
|
||||
bit_lib_set_bit(protocol->encoded_data, 8 + bit_index + 1, 1);
|
||||
}
|
||||
bit_index += 2;
|
||||
}
|
||||
}
|
||||
|
||||
bool protocol_hid_ex_generic_encoder_start(ProtocolHIDEx* protocol) {
|
||||
protocol->encoder.encoded_index = 0;
|
||||
protocol->encoder.pulse = 0;
|
||||
protocol_hid_ex_generic_encode(protocol);
|
||||
|
||||
return true;
|
||||
};
|
||||
|
||||
LevelDuration protocol_hid_ex_generic_encoder_yield(ProtocolHIDEx* 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, HID_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_hid_ex_generic_write_data(ProtocolHIDEx* protocol, void* data) {
|
||||
LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data;
|
||||
bool result = false;
|
||||
|
||||
protocol_hid_ex_generic_encoder_start(protocol);
|
||||
|
||||
if(request->write_type == LFRFIDWriteTypeT5577) {
|
||||
request->t5577.block[0] = LFRFID_T5577_MODULATION_FSK2a | LFRFID_T5577_BITRATE_RF_50 |
|
||||
(6 << 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.block[5] = bit_lib_get_bits_32(protocol->encoded_data, 128, 32);
|
||||
request->t5577.block[6] = bit_lib_get_bits_32(protocol->encoded_data, 160, 32);
|
||||
request->t5577.blocks_to_write = 7;
|
||||
result = true;
|
||||
}
|
||||
return result;
|
||||
};
|
||||
|
||||
void protocol_hid_ex_generic_render_data(ProtocolHIDEx* protocol, string_t result) {
|
||||
// TODO: parser and render functions
|
||||
UNUSED(protocol);
|
||||
string_printf(result, "Generic HID Extended\r\nData: Unknown");
|
||||
};
|
||||
|
||||
const ProtocolBase protocol_hid_ex_generic = {
|
||||
.name = "HIDExt",
|
||||
.manufacturer = "Generic",
|
||||
.data_size = HID_DECODED_DATA_SIZE,
|
||||
.features = LFRFIDFeatureASK,
|
||||
.validate_count = 3,
|
||||
.alloc = (ProtocolAlloc)protocol_hid_ex_generic_alloc,
|
||||
.free = (ProtocolFree)protocol_hid_ex_generic_free,
|
||||
.get_data = (ProtocolGetData)protocol_hid_ex_generic_get_data,
|
||||
.decoder =
|
||||
{
|
||||
.start = (ProtocolDecoderStart)protocol_hid_ex_generic_decoder_start,
|
||||
.feed = (ProtocolDecoderFeed)protocol_hid_ex_generic_decoder_feed,
|
||||
},
|
||||
.encoder =
|
||||
{
|
||||
.start = (ProtocolEncoderStart)protocol_hid_ex_generic_encoder_start,
|
||||
.yield = (ProtocolEncoderYield)protocol_hid_ex_generic_encoder_yield,
|
||||
},
|
||||
.render_data = (ProtocolRenderData)protocol_hid_ex_generic_render_data,
|
||||
.render_brief_data = (ProtocolRenderData)protocol_hid_ex_generic_render_data,
|
||||
.write_data = (ProtocolWriteData)protocol_hid_ex_generic_write_data,
|
||||
};
|
||||
4
lib/lfrfid/protocols/protocol_hid_ex_generic.h
Normal file
4
lib/lfrfid/protocols/protocol_hid_ex_generic.h
Normal file
@@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
|
||||
extern const ProtocolBase protocol_hid_ex_generic;
|
||||
280
lib/lfrfid/protocols/protocol_hid_generic.c
Normal file
280
lib/lfrfid/protocols/protocol_hid_generic.c
Normal file
@@ -0,0 +1,280 @@
|
||||
#include <furi.h>
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
#include <lfrfid/tools/fsk_demod.h>
|
||||
#include <lfrfid/tools/fsk_osc.h>
|
||||
#include "lfrfid_protocols.h"
|
||||
#include <lfrfid/tools/bit_lib.h>
|
||||
|
||||
#define JITTER_TIME (20)
|
||||
#define MIN_TIME (64 - JITTER_TIME)
|
||||
#define MAX_TIME (80 + JITTER_TIME)
|
||||
|
||||
#define HID_DATA_SIZE 11
|
||||
#define HID_PREAMBLE_SIZE 1
|
||||
#define HID_PROTOCOL_SIZE_UNKNOWN 0
|
||||
|
||||
#define HID_ENCODED_DATA_SIZE (HID_PREAMBLE_SIZE + HID_DATA_SIZE + HID_PREAMBLE_SIZE)
|
||||
#define HID_ENCODED_BIT_SIZE ((HID_PREAMBLE_SIZE + HID_DATA_SIZE) * 8)
|
||||
#define HID_DECODED_DATA_SIZE (6)
|
||||
#define HID_DECODED_BIT_SIZE ((HID_ENCODED_BIT_SIZE - HID_PREAMBLE_SIZE * 8) / 2)
|
||||
|
||||
#define HID_PREAMBLE 0x1D
|
||||
|
||||
typedef struct {
|
||||
FSKDemod* fsk_demod;
|
||||
} ProtocolHIDDecoder;
|
||||
|
||||
typedef struct {
|
||||
FSKOsc* fsk_osc;
|
||||
uint8_t encoded_index;
|
||||
uint32_t pulse;
|
||||
} ProtocolHIDEncoder;
|
||||
|
||||
typedef struct {
|
||||
ProtocolHIDDecoder decoder;
|
||||
ProtocolHIDEncoder encoder;
|
||||
uint8_t encoded_data[HID_ENCODED_DATA_SIZE];
|
||||
uint8_t data[HID_DECODED_DATA_SIZE];
|
||||
} ProtocolHID;
|
||||
|
||||
ProtocolHID* protocol_hid_generic_alloc(void) {
|
||||
ProtocolHID* protocol = malloc(sizeof(ProtocolHID));
|
||||
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_hid_generic_free(ProtocolHID* protocol) {
|
||||
fsk_demod_free(protocol->decoder.fsk_demod);
|
||||
fsk_osc_free(protocol->encoder.fsk_osc);
|
||||
free(protocol);
|
||||
};
|
||||
|
||||
uint8_t* protocol_hid_generic_get_data(ProtocolHID* protocol) {
|
||||
return protocol->data;
|
||||
};
|
||||
|
||||
void protocol_hid_generic_decoder_start(ProtocolHID* protocol) {
|
||||
memset(protocol->encoded_data, 0, HID_ENCODED_DATA_SIZE);
|
||||
};
|
||||
|
||||
static bool protocol_hid_generic_can_be_decoded(const uint8_t* data) {
|
||||
// check preamble
|
||||
if(data[0] != HID_PREAMBLE || data[HID_PREAMBLE_SIZE + HID_DATA_SIZE] != HID_PREAMBLE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// check for manchester encoding
|
||||
for(size_t i = HID_PREAMBLE_SIZE; i < (HID_PREAMBLE_SIZE + HID_DATA_SIZE); i++) {
|
||||
for(size_t n = 0; n < 4; n++) {
|
||||
uint8_t bit_pair = (data[i] >> (n * 2)) & 0b11;
|
||||
if(bit_pair == 0b11 || bit_pair == 0b00) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void protocol_hid_generic_decode(const uint8_t* from, uint8_t* to) {
|
||||
size_t bit_index = 0;
|
||||
for(size_t i = HID_PREAMBLE_SIZE; i < (HID_PREAMBLE_SIZE + HID_DATA_SIZE); i++) {
|
||||
for(size_t n = 0; n < 4; n++) {
|
||||
uint8_t bit_pair = (from[i] >> (6 - (n * 2))) & 0b11;
|
||||
if(bit_pair == 0b01) {
|
||||
bit_lib_set_bit(to, bit_index, 0);
|
||||
} else if(bit_pair == 0b10) {
|
||||
bit_lib_set_bit(to, bit_index, 1);
|
||||
}
|
||||
bit_index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Decodes size from the HID Proximity header:
|
||||
* - If any of the first six bits is 1, the key is composed of the bits
|
||||
* following the first 1
|
||||
* - Otherwise, if the first six bits are 0:
|
||||
* - If the seventh bit is 0, the key is composed of the remaining 37 bits.
|
||||
* - If the seventh bit is 1, the size header continues until the next 1 bit,
|
||||
* and the key is composed of however many bits remain.
|
||||
*
|
||||
* HID Proximity keys are 26 bits at minimum. If the header implies a key size
|
||||
* under 26 bits, this function returns HID_PROTOCOL_SIZE_UNKNOWN.
|
||||
*/
|
||||
static uint8_t protocol_hid_generic_decode_protocol_size(ProtocolHID* protocol) {
|
||||
for(size_t bit_index = 0; bit_index < 6; bit_index++) {
|
||||
if(bit_lib_get_bit(protocol->data, bit_index)) {
|
||||
return HID_DECODED_BIT_SIZE - bit_index - 1;
|
||||
}
|
||||
}
|
||||
|
||||
if(!bit_lib_get_bit(protocol->data, 6)) {
|
||||
return 37;
|
||||
}
|
||||
|
||||
size_t bit_index = 7;
|
||||
uint8_t size = 36;
|
||||
while(!bit_lib_get_bit(protocol->data, bit_index) && size >= 26) {
|
||||
size--;
|
||||
bit_index++;
|
||||
}
|
||||
return size < 26 ? HID_PROTOCOL_SIZE_UNKNOWN : size;
|
||||
}
|
||||
|
||||
bool protocol_hid_generic_decoder_feed(ProtocolHID* 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, HID_ENCODED_DATA_SIZE, value);
|
||||
if(protocol_hid_generic_can_be_decoded(protocol->encoded_data)) {
|
||||
protocol_hid_generic_decode(protocol->encoded_data, protocol->data);
|
||||
result = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
};
|
||||
|
||||
static void protocol_hid_generic_encode(ProtocolHID* protocol) {
|
||||
protocol->encoded_data[0] = HID_PREAMBLE;
|
||||
|
||||
size_t bit_index = 0;
|
||||
for(size_t i = 0; i < HID_DECODED_BIT_SIZE; i++) {
|
||||
bool bit = bit_lib_get_bit(protocol->data, i);
|
||||
if(bit) {
|
||||
bit_lib_set_bit(protocol->encoded_data, 8 + bit_index, 1);
|
||||
bit_lib_set_bit(protocol->encoded_data, 8 + bit_index + 1, 0);
|
||||
} else {
|
||||
bit_lib_set_bit(protocol->encoded_data, 8 + bit_index, 0);
|
||||
bit_lib_set_bit(protocol->encoded_data, 8 + bit_index + 1, 1);
|
||||
}
|
||||
bit_index += 2;
|
||||
}
|
||||
}
|
||||
|
||||
bool protocol_hid_generic_encoder_start(ProtocolHID* protocol) {
|
||||
protocol->encoder.encoded_index = 0;
|
||||
protocol->encoder.pulse = 0;
|
||||
protocol_hid_generic_encode(protocol);
|
||||
|
||||
return true;
|
||||
};
|
||||
|
||||
LevelDuration protocol_hid_generic_encoder_yield(ProtocolHID* 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, HID_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_hid_generic_write_data(ProtocolHID* protocol, void* data) {
|
||||
LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data;
|
||||
bool result = false;
|
||||
|
||||
protocol_hid_generic_encoder_start(protocol);
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
static void protocol_hid_generic_string_cat_protocol_bits(ProtocolHID* protocol, uint8_t protocol_size, string_t result) {
|
||||
// round up to the nearest nibble
|
||||
const uint8_t hex_character_count = (protocol_size + 3) / 4;
|
||||
const uint8_t protocol_bit_index = HID_DECODED_BIT_SIZE - protocol_size;
|
||||
|
||||
for(size_t i = 0; i < hex_character_count; i++) {
|
||||
uint8_t nibble =
|
||||
i == 0 ? bit_lib_get_bits(
|
||||
protocol->data, protocol_bit_index, protocol_size % 4 == 0 ? 4 : protocol_size % 4) :
|
||||
bit_lib_get_bits(protocol->data, protocol_bit_index + i * 4, 4);
|
||||
string_cat_printf(result, "%X", nibble & 0xF);
|
||||
}
|
||||
}
|
||||
|
||||
void protocol_hid_generic_render_data(ProtocolHID* protocol, string_t result) {
|
||||
const uint8_t protocol_size = protocol_hid_generic_decode_protocol_size(protocol);
|
||||
|
||||
if(protocol_size == HID_PROTOCOL_SIZE_UNKNOWN) {
|
||||
string_printf(
|
||||
result,
|
||||
"Generic HID Proximity\r\n"
|
||||
"Data: %02X%02X%02X%02X%02X%X",
|
||||
protocol->data[0],
|
||||
protocol->data[1],
|
||||
protocol->data[2],
|
||||
protocol->data[3],
|
||||
protocol->data[4],
|
||||
protocol->data[5] >> 4);
|
||||
} else {
|
||||
string_printf(
|
||||
result,
|
||||
"%hhu-bit HID Proximity\r\n"
|
||||
"Data: ",
|
||||
protocol_size);
|
||||
protocol_hid_generic_string_cat_protocol_bits(protocol, protocol_size, result);
|
||||
}
|
||||
};
|
||||
|
||||
const ProtocolBase protocol_hid_generic = {
|
||||
.name = "HIDProx",
|
||||
.manufacturer = "Generic",
|
||||
.data_size = HID_DECODED_DATA_SIZE,
|
||||
.features = LFRFIDFeatureASK,
|
||||
.validate_count = 6,
|
||||
.alloc = (ProtocolAlloc)protocol_hid_generic_alloc,
|
||||
.free = (ProtocolFree)protocol_hid_generic_free,
|
||||
.get_data = (ProtocolGetData)protocol_hid_generic_get_data,
|
||||
.decoder =
|
||||
{
|
||||
.start = (ProtocolDecoderStart)protocol_hid_generic_decoder_start,
|
||||
.feed = (ProtocolDecoderFeed)protocol_hid_generic_decoder_feed,
|
||||
},
|
||||
.encoder =
|
||||
{
|
||||
.start = (ProtocolEncoderStart)protocol_hid_generic_encoder_start,
|
||||
.yield = (ProtocolEncoderYield)protocol_hid_generic_encoder_yield,
|
||||
},
|
||||
.render_data = (ProtocolRenderData)protocol_hid_generic_render_data,
|
||||
.render_brief_data = (ProtocolRenderData)protocol_hid_generic_render_data,
|
||||
.write_data = (ProtocolWriteData)protocol_hid_generic_write_data,
|
||||
};
|
||||
4
lib/lfrfid/protocols/protocol_hid_generic.h
Normal file
4
lib/lfrfid/protocols/protocol_hid_generic.h
Normal file
@@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
|
||||
extern const ProtocolBase protocol_hid_generic;
|
||||
353
lib/lfrfid/protocols/protocol_indala26.c
Normal file
353
lib/lfrfid/protocols/protocol_indala26.c
Normal file
@@ -0,0 +1,353 @@
|
||||
#include <furi.h>
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
#include <lfrfid/tools/bit_lib.h>
|
||||
#include "lfrfid_protocols.h"
|
||||
|
||||
#define INDALA26_PREAMBLE_BIT_SIZE (33)
|
||||
#define INDALA26_PREAMBLE_DATA_SIZE (5)
|
||||
|
||||
#define INDALA26_ENCODED_BIT_SIZE (64)
|
||||
#define INDALA26_ENCODED_DATA_SIZE \
|
||||
(((INDALA26_ENCODED_BIT_SIZE) / 8) + INDALA26_PREAMBLE_DATA_SIZE)
|
||||
#define INDALA26_ENCODED_DATA_LAST ((INDALA26_ENCODED_BIT_SIZE) / 8)
|
||||
|
||||
#define INDALA26_DECODED_BIT_SIZE (28)
|
||||
#define INDALA26_DECODED_DATA_SIZE (4)
|
||||
|
||||
#define INDALA26_US_PER_BIT (255)
|
||||
#define INDALA26_ENCODER_PULSES_PER_BIT (16)
|
||||
|
||||
typedef struct {
|
||||
uint8_t data_index;
|
||||
uint8_t bit_clock_index;
|
||||
bool last_bit;
|
||||
bool current_polarity;
|
||||
bool pulse_phase;
|
||||
} ProtocolIndalaEncoder;
|
||||
|
||||
typedef struct {
|
||||
uint8_t encoded_data[INDALA26_ENCODED_DATA_SIZE];
|
||||
uint8_t negative_encoded_data[INDALA26_ENCODED_DATA_SIZE];
|
||||
uint8_t corrupted_encoded_data[INDALA26_ENCODED_DATA_SIZE];
|
||||
uint8_t corrupted_negative_encoded_data[INDALA26_ENCODED_DATA_SIZE];
|
||||
|
||||
uint8_t data[INDALA26_DECODED_DATA_SIZE];
|
||||
ProtocolIndalaEncoder encoder;
|
||||
} ProtocolIndala;
|
||||
|
||||
ProtocolIndala* protocol_indala26_alloc(void) {
|
||||
ProtocolIndala* protocol = malloc(sizeof(ProtocolIndala));
|
||||
return protocol;
|
||||
};
|
||||
|
||||
void protocol_indala26_free(ProtocolIndala* protocol) {
|
||||
free(protocol);
|
||||
};
|
||||
|
||||
uint8_t* protocol_indala26_get_data(ProtocolIndala* protocol) {
|
||||
return protocol->data;
|
||||
};
|
||||
|
||||
void protocol_indala26_decoder_start(ProtocolIndala* protocol) {
|
||||
memset(protocol->encoded_data, 0, INDALA26_ENCODED_DATA_SIZE);
|
||||
memset(protocol->negative_encoded_data, 0, INDALA26_ENCODED_DATA_SIZE);
|
||||
memset(protocol->corrupted_encoded_data, 0, INDALA26_ENCODED_DATA_SIZE);
|
||||
memset(protocol->corrupted_negative_encoded_data, 0, INDALA26_ENCODED_DATA_SIZE);
|
||||
};
|
||||
|
||||
static bool protocol_indala26_check_preamble(uint8_t* data, size_t bit_index) {
|
||||
// Preamble 10100000 00000000 00000000 00000000 1
|
||||
if(*(uint32_t*)&data[bit_index / 8] != 0b00000000000000000000000010100000) return false;
|
||||
if(bit_lib_get_bit(data, bit_index + 32) != 1) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool protocol_indala26_can_be_decoded(uint8_t* data) {
|
||||
if(!protocol_indala26_check_preamble(data, 0)) return false;
|
||||
if(!protocol_indala26_check_preamble(data, 64)) return false;
|
||||
if(bit_lib_get_bit(data, 61) != 0) return false;
|
||||
if(bit_lib_get_bit(data, 60) != 0) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool protocol_indala26_decoder_feed_internal(bool polarity, uint32_t time, uint8_t* data) {
|
||||
time += (INDALA26_US_PER_BIT / 2);
|
||||
|
||||
size_t bit_count = (time / INDALA26_US_PER_BIT);
|
||||
bool result = false;
|
||||
|
||||
if(bit_count < INDALA26_ENCODED_BIT_SIZE) {
|
||||
for(size_t i = 0; i < bit_count; i++) {
|
||||
bit_lib_push_bit(data, INDALA26_ENCODED_DATA_SIZE, polarity);
|
||||
if(protocol_indala26_can_be_decoded(data)) {
|
||||
result = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
static void protocol_indala26_decoder_save(uint8_t* data_to, const uint8_t* data_from) {
|
||||
bit_lib_copy_bits(data_to, 0, 22, data_from, 33);
|
||||
bit_lib_copy_bits(data_to, 22, 5, data_from, 55);
|
||||
bit_lib_copy_bits(data_to, 27, 2, data_from, 62);
|
||||
}
|
||||
|
||||
bool protocol_indala26_decoder_feed(ProtocolIndala* protocol, bool level, uint32_t duration) {
|
||||
bool result = false;
|
||||
|
||||
if(duration > (INDALA26_US_PER_BIT / 2)) {
|
||||
if(protocol_indala26_decoder_feed_internal(level, duration, protocol->encoded_data)) {
|
||||
protocol_indala26_decoder_save(protocol->data, protocol->encoded_data);
|
||||
FURI_LOG_D("Indala26", "Positive");
|
||||
result = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
if(protocol_indala26_decoder_feed_internal(
|
||||
!level, duration, protocol->negative_encoded_data)) {
|
||||
protocol_indala26_decoder_save(protocol->data, protocol->negative_encoded_data);
|
||||
FURI_LOG_D("Indala26", "Negative");
|
||||
result = true;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
if(duration > (INDALA26_US_PER_BIT / 4)) {
|
||||
// Try to decode wrong phase synced data
|
||||
if(level) {
|
||||
duration += 120;
|
||||
} else {
|
||||
if(duration > 120) {
|
||||
duration -= 120;
|
||||
}
|
||||
}
|
||||
|
||||
if(protocol_indala26_decoder_feed_internal(
|
||||
level, duration, protocol->corrupted_encoded_data)) {
|
||||
protocol_indala26_decoder_save(protocol->data, protocol->corrupted_encoded_data);
|
||||
FURI_LOG_D("Indala26", "Positive Corrupted");
|
||||
|
||||
result = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
if(protocol_indala26_decoder_feed_internal(
|
||||
!level, duration, protocol->corrupted_negative_encoded_data)) {
|
||||
protocol_indala26_decoder_save(
|
||||
protocol->data, protocol->corrupted_negative_encoded_data);
|
||||
FURI_LOG_D("Indala26", "Negative Corrupted");
|
||||
|
||||
result = true;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
};
|
||||
|
||||
bool protocol_indala26_encoder_start(ProtocolIndala* protocol) {
|
||||
memset(protocol->encoded_data, 0, INDALA26_ENCODED_DATA_SIZE);
|
||||
*(uint32_t*)&protocol->encoded_data[0] = 0b00000000000000000000000010100000;
|
||||
bit_lib_set_bit(protocol->encoded_data, 32, 1);
|
||||
bit_lib_copy_bits(protocol->encoded_data, 33, 22, protocol->data, 0);
|
||||
bit_lib_copy_bits(protocol->encoded_data, 55, 5, protocol->data, 22);
|
||||
bit_lib_copy_bits(protocol->encoded_data, 62, 2, protocol->data, 27);
|
||||
|
||||
protocol->encoder.last_bit =
|
||||
bit_lib_get_bit(protocol->encoded_data, INDALA26_ENCODED_BIT_SIZE - 1);
|
||||
protocol->encoder.data_index = 0;
|
||||
protocol->encoder.current_polarity = true;
|
||||
protocol->encoder.pulse_phase = true;
|
||||
protocol->encoder.bit_clock_index = 0;
|
||||
|
||||
return true;
|
||||
};
|
||||
|
||||
LevelDuration protocol_indala26_encoder_yield(ProtocolIndala* protocol) {
|
||||
LevelDuration level_duration;
|
||||
ProtocolIndalaEncoder* encoder = &protocol->encoder;
|
||||
|
||||
if(encoder->pulse_phase) {
|
||||
level_duration = level_duration_make(encoder->current_polarity, 1);
|
||||
encoder->pulse_phase = false;
|
||||
} else {
|
||||
level_duration = level_duration_make(!encoder->current_polarity, 1);
|
||||
encoder->pulse_phase = true;
|
||||
|
||||
encoder->bit_clock_index++;
|
||||
if(encoder->bit_clock_index >= INDALA26_ENCODER_PULSES_PER_BIT) {
|
||||
encoder->bit_clock_index = 0;
|
||||
|
||||
bool current_bit = bit_lib_get_bit(protocol->encoded_data, encoder->data_index);
|
||||
|
||||
if(current_bit != encoder->last_bit) {
|
||||
encoder->current_polarity = !encoder->current_polarity;
|
||||
}
|
||||
|
||||
encoder->last_bit = current_bit;
|
||||
|
||||
bit_lib_increment_index(encoder->data_index, INDALA26_ENCODED_BIT_SIZE);
|
||||
}
|
||||
}
|
||||
|
||||
return level_duration;
|
||||
};
|
||||
|
||||
// factory code
|
||||
static uint8_t get_fc(const uint8_t* data) {
|
||||
uint8_t fc = 0;
|
||||
|
||||
fc = fc << 1 | bit_lib_get_bit(data, 24);
|
||||
fc = fc << 1 | bit_lib_get_bit(data, 16);
|
||||
fc = fc << 1 | bit_lib_get_bit(data, 11);
|
||||
fc = fc << 1 | bit_lib_get_bit(data, 14);
|
||||
fc = fc << 1 | bit_lib_get_bit(data, 15);
|
||||
fc = fc << 1 | bit_lib_get_bit(data, 20);
|
||||
fc = fc << 1 | bit_lib_get_bit(data, 6);
|
||||
fc = fc << 1 | bit_lib_get_bit(data, 25);
|
||||
|
||||
return fc;
|
||||
}
|
||||
|
||||
// card number
|
||||
static uint16_t get_cn(const uint8_t* data) {
|
||||
uint16_t cn = 0;
|
||||
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 9);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 12);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 10);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 7);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 19);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 3);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 2);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 18);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 13);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 0);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 4);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 21);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 23);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 26);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 17);
|
||||
cn = cn << 1 | bit_lib_get_bit(data, 8);
|
||||
|
||||
return cn;
|
||||
}
|
||||
|
||||
void protocol_indala26_render_data_internal(ProtocolIndala* protocol, string_t result, bool brief) {
|
||||
bool wiegand_correct = true;
|
||||
bool checksum_correct = true;
|
||||
|
||||
const uint8_t fc = get_fc(protocol->data);
|
||||
const uint16_t card = get_cn(protocol->data);
|
||||
const uint32_t fc_and_card = fc << 16 | card;
|
||||
const uint8_t checksum = bit_lib_get_bit(protocol->data, 27) << 1 |
|
||||
bit_lib_get_bit(protocol->data, 28);
|
||||
const bool even_parity = bit_lib_get_bit(protocol->data, 1);
|
||||
const bool odd_parity = bit_lib_get_bit(protocol->data, 5);
|
||||
|
||||
// indala checksum
|
||||
uint8_t checksum_sum = 0;
|
||||
checksum_sum += ((fc_and_card >> 14) & 1);
|
||||
checksum_sum += ((fc_and_card >> 12) & 1);
|
||||
checksum_sum += ((fc_and_card >> 9) & 1);
|
||||
checksum_sum += ((fc_and_card >> 8) & 1);
|
||||
checksum_sum += ((fc_and_card >> 6) & 1);
|
||||
checksum_sum += ((fc_and_card >> 5) & 1);
|
||||
checksum_sum += ((fc_and_card >> 2) & 1);
|
||||
checksum_sum += ((fc_and_card >> 0) & 1);
|
||||
checksum_sum = checksum_sum & 0b1;
|
||||
|
||||
if(checksum_sum == 1 && checksum == 0b01) {
|
||||
} else if(checksum_sum == 0 && checksum == 0b10) {
|
||||
} else {
|
||||
checksum_correct = false;
|
||||
}
|
||||
|
||||
// wiegand parity
|
||||
uint8_t even_parity_sum = 0;
|
||||
for(int8_t i = 12; i < 24; i++) {
|
||||
if(((fc_and_card >> i) & 1) == 1) {
|
||||
even_parity_sum++;
|
||||
}
|
||||
}
|
||||
if(even_parity_sum % 2 != even_parity) wiegand_correct = false;
|
||||
|
||||
uint8_t odd_parity_sum = 1;
|
||||
for(int8_t i = 0; i < 12; i++) {
|
||||
if(((fc_and_card >> i) & 1) == 1) {
|
||||
odd_parity_sum++;
|
||||
}
|
||||
}
|
||||
if(odd_parity_sum % 2 != odd_parity) wiegand_correct = false;
|
||||
|
||||
if(brief) {
|
||||
string_printf(
|
||||
result,
|
||||
"FC: %u\r\nCard: %u, Parity:%s%s",
|
||||
fc,
|
||||
card,
|
||||
(checksum_correct ? "+" : "-"),
|
||||
(wiegand_correct ? "+" : "-"));
|
||||
} else {
|
||||
string_printf(
|
||||
result,
|
||||
"FC: %u\r\n"
|
||||
"Card: %u\r\n"
|
||||
"Checksum: %s\r\n"
|
||||
"W26 Parity: %s",
|
||||
fc,
|
||||
card,
|
||||
(checksum_correct ? "+" : "-"),
|
||||
(wiegand_correct ? "+" : "-"));
|
||||
}
|
||||
}
|
||||
void protocol_indala26_render_data(ProtocolIndala* protocol, string_t result) {
|
||||
protocol_indala26_render_data_internal(protocol, result, false);
|
||||
}
|
||||
void protocol_indala26_render_brief_data(ProtocolIndala* protocol, string_t result) {
|
||||
protocol_indala26_render_data_internal(protocol, result, true);
|
||||
}
|
||||
|
||||
bool protocol_indala26_write_data(ProtocolIndala* protocol, void* data) {
|
||||
LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data;
|
||||
bool result = false;
|
||||
|
||||
protocol_indala26_encoder_start(protocol);
|
||||
|
||||
if(request->write_type == LFRFIDWriteTypeT5577) {
|
||||
request->t5577.block[0] = LFRFID_T5577_BITRATE_RF_32 | LFRFID_T5577_MODULATION_PSK1 |
|
||||
(2 << 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.blocks_to_write = 3;
|
||||
result = true;
|
||||
}
|
||||
return result;
|
||||
};
|
||||
|
||||
const ProtocolBase protocol_indala26 = {
|
||||
.name = "Indala26",
|
||||
.manufacturer = "Motorola",
|
||||
.data_size = INDALA26_DECODED_DATA_SIZE,
|
||||
.features = LFRFIDFeaturePSK,
|
||||
.validate_count = 6,
|
||||
.alloc = (ProtocolAlloc)protocol_indala26_alloc,
|
||||
.free = (ProtocolFree)protocol_indala26_free,
|
||||
.get_data = (ProtocolGetData)protocol_indala26_get_data,
|
||||
.decoder =
|
||||
{
|
||||
.start = (ProtocolDecoderStart)protocol_indala26_decoder_start,
|
||||
.feed = (ProtocolDecoderFeed)protocol_indala26_decoder_feed,
|
||||
},
|
||||
.encoder =
|
||||
{
|
||||
.start = (ProtocolEncoderStart)protocol_indala26_encoder_start,
|
||||
.yield = (ProtocolEncoderYield)protocol_indala26_encoder_yield,
|
||||
},
|
||||
.render_data = (ProtocolRenderData)protocol_indala26_render_data,
|
||||
.render_brief_data = (ProtocolRenderData)protocol_indala26_render_brief_data,
|
||||
.write_data = (ProtocolWriteData)protocol_indala26_write_data,
|
||||
};
|
||||
4
lib/lfrfid/protocols/protocol_indala26.h
Normal file
4
lib/lfrfid/protocols/protocol_indala26.h
Normal file
@@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
|
||||
extern const ProtocolBase protocol_indala26;
|
||||
297
lib/lfrfid/protocols/protocol_io_prox_xsf.c
Normal file
297
lib/lfrfid/protocols/protocol_io_prox_xsf.c
Normal file
@@ -0,0 +1,297 @@
|
||||
#include <furi.h>
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
#include <lfrfid/tools/fsk_demod.h>
|
||||
#include <lfrfid/tools/fsk_osc.h>
|
||||
#include <lfrfid/tools/bit_lib.h>
|
||||
#include "lfrfid_protocols.h"
|
||||
|
||||
#define JITTER_TIME (20)
|
||||
#define MIN_TIME (64 - JITTER_TIME)
|
||||
#define MAX_TIME (80 + JITTER_TIME)
|
||||
|
||||
#define IOPROXXSF_DECODED_DATA_SIZE (4)
|
||||
#define IOPROXXSF_ENCODED_DATA_SIZE (8)
|
||||
|
||||
#define IOPROXXSF_BIT_SIZE (8)
|
||||
#define IOPROXXSF_BIT_MAX_SIZE (IOPROXXSF_BIT_SIZE * IOPROXXSF_ENCODED_DATA_SIZE)
|
||||
|
||||
typedef struct {
|
||||
FSKDemod* fsk_demod;
|
||||
} ProtocolIOProxXSFDecoder;
|
||||
|
||||
typedef struct {
|
||||
FSKOsc* fsk_osc;
|
||||
uint8_t encoded_index;
|
||||
} ProtocolIOProxXSFEncoder;
|
||||
|
||||
typedef struct {
|
||||
ProtocolIOProxXSFEncoder encoder;
|
||||
ProtocolIOProxXSFDecoder decoder;
|
||||
uint8_t encoded_data[IOPROXXSF_ENCODED_DATA_SIZE];
|
||||
uint8_t data[IOPROXXSF_DECODED_DATA_SIZE];
|
||||
} ProtocolIOProxXSF;
|
||||
|
||||
ProtocolIOProxXSF* protocol_io_prox_xsf_alloc(void) {
|
||||
ProtocolIOProxXSF* protocol = malloc(sizeof(ProtocolIOProxXSF));
|
||||
protocol->decoder.fsk_demod = fsk_demod_alloc(MIN_TIME, 8, MAX_TIME, 6);
|
||||
protocol->encoder.fsk_osc = fsk_osc_alloc(8, 10, 64);
|
||||
return protocol;
|
||||
};
|
||||
|
||||
void protocol_io_prox_xsf_free(ProtocolIOProxXSF* protocol) {
|
||||
fsk_demod_free(protocol->decoder.fsk_demod);
|
||||
fsk_osc_free(protocol->encoder.fsk_osc);
|
||||
free(protocol);
|
||||
};
|
||||
|
||||
uint8_t* protocol_io_prox_xsf_get_data(ProtocolIOProxXSF* protocol) {
|
||||
return protocol->data;
|
||||
};
|
||||
|
||||
void protocol_io_prox_xsf_decoder_start(ProtocolIOProxXSF* protocol) {
|
||||
memset(protocol->encoded_data, 0, IOPROXXSF_ENCODED_DATA_SIZE);
|
||||
};
|
||||
|
||||
static uint8_t protocol_io_prox_xsf_compute_checksum(const uint8_t* data) {
|
||||
// Packet structure:
|
||||
//
|
||||
//0 1 2 3 4 5 6 7
|
||||
//v v v v v v v v
|
||||
//01234567 8 9ABCDEF0 1 23456789 A BCDEF012 3 456789AB C DEF01234 5 6789ABCD EF
|
||||
//00000000 0 VVVVVVVV 1 WWWWWWWW 1 XXXXXXXX 1 YYYYYYYY 1 ZZZZZZZZ 1 CHECKSUM 11
|
||||
//
|
||||
// algorithm as observed by the proxmark3 folks
|
||||
// CHECKSUM == 0xFF - (V + W + X + Y + Z)
|
||||
|
||||
uint8_t checksum = 0;
|
||||
|
||||
for(size_t i = 1; i <= 5; i++) {
|
||||
checksum += bit_lib_get_bits(data, 9 * i, 8);
|
||||
}
|
||||
|
||||
return 0xFF - checksum;
|
||||
}
|
||||
|
||||
static bool protocol_io_prox_xsf_can_be_decoded(const uint8_t* encoded_data) {
|
||||
// Packet framing
|
||||
//
|
||||
//0 1 2 3 4 5 6 7
|
||||
//v v v v v v v v
|
||||
//01234567 89ABCDEF 01234567 89ABCDEF 01234567 89ABCDEF 01234567 89ABCDEF
|
||||
//-----------------------------------------------------------------------
|
||||
//00000000 01______ _1______ __1_____ ___1____ ____1___ _____1XX XXXXXX11
|
||||
//
|
||||
// _ = variable data
|
||||
// 0 = preamble 0
|
||||
// 1 = framing 1
|
||||
// X = checksum
|
||||
|
||||
// Validate the packet preamble is there...
|
||||
if(encoded_data[0] != 0b00000000) {
|
||||
return false;
|
||||
}
|
||||
if((encoded_data[1] >> 6) != 0b01) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// ... check for known ones...
|
||||
if(bit_lib_bit_is_not_set(encoded_data[2], 6)) {
|
||||
return false;
|
||||
}
|
||||
if(bit_lib_bit_is_not_set(encoded_data[3], 5)) {
|
||||
return false;
|
||||
}
|
||||
if(bit_lib_bit_is_not_set(encoded_data[4], 4)) {
|
||||
return false;
|
||||
}
|
||||
if(bit_lib_bit_is_not_set(encoded_data[5], 3)) {
|
||||
return false;
|
||||
}
|
||||
if(bit_lib_bit_is_not_set(encoded_data[6], 2)) {
|
||||
return false;
|
||||
}
|
||||
if(bit_lib_bit_is_not_set(encoded_data[7], 1)) {
|
||||
return false;
|
||||
}
|
||||
if(bit_lib_bit_is_not_set(encoded_data[7], 0)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// ... and validate our checksums.
|
||||
uint8_t checksum = protocol_io_prox_xsf_compute_checksum(encoded_data);
|
||||
uint8_t checkval = bit_lib_get_bits(encoded_data, 54, 8);
|
||||
|
||||
if(checksum != checkval) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void protocol_io_prox_xsf_decode(const uint8_t* encoded_data, uint8_t* decoded_data) {
|
||||
// Packet structure:
|
||||
// (Note: the second word seems fixed; but this may not be a guarantee;
|
||||
// it currently has no meaning.)
|
||||
//
|
||||
//0 1 2 3 4 5 6 7
|
||||
//v v v v v v v v
|
||||
//01234567 89ABCDEF 01234567 89ABCDEF 01234567 89ABCDEF 01234567 89ABCDEF
|
||||
//-----------------------------------------------------------------------
|
||||
//00000000 01111000 01FFFFFF FF1VVVVV VVV1CCCC CCCC1CCC CCCCC1XX XXXXXX11
|
||||
//
|
||||
// F = facility code
|
||||
// V = version
|
||||
// C = code
|
||||
// X = checksum
|
||||
|
||||
// Facility code
|
||||
decoded_data[0] = bit_lib_get_bits(encoded_data, 18, 8);
|
||||
|
||||
// Version code.
|
||||
decoded_data[1] = bit_lib_get_bits(encoded_data, 27, 8);
|
||||
|
||||
// Code bytes.
|
||||
decoded_data[2] = bit_lib_get_bits(encoded_data, 36, 8);
|
||||
decoded_data[3] = bit_lib_get_bits(encoded_data, 45, 8);
|
||||
}
|
||||
|
||||
bool protocol_io_prox_xsf_decoder_feed(ProtocolIOProxXSF* protocol, bool level, uint32_t duration) {
|
||||
bool result = false;
|
||||
|
||||
uint32_t count;
|
||||
bool value;
|
||||
|
||||
fsk_demod_feed(protocol->decoder.fsk_demod, level, duration, &value, &count);
|
||||
for(size_t i = 0; i < count; i++) {
|
||||
bit_lib_push_bit(protocol->encoded_data, IOPROXXSF_ENCODED_DATA_SIZE, value);
|
||||
if(protocol_io_prox_xsf_can_be_decoded(protocol->encoded_data)) {
|
||||
protocol_io_prox_xsf_decode(protocol->encoded_data, protocol->data);
|
||||
result = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
};
|
||||
|
||||
static void protocol_io_prox_xsf_encode(const uint8_t* decoded_data, uint8_t* encoded_data) {
|
||||
// Packet to transmit:
|
||||
//
|
||||
// 0 10 20 30 40 50 60
|
||||
// v v v v v v v
|
||||
// 01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
|
||||
// -----------------------------------------------------------------------------
|
||||
// 00000000 0 11110000 1 facility 1 version_ 1 code-one 1 code-two 1 checksum 11
|
||||
|
||||
// Preamble.
|
||||
bit_lib_set_bits(encoded_data, 0, 0b00000000, 8);
|
||||
bit_lib_set_bit(encoded_data, 8, 0);
|
||||
|
||||
bit_lib_set_bits(encoded_data, 9, 0b11110000, 8);
|
||||
bit_lib_set_bit(encoded_data, 17, 1);
|
||||
|
||||
// Facility code.
|
||||
bit_lib_set_bits(encoded_data, 18, decoded_data[0], 8);
|
||||
bit_lib_set_bit(encoded_data, 26, 1);
|
||||
|
||||
// Version
|
||||
bit_lib_set_bits(encoded_data, 27, decoded_data[1], 8);
|
||||
bit_lib_set_bit(encoded_data, 35, 1);
|
||||
|
||||
// Code one
|
||||
bit_lib_set_bits(encoded_data, 36, decoded_data[2], 8);
|
||||
bit_lib_set_bit(encoded_data, 44, 1);
|
||||
|
||||
// Code two
|
||||
bit_lib_set_bits(encoded_data, 45, decoded_data[3], 8);
|
||||
bit_lib_set_bit(encoded_data, 53, 1);
|
||||
|
||||
// Checksum
|
||||
bit_lib_set_bits(encoded_data, 54, protocol_io_prox_xsf_compute_checksum(encoded_data), 8);
|
||||
bit_lib_set_bit(encoded_data, 62, 1);
|
||||
bit_lib_set_bit(encoded_data, 63, 1);
|
||||
}
|
||||
|
||||
bool protocol_io_prox_xsf_encoder_start(ProtocolIOProxXSF* protocol) {
|
||||
protocol_io_prox_xsf_encode(protocol->data, protocol->encoded_data);
|
||||
protocol->encoder.encoded_index = 0;
|
||||
fsk_osc_reset(protocol->encoder.fsk_osc);
|
||||
return true;
|
||||
};
|
||||
|
||||
LevelDuration protocol_io_prox_xsf_encoder_yield(ProtocolIOProxXSF* 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, IOPROXXSF_BIT_MAX_SIZE);
|
||||
}
|
||||
return level_duration_make(level, duration);
|
||||
};
|
||||
|
||||
void protocol_io_prox_xsf_render_data(ProtocolIOProxXSF* protocol, string_t result) {
|
||||
uint8_t* data = protocol->data;
|
||||
string_printf(
|
||||
result,
|
||||
"FC: %u\r\n"
|
||||
"VС: %u\r\n"
|
||||
"Card: %u",
|
||||
data[0],
|
||||
data[1],
|
||||
(uint16_t)((data[2] << 8) | (data[3])));
|
||||
}
|
||||
|
||||
void protocol_io_prox_xsf_render_brief_data(ProtocolIOProxXSF* protocol, string_t result) {
|
||||
uint8_t* data = protocol->data;
|
||||
string_printf(
|
||||
result,
|
||||
"FC: %u, VС: %u\r\n"
|
||||
"Card: %u",
|
||||
data[0],
|
||||
data[1],
|
||||
(uint16_t)((data[2] << 8) | (data[3])));
|
||||
}
|
||||
|
||||
bool protocol_io_prox_xsf_write_data(ProtocolIOProxXSF* protocol, void* data) {
|
||||
LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data;
|
||||
bool result = false;
|
||||
|
||||
protocol_io_prox_xsf_encode(protocol->data, protocol->encoded_data);
|
||||
|
||||
if(request->write_type == LFRFIDWriteTypeT5577) {
|
||||
request->t5577.block[0] = LFRFID_T5577_MODULATION_FSK2a | LFRFID_T5577_BITRATE_RF_64 |
|
||||
(2 << 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.blocks_to_write = 3;
|
||||
result = true;
|
||||
}
|
||||
return result;
|
||||
};
|
||||
|
||||
const ProtocolBase protocol_io_prox_xsf = {
|
||||
.name = "IoProxXSF",
|
||||
.manufacturer = "Kantech",
|
||||
.data_size = IOPROXXSF_DECODED_DATA_SIZE,
|
||||
.features = LFRFIDFeatureASK,
|
||||
.validate_count = 3,
|
||||
.alloc = (ProtocolAlloc)protocol_io_prox_xsf_alloc,
|
||||
.free = (ProtocolFree)protocol_io_prox_xsf_free,
|
||||
.get_data = (ProtocolGetData)protocol_io_prox_xsf_get_data,
|
||||
.decoder =
|
||||
{
|
||||
.start = (ProtocolDecoderStart)protocol_io_prox_xsf_decoder_start,
|
||||
.feed = (ProtocolDecoderFeed)protocol_io_prox_xsf_decoder_feed,
|
||||
},
|
||||
.encoder =
|
||||
{
|
||||
.start = (ProtocolEncoderStart)protocol_io_prox_xsf_encoder_start,
|
||||
.yield = (ProtocolEncoderYield)protocol_io_prox_xsf_encoder_yield,
|
||||
},
|
||||
.render_data = (ProtocolRenderData)protocol_io_prox_xsf_render_data,
|
||||
.render_brief_data = (ProtocolRenderData)protocol_io_prox_xsf_render_brief_data,
|
||||
.write_data = (ProtocolWriteData)protocol_io_prox_xsf_write_data,
|
||||
};
|
||||
4
lib/lfrfid/protocols/protocol_io_prox_xsf.h
Normal file
4
lib/lfrfid/protocols/protocol_io_prox_xsf.h
Normal file
@@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
#include <toolbox/protocols/protocol.h>
|
||||
|
||||
extern const ProtocolBase protocol_io_prox_xsf;
|
||||
Reference in New Issue
Block a user