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