From 1350dcaf63fb9883642b585cbbf272f013a9528f Mon Sep 17 00:00:00 2001 From: Sebastian Mauer Date: Mon, 29 Aug 2022 16:04:17 +0100 Subject: [PATCH] Add support for PAC/Stanley tags (#1648) * Add support for PAC/Stanley tags * Address review comments --- lib/flipper_format/flipper_format_stream.c | 2 +- lib/lfrfid/protocols/lfrfid_protocols.c | 2 + lib/lfrfid/protocols/lfrfid_protocols.h | 1 + lib/lfrfid/protocols/protocol_pac_stanley.c | 227 ++++++++++++++++++++ lib/lfrfid/protocols/protocol_pac_stanley.h | 4 + lib/lfrfid/tools/bit_lib.c | 7 + lib/lfrfid/tools/bit_lib.h | 8 + lib/nfc/nfc_device.c | 2 +- lib/toolbox/hex.c | 20 +- lib/toolbox/hex.h | 20 +- 10 files changed, 287 insertions(+), 6 deletions(-) create mode 100644 lib/lfrfid/protocols/protocol_pac_stanley.c create mode 100644 lib/lfrfid/protocols/protocol_pac_stanley.h diff --git a/lib/flipper_format/flipper_format_stream.c b/lib/flipper_format/flipper_format_stream.c index 81189b69..e4b7b300 100644 --- a/lib/flipper_format/flipper_format_stream.c +++ b/lib/flipper_format/flipper_format_stream.c @@ -356,7 +356,7 @@ bool flipper_format_stream_read_value_line( uint8_t* data = _data; if(string_size(value) >= 2) { // sscanf "%02X" does not work here - if(hex_chars_to_uint8( + if(hex_char_to_uint8( string_get_char(value, 0), string_get_char(value, 1), &data[i])) { diff --git a/lib/lfrfid/protocols/lfrfid_protocols.c b/lib/lfrfid/protocols/lfrfid_protocols.c index 5df01b19..8014c917 100644 --- a/lib/lfrfid/protocols/lfrfid_protocols.c +++ b/lib/lfrfid/protocols/lfrfid_protocols.c @@ -8,6 +8,7 @@ #include "protocol_fdx_b.h" #include "protocol_hid_generic.h" #include "protocol_hid_ex_generic.h" +#include "protocol_pac_stanley.h" const ProtocolBase* lfrfid_protocols[] = { [LFRFIDProtocolEM4100] = &protocol_em4100, @@ -19,4 +20,5 @@ const ProtocolBase* lfrfid_protocols[] = { [LFRFIDProtocolFDXB] = &protocol_fdx_b, [LFRFIDProtocolHidGeneric] = &protocol_hid_generic, [LFRFIDProtocolHidExGeneric] = &protocol_hid_ex_generic, + [LFRFIDProtocolPACStanley] = &protocol_pac_stanley, }; \ No newline at end of file diff --git a/lib/lfrfid/protocols/lfrfid_protocols.h b/lib/lfrfid/protocols/lfrfid_protocols.h index 4b8f6573..2a216412 100644 --- a/lib/lfrfid/protocols/lfrfid_protocols.h +++ b/lib/lfrfid/protocols/lfrfid_protocols.h @@ -17,6 +17,7 @@ typedef enum { LFRFIDProtocolFDXB, LFRFIDProtocolHidGeneric, LFRFIDProtocolHidExGeneric, + LFRFIDProtocolPACStanley, LFRFIDProtocolMax, } LFRFIDProtocol; diff --git a/lib/lfrfid/protocols/protocol_pac_stanley.c b/lib/lfrfid/protocols/protocol_pac_stanley.c new file mode 100644 index 00000000..7ab16a10 --- /dev/null +++ b/lib/lfrfid/protocols/protocol_pac_stanley.c @@ -0,0 +1,227 @@ +#include +#include +#include +#include +#include +#include "lfrfid_protocols.h" + +#define PAC_STANLEY_ENCODED_BIT_SIZE (128) +#define PAC_STANLEY_ENCODED_BYTE_SIZE (((PAC_STANLEY_ENCODED_BIT_SIZE) / 8)) +#define PAC_STANLEY_PREAMBLE_BIT_SIZE (8) +#define PAC_STANLEY_PREAMBLE_BYTE_SIZE (1) +#define PAC_STANLEY_ENCODED_BYTE_FULL_SIZE \ + (PAC_STANLEY_ENCODED_BYTE_SIZE + PAC_STANLEY_PREAMBLE_BYTE_SIZE) +#define PAC_STANLEY_BYTE_LENGTH (10) // start bit, 7 data bits, parity bit, stop bit +#define PAC_STANLEY_DATA_START_INDEX 8 + (3 * PAC_STANLEY_BYTE_LENGTH) + 1 + +#define PAC_STANLEY_DECODED_DATA_SIZE (4) +#define PAC_STANLEY_ENCODED_DATA_SIZE (sizeof(ProtocolPACStanley)) + +#define PAC_STANLEY_CLOCKS_IN_US (32) +#define PAC_STANLEY_CYCLE_LENGTH (256) +#define PAC_STANLEY_MIN_TIME (60) +#define PAC_STANLEY_MAX_TIME (4000) + +typedef struct { + bool inverted; + bool got_preamble; + size_t encoded_index; + uint8_t encoded_data[PAC_STANLEY_ENCODED_BYTE_FULL_SIZE]; + uint8_t data[PAC_STANLEY_DECODED_DATA_SIZE]; +} ProtocolPACStanley; + +ProtocolPACStanley* protocol_pac_stanley_alloc(void) { + ProtocolPACStanley* protocol = malloc(sizeof(ProtocolPACStanley)); + return (void*)protocol; +} + +void protocol_pac_stanley_free(ProtocolPACStanley* protocol) { + free(protocol); +} + +uint8_t* protocol_pac_stanley_get_data(ProtocolPACStanley* protocol) { + return protocol->data; +} + +static void protocol_pac_stanley_decode(ProtocolPACStanley* protocol) { + uint8_t asciiCardId[8]; + for(size_t idx = 0; idx < 8; idx++) { + uint8_t byte = bit_lib_reverse_8_fast(bit_lib_get_bits( + protocol->encoded_data, + PAC_STANLEY_DATA_START_INDEX + (PAC_STANLEY_BYTE_LENGTH * idx), + 8)); + asciiCardId[idx] = byte & 0x7F; // discard the parity bit + } + + hex_chars_to_uint8((char*)asciiCardId, protocol->data); +} + +static bool protocol_pac_stanley_can_be_decoded(ProtocolPACStanley* protocol) { + // Check preamble + if(bit_lib_get_bits(protocol->encoded_data, 0, 8) != 0b11111111) return false; + if(bit_lib_get_bit(protocol->encoded_data, 8) != 0) return false; + if(bit_lib_get_bit(protocol->encoded_data, 9) != 0) return false; + if(bit_lib_get_bit(protocol->encoded_data, 10) != 1) return false; + if(bit_lib_get_bits(protocol->encoded_data, 11, 8) != 0b00000010) return false; + + // Check next preamble + if(bit_lib_get_bits(protocol->encoded_data, 128, 8) != 0b11111111) return false; + + // Checksum + uint8_t checksum = 0; + uint8_t stripped_byte; + for(size_t idx = 0; idx < 9; idx++) { + uint8_t byte = bit_lib_reverse_8_fast(bit_lib_get_bits( + protocol->encoded_data, + PAC_STANLEY_DATA_START_INDEX + (PAC_STANLEY_BYTE_LENGTH * idx), + 8)); + stripped_byte = byte & 0x7F; // discard the parity bit + if(bit_lib_test_parity_32(stripped_byte, BitLibParityOdd) != (byte & 0x80) >> 7) { + return false; + } + if(idx < 8) checksum ^= stripped_byte; + } + if(stripped_byte != checksum) return false; + return true; +} + +void protocol_pac_stanley_decoder_start(ProtocolPACStanley* protocol) { + memset(protocol->data, 0, PAC_STANLEY_DECODED_DATA_SIZE); + protocol->inverted = false; + protocol->got_preamble = false; +} + +bool protocol_pac_stanley_decoder_feed(ProtocolPACStanley* protocol, bool level, uint32_t duration) { + bool pushed = false; + + if(duration > PAC_STANLEY_MAX_TIME) return false; + + uint8_t pulses = (uint8_t)round((float)duration / PAC_STANLEY_CYCLE_LENGTH); + + // Handle last stopbit & preamble (1 sb, 8 bit preamble) + if(pulses >= 9 && !protocol->got_preamble) { + pulses = 8; + protocol->got_preamble = true; + protocol->inverted = !level; + } else if(pulses >= 9 && protocol->got_preamble) { + protocol->got_preamble = false; + } else if(pulses == 0 && duration > PAC_STANLEY_MIN_TIME) { + pulses = 1; + } + + if(pulses) { + for(uint8_t i = 0; i < pulses; i++) { + bit_lib_push_bit( + protocol->encoded_data, + PAC_STANLEY_ENCODED_BYTE_FULL_SIZE, + level ^ protocol->inverted); + } + pushed = true; + } + + if(pushed && protocol_pac_stanley_can_be_decoded(protocol)) { + protocol_pac_stanley_decode(protocol); + return true; + } + + return false; +} + +bool protocol_pac_stanley_encoder_start(ProtocolPACStanley* protocol) { + memset(protocol->encoded_data, 0, PAC_STANLEY_ENCODED_BYTE_SIZE); + + uint8_t idbytes[10]; + idbytes[0] = '2'; + idbytes[1] = '0'; + + uint8_to_hex_chars(protocol->data, &idbytes[2], 8); + + // insert start and stop bits + for(size_t i = 0; i < 16; i++) protocol->encoded_data[i] = 0x40 >> (i + 3) % 5 * 2; + + protocol->encoded_data[0] = 0xFF; // mark + stop + protocol->encoded_data[1] = 0x20; // start + reflect8(STX) + + uint8_t checksum = 0; + for(size_t i = 2; i < 13; i++) { + uint8_t shift = 7 - (i + 3) % 4 * 2; + uint8_t index = i + (i - 1) / 4; + + uint16_t pattern; + if(i < 12) { + pattern = bit_lib_reverse_8_fast(idbytes[i - 2]); + pattern |= bit_lib_test_parity_32(pattern, BitLibParityOdd); + if(i > 3) checksum ^= idbytes[i - 2]; + } else { + pattern = (bit_lib_reverse_8_fast(checksum) & 0xFE) | + (bit_lib_test_parity_32(checksum, BitLibParityOdd)); + } + pattern <<= shift; + + protocol->encoded_data[index] |= pattern >> 8 & 0xFF; + protocol->encoded_data[index + 1] |= pattern & 0xFF; + } + + protocol->encoded_index = 0; + return true; +} + +LevelDuration protocol_pac_stanley_encoder_yield(ProtocolPACStanley* protocol) { + uint16_t length = PAC_STANLEY_CLOCKS_IN_US; + bool bit = bit_lib_get_bit(protocol->encoded_data, protocol->encoded_index); + bit_lib_increment_index(protocol->encoded_index, PAC_STANLEY_ENCODED_BIT_SIZE); + while(bit_lib_get_bit(protocol->encoded_data, protocol->encoded_index) == bit) { + length += PAC_STANLEY_CLOCKS_IN_US; + bit_lib_increment_index(protocol->encoded_index, PAC_STANLEY_ENCODED_BIT_SIZE); + } + + return level_duration_make(bit, length); +} + +bool protocol_pac_stanley_write_data(ProtocolPACStanley* protocol, void* data) { + LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data; + bool result = false; + + protocol_pac_stanley_encoder_start(protocol); + + if(request->write_type == LFRFIDWriteTypeT5577) { + request->t5577.block[0] = LFRFID_T5577_MODULATION_DIRECT | 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; +} + +void protocol_pac_stanley_render_data(ProtocolPACStanley* protocol, string_t result) { + uint8_t* data = protocol->data; + string_printf(result, "CIN: %02X%02X%02X%02X", data[0], data[1], data[2], data[3]); +} + +const ProtocolBase protocol_pac_stanley = { + .name = "PAC/Stanley", + .manufacturer = "N/A", + .data_size = PAC_STANLEY_DECODED_DATA_SIZE, + .features = LFRFIDFeatureASK, + .validate_count = 3, + .alloc = (ProtocolAlloc)protocol_pac_stanley_alloc, + .free = (ProtocolFree)protocol_pac_stanley_free, + .get_data = (ProtocolGetData)protocol_pac_stanley_get_data, + .decoder = + { + .start = (ProtocolDecoderStart)protocol_pac_stanley_decoder_start, + .feed = (ProtocolDecoderFeed)protocol_pac_stanley_decoder_feed, + }, + .encoder = + { + .start = (ProtocolEncoderStart)protocol_pac_stanley_encoder_start, + .yield = (ProtocolEncoderYield)protocol_pac_stanley_encoder_yield, + }, + .render_data = (ProtocolRenderData)protocol_pac_stanley_render_data, + .render_brief_data = (ProtocolRenderData)protocol_pac_stanley_render_data, + .write_data = (ProtocolWriteData)protocol_pac_stanley_write_data, +}; \ No newline at end of file diff --git a/lib/lfrfid/protocols/protocol_pac_stanley.h b/lib/lfrfid/protocols/protocol_pac_stanley.h new file mode 100644 index 00000000..3ca329cf --- /dev/null +++ b/lib/lfrfid/protocols/protocol_pac_stanley.h @@ -0,0 +1,4 @@ +#pragma once +#include + +extern const ProtocolBase protocol_pac_stanley; \ No newline at end of file diff --git a/lib/lfrfid/tools/bit_lib.c b/lib/lfrfid/tools/bit_lib.c index b57bda8a..2df12707 100644 --- a/lib/lfrfid/tools/bit_lib.c +++ b/lib/lfrfid/tools/bit_lib.c @@ -262,6 +262,13 @@ uint16_t bit_lib_reverse_16_fast(uint16_t data) { return result; } +uint8_t bit_lib_reverse_8_fast(uint8_t byte) { + byte = (byte & 0xF0) >> 4 | (byte & 0x0F) << 4; + byte = (byte & 0xCC) >> 2 | (byte & 0x33) << 2; + byte = (byte & 0xAA) >> 1 | (byte & 0x55) << 1; + return byte; +} + uint16_t bit_lib_crc16( uint8_t const* data, size_t data_size, diff --git a/lib/lfrfid/tools/bit_lib.h b/lib/lfrfid/tools/bit_lib.h index 24aae0e9..482ae36b 100644 --- a/lib/lfrfid/tools/bit_lib.h +++ b/lib/lfrfid/tools/bit_lib.h @@ -194,6 +194,14 @@ void bit_lib_print_regions( */ uint16_t bit_lib_reverse_16_fast(uint16_t data); +/** + * @brief Reverse bits in uint8_t, faster than generic bit_lib_reverse_bits. + * + * @param byte Byte + * @return uint8_t the reversed byte + */ +uint8_t bit_lib_reverse_8_fast(uint8_t byte); + /** * @brief Slow, but generic CRC16 implementation * diff --git a/lib/nfc/nfc_device.c b/lib/nfc/nfc_device.c index 0bfdb3da..c4691921 100644 --- a/lib/nfc/nfc_device.c +++ b/lib/nfc/nfc_device.c @@ -782,7 +782,7 @@ static void nfc_device_load_mifare_classic_block( char hi = string_get_char(block_str, 3 * i); char low = string_get_char(block_str, 3 * i + 1); uint8_t byte = 0; - if(hex_chars_to_uint8(hi, low, &byte)) { + if(hex_char_to_uint8(hi, low, &byte)) { block_tmp.value[i] = byte; } else { FURI_BIT_SET(block_unknown_bytes_mask, i); diff --git a/lib/toolbox/hex.c b/lib/toolbox/hex.c index 41bb24bb..7b2719b7 100644 --- a/lib/toolbox/hex.c +++ b/lib/toolbox/hex.c @@ -15,7 +15,7 @@ bool hex_char_to_hex_nibble(char c, uint8_t* nibble) { } } -bool hex_chars_to_uint8(char hi, char low, uint8_t* value) { +bool hex_char_to_uint8(char hi, char low, uint8_t* value) { uint8_t hi_nibble_value, low_nibble_value; if(hex_char_to_hex_nibble(hi, &hi_nibble_value) && @@ -27,13 +27,29 @@ bool hex_chars_to_uint8(char hi, char low, uint8_t* value) { } } +bool hex_chars_to_uint8(const char* value_str, uint8_t* value) { + bool parse_success = false; + while(*value_str && value_str[1]) { + parse_success = hex_char_to_uint8(*value_str, value_str[1], value++); + if(!parse_success) break; + value_str += 2; + } + return parse_success; +} + bool hex_chars_to_uint64(const char* value_str, uint64_t* value) { uint8_t* _value = (uint8_t*)value; bool parse_success = false; for(uint8_t i = 0; i < 8; i++) { - parse_success = hex_chars_to_uint8(value_str[i * 2], value_str[i * 2 + 1], &_value[7 - i]); + parse_success = hex_char_to_uint8(value_str[i * 2], value_str[i * 2 + 1], &_value[7 - i]); if(!parse_success) break; } return parse_success; } + +void uint8_to_hex_chars(const uint8_t* src, uint8_t* target, int length) { + const char chars[] = "0123456789ABCDEF"; + while(--length >= 0) + target[length] = chars[(src[length >> 1] >> ((1 - (length & 1)) << 2)) & 0xF]; +} diff --git a/lib/toolbox/hex.h b/lib/toolbox/hex.h index c6683a52..740f23b7 100644 --- a/lib/toolbox/hex.h +++ b/lib/toolbox/hex.h @@ -14,14 +14,22 @@ extern "C" { */ bool hex_char_to_hex_nibble(char c, uint8_t* nibble); -/** Convert ASCII hex values to byte +/** Convert ASCII hex value to byte * @param hi hi nibble text * @param low low nibble text * @param value output value * * @return bool conversion status */ -bool hex_chars_to_uint8(char hi, char low, uint8_t* value); +bool hex_char_to_uint8(char hi, char low, uint8_t* value); + +/** Convert ASCII hex values to uint8_t + * @param value_str ASCII data + * @param value output value + * + * @return bool conversion status + */ +bool hex_chars_to_uint8(const char* value_str, uint8_t* value); /** Convert ASCII hex values to uint64_t * @param value_str ASCII 64 bi data @@ -31,6 +39,14 @@ bool hex_chars_to_uint8(char hi, char low, uint8_t* value); */ bool hex_chars_to_uint64(const char* value_str, uint64_t* value); +/** Convert uint8_t to ASCII hex values + * @param src source data + * @param target output value + * @param length data length + * + */ +void uint8_to_hex_chars(const uint8_t* src, uint8_t* target, int length); + #ifdef __cplusplus } #endif