From fdc41e5856db9ee691d3418ac31cfe86ee185a90 Mon Sep 17 00:00:00 2001
From: Pol Henarejos <pol.henarejos@cttc.es>
Date: Wed, 17 Apr 2024 11:50:44 +0200
Subject: [PATCH] Add support for PHY command to store and change VIDPID and
 LED no. dynamically on reboot.

Signed-off-by: Pol Henarejos <pol.henarejos@cttc.es>
---
 src/hsm/cmd_extras.c | 34 ++++++++++++++++++++++++++++++++++
 1 file changed, 34 insertions(+)

diff --git a/src/hsm/cmd_extras.c b/src/hsm/cmd_extras.c
index a5c4499..aea3fca 100644
--- a/src/hsm/cmd_extras.c
+++ b/src/hsm/cmd_extras.c
@@ -195,6 +195,40 @@ int cmd_extras() {
             }
         }
     }
+#ifndef ENABLE_EMULATION
+    else if (P1(apdu) == 0x1B) { // Set PHY
+        if (apdu.nc == 0) {
+            if (file_has_data(ef_phy)) {
+                res_APDU_size = file_get_size(ef_phy);
+                memcpy(res_APDU, file_get_data(ef_phy), res_APDU_size);
+            }
+        }
+        else {
+            uint8_t tmp[PHY_MAX_SIZE];
+            memset(tmp, 0, sizeof(tmp));
+            if (file_has_data(ef_phy)) {
+                memcpy(tmp, file_get_data(ef_phy), MIN(sizeof(tmp), file_get_size(ef_phy)));
+            }
+            if (P2(apdu) == PHY_VID) { // VIDPID
+                if (apdu.nc != 4) {
+                    return SW_WRONG_LENGTH();
+                }
+                memcpy(tmp + PHY_VID, apdu.data, 4);
+            }
+            else if (P2(apdu) == PHY_LED_GPIO || P2(apdu) == PHY_LED_MODE) {
+                if (apdu.nc != 1) {
+                    return SW_WRONG_LENGTH();
+                }
+                tmp[P2(apdu)] = apdu.data[0];
+            }
+            else {
+                return SW_INCORRECT_P1P2();
+            }
+            flash_write_data_to_file(ef_phy, tmp, sizeof(tmp));
+            low_flash_available();
+        }
+    }
+#endif
     else {
         return SW_INCORRECT_P1P2();
     }