From 84f79528178a02b81a0cf605c15d0e351c91e73d Mon Sep 17 00:00:00 2001
From: Pol Henarejos <pol.henarejos@cttc.es>
Date: Sat, 15 Nov 2025 20:13:35 +0100
Subject: [PATCH] Add support for PHY read.

Signed-off-by: Pol Henarejos <pol.henarejos@cttc.es>
---
 src/led/led.c | 28 ++++++++++++++++++++++++----
 src/rescue.c  | 26 ++++++++++++++++++++++++--
 2 files changed, 48 insertions(+), 6 deletions(-)

diff --git a/src/led/led.c b/src/led/led.c
index 1af30f9..cf24780 100644
--- a/src/led/led.c
+++ b/src/led/led.c
@@ -112,16 +112,34 @@ void led_init() {
     led_driver = &led_driver_dummy;
 #if defined(PICO_PLATFORM) || defined(ESP_PLATFORM)
     // Guess default driver
-#ifdef PICO_DEFAULT_LED_PIN
-    led_driver = &led_driver_pico;
+#if defined(PIMORONI_TINY2040) || defined(PIMORONI_TINY2350)
+    led_driver = &led_driver_pimoroni;
+    phy_data.led_driver = phy_data.led_driver_present ? phy_data.led_driver : PHY_LED_DRIVER_PIMORONI;
 #elif defined(CYW43_WL_GPIO_LED_PIN)
     led_driver = &led_driver_cyw43;
+    phy_data.led_driver = phy_data.led_driver_present ? phy_data.led_driver : PHY_LED_DRIVER_CYW43;
+    phy_data.led_gpio = phy_data.led_gpio_present ? phy_data.led_gpio : CYW43_WL_GPIO_LED_PIN;
 #elif defined(PICO_DEFAULT_WS2812_PIN)
     led_driver = &led_driver_ws2812;
+    phy_data.led_driver = phy_data.led_driver_present ? phy_data.led_driver : PHY_LED_DRIVER_WS2812;
+    phy_data.led_gpio = phy_data.led_gpio_present ? phy_data.led_gpio : PICO_DEFAULT_WS2812_PIN;
 #elif defined(ESP_PLATFORM)
+    #if defined(CONFIG_IDF_TARGET_ESP32S3)
+        #define NEOPIXEL_PIN GPIO_NUM_48
+    #elif defined(CONFIG_IDF_TARGET_ESP32S2)
+        #define NEOPIXEL_PIN GPIO_NUM_15
+    #elif defined(CONFIG_IDF_TARGET_ESP32C6)
+        #define NEOPIXEL_PIN GPIO_NUM_8
+    #else
+        #define NEOPIXEL_PIN GPIO_NUM_27
+    #endif
     led_driver = &led_driver_neopixel;
-#elif defined(PIMORONI_TINY2040) || defined(PIMORONI_TINY2350)
-    led_driver = &led_driver_pimoroni;
+    phy_data.led_driver = phy_data.led_driver_present ? phy_data.led_driver : PHY_LED_DRIVER_NEOPIXEL;
+    phy_data.led_gpio = phy_data.led_gpio_present ? phy_data.led_gpio : NEOPIXEL_PIN;
+#elif defined(PICO_DEFAULT_LED_PIN)
+    led_driver = &led_driver_pico;
+    phy_data.led_driver = phy_data.led_driver_present ? phy_data.led_driver : PHY_LED_DRIVER_PICO;
+    phy_data.led_gpio = phy_data.led_gpio_present ? phy_data.led_gpio : PICO_DEFAULT_LED_PIN;
 #endif
     if (phy_data.led_driver_present) {
         switch (phy_data.led_driver) {
@@ -149,6 +167,8 @@ void led_init() {
                 break;
         }
     }
+    phy_data.led_driver_present = true;
+    phy_data.led_gpio_present = true;
     led_driver->init();
     led_set_mode(MODE_NOT_MOUNTED);
 #endif
diff --git a/src/rescue.c b/src/rescue.c
index 1413093..f517979 100644
--- a/src/rescue.c
+++ b/src/rescue.c
@@ -39,6 +39,8 @@ const uint8_t rescue_aid[] = {
 #endif
 
 extern uint8_t PICO_PRODUCT;
+extern uint8_t PICO_VERSION_MAJOR;
+extern uint8_t PICO_VERSION_MINOR;
 
 int rescue_select(app_t *a, uint8_t force) {
     a->process_apdu = rescue_process_apdu;
@@ -46,8 +48,8 @@ int rescue_select(app_t *a, uint8_t force) {
     res_APDU_size = 0;
     res_APDU[res_APDU_size++] = PICO_MCU;
     res_APDU[res_APDU_size++] = PICO_PRODUCT;
-    res_APDU[res_APDU_size++] = PICO_KEYS_SDK_VERSION_MAJOR;
-    res_APDU[res_APDU_size++] = PICO_KEYS_SDK_VERSION_MINOR;
+    res_APDU[res_APDU_size++] = PICO_VERSION_MAJOR;
+    res_APDU[res_APDU_size++] = PICO_VERSION_MINOR;
     apdu.ne = res_APDU_size;
     if (force) {
         scan_flash();
@@ -81,6 +83,24 @@ int cmd_write() {
     return SW_OK();
 }
 
+int cmd_read() {
+    if (apdu.nc != 0) {
+        return SW_WRONG_LENGTH();
+    }
+
+    if (P1(apdu) == 0x1) { // PHY
+#ifndef ENABLE_EMULATION
+        uint16_t len = 0;
+        int ret = phy_serialize_data(&phy_data, apdu.rdata, &len);
+        if (ret != PICOKEY_OK) {
+            return SW_EXEC_ERROR();
+        }
+        res_APDU_size = len;
+#endif
+    }
+    return SW_OK();
+}
+
 #if defined(PICO_RP2350) || defined(ESP_PLATFORM)
 int cmd_secure() {
     if (apdu.nc != 0) {
@@ -100,12 +120,14 @@ int cmd_secure() {
 
 #define INS_WRITE            0x1C
 #define INS_SECURE           0x1D
+#define INS_READ             0x1E
 
 static const cmd_t cmds[] = {
     { INS_WRITE, cmd_write },
 #if defined(PICO_RP2350) || defined(ESP_PLATFORM)
     { INS_SECURE, cmd_secure },
 #endif
+    { INS_READ, cmd_read },
     { 0x00, 0x0 }
 };