From 08d4dc58aa29030b3d005f814639dd8a1c9fe68d Mon Sep 17 00:00:00 2001
From: Pol Henarejos <pol.henarejos@cttc.es>
Date: Sun, 13 Oct 2024 20:25:59 +0200
Subject: [PATCH] Add OTP extra command to read/write OTP without bootmode.

Signed-off-by: Pol Henarejos <pol.henarejos@cttc.es>
---
 pico-keys-sdk        |  2 +-
 src/hsm/cmd_extras.c | 24 ++++++++++++++++++++++++
 2 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/pico-keys-sdk b/pico-keys-sdk
index 84c3efd..6216cd2 160000
--- a/pico-keys-sdk
+++ b/pico-keys-sdk
@@ -1 +1 @@
-Subproject commit 84c3efd78292ebbb8473df667ab80310488d30a7
+Subproject commit 6216cd24be025d6ccd43c3358cc48ec7eac4556f
diff --git a/src/hsm/cmd_extras.c b/src/hsm/cmd_extras.c
index efded5a..375a1b3 100644
--- a/src/hsm/cmd_extras.c
+++ b/src/hsm/cmd_extras.c
@@ -37,6 +37,7 @@
 #define SECURE_LOCK_MASK 0x3
 #define SECURE_LOCK_DISABLE 0x4
 #define CMD_PHY 0x1B
+#define CMD_OTP 0x4C
 
 int cmd_extras() {
 #ifndef ENABLE_EMULATION
@@ -249,6 +250,29 @@ int cmd_extras() {
             low_flash_available();
         }
     }
+#endif
+#if RP2350
+    else if (P1(apdu) == CMD_OTP) {
+        if (apdu.nc < 2) {
+            return SW_WRONG_LENGTH();
+        }
+        uint16_t row = (apdu.data[0] << 8) | apdu.data[1];
+        apdu.nc -= 2;
+        apdu.data += 2;
+        if (apdu.nc == 2) {
+            memcpy(res_APDU, otp_buffer(row), apdu.ne);
+            res_APDU_size = apdu.ne;
+        }
+        else {
+            if (!(apdu.nc % 16)) {
+                return SW_WRONG_DATA();
+            }
+            int ret = otp_write_data(row, apdu.data, apdu.nc);
+            if (ret != 0) {
+                return SW_EXEC_ERROR();
+            }
+        }
+    }
 #endif
     else {
         return SW_INCORRECT_P1P2();