Add parse phy byte string.

Signed-off-by: Pol Henarejos <pol.henarejos@cttc.es>

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

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 {