1 files changed, 206 insertions, 1 deletions
diff --git a/halimpl/hal/phNxpUciHal_ext.cc b/halimpl/hal/phNxpUciHal_ext.cc
index 9d6f3e4..55f5aff 100644
--- a/halimpl/hal/phNxpUciHal_ext.cc
+++ b/halimpl/hal/phNxpUciHal_ext.cc
@@ -18,6 +18,8 @@
 
 #include <atomic>
 #include <bitset>
+#include <map>
+#include <vector>
 
 #include <cutils/properties.h>
 
@@ -730,7 +732,7 @@ void phNxpUciHal_extcal_handle_coreinit(void)
   extcal_do_ant_delay();
 }
 
-extern bool isCountryCodeMapCreated;
+static bool isCountryCodeMapCreated;
 
 void apply_per_country_calibrations(void)
 {
@@ -943,3 +945,206 @@ bool phNxpUciHal_handle_set_app_config(uint16_t *data_len, uint8_t *p_data)
 
   return false;
 }
+
+extern std::map<uint16_t, std::vector<uint16_t>> input_map;
+extern std::map<uint16_t, std::vector<uint16_t>> conf_map;
+
+/******************************************************************************
+ * Function         phNxpUciHal_parse_get_capsInfo
+ *
+ * Description      parse the caps info response as per FIRA 2.0.
+ *
+ * Returns          void.
+ *
+ ******************************************************************************/
+static void phNxpUciHal_parse_get_capsInfo(uint16_t data_len, uint8_t *p_data)
+{
+  uint8_t *p = p_data;
+  uint8_t pDeviceCapsInfo[UCI_MAX_DATA_LEN];
+  uint8_t *pp = pDeviceCapsInfo;
+  uint8_t tagId = 0, subTagId = 0, len = 0;
+  uint8_t mt = 0, gid = 0, oid = 0;
+  uint8_t capsLen = p_data[5];
+  uint8_t dataLen = p_data[3];
+  mt = (*(p_data)&UCI_MT_MASK) >> UCI_MT_SHIFT;
+  gid = p_data[0] & UCI_GID_MASK;
+  oid = p_data[1] & UCI_OID_MASK;
+
+  if (p_data[4] == 0) {
+      for (uint16_t index = 6; index < data_len;) {
+        tagId = p_data[index++];
+        if (tagId != 0xE0 && tagId != 0xE3) {
+          len = p_data[index++];
+          if ((index + len) > data_len) {
+            break;
+          }
+          uint8_t *p_caps_value = (uint8_t *)(p_data + index);
+          /* b0 = Azimuth AoA -90 degree to 90 degree
+           * b1 = Azimuth AoA -180 degree to 180 degree
+           * b2 = Elevation AoA
+           * b3 = AoA FOM
+           */
+          if (AOA_SUPPORT_TAG_ID == tagId) {
+            if (nxpucihal_ctrl.numberOfAntennaPairs == 1) {
+              *p_caps_value = 1;
+            } else if (nxpucihal_ctrl.numberOfAntennaPairs > 1) {
+              // If antenna pair more than 1 then it will support both b0
+              // = Azimuth AoA -90° to 90° and b2=Elevation AoA then value
+              // will set to 5
+              *p_caps_value = 5;
+            } else {
+              *p_caps_value = 0;
+            }
+          }
+          UINT8_TO_STREAM(pp, tagId);
+          UINT8_TO_STREAM(pp, len);
+          if (tagId == CCC_SUPPORTED_PROTOCOL_VERSIONS_ID) {
+            UINT8_TO_STREAM(pp, p_caps_value[len - 1]);
+            UINT8_TO_STREAM(pp, p_caps_value[0]);
+          } else {
+            ARRAY_TO_STREAM(pp, p_caps_value, len);
+          }
+          index = index + len;
+        } else { // ignore vendor specific data
+          if ((index + 1) >= data_len) {
+            break;
+          }
+          subTagId = p_data[index++];
+          if ((index + 1) > data_len) {
+            break;
+          }
+          len = p_data[index++];
+          index = index + len;
+          capsLen--;
+          dataLen =
+              dataLen - (len + 3); // from datalen substract tagId,
+                                   // subTagId, len and value of config
+        }
+      }
+
+      // mapping device caps according to Fira 2.0
+      std::array<uint8_t, NXP_MAX_CONFIG_STRING_LEN> buffer;
+      buffer.fill(0);
+      uint8_t *vendorConfig = NULL;
+      long retlen = 0;
+      int numberOfParams = 0;
+
+      if (NxpConfig_GetByteArray(NAME_UWB_VENDOR_CAPABILITY,
+                                     buffer.data(), buffer.size(),
+                                     &retlen)) {
+        if (retlen > 0) {
+          vendorConfig = buffer.data();
+          ARRAY_TO_STREAM(pp, vendorConfig, retlen);
+          dataLen += retlen;
+
+          // calculate number of params
+          int index = 0, paramId, length;
+          do {
+            paramId = vendorConfig[index++];
+            length = vendorConfig[index++];
+            index = index + length;
+            numberOfParams++;
+          } while (index < retlen);
+
+          NXPLOG_UCIHAL_D("Get caps read info from config file, length: "
+                          "%ld, numberOfParams: %d",
+                          retlen, numberOfParams);
+
+          nxpucihal_ctrl.rx_data_len = UCI_MSG_HDR_SIZE + dataLen;
+          UCI_MSG_BLD_HDR0(p, UCI_MT_RSP, UCI_GID_CORE);
+          UCI_MSG_BLD_HDR1(p, UCI_MSG_CORE_GET_CAPS_INFO);
+          UINT8_TO_STREAM(p, 0x00);
+          UINT8_TO_STREAM(p, dataLen);
+          UINT8_TO_STREAM(p, 0x00); // status
+          UINT8_TO_STREAM(p, (capsLen + numberOfParams));
+          ARRAY_TO_STREAM(p, pDeviceCapsInfo, dataLen);
+        } else {
+          NXPLOG_UCIHAL_E("Reading config file for %s failed!!!",
+                          NAME_UWB_VENDOR_CAPABILITY);
+        }
+      }
+  }
+  phNxpUciHal_print_packet(NXP_TML_UCI_RSP_NTF_UWBS_2_AP, nxpucihal_ctrl.p_rx_data,
+                           nxpucihal_ctrl.rx_data_len);
+}
+
+void phNxpUciHal_handle_get_caps_info(uint16_t data_len, uint8_t *p_data)
+{
+  phNxpUciHal_parse_get_capsInfo(data_len, p_data);
+
+  std::map<uint16_t, std::vector<uint16_t>>::iterator itr;
+  std::vector<uint16_t>::iterator v_itr;
+  uint16_t arrLen, tag, idx;
+
+  // do not modify caps if the country code is not received from upper
+  // layer.
+  if (isCountryCodeMapCreated == false) {
+    return;
+  }
+  // Check UWBS Caps response status
+  if (p_data[4] == 0) {
+    idx = UCI_PKT_HDR_LEN + UCI_PKT_PAYLOAD_STATUS_LEN +
+          UCI_PKT_NUM_CAPS_LEN;
+    if (get_input_map(p_data, data_len, idx)) {
+      NXPLOG_UCIHAL_D("Input Map created");
+    } else {
+      NXPLOG_UCIHAL_D("Input Map creation failed");
+      return;
+    }
+  } else {
+    return;
+  }
+  // Compare the maps for Tags and modify input map if Values are
+  // different
+  for (itr = input_map.begin(); itr != input_map.end(); ++itr) {
+    tag = itr->first;
+    // Check for the Tag in both maps
+    if ((conf_map.count(tag)) == 1) {
+      if (tag == UWB_CHANNELS || tag == CCC_UWB_CHANNELS) {
+        NXPLOG_UCIHAL_D(
+            "Tag = 0x%02X , modify UWB_CHANNELS based on country conf ",
+            tag);
+        for (uint32_t j = 0; j < (itr->second).size(); j++) {
+          (input_map[tag])[j] =
+              ((conf_map[tag])[j]) & ((input_map[tag])[j]);
+        }
+      }
+    } else {
+      // TAG not found do nothing
+    }
+  }
+  // convert the modified input map to p_caps_resp array
+  memset(nxpucihal_ctrl.p_caps_resp, 0, UCI_MAX_DATA_LEN);
+  // Header information from Input array is updated in initial bytes
+  nxpucihal_ctrl.p_caps_resp[0] = p_data[0];
+  nxpucihal_ctrl.p_caps_resp[1] = p_data[1];
+  nxpucihal_ctrl.p_caps_resp[2] = p_data[2];
+  nxpucihal_ctrl.p_caps_resp[4] = p_data[4];
+  for (itr = input_map.begin(); itr != input_map.end(); ++itr) {
+    tag = itr->first;
+    // If Tag is 0xE0 or 0xE1 or 0xE2,Tag will be of 2 bytes
+    if (((tag >> 8) >= 0xE0) && ((tag >> 8) <= 0xE2)) {
+      nxpucihal_ctrl.p_caps_resp[idx++] = (tag & 0xFF00) >> 8;
+      nxpucihal_ctrl.p_caps_resp[idx++] = (tag & 0x00FF);
+    } else {
+      nxpucihal_ctrl.p_caps_resp[idx++] = tag;
+    }
+    for (v_itr = itr->second.begin(); v_itr != itr->second.end();
+         ++v_itr) {
+      nxpucihal_ctrl.p_caps_resp[idx++] = (*v_itr);
+    }
+  }
+  arrLen = idx;
+  // exclude the initial header data
+  nxpucihal_ctrl.p_caps_resp[3] = arrLen - UCI_PKT_HDR_LEN;
+  // update the number of parameter TLVs.
+  nxpucihal_ctrl.p_caps_resp[5] = input_map.size();
+  input_map.clear();
+
+  // send GET CAPS INFO response to the Upper Layer
+  (*nxpucihal_ctrl.p_uwb_stack_data_cback)(arrLen,
+                                           nxpucihal_ctrl.p_caps_resp);
+  // skip the incoming packet as we have send the modified response
+  // already
+  nxpucihal_ctrl.isSkipPacket = 1;
+}