From 4ae1912772ade3eb351f1713f081004dbf05b2da Mon Sep 17 00:00:00 2001
From: merlokk <807634+merlokk@users.noreply.github.com>
Date: Thu, 19 Aug 2021 16:38:19 +0300
Subject: [PATCH] lrp maced tx works

---
 client/src/mifare/desfiresecurechan.c | 39 +++++++++++++++++++++++++++
 1 file changed, 39 insertions(+)

diff --git a/client/src/mifare/desfiresecurechan.c b/client/src/mifare/desfiresecurechan.c
index a52118547..640273a59 100644
--- a/client/src/mifare/desfiresecurechan.c
+++ b/client/src/mifare/desfiresecurechan.c
@@ -381,6 +381,43 @@ static void DesfireSecureChannelEncodeEV2(DesfireContext *ctx, uint8_t cmd, uint
     }
 }
 
+static void DesfireSecureChannelEncodeLRP(DesfireContext *ctx, uint8_t cmd, uint8_t *srcdata, size_t srcdatalen, uint8_t *dstdata, size_t *dstdatalen) {
+    uint8_t data[1050] = {0};
+    size_t rlen = 0;
+
+    memcpy(dstdata, srcdata, srcdatalen);
+    *dstdatalen = srcdatalen;
+
+    uint8_t hdrlen = DesfireGetCmdHeaderLen(cmd);
+
+    if (ctx->commMode == DCMMACed) {
+        uint8_t cmac[DESFIRE_MAX_CRYPTO_BLOCK_SIZE] = {0};
+        DesfireLRPCalcCMAC(ctx, cmd, srcdata, srcdatalen, cmac);
+
+        memcpy(&dstdata[srcdatalen], cmac, DesfireGetMACLength(ctx));
+        *dstdatalen = srcdatalen + DesfireGetMACLength(ctx);
+    } else if (ctx->commMode == DCMEncrypted || ctx->commMode == DCMEncryptedWithPadding || ctx->commMode == DCMEncryptedPlain) {
+        memcpy(dstdata, srcdata, hdrlen);
+
+        if (srcdatalen > hdrlen) {
+            rlen = padded_data_length(srcdatalen + 1 - hdrlen, desfire_get_key_block_length(ctx->keyType));
+            memcpy(data, &srcdata[hdrlen], srcdatalen - hdrlen);
+            data[srcdatalen - hdrlen] = 0x80; // padding
+
+            DesfireEV2FillIV(ctx, true, NULL); // fill IV to ctx
+            DesfireCryptoEncDec(ctx, DCOSessionKeyEnc, data, rlen, &dstdata[hdrlen], true);
+        }
+
+        uint8_t cmac[DESFIRE_MAX_CRYPTO_BLOCK_SIZE] = {0};
+        DesfireLRPCalcCMAC(ctx, cmd, dstdata, hdrlen + rlen, cmac);
+
+        memcpy(&dstdata[hdrlen + rlen], cmac, DesfireGetMACLength(ctx));
+
+        *dstdatalen = hdrlen + rlen + DesfireGetMACLength(ctx);
+        ctx->commMode = DCMEncrypted;
+    }
+}
+
 void DesfireSecureChannelEncode(DesfireContext *ctx, uint8_t cmd, uint8_t *srcdata, size_t srcdatalen, uint8_t *dstdata, size_t *dstdatalen) {
     ctx->lastCommand = cmd;
     ctx->lastRequestZeroLen = (srcdatalen <= DesfireGetCmdHeaderLen(cmd));
@@ -396,6 +433,8 @@ void DesfireSecureChannelEncode(DesfireContext *ctx, uint8_t cmd, uint8_t *srcda
             DesfireSecureChannelEncodeEV2(ctx, cmd, srcdata, srcdatalen, dstdata, dstdatalen);
             break;
         case DACLRP:
+            DesfireSecureChannelEncodeLRP(ctx, cmd, srcdata, srcdatalen, dstdata, dstdatalen);
+            break;
         case DACNone:
             memcpy(dstdata, srcdata, srcdatalen);
             *dstdatalen = srcdatalen;