From 6f7d92a5913d4a985cbaa71a0f74df04405ce162 Mon Sep 17 00:00:00 2001
From: Pol Henarejos <pol.henarejos@cttc.es>
Date: Mon, 4 Nov 2024 18:25:05 +0100
Subject: [PATCH] Add parse phy byte string.

Signed-off-by: Pol Henarejos <pol.henarejos@cttc.es>
---
 src/fs/file.c | 51 +++++++++++++++++++++++++++++++++++++++++++++++++++
 src/fs/file.h |  4 ++++
 2 files changed, 55 insertions(+)

diff --git a/src/fs/file.c b/src/fs/file.c
index fa47599..1489aad 100644
--- a/src/fs/file.c
+++ b/src/fs/file.c
@@ -528,3 +528,54 @@ int delete_file(file_t *ef) {
     low_flash_available();
     return CCID_OK;
 }
+
+#ifndef ENABLE_EMULATION
+int parse_phy_data(const uint8_t *data, uint8_t len) {
+    file_t *ef_phy = search_by_fid(EF_PHY, NULL, SPECIFY_EF);
+    if (!ef_phy) {
+        return CCID_ERR_FILE_NOT_FOUND;
+    }
+    uint8_t tmp[PHY_MAX_SIZE];
+    memset(tmp, 0, sizeof(tmp));
+    uint16_t opts = 0;
+    if (file_has_data(ef_phy)) {
+        memcpy(tmp, file_get_data(ef_phy), MIN(sizeof(tmp), file_get_size(ef_phy)));
+        if (file_get_size(ef_phy) >= 8) {
+            opts = (tmp[PHY_OPTS] << 8) | tmp[PHY_OPTS + 1];
+        }
+    }
+    const uint8_t *p = data;
+    while (p < data + len) {
+        uint8_t tag = *p++;
+        switch (tag) {
+            case PHY_VID:
+                memcpy(tmp + PHY_VID, p, 2);
+                opts |= PHY_OPT_VPID;
+                p += 2;
+                break;
+            case PHY_PID:
+                memcpy(tmp + PHY_PID, p, 2);
+                opts |= PHY_OPT_VPID;
+                p += 2;
+                break;
+            case PHY_LED_GPIO:
+                tmp[PHY_LED_GPIO] = *p++;
+                opts |= PHY_OPT_GPIO;
+                break;
+            case PHY_LED_BTNESS:
+                tmp[PHY_LED_BTNESS] = *p++;
+                opts |= PHY_OPT_BTNESS;
+                break;
+            case PHY_OPTS:
+                opts = (opts & ~PHY_OPT_MASK) | (((*p << 8) | *(p + 1)) & PHY_OPT_MASK);
+                p += 2;
+                break;
+        }
+    }
+    tmp[PHY_OPTS] = opts >> 8;
+    tmp[PHY_OPTS + 1] = opts & 0xff;
+    file_put_data(ef_phy, tmp, sizeof(tmp));
+    low_flash_available();
+    return CCID_OK;
+}
+#endif
diff --git a/src/fs/file.h b/src/fs/file.h
index dc6bd18..4a24504 100644
--- a/src/fs/file.h
+++ b/src/fs/file.h
@@ -85,6 +85,10 @@
 
 #define PHY_MAX_SIZE    8
 
+#ifndef ENABLE_EMULATION
+extern int parse_phy_data(const uint8_t *data, uint8_t len);
+#endif
+
 #define MAX_DEPTH 4
 
 typedef struct file {