From 96502e21aed5fb4c42a0fe4233054712e163bbfc Mon Sep 17 00:00:00 2001
From: MX <10697207+xMasterX@users.noreply.github.com>
Date: Tue, 4 Oct 2022 21:06:16 +0300
Subject: [PATCH] RFID: write fix for some protocols

OFW PR 1828 by nminaylov
---
 lib/lfrfid/protocols/protocol_awid.c    | 7 +++++++
 lib/lfrfid/protocols/protocol_fdx_a.c   | 9 +++++++++
 lib/lfrfid/protocols/protocol_keri.c    | 3 +++
 lib/lfrfid/protocols/protocol_pyramid.c | 1 +
 4 files changed, 20 insertions(+)

diff --git a/lib/lfrfid/protocols/protocol_awid.c b/lib/lfrfid/protocols/protocol_awid.c
index 7131d30dc..7472b5693 100644
--- a/lib/lfrfid/protocols/protocol_awid.c
+++ b/lib/lfrfid/protocols/protocol_awid.c
@@ -205,8 +205,15 @@ bool protocol_awid_write_data(ProtocolAwid* protocol, void* data) {
     LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data;
     bool result = false;
 
+    // Fix incorrect length byte
+    if(protocol->data[0] != 26 && protocol->data[0] != 50 && protocol->data[0] != 37 &&
+       protocol->data[0] != 34) {
+        protocol->data[0] = 26;
+    }
+
     // Correct protocol data by redecoding
     protocol_awid_encode(protocol->data, (uint8_t*)protocol->encoded_data);
+    bit_lib_remove_bit_every_nth((uint8_t*)protocol->encoded_data, 8, 88, 4);
     protocol_awid_decode(protocol->encoded_data, protocol->data);
 
     protocol_awid_encode(protocol->data, (uint8_t*)protocol->encoded_data);
diff --git a/lib/lfrfid/protocols/protocol_fdx_a.c b/lib/lfrfid/protocols/protocol_fdx_a.c
index 554b9071e..07cff08e5 100644
--- a/lib/lfrfid/protocols/protocol_fdx_a.c
+++ b/lib/lfrfid/protocols/protocol_fdx_a.c
@@ -79,6 +79,14 @@ static bool protocol_fdx_a_decode(const uint8_t* from, uint8_t* to) {
     return true;
 }
 
+static void protocol_fdx_a_fix_parity(ProtocolFDXA* protocol) {
+    for(size_t i = 0; i < FDXA_DECODED_DATA_SIZE; i++) {
+        if(bit_lib_test_parity_32(protocol->data[i], BitLibParityOdd)) {
+            protocol->data[i] ^= (1 << 7);
+        }
+    }
+}
+
 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 ||
@@ -179,6 +187,7 @@ bool protocol_fdx_a_write_data(ProtocolFDXA* protocol, void* data) {
     bool result = false;
 
     // Correct protocol data by redecoding
+    protocol_fdx_a_fix_parity(protocol);
     protocol_fdx_a_encoder_start(protocol);
     protocol_fdx_a_decode(protocol->encoded_data, protocol->data);
 
diff --git a/lib/lfrfid/protocols/protocol_keri.c b/lib/lfrfid/protocols/protocol_keri.c
index 7e6625546..8d77c0216 100644
--- a/lib/lfrfid/protocols/protocol_keri.c
+++ b/lib/lfrfid/protocols/protocol_keri.c
@@ -170,6 +170,7 @@ bool protocol_keri_encoder_start(ProtocolKeri* protocol) {
     memset(protocol->encoded_data, 0, KERI_ENCODED_DATA_SIZE);
     *(uint32_t*)&protocol->encoded_data[0] = 0b00000000000000000000000011100000;
     bit_lib_copy_bits(protocol->encoded_data, 32, 32, protocol->data, 0);
+    bit_lib_set_bits(protocol->encoded_data, 32, 1, 1);
 
     protocol->encoder.last_bit =
         bit_lib_get_bit(protocol->encoded_data, KERI_ENCODED_BIT_SIZE - 1);
@@ -224,6 +225,8 @@ bool protocol_keri_write_data(ProtocolKeri* protocol, void* data) {
     LFRFIDWriteRequest* request = (LFRFIDWriteRequest*)data;
     bool result = false;
 
+    // Start bit should be always set
+    protocol->data[0] |= (1 << 7);
     protocol_keri_encoder_start(protocol);
 
     if(request->write_type == LFRFIDWriteTypeT5577) {
diff --git a/lib/lfrfid/protocols/protocol_pyramid.c b/lib/lfrfid/protocols/protocol_pyramid.c
index 1bba98718..1ec7d7531 100644
--- a/lib/lfrfid/protocols/protocol_pyramid.c
+++ b/lib/lfrfid/protocols/protocol_pyramid.c
@@ -221,6 +221,7 @@ bool protocol_pyramid_write_data(ProtocolPyramid* protocol, void* data) {
 
     // Correct protocol data by redecoding
     protocol_pyramid_encode(protocol);
+    bit_lib_remove_bit_every_nth(protocol->encoded_data, 8, 15 * 8, 8);
     protocol_pyramid_decode(protocol);
 
     protocol_pyramid_encoder_start(protocol);