From f71eaaec55e2b5125f6e23ca202ec9d4815c503c Mon Sep 17 00:00:00 2001
From: Yann GASCUEL <34003959+lnv42@users.noreply.github.com>
Date: Fri, 19 Jan 2024 16:30:11 +0100
Subject: [PATCH] iso15sim: add suppot for SET_PASSWORD

---
 armsrc/iso15693.c | 23 +++++++++++++++++++++--
 armsrc/iso15693.h |  2 ++
 2 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/armsrc/iso15693.c b/armsrc/iso15693.c
index 73274859f..46016f725 100644
--- a/armsrc/iso15693.c
+++ b/armsrc/iso15693.c
@@ -2176,6 +2176,7 @@ void SimTagIso15693(uint8_t *uid, uint8_t block_size) {
     uint8_t error = 0;
     uint8_t pageNum = 0;
     uint8_t nbPages = 0;
+    uint8_t pwdId = 0;
 
     while (exit_loop == false) {
 
@@ -2495,11 +2496,29 @@ void SimTagIso15693(uint8_t *uid, uint8_t block_size) {
                 break;
             case ISO15693_GET_RANDOM_NUMBER:
                 if (g_dbglevel >= DBG_DEBUG) Dbprintf("GetRandomNumber cmd");
+                tag->random[0] = (uint8_t)(reader_eof_time) ^ 0xFF; // poor random number
+                tag->random[1] = (uint8_t)(reader_eof_time >> 8) ^ 0xFF;
                 recv[0] = ISO15_NOERROR;
-                recv[1] = (uint8_t)(reader_eof_time) ^ 0xFF; // poor random number
-                recv[2] = (uint8_t)(reader_eof_time >> 8) ^ 0xFF;
+                recv[1] = tag->random[0]; // poor random number
+                recv[2] = tag->random[1];
                 recvLen = 3;
                 break;
+            case ISO15693_SET_PASSWORD:
+                if (g_dbglevel >= DBG_DEBUG) Dbprintf("SetPassword cmd");
+                if (cmd[cmdCpt++] == tag->ic)
+                {
+                    pwdId = cmd[cmdCpt++];
+                    if (pwdId == 4) // Privacy password
+                    {
+                        tag->privacyPasswd[0] = cmd[cmdCpt] ^ tag->random[0];
+                        tag->privacyPasswd[1] = cmd[cmdCpt+1] ^ tag->random[1];
+                        tag->privacyPasswd[2] = cmd[cmdCpt+2] ^ tag->random[0];
+                        tag->privacyPasswd[3] = cmd[cmdCpt+3] ^ tag->random[1];
+                    }
+                }
+                recv[0] = ISO15_NOERROR;
+                recvLen = 1;
+                break;
             case ISO15693_ENABLE_PRIVACY:
                 if (g_dbglevel >= DBG_DEBUG) Dbprintf("EnablePrivacy cmd");
                 // not realy entering privacy mode
diff --git a/armsrc/iso15693.h b/armsrc/iso15693.h
index 31086f50d..1de06520d 100644
--- a/armsrc/iso15693.h
+++ b/armsrc/iso15693.h
@@ -47,6 +47,8 @@ typedef struct iso15693_tag {
     uint8_t ic;
     uint8_t locks[ISO15693_TAG_MAX_PAGES];
     uint8_t data[ISO15693_TAG_MAX_SIZE];
+    uint8_t random[2];
+    uint8_t privacyPasswd[4];
     enum {
         TAG_STATE_NO_FIELD,
         TAG_STATE_READY,