Snap for 11754915 from df363d8295b52d80608bdc2014d5d93661f9775d to sdk-releasesdk-release

Change-Id: I5bd753a83f0bde738e63727ca7c06b62d6140614

12 files changed, 465 insertions, 589 deletions

diff --git a/extns/inc/uci_defs.h b/extns/inc/uci_defs.h
index 163a93b..f20d39f 100644
--- a/extns/inc/uci_defs.h
+++ b/extns/inc/uci_defs.h
@@ -64,9 +64,6 @@
 #define UCI_GID_PROPRIETARY_0X0F  0x0F /* Proprietary Group */
 #define UCI_GID_INTERNAL          0x0B /* Internal Group */
 
-/* 0100b - 1100b RFU */
-#define UCI_OID_GET_CAPS_INFO 0x03
-
 /* OID: Opcode Identifier (byte 1) */
 #define UCI_OID_MASK 0x3F
 #define UCI_OID_SHIFT 0
@@ -117,6 +114,9 @@
 #define UCI_MSG_CORE_DEVICE_STATUS_NTF 1
 #define UCI_MSG_CORE_DEVICE_INFO 2
 #define UCI_MSG_CORE_GET_CAPS_INFO 3
+#define UCI_MSG_CORE_GET_CAPS_INFO_NR_OFFSET  5
+#define UCI_MSG_CORE_GET_CAPS_INFO_TLV_OFFSET 6
+
 #define UCI_MSG_CORE_SET_CONFIG 4
 #define UCI_MSG_CORE_GENERIC_ERROR_NTF 7
 
@@ -210,9 +210,7 @@ constexpr uint8_t kSessionType_CCCRanging = 0xA0;
 /* Generic Status Codes */
 #define UCI_STATUS_OK 0x00
 #define UCI_STATUS_FAILED 0x02
-#define UCI_STATUS_SYNTAX_ERROR  0x03
 #define UCI_STATUS_INVALID_PARAM 0x04
-#define UCI_STATUS_INVALID_MSG_SIZE 0x06
 #define UCI_STATUS_COMMAND_RETRY 0x0A
 #define UCI_STATUS_UNKNOWN 0x0B
 #define UCI_STATUS_THERMAL_RUNAWAY 0x54
diff --git a/halimpl/hal/phNxpUciHal.cc b/halimpl/hal/phNxpUciHal.cc
index 8b759c8..f82706a 100644
--- a/halimpl/hal/phNxpUciHal.cc
+++ b/halimpl/hal/phNxpUciHal.cc
@@ -42,9 +42,6 @@
 using namespace std;
 using android::base::StringPrintf;
 
-extern map<uint16_t, vector<uint16_t>> input_map;
-extern map<uint16_t, vector<uint16_t>> conf_map;
-
 /*********************** Global Variables *************************************/
 /* UCI HAL Control structure */
 phNxpUciHal_Control_t nxpucihal_ctrl;
@@ -58,10 +55,6 @@ uint32_t timeoutTimerId = 0;
 char persistant_log_path[120];
 static uint8_t Rx_data[UCI_MAX_DATA_LEN];
 
-/* AOA support handling */
-bool isAntennaRxPairDefined = false;
-int numberOfAntennaPairs = 0;
-
 /**************** local methods used in this file only ************************/
 static void phNxpUciHal_write_complete(void* pContext,
                                        phTmlUwb_TransactInfo_t* pInfo);
@@ -224,7 +217,6 @@ static void phNxpUciHal_client_thread(phNxpUciHal_Control_t* p_nxpucihal_ctrl)
   NXPLOG_UCIHAL_D("NxpUciHal thread stopped");
 }
 
-bool isCountryCodeMapCreated = false;
 /******************************************************************************
  * Function         phNxpUciHal_parse
  *
@@ -267,84 +259,6 @@ bool phNxpUciHal_parse(uint16_t data_len, const uint8_t *p_data)
     } else if ((gid == UCI_GID_SESSION_MANAGE) && (oid == UCI_MSG_SESSION_STATE_INIT)) {
       SessionTrack_onSessionInit(nxpucihal_ctrl.cmd_len, nxpucihal_ctrl.p_cmd_data);
     }
-  } else if (mt == UCI_MT_RSP) {
-    if ((gid == UCI_GID_CORE) && (oid == UCI_MSG_CORE_GET_CAPS_INFO)) {
-        map<uint16_t, vector<uint16_t>>::iterator itr;
-        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 false;
-        }
-        // 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 false;
-          }
-        } else {
-          return false;
-        }
-        // 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;
-        ret = false;
-    }
   } else {
     ret = false;
   }
@@ -546,7 +460,7 @@ static void phNxpUciHal_write_complete(void* pContext,
   phNxpUciHal_Sem_t* p_cb_data = (phNxpUciHal_Sem_t*)pContext;
 
   if (pInfo->wStatus == UWBSTATUS_SUCCESS) {
-    NXPLOG_UCIHAL_D("write successful status = 0x%x", pInfo->wStatus);
+    NXPLOG_UCIHAL_V("write successful status = 0x%x", pInfo->wStatus);
   } else {
     NXPLOG_UCIHAL_E("write error status = 0x%x", pInfo->wStatus);
   }
@@ -558,128 +472,6 @@ static void phNxpUciHal_write_complete(void* pContext,
 }
 
 /******************************************************************************
- * Function         phNxpUciHal_parse_get_capsInfo
- *
- * Description      parse the caps info response as per FIRA 2.0.
- *
- * Returns          void.
- *
- ******************************************************************************/
-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;
-  uint8_t *p_caps_value;
-  if (mt == UCI_MT_RSP) {
-    if ((gid == UCI_GID_CORE) && (oid == UCI_MSG_CORE_GET_CAPS_INFO)) {
-      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++];
-              /* 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 (isAntennaRxPairDefined) {
-                  if (numberOfAntennaPairs == 1) {
-                    *p_caps_value = 1;
-                  } else if (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;
-                }
-              } else {
-                p_caps_value = (uint8_t *)(p_data + index);
-              }
-              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);
-    }
-  }
-}
-
-/******************************************************************************
  * Function         phNxpUciHal_read_complete
  *
  * Description      This function is called whenever there is an UCI packet
@@ -710,10 +502,10 @@ void phNxpUciHal_read_complete(void* pContext,
      length = (length << EXTENDED_MODE_LEN_SHIFT) | pInfo->pBuff[index + EXTENDED_MODE_LEN_OFFSET] ;
     }
     length += UCI_MSG_HDR_SIZE;
-    NXPLOG_UCIHAL_D("read successful length = %d", length);
+    NXPLOG_UCIHAL_V("read successful length = %d", length);
 
     if (pInfo->wStatus == UWBSTATUS_SUCCESS) {
-      NXPLOG_UCIHAL_D("read successful status = 0x%x", pInfo->wStatus);
+      NXPLOG_UCIHAL_V("read successful status = 0x%x", pInfo->wStatus);
       nxpucihal_ctrl.p_rx_data = &pInfo->pBuff[index];
       nxpucihal_ctrl.rx_data_len = length;
       phNxpUciHal_print_packet(NXP_TML_UCI_RSP_NTF_UWBS_2_AP, nxpucihal_ctrl.p_rx_data, nxpucihal_ctrl.rx_data_len);
@@ -723,16 +515,15 @@ void phNxpUciHal_read_complete(void* pContext,
       oid = nxpucihal_ctrl.p_rx_data[1] & UCI_OID_MASK;
       pbf = (nxpucihal_ctrl.p_rx_data[0] & UCI_PBF_MASK) >> UCI_PBF_SHIFT;
 
-      // mapping device caps according to Fira 2.0
-      if (gid == UCI_GID_CORE && oid == UCI_OID_GET_CAPS_INFO) {
-        phNxpUciHal_parse_get_capsInfo(nxpucihal_ctrl.rx_data_len,
-         nxpucihal_ctrl.p_rx_data);
-      }
-
       nxpucihal_ctrl.isSkipPacket = 0;
-      phNxpUciHal_parse(nxpucihal_ctrl.rx_data_len, nxpucihal_ctrl.p_rx_data);
+
       phNxpUciHal_rx_handler_check(pInfo->wLength, pInfo->pBuff);
 
+      // mapping device caps according to Fira 2.0
+      if (mt == UCI_MT_RSP && gid == UCI_GID_CORE && oid == UCI_MSG_CORE_GET_CAPS_INFO) {
+        phNxpUciHal_handle_get_caps_info(nxpucihal_ctrl.rx_data_len, nxpucihal_ctrl.p_rx_data);
+      }
+
       // phNxpUciHal_process_ext_cmd_rsp() is waiting for the response packet
       // set this true to wake it up for other reasons
       bool bWakeupExtCmd = (mt == UCI_MT_RSP);
@@ -759,9 +550,6 @@ void phNxpUciHal_read_complete(void* pContext,
             nxpucihal_ctrl.ext_cb_data.status = UWBSTATUS_COMMAND_RETRANSMIT;
             nxpucihal_ctrl.isSkipPacket = 1;
             bWakeupExtCmd = true;
-          } else  if (status_code == UCI_STATUS_INVALID_MSG_SIZE || status_code == UCI_STATUS_SYNTAX_ERROR) {
-            nxpucihal_ctrl.ext_cb_data.status = UWBSTATUS_INVALID_COMMAND_LENGTH;
-            bWakeupExtCmd = true;
           }
         }
       }
@@ -921,9 +709,8 @@ static void parseAntennaConfig(const char *configName)
     length = data[index++];
     if ((ANTENNA_RX_PAIR_DEFINE_TAG_ID == tagId) &&
         (ANTENNA_RX_PAIR_DEFINE_SUB_TAG_ID == subTagId)) {
-      isAntennaRxPairDefined = true;
-      numberOfAntennaPairs = data[index];
-      NXPLOG_UCIHAL_D("numberOfAntennaPairs:%d", numberOfAntennaPairs);
+      nxpucihal_ctrl.numberOfAntennaPairs = data[index];
+      NXPLOG_UCIHAL_D("numberOfAntennaPairs:%d", nxpucihal_ctrl.numberOfAntennaPairs);
       break;
     } else {
       index = index + length;
@@ -1339,4 +1126,4 @@ void phNxpUciHal_send_dev_error_status_ntf()
  nxpucihal_ctrl.rx_data_len = 5;
  static uint8_t rsp_data[5] = {0x60, 0x01, 0x00, 0x01, 0xFF};
  (*nxpucihal_ctrl.p_uwb_stack_data_cback)(nxpucihal_ctrl.rx_data_len, rsp_data);
-}
\ No newline at end of file
+}
diff --git a/halimpl/hal/phNxpUciHal.h b/halimpl/hal/phNxpUciHal.h
index 87ee996..b3172cd 100644
--- a/halimpl/hal/phNxpUciHal.h
+++ b/halimpl/hal/phNxpUciHal.h
@@ -189,7 +189,6 @@ typedef struct phNxpUciHal_Control {
   uint8_t p_cmd_data[UCI_MAX_DATA_LEN];
   uint16_t rsp_len;
   uint8_t p_rsp_data[UCI_MAX_DATA_LEN];
-  uint8_t p_caps_resp[UCI_MAX_DATA_LEN];
 
   /* CORE_DEVICE_INFO_RSP cache */
   bool isDevInfoCached;
@@ -206,6 +205,9 @@ typedef struct phNxpUciHal_Control {
   // Per-country settings
   phNxpUciHal_Runtime_Settings_t rt_settings;
 
+  // AOA support handling
+  int numberOfAntennaPairs;
+
   // Extra calibration
   // Antenna Definitions for extra calibration, b0=Antenna1, b1=Antenna2, ...
   uint8_t cal_rx_antenna_mask;
diff --git a/halimpl/hal/phNxpUciHal_ext.cc b/halimpl/hal/phNxpUciHal_ext.cc
index 4c56a74..0393a89 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>
 
@@ -32,7 +34,6 @@
 
 /* Timeout value to wait for response from DEVICE_TYPE_SR1xx */
 #define MAX_COMMAND_RETRY_COUNT           5
-#define MAX_COMMAND_RETRY_ON_INVALID_LEN  2
 #define HAL_EXTNS_WRITE_RSP_TIMEOUT_MS    100
 #define HAL_HW_RESET_NTF_TIMEOUT          10000 /* 10 sec wait */
 
@@ -84,7 +85,6 @@ tHAL_UWB_STATUS phNxpUciHal_process_ext_cmd_rsp(uint16_t cmd_len,
   tHAL_UWB_STATUS status = UWBSTATUS_FAILED;
   int nr_retries = 0;
   int nr_timedout = 0;
-  int nr_unrecognized = 0;
   bool exit_loop = false;
 
   while(!exit_loop) {
@@ -118,17 +118,13 @@ tHAL_UWB_STATUS phNxpUciHal_process_ext_cmd_rsp(uint16_t cmd_len,
       // Upper layer should take care of it.
       nr_retries++;
       break;
-    case UWBSTATUS_INVALID_COMMAND_LENGTH:
-      // Something went wrong, just report this to upper-layer as is.
-      nr_unrecognized++;
-      break;
     default:
       status = nxpucihal_ctrl.ext_cb_data.status;
       exit_loop = true;
       break;
     }
 
-    if (nr_retries >= MAX_COMMAND_RETRY_COUNT || nr_unrecognized >= MAX_COMMAND_RETRY_ON_INVALID_LEN) {
+    if (nr_retries >= MAX_COMMAND_RETRY_COUNT) {
       NXPLOG_UCIHAL_E("Failed to process cmd/rsp 0x%x", nxpucihal_ctrl.ext_cb_data.status);
       status = UWBSTATUS_FAILED;
       exit_loop = true;
@@ -136,9 +132,9 @@ tHAL_UWB_STATUS phNxpUciHal_process_ext_cmd_rsp(uint16_t cmd_len,
     }
   }
 
-  if (nr_timedout > 0 || nr_unrecognized > 0) {
-    NXPLOG_UCIHAL_E("Warning: CMD/RSP retried %d times (timeout:%d, unrecognized:%d)\n",
-                    nr_retries, nr_timedout, nr_unrecognized);
+  if (nr_timedout > 0) {
+    NXPLOG_UCIHAL_E("Warning: CMD/RSP retried %d times (timeout:%d)\n",
+                    nr_retries, nr_timedout);
   }
 
 clean_and_return:
@@ -283,8 +279,6 @@ tHAL_UWB_STATUS phNxpUciHal_process_ext_rsp(uint16_t rsp_len, uint8_t* p_buff){
  return status;
 }
 
-static bool phNxpUciHal_setCalibParamTxPower(void);
-
 /*******************************************************************************
  * Function      phNxpUciHal_resetRuntimeSettings
  *
@@ -297,21 +291,18 @@ static void phNxpUciHal_resetRuntimeSettings(void)
   rt_set->uwb_enable = true;
   rt_set->restricted_channel_mask = 0;
   rt_set->tx_power_offset = 0;
-
 }
 
 /*******************************************************************************
  * Function      phNxpUciHal_applyCountryCaps
  *
- * Description   Creates supported channel's and Tx power TLV format for
- *specific country code  and updates map.
+ * Description   Update runtime settings with given COUNTRY_CODE_CAPS byte array
  *
  * Returns       void
  *
  *******************************************************************************/
 static void phNxpUciHal_applyCountryCaps(const char country_code[2],
-    const uint8_t *cc_resp, uint32_t cc_resp_len,
-    uint8_t *cc_data, uint32_t *cc_data_len)
+    const uint8_t *cc_resp, uint32_t cc_resp_len)
 {
   phNxpUciHal_Runtime_Settings_t *rt_set = &nxpucihal_ctrl.rt_settings;
 
@@ -346,8 +337,6 @@ static void phNxpUciHal_applyCountryCaps(const char country_code[2],
         if (len == 2) {
           rt_set->tx_power_offset = (short)((cc_resp[idx + 0]) | (((cc_resp[idx + 1]) << RMS_TX_POWER_SHIFT) & 0xFF00));
           NXPLOG_UCIHAL_D("CountryCaps tx_power_offset = %d", rt_set->tx_power_offset);
-
-          phNxpUciHal_setCalibParamTxPower();
         }
         break;
       default:
@@ -359,40 +348,6 @@ static void phNxpUciHal_applyCountryCaps(const char country_code[2],
     }
     idx += len;
   }
-
-  // Force UWB disabled even COUNTRY_CODE_CAPS provides it as enabled
-  if (!is_valid_country_code(country_code) && rt_set->uwb_enable) {
-    NXPLOG_UCIHAL_E("UWB is enabled by COUNTRY-CODE_CAPS with invalid country code %c%c,"
-                    " forcing disabled", country_code[0], country_code[1]);
-    rt_set->uwb_enable = false;
-  }
-
-  // consist up 'cc_data' TLVs
-  uint8_t fira_channels = 0xff;
-  if (rt_set->restricted_channel_mask & (1 << 5))
-    fira_channels &= CHANNEL_5_MASK;
-  if (rt_set->restricted_channel_mask & (1 << 9))
-    fira_channels &= CHANNEL_9_MASK;
-
-  uint8_t ccc_channels = 0;
-  if (!(rt_set->restricted_channel_mask & (1 << 5)))
-    ccc_channels |= 0x01;
-  if (!(rt_set->restricted_channel_mask & (1 << 9)))
-    ccc_channels |= 0x02;
-
-  uint8_t index = 0;
-  if ((index + 3) <= *cc_data_len) {
-    cc_data[index++] = UWB_CHANNELS;
-    cc_data[index++] = 0x01;
-    cc_data[index++] = fira_channels;
-  }
-
-  if ((index + 3) <= *cc_data_len) {
-    cc_data[index++] = CCC_UWB_CHANNELS;
-    cc_data[index++] = 0x01;
-    cc_data[index++] = ccc_channels;
-  }
-  *cc_data_len = index;
 }
 
 /*******************************************************************************
@@ -412,15 +367,15 @@ static bool phNxpUciHal_is_retry_not_required(uint8_t uci_octet0) {
 }
 
 /******************************************************************************
- * Function         phNxpUciHal_updateTxPower
+ * Function         CountryCodeCapsGenTxPowerPacket
  *
- * Description      This function updates the tx antenna power,
- *                  apply runtime_settings to gtx_power[]
+ * Description      This function makes tx antenna power calibration command
+ *                  with gtx_power[] + tx_power_offset
  *
- * Returns          true if gtx_power has valid packet data.
+ * Returns          true if packet has been updated
  *
  ******************************************************************************/
-static bool phNxpUciHal_updateTxPower(void)
+static bool CountryCodeCapsGenTxPowerPacket(uint8_t *packet, size_t packet_len, uint16_t *out_len)
 {
   phNxpUciHal_Runtime_Settings_t *rt_set = &nxpucihal_ctrl.rt_settings;
 
@@ -430,26 +385,52 @@ static bool phNxpUciHal_updateTxPower(void)
   if (gtx_power.empty())
     return false;
 
-  uint8_t index = 2;  // channel + param ID
-
-  if (gtx_power[index++]) { // number of entries
-    uint8_t num_of_antennas = gtx_power[index++];
-    while (num_of_antennas--) {
-      index += 3; // antenna Id(1) + Peak Tx(2)
-      long tx_power_long = gtx_power[index]  | ((gtx_power[index + 1] << RMS_TX_POWER_SHIFT) & 0xFF00);
-      tx_power_long += rt_set->tx_power_offset;
-
-      // long to 16bit little endian
-      if (tx_power_long < 0)
-        tx_power_long = 0;
-      uint16_t tx_power_u16 = (uint16_t)tx_power_long;
-      gtx_power[index++] = tx_power_u16 & 0xff;
-      gtx_power[index++] = tx_power_u16 >> RMS_TX_POWER_SHIFT;
-    }
-    return true;
-  } else {
+  if (gtx_power.size() > packet_len)
     return false;
+
+  uint16_t gtx_power_len = gtx_power.size();
+  memcpy(packet, gtx_power.data(), gtx_power_len);
+  uint8_t index = UCI_MSG_HDR_SIZE + 2;  // channel + Tag
+
+  // Length
+  if (index > gtx_power_len) {
+    NXPLOG_UCIHAL_E("Upper-layer calibration command for tx_power is invalid.");
+    return false;
+  }
+  if (!packet[index] || (packet[index] + index) > gtx_power_len) {
+    NXPLOG_UCIHAL_E("Upper-layer calibration command for tx_power is invalid.");
+    return false;
+  }
+  index++;
+
+  // Value[0] = Number of antennas
+  uint8_t num_of_antennas = packet[index++];
+
+  while (num_of_antennas--) {
+    // each entry = { antenna-id(1) + peak_tx(2) + id_rms(2) }
+    if ((index + 5) < gtx_power_len) {
+      NXPLOG_UCIHAL_E("Upper-layer calibration command for tx_power is invalid.");
+      return false;
+    }
+
+    index += 3; // antenna Id(1) + Peak Tx(2)
+
+    // Adjust id_rms part
+    long tx_power_long = packet[index]  | ((packet[index + 1] << RMS_TX_POWER_SHIFT) & 0xFF00);
+    tx_power_long += rt_set->tx_power_offset;
+
+    if (tx_power_long < 0)
+      tx_power_long = 0;
+
+    uint16_t tx_power_u16 = (uint16_t)tx_power_long;
+    packet[index++] = tx_power_u16 & 0xff;
+    packet[index++] = tx_power_u16 >> RMS_TX_POWER_SHIFT;
   }
+
+  if (out_len)
+    *out_len = gtx_power_len;
+
+  return true;
 }
 
 /*******************************************************************************
@@ -482,31 +463,34 @@ void phNxpUciHal_handle_set_calibration(const uint8_t *p_data, uint16_t data_len
   // RMS Tx power -> Octet [4, 5] in calib data
   NXPLOG_UCIHAL_D("phNxpUciHal_handle_set_calibration channel=%u tx_power_offset=%d", channel, rt_set->tx_power_offset);
 
-  gtx_power = std::move(std::vector<uint8_t> {p_data + UCI_MSG_HDR_SIZE, p_data + data_len});
+  // Backup the packet to gtx_power[]
+  gtx_power = std::move(std::vector<uint8_t> {p_data, p_data + data_len});
 
-  if (phNxpUciHal_updateTxPower())
-    memcpy(&nxpucihal_ctrl.p_cmd_data[UCI_MSG_HDR_SIZE], gtx_power.data(),  gtx_power.size());
+  // Patch SET_CALIBRATION_CMD per gtx_power + tx_power_offset
+  CountryCodeCapsGenTxPowerPacket(nxpucihal_ctrl.p_cmd_data, sizeof(nxpucihal_ctrl.p_cmd_data), &nxpucihal_ctrl.cmd_len);
 }
 
 /******************************************************************************
- * Function         phNxpUciHal_setCalibParamTxPower
+ * Function         CountryCodeCapsApplyTxPower
  *
- * Description      This function sets the TX power
+ * Description      This function sets the TX power from COUNTRY_CODE_CAPS
  *
- * Returns          true/false
+ * Returns          false if no tx_power_offset or no Upper-layer Calibration cmd was given
+ *                  true if it was successfully applied.
  *
  ******************************************************************************/
-static bool phNxpUciHal_setCalibParamTxPower(void)
+static bool CountryCodeCapsApplyTxPower(void)
 {
-  if (!phNxpUciHal_updateTxPower())
+  if (gtx_power.empty())
     return false;
 
-  // GID : 0xF / OID : 0x21
-  std::vector<uint8_t> packet{0x2f, 0x21, 0x00, 0x00};
-  packet.insert(packet.end(), gtx_power.begin(), gtx_power.end());
-  packet[3] = gtx_power.size();
+  // use whole packet as-is from upper-layer command (gtx_power[])
+  std::vector<uint8_t> packet(gtx_power.size());
+  uint16_t packet_size = 0;
+  if (!CountryCodeCapsGenTxPowerPacket(packet.data(), packet.size(), &packet_size))
+    return false;
 
-  tHAL_UWB_STATUS status = phNxpUciHal_send_ext_cmd(packet.size(), packet.data());
+  tHAL_UWB_STATUS status = phNxpUciHal_send_ext_cmd(packet_size, packet.data());
   if (status != UWBSTATUS_SUCCESS) {
       NXPLOG_UCIHAL_D("%s: send failed", __func__);
   }
@@ -736,18 +720,13 @@ void phNxpUciHal_extcal_handle_coreinit(void)
   extcal_do_ant_delay();
 }
 
-extern bool isCountryCodeMapCreated;
-
 void apply_per_country_calibrations(void)
 {
   // TX-POWER can be provided by
   // 1) COUNTRY_CODE_CAPS with offset values.
   // 2) Extra calibration files with absolute tx power values
   // only one should be applied if both were provided by platform
-  if (isCountryCodeMapCreated) {
-    // Apply COUNTRY_CODE_CAPS's txpower offset
-    phNxpUciHal_setCalibParamTxPower();
-  } else {
+  if (!CountryCodeCapsApplyTxPower()) {
     extcal_do_tx_power();
   }
 
@@ -771,10 +750,9 @@ void phNxpUciHal_handle_set_country_code(const char country_code[2])
 
   phNxpUciHal_Runtime_Settings_t *rt_set = &nxpucihal_ctrl.rt_settings;
   phNxpUciHal_resetRuntimeSettings();
-  isCountryCodeMapCreated = false;
 
   if (!is_valid_country_code(country_code)) {
-    NXPLOG_UCIHAL_D("Country code %c%c is invalid, disable UWB", country_code[0], country_code[1]);
+    NXPLOG_UCIHAL_D("Country code %c%c is invalid, UWB should be disabled", country_code[0], country_code[1]);
   }
 
   if (NxpConfig_SetCountryCode(country_code)) {
@@ -791,28 +769,21 @@ void phNxpUciHal_handle_set_country_code(const char country_code[2])
       rt_set->uwb_enable = !uwb_disable;
     }
 
-    if (!is_valid_country_code(country_code) && rt_set->uwb_enable) {
-      NXPLOG_UCIHAL_E("UWB is enabled by ExtCal with invalid country code %c%c,"
-                      " forcing disabled",
-                      country_code[0], country_code[1]);
-    }
-
-    // Load 'COUNTRY_CODE_CAPS' restrictions (via 'conf_map')
+    // Apply COUNTRY_CODE_CAPS
     uint8_t cc_caps[UCI_MAX_DATA_LEN];
     long retlen = 0;
     if (NxpConfig_GetByteArray(NAME_NXP_UWB_COUNTRY_CODE_CAPS, cc_caps, sizeof(cc_caps), &retlen) && retlen) {
       NXPLOG_UCIHAL_D("COUNTRY_CODE_CAPS is provided.");
+      phNxpUciHal_applyCountryCaps(country_code, cc_caps, retlen);
+    }
 
-      uint32_t cc_caps_len = retlen;
-      uint8_t cc_data[UCI_MAX_DATA_LEN];
-      uint32_t cc_data_len = sizeof(cc_data);
-      phNxpUciHal_applyCountryCaps(country_code, cc_caps, cc_caps_len, cc_data, &cc_data_len);
-
-      if (get_conf_map(cc_data, cc_data_len)) {
-        isCountryCodeMapCreated = true;
-        NXPLOG_UCIHAL_D("Country code caps loaded");
-      }
+    // Check country code validity
+    if (!is_valid_country_code(country_code) && rt_set->uwb_enable) {
+      NXPLOG_UCIHAL_E("UWB is not disabled by configuration files with invalid country code %c%c,"
+                      " forcing it disabled", country_code[0], country_code[1]);
+      rt_set->uwb_enable = false;
     }
+
     // Apply per-country calibration, it's handled by SessionTrack
     SessionTrack_onCountryCodeChanged();
   }
@@ -949,3 +920,104 @@ bool phNxpUciHal_handle_set_app_config(uint16_t *data_len, uint8_t *p_data)
 
   return false;
 }
+
+void phNxpUciHal_handle_get_caps_info(uint16_t data_len, uint8_t *p_data)
+{
+  if (data_len < UCI_MSG_CORE_GET_CAPS_INFO_NR_OFFSET)
+    return;
+
+  uint8_t status = p_data[UCI_RESPONSE_STATUS_OFFSET];
+  uint8_t nr = p_data[UCI_MSG_CORE_GET_CAPS_INFO_NR_OFFSET];
+  if (status != UWBSTATUS_SUCCESS || nr < 1)
+    return;
+
+  auto tlvs = decodeTlvBytes({0xe0, 0xe1, 0xe2, 0xe3}, &p_data[UCI_MSG_CORE_GET_CAPS_INFO_TLV_OFFSET], data_len - UCI_MSG_CORE_GET_CAPS_INFO_TLV_OFFSET);
+  if (tlvs.size() != nr) {
+    NXPLOG_UCIHAL_E("Failed to parse DevCaps %zu != %u", tlvs.size(), nr);
+  }
+
+  // Remove all NXP vendor specific parameters
+  for (auto it = tlvs.begin(); it != tlvs.end();) {
+    if (it->first > 0xff)
+      it = tlvs.erase(it);
+    else
+      it++;
+  }
+
+  // Override AOA_SUPPORT_TAG_ID
+  auto it = tlvs.find(AOA_SUPPORT_TAG_ID);
+  if (it != tlvs.end()) {
+    if (nxpucihal_ctrl.numberOfAntennaPairs == 1) {
+      it->second = std::vector<uint8_t>{0x01};
+    } else if (nxpucihal_ctrl.numberOfAntennaPairs > 1) {
+      it->second = std::vector<uint8_t>{0x05};
+    } else {
+      it->second = std::vector<uint8_t>{0x00};
+    }
+  }
+
+  // Byteorder of CCC_SUPPORTED_PROTOCOL_VERSIONS_ID
+  it = tlvs.find(CCC_SUPPORTED_PROTOCOL_VERSIONS_ID);
+  if (it != tlvs.end() && it->second.size() == 2) {
+    std::swap(it->second[0], it->second[1]);
+  }
+
+  // Append UWB_VENDOR_CAPABILITY from configuration files
+  {
+    std::array<uint8_t, NXP_MAX_CONFIG_STRING_LEN> buffer;
+    long retlen = 0;
+    if (NxpConfig_GetByteArray(NAME_UWB_VENDOR_CAPABILITY, buffer.data(),
+                               buffer.size(), &retlen) && retlen) {
+      auto vendorTlvs = decodeTlvBytes({}, buffer.data(), retlen);
+      for (auto const& [key, val] : vendorTlvs) {
+        tlvs[key] = val;
+      }
+    }
+  }
+
+  // Apply restrictions
+  const phNxpUciHal_Runtime_Settings_t *rt_set = &nxpucihal_ctrl.rt_settings;
+
+  uint8_t fira_channels = 0xff;
+  if (rt_set->restricted_channel_mask & (1 << 5))
+    fira_channels &= CHANNEL_5_MASK;
+  if (rt_set->restricted_channel_mask & (1 << 9))
+    fira_channels &= CHANNEL_9_MASK;
+
+  uint8_t ccc_channels = 0;
+  if (!(rt_set->restricted_channel_mask & (1 << 5)))
+    ccc_channels |= 0x01;
+  if (!(rt_set->restricted_channel_mask & (1 << 9)))
+    ccc_channels |= 0x02;
+
+  tlvs[UWB_CHANNELS] = std::vector{fira_channels};
+  tlvs[CCC_UWB_CHANNELS] = std::vector{ccc_channels};
+
+  // Convert it back to raw packet bytes
+  uint8_t packet[256];
+
+  // header
+  memcpy(packet, p_data, UCI_MSG_HDR_SIZE);
+  // status
+  packet[UCI_RESPONSE_STATUS_OFFSET] = UWBSTATUS_SUCCESS;
+  // nr
+  packet[UCI_MSG_CORE_GET_CAPS_INFO_NR_OFFSET] = tlvs.size();
+
+  // tlvs
+  auto tlv_bytes = encodeTlvBytes(tlvs);
+  if ((tlv_bytes.size() + UCI_MSG_CORE_GET_CAPS_INFO_TLV_OFFSET) > sizeof(packet)) {
+    NXPLOG_UCIHAL_E("DevCaps overflow!");
+  } else {
+    uint8_t packet_len = UCI_MSG_CORE_GET_CAPS_INFO_TLV_OFFSET + tlv_bytes.size();
+    packet[UCI_PAYLOAD_LENGTH_OFFSET] = packet_len - UCI_MSG_HDR_SIZE;
+    memcpy(&packet[UCI_MSG_CORE_GET_CAPS_INFO_TLV_OFFSET], tlv_bytes.data(), tlv_bytes.size());
+
+    phNxpUciHal_print_packet(NXP_TML_UCI_RSP_NTF_UWBS_2_AP, packet, packet_len);
+
+    // send GET CAPS INFO response to the Upper Layer
+    (*nxpucihal_ctrl.p_uwb_stack_data_cback)(packet_len, packet);
+    // skip the incoming packet as we have send the modified response
+    // already
+    nxpucihal_ctrl.isSkipPacket = 1;
+  }
+}
diff --git a/halimpl/hal/phNxpUciHal_ext.h b/halimpl/hal/phNxpUciHal_ext.h
index 634fca0..147847a 100644
--- a/halimpl/hal/phNxpUciHal_ext.h
+++ b/halimpl/hal/phNxpUciHal_ext.h
@@ -65,5 +65,6 @@ void phNxpUciHal_extcal_handle_coreinit(void);
 void phNxpUciHal_process_response();
 void phNxpUciHal_handle_set_country_code(const char country_code[2]);
 bool phNxpUciHal_handle_set_app_config(uint16_t *data_len, uint8_t *p_data);
+void phNxpUciHal_handle_get_caps_info(uint16_t data_len, uint8_t *p_data);
 void apply_per_country_calibrations(void);
 #endif /* _PHNXPNICHAL_EXT_H_ */
diff --git a/halimpl/hal/phNxpUwbCalib.cc b/halimpl/hal/phNxpUwbCalib.cc
new file mode 100644
index 0000000..fdab160
--- /dev/null
+++ b/halimpl/hal/phNxpUwbCalib.cc
@@ -0,0 +1,182 @@
+/*
+ * Copyright 2021-2023 NXP
+ * Copyright 2024 Google
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#include "phNxpUwbCalib.h"
+#include "phUwbStatus.h"
+#include "phNxpUciHal_ext.h"
+
+/* SR1XX is same as SR2XX */
+static tHAL_UWB_STATUS sr1xx_apply_calibration(extcal_param_id_t id, const uint8_t ch, const uint8_t *data, size_t data_len);
+static tHAL_UWB_STATUS sr1xx_set_conf(const std::vector<uint8_t> &tlv);
+static tHAL_UWB_STATUS sr1xx_set_calibration(uint8_t channel, const std::vector<uint8_t> &tlv);
+
+
+tHAL_UWB_STATUS phNxpUwbCalib_apply_calibration(extcal_param_id_t id, const uint8_t ch, const uint8_t *data, size_t data_len) {
+  return sr1xx_apply_calibration(id, ch, data, data_len);
+}
+
+//
+// SR1XX Device Calibrations:
+//
+// Based on NXP SR1xx UCI v2.0.5
+// current HAL impl only supports "xtal" read from otp
+// others should be existed in .conf files
+
+static tHAL_UWB_STATUS sr1xx_set_calibration(uint8_t channel, const std::vector<uint8_t> &tlv)
+{
+  // SET_CALIBRATION_CMD header: GID=0xF OID=0x21
+  std::vector<uint8_t> packet({ (0x20 | UCI_GID_PROPRIETARY_0X0F), UCI_MSG_SET_DEVICE_CALIBRATION, 0x00, 0x00});
+
+  // use 9 for channel-independent parameters
+  if (!channel) {
+    channel = 9;
+  }
+  packet.push_back(channel);
+  packet.insert(packet.end(), tlv.begin(), tlv.end());
+  packet[3] = packet.size() - 4;
+  return phNxpUciHal_send_ext_cmd(packet.size(), packet.data());
+}
+
+static tHAL_UWB_STATUS sr1xx_set_conf(const std::vector<uint8_t> &tlv)
+{
+  // SET_CALIBRATION_CMD header: GID=0xF OID=0x21
+  std::vector<uint8_t> packet({ (0x20 | UCI_GID_CORE), UCI_MSG_CORE_SET_CONFIG, 0x00, 0x00});
+  packet.push_back(1);  // number of parameters
+  packet.insert(packet.end(), tlv.begin(), tlv.end());
+  packet[3] = packet.size() - 4;
+  return phNxpUciHal_send_ext_cmd(packet.size(), packet.data());
+}
+
+static tHAL_UWB_STATUS sr1xx_apply_calibration(extcal_param_id_t id, const uint8_t ch, const uint8_t *data, size_t data_len)
+{
+  // Device Calibration
+  const uint8_t UCI_PARAM_ID_RF_CLK_ACCURACY_CALIB    = 0x01;
+  const uint8_t UCI_PARAM_ID_RX_ANT_DELAY_CALIB       = 0x02;
+  const uint8_t UCI_PARAM_ID_TX_POWER_PER_ANTENNA     = 0x04;
+
+  // Device Configurations
+  const uint16_t UCI_PARAM_ID_TX_BASE_BAND_CONFIG     = 0xe426;
+  const uint16_t UCI_PARAM_ID_DDFS_TONE_CONFIG        = 0xe427;
+  const uint16_t UCI_PARAM_ID_TX_PULSE_SHAPE_CONFIG   = 0xe428;
+
+  switch (id) {
+  case EXTCAL_PARAM_CLK_ACCURACY:
+    {
+      if (data_len != 6) {
+        return UWBSTATUS_FAILED;
+      }
+
+      std::vector<uint8_t> tlv;
+      // Tag
+      tlv.push_back(UCI_PARAM_ID_RF_CLK_ACCURACY_CALIB);
+      // Length
+      tlv.push_back((uint8_t)data_len + 1);
+      // Value
+      tlv.push_back(3); // number of register (must be 0x03)
+      tlv.insert(tlv.end(), data, data + data_len);
+
+      return sr1xx_set_calibration(ch, tlv);
+    }
+  case EXTCAL_PARAM_RX_ANT_DELAY:
+    {
+      if (!ch || data_len < 1 || !data[0] || (data[0] * 3) != (data_len - 1)) {
+        return UWBSTATUS_FAILED;
+      }
+
+      std::vector<uint8_t> tlv;
+      // Tag
+      tlv.push_back(UCI_PARAM_ID_RX_ANT_DELAY_CALIB);
+      // Length
+      tlv.push_back((uint8_t)data_len);
+      // Value
+      tlv.insert(tlv.end(), data, data + data_len);
+
+      return sr1xx_set_calibration(ch, tlv);
+    }
+  case EXTCAL_PARAM_TX_POWER:
+    {
+      if (!ch || data_len < 1 || !data[0] || (data[0] * 5) != (data_len - 1)) {
+        return UWBSTATUS_FAILED;
+      }
+
+      std::vector<uint8_t> tlv;
+      // Tag
+      tlv.push_back(UCI_PARAM_ID_TX_POWER_PER_ANTENNA);
+      // Length
+      tlv.push_back((uint8_t)data_len);
+      // Value
+      tlv.insert(tlv.end(), data, data + data_len);
+
+      return sr1xx_set_calibration(ch, tlv);
+    }
+  case EXTCAL_PARAM_TX_BASE_BAND_CONTROL:
+    {
+      if (data_len != 1) {
+        return UWBSTATUS_FAILED;
+      }
+
+      std::vector<uint8_t> tlv;
+      // Tag
+      tlv.push_back(UCI_PARAM_ID_TX_BASE_BAND_CONFIG >> 8);
+      tlv.push_back(UCI_PARAM_ID_TX_BASE_BAND_CONFIG & 0xff);
+      // Length
+      tlv.push_back(1);
+      // Value
+      tlv.push_back(data[0]);
+
+      return sr1xx_set_conf(tlv);
+    }
+  case EXTCAL_PARAM_DDFS_TONE_CONFIG:
+    {
+      if (!data_len) {
+        return UWBSTATUS_FAILED;
+      }
+
+      std::vector<uint8_t> tlv;
+      // Tag
+      tlv.push_back(UCI_PARAM_ID_DDFS_TONE_CONFIG >> 8);
+      tlv.push_back(UCI_PARAM_ID_DDFS_TONE_CONFIG & 0xff);
+      // Length
+      tlv.push_back(data_len);
+      // Value
+      tlv.insert(tlv.end(), data, data + data_len);
+
+      return sr1xx_set_conf(tlv);
+    }
+  case EXTCAL_PARAM_TX_PULSE_SHAPE:
+    {
+      if (!data_len) {
+        return UWBSTATUS_FAILED;
+      }
+
+      std::vector<uint8_t> tlv;
+      // Tag
+      tlv.push_back(UCI_PARAM_ID_TX_PULSE_SHAPE_CONFIG >> 8);
+      tlv.push_back(UCI_PARAM_ID_TX_PULSE_SHAPE_CONFIG & 0xff);
+      // Length
+      tlv.push_back(data_len);
+      // Value
+      tlv.insert(tlv.end(), data, data + data_len);
+
+      return sr1xx_set_conf(tlv);
+    }
+  default:
+    NXPLOG_UCIHAL_E("Unsupported parameter: 0x%x", id);
+    return UWBSTATUS_FAILED;
+  }
+}
diff --git a/halimpl/hal/phNxpUwbCalib.h b/halimpl/hal/phNxpUwbCalib.h
new file mode 100644
index 0000000..a88a2b4
--- /dev/null
+++ b/halimpl/hal/phNxpUwbCalib.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright 2024 Google
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#pragma once
+
+#include "NxpUwbChip.h"
+
+tHAL_UWB_STATUS phNxpUwbCalib_apply_calibration(extcal_param_id_t id, const uint8_t ch, const uint8_t *data, size_t data_len);
diff --git a/halimpl/hal/sr1xx/NxpUwbChipSr1xx.cc b/halimpl/hal/sr1xx/NxpUwbChipSr1xx.cc
index f39ccae..679e5a4 100644
--- a/halimpl/hal/sr1xx/NxpUwbChipSr1xx.cc
+++ b/halimpl/hal/sr1xx/NxpUwbChipSr1xx.cc
@@ -5,6 +5,7 @@
 #include "phNxpUciHal_utils.h"
 #include "phUwbStatus.h"
 #include "phUwbTypes.h"
+#include "phNxpUwbCalib.h"
 #include "uci_defs.h"
 
 #define UCI_MSG_UWB_ESE_BINDING_LEN                   11
@@ -29,157 +30,6 @@ static void report_binding_status(uint8_t binding_status)
   }
 }
 
-//
-// SR1XX Device Calibrations:
-//
-// Based on NXP SR1xx UCI v2.0.5
-// current HAL impl only supports "xtal" read from otp
-// others should be existed in .conf files
-
-static tHAL_UWB_STATUS sr1xx_set_calibration(uint8_t channel, const std::vector<uint8_t> &tlv)
-{
-  // SET_CALIBRATION_CMD header: GID=0xF OID=0x21
-  std::vector<uint8_t> packet({ (0x20 | UCI_GID_PROPRIETARY_0X0F), UCI_MSG_SET_DEVICE_CALIBRATION, 0x00, 0x00});
-
-  // use 9 for channel-independent parameters
-  if (!channel) {
-    channel = 9;
-  }
-  packet.push_back(channel);
-  packet.insert(packet.end(), tlv.begin(), tlv.end());
-  packet[3] = packet.size() - 4;
-  return phNxpUciHal_send_ext_cmd(packet.size(), packet.data());
-}
-
-static tHAL_UWB_STATUS sr1xx_set_conf(const std::vector<uint8_t> &tlv)
-{
-  // SET_CALIBRATION_CMD header: GID=0xF OID=0x21
-  std::vector<uint8_t> packet({ (0x20 | UCI_GID_CORE), UCI_MSG_CORE_SET_CONFIG, 0x00, 0x00});
-  packet.push_back(1);  // number of parameters
-  packet.insert(packet.end(), tlv.begin(), tlv.end());
-  packet[3] = packet.size() - 4;
-  return phNxpUciHal_send_ext_cmd(packet.size(), packet.data());
-}
-
-static tHAL_UWB_STATUS sr1xx_apply_calibration(extcal_param_id_t id, const uint8_t ch, const uint8_t *data, size_t data_len)
-{
-  // Device Calibration
-  const uint8_t UCI_PARAM_ID_RF_CLK_ACCURACY_CALIB    = 0x01;
-  const uint8_t UCI_PARAM_ID_RX_ANT_DELAY_CALIB       = 0x02;
-  const uint8_t UCI_PARAM_ID_TX_POWER_PER_ANTENNA     = 0x04;
-
-  // Device Configurations
-  const uint16_t UCI_PARAM_ID_TX_BASE_BAND_CONFIG     = 0xe426;
-  const uint16_t UCI_PARAM_ID_DDFS_TONE_CONFIG        = 0xe427;
-  const uint16_t UCI_PARAM_ID_TX_PULSE_SHAPE_CONFIG   = 0xe428;
-
-  switch (id) {
-  case EXTCAL_PARAM_CLK_ACCURACY:
-    {
-      if (data_len != 6) {
-        return UWBSTATUS_FAILED;
-      }
-
-      std::vector<uint8_t> tlv;
-      // Tag
-      tlv.push_back(UCI_PARAM_ID_RF_CLK_ACCURACY_CALIB);
-      // Length
-      tlv.push_back((uint8_t)data_len + 1);
-      // Value
-      tlv.push_back(3); // number of register (must be 0x03)
-      tlv.insert(tlv.end(), data, data + data_len);
-
-      return sr1xx_set_calibration(ch, tlv);
-    }
-  case EXTCAL_PARAM_RX_ANT_DELAY:
-    {
-      if (!ch || data_len < 1 || !data[0] || (data[0] * 3) != (data_len - 1)) {
-        return UWBSTATUS_FAILED;
-      }
-
-      std::vector<uint8_t> tlv;
-      // Tag
-      tlv.push_back(UCI_PARAM_ID_RX_ANT_DELAY_CALIB);
-      // Length
-      tlv.push_back((uint8_t)data_len);
-      // Value
-      tlv.insert(tlv.end(), data, data + data_len);
-
-      return sr1xx_set_calibration(ch, tlv);
-    }
-  case EXTCAL_PARAM_TX_POWER:
-    {
-      if (!ch || data_len < 1 || !data[0] || (data[0] * 5) != (data_len - 1)) {
-        return UWBSTATUS_FAILED;
-      }
-
-      std::vector<uint8_t> tlv;
-      // Tag
-      tlv.push_back(UCI_PARAM_ID_TX_POWER_PER_ANTENNA);
-      // Length
-      tlv.push_back((uint8_t)data_len);
-      // Value
-      tlv.insert(tlv.end(), data, data + data_len);
-
-      return sr1xx_set_calibration(ch, tlv);
-    }
-  case EXTCAL_PARAM_TX_BASE_BAND_CONTROL:
-    {
-      if (data_len != 1) {
-        return UWBSTATUS_FAILED;
-      }
-
-      std::vector<uint8_t> tlv;
-      // Tag
-      tlv.push_back(UCI_PARAM_ID_TX_BASE_BAND_CONFIG >> 8);
-      tlv.push_back(UCI_PARAM_ID_TX_BASE_BAND_CONFIG & 0xff);
-      // Length
-      tlv.push_back(1);
-      // Value
-      tlv.push_back(data[0]);
-
-      return sr1xx_set_conf(tlv);
-    }
-  case EXTCAL_PARAM_DDFS_TONE_CONFIG:
-    {
-      if (!data_len) {
-        return UWBSTATUS_FAILED;
-      }
-
-      std::vector<uint8_t> tlv;
-      // Tag
-      tlv.push_back(UCI_PARAM_ID_DDFS_TONE_CONFIG >> 8);
-      tlv.push_back(UCI_PARAM_ID_DDFS_TONE_CONFIG & 0xff);
-      // Length
-      tlv.push_back(data_len);
-      // Value
-      tlv.insert(tlv.end(), data, data + data_len);
-
-      return sr1xx_set_conf(tlv);
-    }
-  case EXTCAL_PARAM_TX_PULSE_SHAPE:
-    {
-      if (!data_len) {
-        return UWBSTATUS_FAILED;
-      }
-
-      std::vector<uint8_t> tlv;
-      // Tag
-      tlv.push_back(UCI_PARAM_ID_TX_PULSE_SHAPE_CONFIG >> 8);
-      tlv.push_back(UCI_PARAM_ID_TX_PULSE_SHAPE_CONFIG & 0xff);
-      // Length
-      tlv.push_back(data_len);
-      // Value
-      tlv.insert(tlv.end(), data, data + data_len);
-
-      return sr1xx_set_conf(tlv);
-    }
-  default:
-    NXPLOG_UCIHAL_E("Unsupported parameter: 0x%x", id);
-    return UWBSTATUS_FAILED;
-  }
-}
-
 /******************************************************************************
  * Function         otp_read_data
  *
@@ -627,7 +477,7 @@ tHAL_UWB_STATUS NxpUwbChipSr1xx::read_otp(extcal_param_id_t id, uint8_t *data, s
 
 tHAL_UWB_STATUS NxpUwbChipSr1xx::apply_calibration(extcal_param_id_t id, const uint8_t ch, const uint8_t *data, size_t data_len)
 {
-  return sr1xx_apply_calibration(id, ch, data, data_len);
+  return phNxpUwbCalib_apply_calibration(id, ch, data, data_len);
 }
 
 int16_t NxpUwbChipSr1xx::extra_group_delay(void) {
diff --git a/halimpl/log/phNxpLog.h b/halimpl/log/phNxpLog.h
index fe723fd..fd9ec52 100644
--- a/halimpl/log/phNxpLog.h
+++ b/halimpl/log/phNxpLog.h
@@ -65,6 +65,7 @@ extern uci_log_level_t gLog_level;
 #define NXPLOG_LOG_ERROR_LOGLEVEL 0x01
 #define NXPLOG_LOG_WARN_LOGLEVEL 0x02
 #define NXPLOG_LOG_DEBUG_LOGLEVEL 0x03
+#define NXPLOG_LOG_VERBOSE_LOGLEVEL 0x04
 /* ####################### Set the default logging level for EVERY COMPONENT
  * here ########################## :END: */
 
@@ -130,6 +131,11 @@ extern const char* NXPLOG_ITEM_HCPR; /* Android logging tag for NxpHcpR   */
 
 /* Logging APIs used by NxpUciHal module */
 #if (ENABLE_HAL_TRACES == TRUE)
+#define NXPLOG_UCIHAL_V(...)                                       \
+  {                                                                \
+    if ((gLog_level.hal_log_level >= NXPLOG_LOG_VERBOSE_LOGLEVEL))   \
+      LOG_PRI(ANDROID_LOG_VERBOSE, NXPLOG_ITEM_UCIHAL, __VA_ARGS__); \
+  }
 #define NXPLOG_UCIHAL_D(...)                                       \
   {                                                                \
     if ((gLog_level.hal_log_level >= NXPLOG_LOG_DEBUG_LOGLEVEL))   \
@@ -146,6 +152,7 @@ extern const char* NXPLOG_ITEM_HCPR; /* Android logging tag for NxpHcpR   */
       LOG_PRI(ANDROID_LOG_ERROR, NXPLOG_ITEM_UCIHAL, __VA_ARGS__); \
   }
 #else
+#define NXPLOG_UCIHAL_V(...)
 #define NXPLOG_UCIHAL_D(...)
 #define NXPLOG_UCIHAL_W(...)
 #define NXPLOG_UCIHAL_E(...)
diff --git a/halimpl/utils/phNxpConfig.h b/halimpl/utils/phNxpConfig.h
index 5afd9eb..1352d53 100644
--- a/halimpl/utils/phNxpConfig.h
+++ b/halimpl/utils/phNxpConfig.h
@@ -61,7 +61,6 @@ int NxpConfig_GetStrArrayVal(const char* name, int index, char* pValue, unsigned
 #define NAME_NXP_UWB_EXT_APP_SR1XX_T_CONFIG "NXP_UWB_EXT_APP_SR1XX_T_CONFIG"
 #define NAME_NXP_UWB_EXT_APP_SR1XX_S_CONFIG "NXP_UWB_EXT_APP_SR1XX_S_CONFIG"
 #define NAME_UWB_USER_FW_BOOT_MODE_CONFIG "UWB_USER_FW_BOOT_MODE_CONFIG"
-#define NAME_NXP_COUNTRY_CODE_CONFIG "NXP_COUNTRY_CODE_CONFIG"
 #define NAME_NXP_UWB_COUNTRY_CODE_CAPS "UWB_COUNTRY_CODE_CAPS"
 
 #define NAME_NXP_SECURE_CONFIG_BLK "NXP_SECURE_CONFIG_BLK_"
diff --git a/halimpl/utils/phNxpUciHal_utils.cc b/halimpl/utils/phNxpUciHal_utils.cc
index b1450e2..42af937 100644
--- a/halimpl/utils/phNxpUciHal_utils.cc
+++ b/halimpl/utils/phNxpUciHal_utils.cc
@@ -541,104 +541,57 @@ double phNxpUciHal_byteArrayToDouble(const uint8_t* p_data) {
   return d;                                                       \
 }
 
-/*******************************************************************************
- * Function      get_input_map
- *
- * Description   Creates a map from the USBS CAPS Response with key as Tag and
- *               value as a vector containing Length and Values of the Tag.
- *
- * Returns       true if the map creation successful
- *
- *******************************************************************************/
-bool get_input_map(const uint8_t *i_data, uint16_t iData_len,
-                   uint8_t startIndex) {
-  vector<uint16_t> input_vec;
-  bool ret = true;
-  uint16_t i = startIndex, j = 0, tag = 0, len = 0;
-  if (i_data == NULL) {
-    NXPLOG_UCIHAL_D("input map creation failed, i_data is NULL");
-    return false;
-  }
-
-  while (i < iData_len) {
-    if (i + 1 >= iData_len) {
-      ret = false;
-      break;
-    }
-    tag = i_data[i++];
-    // Tag IDs from 0xE0 to 0xE2 are extended tag IDs with 2 bytes length.
-    if ((tag >= 0xE0) && (tag <= 0xE2)) {
-      if (i + 1 >= iData_len) {
-        ret = false;
+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 = (tag << 8) | i_data[i++];
+      tag = (byte0 << 8) | byte1; // 2 bytes tag as big endiann
+      len = tlv_bytes[i++];
+    } else {
+      tag = byte0;
+      len = byte1;
     }
-    if (i + 1 >= iData_len) {
-      ret = false;
+    if ((i + len) > tlv_len) {
+      NXPLOG_UCIHAL_E("Failed to decode TLV bytes (offset=%zu).", i);
       break;
     }
-    len = i_data[i++];
-    input_vec.insert(input_vec.begin(), len);
-    if (i + len > iData_len) {
-      ret = false;
-      break;
-    }
-    for (j = 1; j <= len; j++) {
-      input_vec.insert(input_vec.begin() + j, i_data[i++]);
-    }
-    input_map[tag] = input_vec;
-    input_vec.clear();
+    ret[tag] = std::vector(&tlv_bytes[i], &tlv_bytes[i + len]);
+    i += len;
   }
+
   return ret;
 }
 
-/*******************************************************************************
- * Function      get_conf_map
- *
- * Description   Creates a map from the Country code conf with key as Tag and
- *               value as a vector containing Length and Values of the Tag.
- *
- * Returns       true if the map creation successful
- *
- *******************************************************************************/
-bool get_conf_map(uint8_t *c_data, uint16_t cData_len) {
-  vector<uint16_t> conf_vec;
-  bool ret = true;
-  uint16_t i = 0, j = 0, tag = 0, len = 0;
-  if (c_data == NULL) {
-    NXPLOG_UCIHAL_D("Country code conf map creation failed, c_data is NULL");
-    return false;
-  }
-  while (i < cData_len) {
-    if (i + 1 >= cData_len) {
-      ret = false;
-      break;
-    }
-    tag = c_data[i++];
-    // Tag IDs from 0xE0 to 0xE2 are extended tag IDs with 2 bytes length.
-    if ((tag >= 0xE0) && (tag <= 0xE2)) {
-      if (i + 1 >= cData_len) {
-        ret = false;
-        break;
-      }
-      tag = (tag << 8) | c_data[i++];
-    }
-    if (i + 1 >= cData_len) {
-      ret = false;
-      break;
-    }
-    len = c_data[i++];
-    conf_vec.insert(conf_vec.begin(), len);
-    if (i + len > cData_len) {
-      ret = false;
-      break;
-    }
-    for (j = 1; j <= len; j++) {
-      conf_vec.insert(conf_vec.begin() + j, c_data[i++]);
+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);
     }
-    conf_map[tag] = conf_vec;
-    conf_vec.clear();
+    bytes.push_back(tag & 0xff);
+
+    // Length
+    bytes.push_back(val.size());
+
+    // Value
+    bytes.insert(bytes.end(), val.begin(), val.end());
   }
-  return ret;
+
+  return bytes;
 }
diff --git a/halimpl/utils/phNxpUciHal_utils.h b/halimpl/utils/phNxpUciHal_utils.h
index bdf095e..6639fb1 100644
--- a/halimpl/utils/phNxpUciHal_utils.h
+++ b/halimpl/utils/phNxpUciHal_utils.h
@@ -174,9 +174,6 @@ void phNxpUciHal_print_packet(enum phNxpUciHal_Pkt_Type what, const uint8_t* p_d
                               uint16_t len);
 void phNxpUciHal_emergency_recovery(void);
 double phNxpUciHal_byteArrayToDouble(const uint8_t* p_data);
-bool get_input_map(const uint8_t *i_data, uint16_t iData_len,
-                   uint8_t startIndex);
-bool get_conf_map(uint8_t *c_data, uint16_t cData_len);
 
 template <typename T>
 static inline T le_bytes_to_cpu(const uint8_t *p)
@@ -223,4 +220,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_ */