Mirrors/unleashed-firmware
2
0
Fork 0
mirror of https://github.com/DarkFlippers/unleashed-firmware synced 2024-12-18 08:43:10 +00:00
Code Issues Projects Releases Packages Wiki Activity
unleashed-firmware/lib/lfrfid/protocols/protocol_fdx_b.c
MX 4a61a28f57
Protocol data redecoding before write 
OFW PR 1723 by nminaylov
2022-09-14 16:18:23 +03:00

378 lines
No EOL
13 KiB
C
Raw Blame History

#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;
// Correct protocol data by redecoding
protocol_fdx_b_encoder_start(protocol);
protocol_fdx_b_decode(protocol);
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,
};
Reference in a new issue View git blame Copy permalink