Cleanup: add tlv parsing functions

decodeTlvBytes(): parse TLV bytes into map<key=T, val=LV>.
encodeTlvBytes(): convert map<key=T, val=LV> into raw bytes.

Bug: 321604848
Test: Check DevCapResp packet from logcat
Change-Id: I6dd497813691e009d209d79bf8cd207a8f337dc9

2 files changed, 63 insertions, 0 deletions

diff --git a/halimpl/utils/phNxpUciHal_utils.cc b/halimpl/utils/phNxpUciHal_utils.cc
index b1450e2..eee628b 100644
--- a/halimpl/utils/phNxpUciHal_utils.cc
+++ b/halimpl/utils/phNxpUciHal_utils.cc
@@ -642,3 +642,59 @@ bool get_conf_map(uint8_t *c_data, uint16_t cData_len) {
   }
   return ret;
 }
+
+// Decode bytes into map<key=T, val=LV>
+std::map<uint16_t, std::vector<uint8_t>>
+decodeTlvBytes(const std::vector<uint8_t> &ext_ids, const uint8_t *tlv_bytes, size_t tlv_len)
+{
+  std::map<uint16_t, std::vector<uint8_t>> ret;
+
+  size_t i = 0;
+  while ((i + 1) < tlv_len) {
+    uint16_t tag;
+    uint8_t len;
+
+    uint8_t byte0 = tlv_bytes[i++];
+    uint8_t byte1 = tlv_bytes[i++];
+    if (std::find(ext_ids.begin(), ext_ids.end(), byte0) != ext_ids.end()) {
+      if (i >= tlv_len) {
+        NXPLOG_UCIHAL_E("Failed to decode TLV bytes (offset=%zu).", i);
+        break;
+      }
+      tag = (byte0 << 8) | byte1; // 2 bytes tag as big endiann
+      len = tlv_bytes[i++];
+    } else {
+      tag = byte0;
+      len = byte1;
+    }
+    if ((i + len) > tlv_len) {
+      NXPLOG_UCIHAL_E("Failed to decode TLV bytes (offset=%zu).", i);
+      break;
+    }
+    ret[tag] = std::vector(&tlv_bytes[i], &tlv_bytes[i + len]);
+    i += len;
+  }
+
+  return ret;
+}
+
+std::vector<uint8_t> encodeTlvBytes(const std::map<uint16_t, std::vector<uint8_t>> &tlvs)
+{
+  std::vector<uint8_t> bytes;
+
+  for (auto const & [tag, val] : tlvs) {
+    // Tag
+    if (tag > 0xff) {
+      bytes.push_back(tag >> 8);
+    }
+    bytes.push_back(tag & 0xff);
+
+    // Length
+    bytes.push_back(val.size());
+
+    // Value
+    bytes.insert(bytes.end(), val.begin(), val.end());
+  }
+
+  return bytes;
+}
diff --git a/halimpl/utils/phNxpUciHal_utils.h b/halimpl/utils/phNxpUciHal_utils.h
index bdf095e..0edcbb8 100644
--- a/halimpl/utils/phNxpUciHal_utils.h
+++ b/halimpl/utils/phNxpUciHal_utils.h
@@ -223,4 +223,11 @@ static inline void cpu_to_le_bytes(uint8_t *p, const T num)
   if (phNxpUciHal_get_monitor()) \
   pthread_mutex_unlock(&phNxpUciHal_get_monitor()->concurrency_mutex)
 
+// Decode bytes into map<key=T, val=LV>
+std::map<uint16_t, std::vector<uint8_t>>
+decodeTlvBytes(const std::vector<uint8_t> &ext_ids, const uint8_t *tlv_bytes, size_t tlv_len);
+
+// Encode map<key=T, val=LV> into TLV bytes
+std::vector<uint8_t> encodeTlvBytes(const std::map<uint16_t, std::vector<uint8_t>> &tlvs);
+
 #endif /* _PHNXPUCIHAL_UTILS_H_ */