Add support for PAC/Stanley tags (#1648)

* Add support for PAC/Stanley tags
* Address review comments

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])) {

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,
 };

lib/lfrfid/protocols/lfrfid_protocols.h

@@ -17,6 +17,7 @@ typedef enum {
     LFRFIDProtocolFDXB,
     LFRFIDProtocolHidGeneric,
     LFRFIDProtocolHidExGeneric,
+    LFRFIDProtocolPACStanley,
 
     LFRFIDProtocolMax,
 } LFRFIDProtocol;

lib/lfrfid/protocols/protocol_pac_stanley.c

@@ -0,0 +1,227 @@
+#include <furi.h>
+#include <math.h>
+#include <toolbox/protocols/protocol.h>
+#include <toolbox/hex.h>
+#include <lfrfid/tools/bit_lib.h>
+#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,
+};

lib/lfrfid/protocols/protocol_pac_stanley.h

@@ -0,0 +1,4 @@
+#pragma once
+#include <toolbox/protocols/protocol.h>
+
+extern const ProtocolBase protocol_pac_stanley;

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,

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
  * 

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);

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];
+}

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