xref: /aosp_15_r20/hardware/nxp/uwb/halimpl/hal/sessionTrack.cc

#include <bit>
#include <mutex>
#include <random>
#include <thread>
#include <unordered_map>
#include <vector>

#include "phNxpConfig.h"
#include "phNxpUciHal.h"
#include "phNxpUciHal_ext.h"
#include "phNxpUciHal_utils.h"

extern phNxpUciHal_Control_t nxpucihal_ctrl;

//
// SessionTrack
//
// Keeps track of device/session state.
//
// 1. Per-country calibrations
//
// When the country code is switching from A to B,
// a. Active Sessions restricted by B country should be stopped.
// b. Per-country device calibrations should be delayed to where
//    device stays in IDLE.
//
// 2. Issue URSK_DELETE_CMD on SESSION_DEINIT_RSP (optional/experimental)
//
// Calls URSK_DELETE_CMD for every CCC session closing,
// for the cases where CCC session ID was created but not started.
// (This is only activated when DELETE_URSK_FOR_CCC_SESSION=1 is set
// from config)
//
// 3. Call suspend to kernel driver on idle (optional/experimental)
//
// (This is only activated when AUTO_SUSPEND_ENABLED=1 is set from config)
// Tracks the each session's status and automatically requests suspend
// to kernel driver when it's idle for a given duration,
// and resumes the device before sending any commands.
// SessionTracks detects UWBS is in idle when there's no session created.
//
//
// For CCC Session, in case `OVERRIDE_STS_INDEX_FOR_CCC_SESSION` is set,
// 1) Set STS Index as as non Zero in case new ranging session
// 2) Set STS Index to Last CCC STS Index + 1 in case of restarting a session.
//

class SessionTrack {
private:
  // Session
  struct SessionInfo {
    uint32_t  session_id_;
    uint8_t   session_type_;
    uint8_t   session_state_;
    uint8_t   channel_;
    bool      ranging_started_;
    SessionInfo(uint32_t session_id, uint8_t session_type) :
      session_id_(session_id),
      session_type_(session_type),
      session_state_(UCI_MSG_SESSION_STATE_UNDEFINED),
      channel_(0),
      ranging_started_(false) {
    }
  };
  enum class SessionTrackWorkType {
    IDLE = 0,
    REFRESH_IDLE,
    ACTIVATE,
    IDLE_TIMER_FIRED,
    DELETE_URSK,
    STOP,
  };
  enum class PowerState {
    SUSPEND = 0,
    IDLE,
    ACTIVE,
  };
  struct SessionTrackMsg {
    SessionTrackWorkType type_;
    std::shared_ptr<SessionInfo> session_info_;
    bool sync_;
    bool cond_flag;
    std::condition_variable cond_;

    SessionTrackMsg(SessionTrackWorkType type, bool sync)
        : type_(type), session_info_(nullptr), sync_(sync), cond_flag(false) {}

    // Per-session work item
    SessionTrackMsg(SessionTrackWorkType type,
                    std::shared_ptr<SessionInfo> session_info, bool sync)
        : type_(type), session_info_(session_info), sync_(sync),
          cond_flag(false) {}
  };
  static constexpr unsigned long kAutoSuspendTimeoutDefaultMs_ = (30 * 1000);
  static constexpr long kQueueTimeoutMs = 2000;
  static constexpr long kUrskDeleteNtfTimeoutMs = 500;

private:
  std::shared_ptr<phNxpUciHal_RxHandler> rx_handler_session_status_ntf_;
  std::unordered_map<uint32_t, std::shared_ptr<SessionInfo>> sessions_;
  std::mutex sessions_lock_;

  bool auto_suspend_enabled_;
  bool delete_ursk_ccc_enabled_;
  bool calibration_delayed_;
  std::atomic<PowerState> power_state_;
  bool idle_timer_started_;
  unsigned long idle_timeout_ms_;
  bool override_sts_index_for_ccc_;

  std::thread worker_thread_;
  std::mutex sync_mutex_;
  uint32_t idle_timer_;
  std::unique_ptr<MessageQueue<SessionTrackMsg>> msgq_;

public:
  SessionTrack() :
    auto_suspend_enabled_(false),
    delete_ursk_ccc_enabled_(false),
    calibration_delayed_(false),
    power_state_(PowerState::IDLE),
    idle_timer_started_(false),
    idle_timeout_ms_(kAutoSuspendTimeoutDefaultMs_),
    override_sts_index_for_ccc_(true)
  {
    sessions_.clear();

    msgq_ = std::make_unique<MessageQueue<SessionTrackMsg>>("SessionTrack");
    worker_thread_ = std::thread(&SessionTrack::PowerManagerWorker, this);

    unsigned long numval = 0;

    if (NxpConfig_GetNum(NAME_DELETE_URSK_FOR_CCC_SESSION, &numval, sizeof(numval)) && numval) {
      delete_ursk_ccc_enabled_ = true;
    }

    // Default on
    if (NxpConfig_GetNum(NAME_OVERRIDE_STS_INDEX_FOR_CCC_SESSION, &numval, sizeof(numval)) && !numval) {
      override_sts_index_for_ccc_ = false;
    }

    if (NxpConfig_GetNum(NAME_AUTO_SUSPEND_ENABLE, &numval, sizeof(numval)) && numval) {
      auto_suspend_enabled_ = true;

      NxpConfig_GetNum(NAME_AUTO_SUSPEND_TIMEOUT_MS, &idle_timeout_ms_, sizeof(idle_timeout_ms_));

      // Idle timer is only activated when AUTO_SUSPEND_ENABLED=1
      // device suspend won't be triggered when it's not activated.
      idle_timer_ = phOsalUwb_Timer_Create();
      RefreshIdle();
    }

    // register SESSION_STATUS_NTF rx handler
    rx_handler_session_status_ntf_ = phNxpUciHal_rx_handler_add(
      UCI_MT_NTF, UCI_GID_SESSION_MANAGE, UCI_MSG_SESSION_STATUS_NTF,
      false,
      std::bind(&SessionTrack::OnSessionStatusNtf, this, std::placeholders::_1, std::placeholders::_2));
  }

  virtual ~SessionTrack() {
    phNxpUciHal_rx_handler_del(rx_handler_session_status_ntf_);

    if (auto_suspend_enabled_) {
      phOsalUwb_Timer_Delete(idle_timer_);
    }
    QueueSessionTrackWork(SessionTrackWorkType::STOP);
    worker_thread_.join();
  }

  // Called upon upper-layer's SESSION_INIT_CMD
  void OnSessionInit(size_t packet_len, const uint8_t *packet)
  {
    if (packet_len != UCI_MSG_SESSION_STATE_INIT_CMD_LEN)
      return;

    uint32_t session_id = le_bytes_to_cpu<uint32_t>(&packet[UCI_MSG_SESSION_STATE_INIT_CMD_ID_OFFSET]);
    uint8_t session_type = packet[UCI_MSG_SESSION_STATE_INIT_CMD_TYPE_OFFSET];

    // Check SESSION_INIT_RSP for SessionID - Handle matching
    auto session_init_rsp_cb =
      [this, session_id, session_type](size_t packet_len, const uint8_t *packet) -> bool
    {
      if (packet_len != UCI_MSG_SESSION_STATE_INIT_RSP_LEN )
        return false;

      uint8_t status = packet[UCI_MSG_SESSION_STATE_INIT_RSP_STATUS_OFFSET];
      uint32_t handle = le_bytes_to_cpu<uint32_t>(&packet[UCI_MSG_SESSION_STATE_INIT_RSP_HANDLE_OFFSET]);
      if (status != UWBSTATUS_SUCCESS)
        return false;

      bool was_idle;
      {
        std::lock_guard<std::mutex> lock(sessions_lock_);

        was_idle = IsDeviceIdle();

        sessions_.emplace(std::make_pair(handle,
                                         std::make_shared<SessionInfo>(session_id, session_type)));
      }
      if (was_idle) {
        NXPLOG_UCIHAL_D("Queue Active");
        QueueSessionTrackWork(SessionTrackWorkType::ACTIVATE);
      }

      return false;
    };

    // XXX: This rx handler can be called multiple times on
    // UCI_STATUS_COMMAND_RETRY(0xA) from SESSION_INIT_CMD
    phNxpUciHal_rx_handler_add(UCI_MT_RSP, UCI_GID_SESSION_MANAGE,
      UCI_MSG_SESSION_STATE_INIT, true, session_init_rsp_cb);
  }

  // Called by upper-layer's SetAppConfig command handler
  void OnChannelConfig(uint32_t session_handle, uint8_t channel) {
    // Update channel info
    std::lock_guard<std::mutex> lock(sessions_lock_);
    auto pSessionInfo = GetSessionInfo(session_handle);
    if (!pSessionInfo)
      return;
    pSessionInfo->channel_ = channel;
  }

  // Called by upper-layer's SetCountryCode command handler,
  // Check whether per-country calibration can be executed.
  // phNxpUciHal_Runtime_Settings_t should've been updated.
  void OnCountryCodeChanged() {
    phNxpUciHal_Runtime_Settings_t *rt_set = &nxpucihal_ctrl.rt_settings;
    NXPLOG_UCIHAL_D("SessionTrack: OnCountryCodeChanged");

    calibration_delayed_ = false;
    std::vector<uint32_t> blocked_session_handles;
    {
      std::lock_guard<std::mutex> lock(sessions_lock_);
      for (const auto elem : sessions_) {
        auto session_handle = elem.first;
        auto pSessionInfo = elem.second;

        if(pSessionInfo->session_state_ != UCI_MSG_SESSION_STATE_ACTIVE)
          continue;
        // there's active sessions existed, delay per-country calibrations
        calibration_delayed_ = true;
        if (!rt_set->uwb_enable || rt_set->restricted_channel_mask & (1 << pSessionInfo->channel_)) {
          blocked_session_handles.push_back(session_handle);
        }
      }
    }

    if (rt_set->uwb_enable && !calibration_delayed_) {
      NXPLOG_UCIHAL_D("SessionTrack: no active sessions, execute per-country cal now.")
      apply_per_country_calibrations();
    } else {
      NXPLOG_UCIHAL_D("SessionTrack: device is in active state, delay per-country cal.")
      // stop all sessions affected by new country code's restrictions
      for (auto session_handle : blocked_session_handles) {
        NXPLOG_UCIHAL_D("SessionTrack: stop session (handle=0x%08x) due to country code restrictions", session_handle);
        // Can issue an UCI command. This function is only called from upper-layer thread.
        StopRanging(session_handle);
      }
    }
  }

  void RefreshIdle() {
    QueueSessionTrackWork(SessionTrackWorkType::REFRESH_IDLE);
  }

  void OnSessionStart(size_t packet_len, const uint8_t *packet) {
    if (packet_len != UCI_MSG_SESSION_START_CMD_LENGTH)
      return;

    if (!override_sts_index_for_ccc_) {
      return; /* Not needed / used... */
    }

    uint32_t session_handle = le_bytes_to_cpu<uint32_t>(&packet[UCI_MSG_SESSION_START_HANDLE_OFFSET]);

    std::shared_ptr<SessionInfo> pSessionInfo;
    {
      std::lock_guard<std::mutex> lock(sessions_lock_);
      pSessionInfo = GetSessionInfo(session_handle);
    }

    // Check STS_INDEX and fetch if it was not set by upper-layer
    if (!pSessionInfo || pSessionInfo->session_type_ != kSessionType_CCCRanging)
      return;

    auto result = QueryStsIndex(session_handle);
    if (!result.first) {
      NXPLOG_UCIHAL_E("SessionTrack: failed to query sts index, session_handle=0x%x", session_handle);
      return;
    }

    // When it's resuming session, FW gives 0xFFFFFFFF when STS_INDEX was not set (SR100 D50.21)
    if (result.second != 0 && result.second != 0xFFFFFFFF) {
      NXPLOG_UCIHAL_D("SessionTrack: sts_index0 already set, skip fetching it");
      return;
    }

    if (!pSessionInfo->ranging_started_) {
      // first ranging
      NXPLOG_UCIHAL_D("SessionTrack: session handle 0x%x has zero sts_index0, fetch it", session_handle);
      uint32_t new_sts_index = PickRandomStsIndex();
      SetStsIndex(session_handle, new_sts_index);
    } else {
      // resuming ranging, sts_index0 should be incremented
      NXPLOG_UCIHAL_D("SessionTrack: session handle 0x%x doesn't have valid sts_index0, increment it", session_handle);

      result = QueryLastStsIndexUsed(session_handle);
      if (!result.first) {
        NXPLOG_UCIHAL_E("SessionTrack: failed to query last sts index used, session_handle=0x%x", session_handle);
        return;
      }
      // increment it from last sts_index (just +1)
      uint32_t new_sts_index = (result.second + 1) & ((1 << 30) - 1);
      SetStsIndex(session_handle, new_sts_index);
    }
  }

private:
  // Send SESSION_STOP_CMD
  void StopRanging(uint32_t session_handle) {
    uint8_t session_handle_bytes[4];
    cpu_to_le_bytes(session_handle_bytes, session_handle);

    std::vector<uint8_t> packet{(UCI_MT_CMD << UCI_MT_SHIFT) | UCI_GID_SESSION_CONTROL, UCI_MSG_SESSION_STOP, 0, 0};
    packet.insert(packet.end(), std::begin(session_handle_bytes), std::end(session_handle_bytes));
    packet[UCI_PAYLOAD_LENGTH_OFFSET] = packet.size() - UCI_MSG_HDR_SIZE;

    auto ret = phNxpUciHal_send_ext_cmd(packet.size(), packet.data());
    if (ret != UWBSTATUS_SUCCESS) {
      NXPLOG_UCIHAL_E("SessionTrack: Failed to stop session handle 0x%08x", session_handle);
    }
  }

  // Send URSK_DELETE_CMD
  void DeleteUrsk(std::shared_ptr<SessionInfo> session_info) {
    if (!session_info)
      return;

    phNxpUciHal_Sem_t urskDeleteNtfWait;
    phNxpUciHal_init_cb_data(&urskDeleteNtfWait, NULL);

    phNxpUciHal_rx_handler_add(UCI_MT_RSP, UCI_GID_PROPRIETARY_0X0F,
      UCI_MSG_URSK_DELETE, true,
      [](size_t packet_len, const uint8_t *packet) -> bool {
        if (packet_len < 5)
          return true;
        if (packet[4] != UWBSTATUS_SUCCESS) {
          NXPLOG_UCIHAL_E("SessionTrack: URSR_DELETE failed, rsp status=0x%x", packet[4]);
        }
        return true;
      }
    );
    phNxpUciHal_rx_handler_add(UCI_MT_NTF, UCI_GID_PROPRIETARY_0X0F,
      UCI_MSG_URSK_DELETE, true,
      [&urskDeleteNtfWait](size_t packet_len, const uint8_t *packet) -> bool {
        if (packet_len < 6)
          return true;
        uint8_t status = packet[4];
        uint8_t nr = packet[5];

        // We always issue URSK_DELETE_CMD with one Session ID and wait for it,
        // Number of entries should be 1.
        if (nr != 1 || packet_len != (6 + 5 * nr)) {
          NXPLOG_UCIHAL_E("SessionTrack: unrecognized packet type of URSK_DELETE_NTF");
          urskDeleteNtfWait.status = UWB_DEVICE_ERROR;
        } else {
          if (status != UWBSTATUS_SUCCESS) {
            NXPLOG_UCIHAL_E("SessionTrack: URSK_DELETE failed, ntf status=0x%x", status);
            urskDeleteNtfWait.status = status;
          } else {
            uint32_t session_id = le_bytes_to_cpu<uint32_t>(&packet[6]);
            uint8_t del_status = packet[10];
            NXPLOG_UCIHAL_D("SessionTrack: URSK_DELETE done, deletion_status=%u", del_status);

            // 0: RDS removed successfully
            // 1: RDS not found
            // 2: Interface error encountered
            if (del_status == 0 || del_status == 1) {
              urskDeleteNtfWait.status = status;
            } else {
              urskDeleteNtfWait.status = UWB_DEVICE_ERROR;
            }
          }
        }
        SEM_POST(&urskDeleteNtfWait);
        return true;
      }
    );

    NXPLOG_UCIHAL_D("SessionTrack: URSK_DELETE for session ID 0x%x", session_info->session_id_);
    uint8_t session_id_bytes[4];
    cpu_to_le_bytes(session_id_bytes, session_info->session_id_);

    std::vector<uint8_t> packet{(UCI_MT_CMD << UCI_MT_SHIFT) | UCI_GID_PROPRIETARY_0X0F,
      UCI_MSG_URSK_DELETE, 0, 0};

    packet.push_back(1);  // Num of Session IDs = 1
    packet.insert(packet.end(), std::begin(session_id_bytes), std::end(session_id_bytes));
    packet[UCI_PAYLOAD_LENGTH_OFFSET] = packet.size() - UCI_MSG_HDR_SIZE;

    auto ret = phNxpUciHal_send_ext_cmd(packet.size(), packet.data());
    if (ret != UWBSTATUS_SUCCESS) {
      NXPLOG_UCIHAL_E("SessionTrack: Failed to delete URSK for session id 0x%08x",
        session_info->session_id_);
    }

    if (phNxpUciHal_sem_timed_wait_msec(&urskDeleteNtfWait, kUrskDeleteNtfTimeoutMs)) {
      NXPLOG_UCIHAL_E("SessionTrack: Failed(timeout) to delete URSK for session id 0x%08x",
        session_info->session_id_);
    }

    phNxpUciHal_cleanup_cb_data(&urskDeleteNtfWait);
  }

  std::pair<bool, uint32_t> GetAppConfLe32(uint32_t session_handle, uint8_t tag)
  {
    uint32_t val = 0;
    bool result = false;

    phNxpUciHal_rx_handler_add(UCI_MT_RSP, UCI_GID_SESSION_MANAGE,
      UCI_MSG_SESSION_GET_APP_CONFIG, true,
      [&val, &result, tag](size_t packet_len, const uint8_t *packet) -> bool {
        if (packet_len != 12)
          return true;

        if (packet[4] != UWBSTATUS_SUCCESS) {
          NXPLOG_UCIHAL_E("SessionTrack: GetAppConfig failed, status=0x%02x", packet[4]);
          return true;
        }
        if (packet[5] != 1) {
          NXPLOG_UCIHAL_E("SessionTrack: GetAppConfig failed, nr=%u", packet[5]);
          return true;
        }
        if (packet[6] != tag) {
          NXPLOG_UCIHAL_E("SessionTrack: GetAppConfig failed, tag=0x%02x, expected=0x%02x", packet[6], tag);
          return true;
        }
        if (packet[7] != 4) {
          NXPLOG_UCIHAL_E("SessionTrack: GetAppConfig failed, len=%u", packet[7]);
          return true;
        }
        val = le_bytes_to_cpu<uint32_t>(&packet[8]);
        result = true;
        return true;
      }
    );

    std::vector<uint8_t> packet{(UCI_MT_CMD << UCI_MT_SHIFT) | UCI_GID_SESSION_MANAGE,
      UCI_MSG_SESSION_GET_APP_CONFIG, 0,  0};

    uint8_t session_handle_bytes[4];
    cpu_to_le_bytes(session_handle_bytes, session_handle);

    packet.insert(packet.end(), std::begin(session_handle_bytes), std::end(session_handle_bytes));
    packet.push_back(1);  // Num of Prameters
    packet.push_back(tag);  // The parameter. STS Index
    packet[UCI_PAYLOAD_LENGTH_OFFSET] = packet.size() - UCI_MSG_HDR_SIZE;

    auto ret = phNxpUciHal_send_ext_cmd(packet.size(), packet.data());
    if (ret != UWBSTATUS_SUCCESS) {
      return std::pair(false, 0);
    } else {
      return std::pair(result, val);
    }
  }

  std::pair<bool, uint32_t> QueryStsIndex(uint32_t session_handle)
  {
    return GetAppConfLe32(session_handle, UCI_APP_CONFIG_FIRA_STS_INDEX);
  }

  std::pair<bool, uint32_t> QueryLastStsIndexUsed(uint32_t session_handle)
  {
    return GetAppConfLe32(session_handle, UCI_APP_CONFIG_CCC_LAST_STS_INDEX_USED);
  }

  bool SetStsIndex(uint32_t session_handle, uint32_t sts_index)
  {
    NXPLOG_UCIHAL_D("SessionTrack: SetStsIndex 0x%x for session handle 0x%x", sts_index, session_handle);

    uint8_t session_handle_bytes[4];
    uint8_t sts_index_bytes[4];

    cpu_to_le_bytes(session_handle_bytes, session_handle);
    cpu_to_le_bytes(sts_index_bytes, sts_index);

    std::vector<uint8_t> packet{(UCI_MT_CMD << UCI_MT_SHIFT) | UCI_GID_SESSION_MANAGE,
      UCI_MSG_SESSION_SET_APP_CONFIG, 0,  0};

    packet.insert(packet.end(), std::begin(session_handle_bytes), std::end(session_handle_bytes));
    packet.push_back(1);  // Num of Prameters
    packet.push_back(UCI_APP_CONFIG_FIRA_STS_INDEX);  // The parameter. STS Index
    packet.push_back(4);  // Sts Index Size...
    packet.insert(packet.end(), std::begin(sts_index_bytes), std::end(sts_index_bytes));
    packet[UCI_PAYLOAD_LENGTH_OFFSET] = packet.size() - UCI_MSG_HDR_SIZE;

    auto ret = phNxpUciHal_send_ext_cmd(packet.size(), packet.data());
    if (ret != UWBSTATUS_SUCCESS) {
      NXPLOG_UCIHAL_E("SessionTrack: Failed to SetStsIndex");
      return false;
    }
    return true;
  }

  uint32_t PickRandomStsIndex()
  {
    std::random_device rdev;
    std::mt19937 rng(rdev());

    // valid range is [1, 2~30), but use half of it to prevent roll over
    std::uniform_int_distribution<std::mt19937::result_type> sts_index(1, (1 << 16) - 1);
    return sts_index(rng);
  }

  // UCI_MSG_SESSION_STATUS_NTF rx handler
  bool OnSessionStatusNtf(size_t packet_len, const uint8_t* packet) {
    if (packet_len != UCI_MSG_SESSION_STATUS_NTF_LENGTH) {
      NXPLOG_UCIHAL_E("SessionTrack: SESSION_STATUS_NTF packet parse error");
      return false;
    }

    uint32_t session_handle = le_bytes_to_cpu<uint32_t>(&packet[UCI_MSG_SESSION_STATUS_NTF_HANDLE_OFFSET]);
    uint8_t session_state = packet[UCI_MSG_SESSION_STATUS_NTF_STATE_OFFSET];

    bool is_idle = false;
    {
      std::lock_guard<std::mutex> lock(sessions_lock_);

      auto pSessionInfo = GetSessionInfo(session_handle);
      if (pSessionInfo) {
        NXPLOG_UCIHAL_D("SessionTrack: update session handle 0x%08x state %u", session_handle, session_state);
        pSessionInfo->session_state_ = session_state;
      }

      if (session_state == UCI_MSG_SESSION_STATE_DEINIT) {
        NXPLOG_UCIHAL_D("SessionTrack: remove session handle 0x%08x", session_handle);

        if (delete_ursk_ccc_enabled_ && pSessionInfo &&
            pSessionInfo->session_type_ == kSessionType_CCCRanging) {

          // If this CCC ranging session, issue DELETE_URSK_CMD for this session.
          // This is executed on client thread, we shouldn't block the execution of this thread.
          QueueDeleteUrsk(pSessionInfo);
        }
        sessions_.erase(session_handle);
        is_idle = IsDeviceIdle();
      } else if (session_state == UCI_MSG_SESSION_STATE_ACTIVE) {
        // mark this session has been started at
        pSessionInfo->ranging_started_ = true;
      }
    }

    if (is_idle) { // transition to IDLE
      NXPLOG_UCIHAL_D("Queue Idle");
      QueueSessionTrackWork(SessionTrackWorkType::IDLE);
    }

    return false;
  }

  static void IdleTimerCallback(uint32_t TimerId, void* pContext) {
    SessionTrack *mgr = static_cast<SessionTrack*>(pContext);
    mgr->QueueSessionTrackWork(SessionTrackWorkType::IDLE_TIMER_FIRED);
  }

  void PowerIdleTimerStop() {
    if (!auto_suspend_enabled_)
      return;

    NXPLOG_UCIHAL_D("SessionTrack: stop idle timer");
    if (idle_timer_started_) {
      if (phOsalUwb_Timer_Stop(idle_timer_) != UWBSTATUS_SUCCESS) {
        NXPLOG_UCIHAL_E("SessionTrack: idle timer stop failed");
      }
      idle_timer_started_ = false;
    }
  }
  void PowerIdleTimerRefresh() {
    if (!auto_suspend_enabled_)
      return;

    NXPLOG_UCIHAL_D("SessionTrack: refresh idle timer, %lums", idle_timeout_ms_);
    if (idle_timer_started_) {
      if (phOsalUwb_Timer_Stop(idle_timer_) != UWBSTATUS_SUCCESS) {
        NXPLOG_UCIHAL_E("SessionTrack: idle timer stop failed");
      }
    }
    if (phOsalUwb_Timer_Start(idle_timer_, idle_timeout_ms_, IdleTimerCallback, this) != UWBSTATUS_SUCCESS) {
      NXPLOG_UCIHAL_E("SessionTrack: idle timer start failed");
    }
    idle_timer_started_ = true;
  }

  // Worker thread for auto suspend
  void PowerManagerWorker() {
    NXPLOG_UCIHAL_D("SessionTrack: worker thread started.")

    bool stop_thread = false;
    while (!stop_thread) {
      auto msg = msgq_->recv();
      if (!msg) {
        NXPLOG_UCIHAL_E("Power State: CRITICAL: worker thread received a bad message!, stop the queue");
        break;
      }
      NXPLOG_UCIHAL_D("SessionTrack: work %d state %d",
        static_cast<int>(msg->type_), static_cast<int>(power_state_.load()));

      switch (msg->type_) {
      case SessionTrackWorkType::IDLE:
        if (calibration_delayed_) {
          NXPLOG_UCIHAL_D("SessionTrack: No active session, execute per-country calibrations");
          CONCURRENCY_LOCK();
          apply_per_country_calibrations();
          CONCURRENCY_UNLOCK();
          calibration_delayed_ = false;
        }
        power_state_ = PowerState::IDLE;
        PowerIdleTimerRefresh();
        break;
      case SessionTrackWorkType::REFRESH_IDLE:
        if (power_state_ == PowerState::SUSPEND) {
          NXPLOG_UCIHAL_D("SessionTrack: resume");
          phTmlUwb_Resume();
          power_state_ = PowerState::IDLE;
        }
        if (power_state_ == PowerState::IDLE) {
          PowerIdleTimerRefresh();
        }
        break;
      case SessionTrackWorkType::ACTIVATE:
        if (power_state_ == PowerState::SUSPEND) {
          NXPLOG_UCIHAL_E("SessionTrack: activated while in suspend!");
          phTmlUwb_Resume();
        }
        PowerIdleTimerStop();
        power_state_ = PowerState::ACTIVE;
        break;
      case SessionTrackWorkType::IDLE_TIMER_FIRED:
        if (power_state_ == PowerState::IDLE) {
          NXPLOG_UCIHAL_D("SessionTrack: idle timer expired, go suspend");
          power_state_ = PowerState::SUSPEND;
          phTmlUwb_Suspend();
        } else {
          NXPLOG_UCIHAL_E("SessionTrack: idle timer expired while in %d",
            static_cast<int>(power_state_.load()));
        }
        break;
      case SessionTrackWorkType::DELETE_URSK:
        CONCURRENCY_LOCK();
        DeleteUrsk(msg->session_info_);
        CONCURRENCY_UNLOCK();
        break;
      case SessionTrackWorkType::STOP:
        stop_thread = true;
        break;
      default:
        NXPLOG_UCIHAL_E("SessionTrack: worker thread received a bad message!");
        break;
      }
      if (msg->sync_) {
        std::lock_guard<std::mutex> lock(sync_mutex_);
        msg->cond_flag = true;
        msg->cond_.notify_one();
      }
    }
    if (idle_timer_started_) {
      PowerIdleTimerStop();
    }

    NXPLOG_UCIHAL_D("SessionTrack: worker thread exit.");
  }

  void QueueSessionTrackWork(std::shared_ptr<SessionTrackMsg> msg) {
    msgq_->send(msg);

    if (msg->sync_) {
      std::unique_lock<std::mutex> lock(sync_mutex_);
      if (!msg->cond_.wait_for(lock, std::chrono::milliseconds(kQueueTimeoutMs),
                               [&msg] { return msg->cond_flag; })) {
        NXPLOG_UCIHAL_E("SessionTrack: timeout to process %d", static_cast<int>(msg->type_));
      }
    }
  }

  void QueueSessionTrackWork(SessionTrackWorkType work) {
    // When sync is true, the job shouldn't trigger another transaction.
    // TODO: strict checking of each job is not executing UCI transactions.
    bool sync = (work == SessionTrackWorkType::STOP ||
                 work == SessionTrackWorkType::REFRESH_IDLE);
    auto msg = std::make_shared<SessionTrackMsg>(work, sync);
    QueueSessionTrackWork(msg);
  }

  void QueueDeleteUrsk(std::shared_ptr<SessionInfo> pSessionInfo) {
    // This job will execute another UCI transaction.
    auto msg = std::make_shared<SessionTrackMsg>(
      SessionTrackWorkType::DELETE_URSK, pSessionInfo, false);
    QueueSessionTrackWork(msg);
  }

  std::shared_ptr<SessionInfo> GetSessionInfo(uint32_t session_handle) {
    auto it = sessions_.find(session_handle);
    if (it == sessions_.end()) {
      NXPLOG_UCIHAL_E("SessionTrack: Session 0x%08x not registered", session_handle);
      return NULL;
    }
    return it->second;
  }

  bool IsDeviceIdle() {
    return sessions_.size() == 0;
  }
};

static std::unique_ptr<SessionTrack> gSessionTrack;

void SessionTrack_init()
{
  gSessionTrack = std::make_unique<SessionTrack>();
}

void SessionTrack_deinit()
{
  gSessionTrack.reset();
}

void SessionTrack_onCountryCodeChanged()
{
  if (gSessionTrack)
    gSessionTrack->OnCountryCodeChanged();
}

void SessionTrack_onAppConfig(uint32_t session_handle, uint8_t channel)
{
  if (gSessionTrack)
    gSessionTrack->OnChannelConfig(session_handle, channel);
}

void SessionTrack_keepAlive()
{
  if (gSessionTrack)
    gSessionTrack->RefreshIdle();
}

void SessionTrack_onSessionInit(size_t packet_len, const uint8_t *packet)
{
  if (gSessionTrack)
    gSessionTrack->OnSessionInit(packet_len, packet);
}

void SessionTrack_onSessionStart(size_t packet_len, const uint8_t *packet)
{
  if (gSessionTrack)
    gSessionTrack->OnSessionStart(packet_len, packet);
}