/*
|
* Copyright (c) 2019-2023 Beijing Hanwei Innovation Technology Ltd. Co. and
|
* its subsidiaries and affiliates (collectly called MKSEMI).
|
*
|
* All rights reserved.
|
*
|
* Redistribution and use in source and binary forms, with or without
|
* modification, are permitted provided that the following conditions are met:
|
*
|
* 1. Redistributions of source code must retain the above copyright notice,
|
* this list of conditions and the following disclaimer.
|
*
|
* 2. Redistributions in binary form, except as embedded into an MKSEMI
|
* integrated circuit in a product or a software update for such product,
|
* must reproduce the above copyright notice, this list of conditions and
|
* the following disclaimer in the documentation and/or other materials
|
* provided with the distribution.
|
*
|
* 3. Neither the name of MKSEMI nor the names of its contributors may be used
|
* to endorse or promote products derived from this software without
|
* specific prior written permission.
|
*
|
* 4. This software, with or without modification, must only be used with a
|
* MKSEMI integrated circuit.
|
*
|
* 5. Any software provided in binary form under this license must not be
|
* reverse engineered, decompiled, modified and/or disassembled.
|
*
|
* THIS SOFTWARE IS PROVIDED BY MKSEMI "AS IS" AND ANY EXPRESS OR IMPLIED
|
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
|
* MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
* DISCLAIMED. IN NO EVENT SHALL MKSEMI OR CONTRIBUTORS BE LIABLE FOR ANY
|
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
|
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
*/
|
|
#include "mk_trace.h"
|
#include "mk_uwb.h"
|
#include "wsf_msg.h"
|
#include "ranging_ccc.h"
|
#include "lib_ccc.h"
|
#include "lib_aoa.h"
|
#include "lib_ranging.h"
|
#include "uwb_api.h"
|
#ifdef RADAR_EN
|
#include "uwb_radar.h"
|
#endif
|
#if KF_EN
|
#include "lib_kf.h"
|
#endif
|
|
#if PDOA_3D_EN
|
#include "lib_pdoa_3d.h"
|
#endif
|
|
#if PDOA_3D_EN
|
#define PDOA_3D_SUPPORT_NUM 10
|
#define PDOA_3D_TIMEOUT_MS 2000
|
static struct PDOA_3D_MAC_ADDR_T mac_addr_cache[PDOA_3D_SUPPORT_NUM];
|
static struct PDOA_3D_PDOA_DATA_T pdoa_data_cache[PDOA_3D_SUPPORT_NUM];
|
#endif
|
|
#if KF_EN && FILTER_EN
|
#define KF_SUPPORT_NUM 6
|
#define KF_TIMEOUT_MS 1000
|
static struct KF_MAC_ADDR_T kf_mac_addr_cache[KF_SUPPORT_NUM];
|
static struct KF_CHANNEL_CACHE_T kf_channel_cache[KF_SUPPORT_NUM];
|
static struct KF_MAT_VALUE_CACHE_T kf_mat_value_cache[KF_SUPPORT_NUM];
|
#endif
|
|
static struct RANGING_CB_T ranging_cb;
|
|
static struct UWB_OP_T op = {
|
.session_configure = ranging_configure,
|
.session_start = ranging_start,
|
.session_stop = ranging_stop,
|
.session_local_addr_set = ranging_local_addr_set,
|
.session_peer_addr_set = ranging_peer_addr_set,
|
.session_responder_addr_add = ranging_responder_addr_add,
|
.session_responder_list_clr = ranging_responder_list_clr,
|
.session_responder_num_get = ranging_responder_num_get,
|
.session_responder_addr_get = ranging_responder_addr_get,
|
.session_set_ccc_ursk = ranging_set_ccc_ursk,
|
#ifdef RADAR_EN
|
.vendor_session_configure = uwb_radar_configure,
|
.vendor_session_start = uwb_radar_start,
|
.vendor_session_stop = uwb_radar_stop,
|
#else
|
.vendor_session_configure = NULL,
|
.vendor_session_start = NULL,
|
.vendor_session_stop = NULL,
|
#endif
|
};
|
|
void app_session_init(void);
|
extern void ccc_tx_process(struct MAC_HW_REPORT_T *tx_report);
|
extern void ccc_rx_process(struct MAC_HW_REPORT_T *rx_report);
|
|
//------------------------------------------------------------------------------
|
int ranging_init(uint8_t handle_id)
|
{
|
/* store handler ID */
|
ranging_cb.handle_id = handle_id;
|
|
/* init rx queue */
|
WSF_QUEUE_INIT(&ranging_cb.msg_queue);
|
|
ranging_cb.daemon_timer.handlerId = handle_id;
|
ranging_cb.daemon_timer.msg.event = RANGING_DAEMON_TIMER_MSG;
|
|
LOG_INFO(TRACE_MODULE_APP, "Ranging lib version: %s\r\n", MK8000_get_rangelib_version());
|
|
#if AOA_EN
|
LOG_INFO(TRACE_MODULE_APP, "AoA lib version: %s\r\n", MK8000_get_aoalib_version());
|
#endif
|
|
return 0;
|
}
|
|
int ranging_deinit(void)
|
{
|
return 0;
|
}
|
|
/**
|
* @brief Initialize ranging session.
|
* @note This function will be called by uwbapi_session_init().
|
*/
|
void app_session_init(void)
|
{
|
// register process handler for MAC TX done and RX done
|
mac_register_process_handler(ccc_tx_process, ccc_rx_process);
|
|
uwbs_handler_init(&op);
|
}
|
|
void ranging_configure(void)
|
{
|
ranging_env.uwb_evt_prefetch_time = UWB_EVT_PREFETCH_TIME;
|
ranging_env.uwb_rx_open_in_advance = UWB_RX_OPEN_IN_ADVANCE;
|
ranging_env.uwb_rx_window = UWB_RX_WINDOW;
|
|
uint32_t compensation = 0;
|
#if LOW_POWER_EN
|
compensation += LPM_PPM_COMPENSATION(LOW_POWER_CLOCK_PPM, uwb_app_config.session_param.ranging_interval) + US_TO_PHY_TIMER_COUNT(80);
|
#endif
|
ranging_env.uwb_period_prefetch_time = UWB_PERIOD_PREFETCH_TIME + compensation;
|
ranging_env.uwb_rx_open_in_advance_wakeup = UWB_RX_OPEN_IN_ADVANCE + compensation;
|
ranging_env.uwb_rx_window_wakeup = UWB_RX_WINDOW + 2 * compensation;
|
|
uwbs_configure(PHY_TX | PHY_RX, uwb_app_config.session_param.tx_power_level);
|
|
#if CSI_EN
|
ranging_aux_out_opt_set(CH_LEN_DEFAULT, 3);
|
#endif
|
|
#if (ANT_PATTERN == ANT_PATTERN_SQUARE)
|
struct AOA_ANGLE_SPAN_T aoa_span;
|
aoa_span.Ndim = 2;
|
aoa_span.el_low = 90;
|
aoa_span.el_high = 90;
|
aoa_span.el_step = 1;
|
aoa_span.az_low = 0;
|
aoa_span.az_high = 359;
|
aoa_span.az_step = 1;
|
aoa_angle_search_span_set(&aoa_span);
|
#endif
|
|
#if AOA_EN
|
aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM);
|
aoa_steering_vector_set((const float *)((uint32_t)((uwb_app_config.ppdu_params.ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE));
|
#else
|
aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI);
|
#endif
|
|
aoa_param_config();
|
|
#if PDOA_3D_EN
|
pdoa_3d_param_config(ANT_PATTERN, ANT_LAYOUT, PDOA_3D_AMBIGUITY_LEVEL_HIGH, mac_addr_cache, pdoa_data_cache, PDOA_3D_SUPPORT_NUM, PDOA_3D_TIMEOUT_MS);
|
// pdoa_angle_reverse_set(1, 0);
|
#endif
|
|
LOG_INFO(TRACE_MODULE_CCC, "AoA status:[%d]\r\n", uwb_app_config.session_param.aoa_result_req);
|
|
#if FILTER_EN
|
if (uwb_app_config.filter_en)
|
{
|
#if KF_EN
|
loc_post_kf_config(uwb_app_config.session_param.ranging_interval, kf_mac_addr_cache, kf_channel_cache, kf_mat_value_cache, KF_SUPPORT_NUM,
|
KF_TIMEOUT_MS);
|
#else
|
loc_post_filter_config(uwb_app_config.session_param.ranging_interval, 1, uwb_app_config.session_param.aoa_result_req);
|
#endif
|
}
|
LOG_INFO(TRACE_MODULE_CCC, "Ranging filter status:[%d]\r\n", uwb_app_config.filter_en);
|
#endif
|
|
#if RANGING_FOM_FILTER_EN
|
ranging_debug_csi_en_set(1);
|
#endif
|
|
if (uwb_app_config.session_param.sts_config == STS_DYNAMIC)
|
{
|
mac_ifs_set(PHY_LIFS_PERIOD_MACCLK_LONG, PHY_SIFS_PERIOD_MACCLK_LONG);
|
}
|
|
#if 1
|
LOG_INFO(TRACE_MODULE_CCC, "ranging interval:[%u], slot duration:[%u]\r\n", uwb_app_config.session_param.ranging_interval,
|
uwb_app_config.session_param.slot_duration);
|
LOG_INFO(TRACE_MODULE_CCC, "responders:[%u], responder slot idx:[%u]\r\n", uwb_app_config.session_param.controlees_num,
|
uwb_app_config.session_param.ccc_responder_slot_idx);
|
|
uint8_t rounds_per_block = (uint8_t)(MS_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.ranging_interval) /
|
(uwb_app_config.session_param.slots_per_round * RSTU_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.slot_duration)));
|
|
LOG_INFO(TRACE_MODULE_CCC, "rounds per block:[%u], slots per round:[%u]\r\n", rounds_per_block, uwb_app_config.session_param.slots_per_round);
|
LOG_INFO(TRACE_MODULE_CCC, "channel:[%u], sync_ci:[%u]\r\n", uwb_app_config.ppdu_params.ch_num, uwb_app_config.ppdu_params.code_index);
|
LOG_INFO(TRACE_MODULE_CCC, "hopping_mode:[0x%02x], hop_mode_key:[0x%08x]\r\n", uwb_app_config.session_param.hopping_mode,
|
uwb_app_config.session_param.hop_mode_key);
|
#endif
|
|
uwbs_security_enable_set(true);
|
}
|
|
void uwb_pkt_tx_done_ind(const struct MAC_HW_REPORT_T *tx, enum RANGING_STAGE_T stage)
|
{
|
// struct UWB_PKT_TX_DONE_IND_T *ind = WsfMsgAlloc(sizeof(struct UWB_PKT_TX_DONE_IND_T) + tx->pkt_len);
|
struct UWB_PKT_TX_DONE_IND_T *ind = WsfMsgAlloc(sizeof(struct UWB_PKT_TX_DONE_IND_T));
|
|
if (ind != NULL)
|
{
|
ind->hdr.event = UWB_PKT_TX_DONE_MSG;
|
|
ind->block_idx = ranging_env.block_idx;
|
ind->round_idx = ranging_env.round_idx;
|
ind->sts_idx = ranging_env.sts_idx;
|
|
ind->ranging_stage = (uint8_t)stage;
|
ind->status = tx->err_code;
|
|
ind->tx_len = tx->pkt_len;
|
|
if ((ind->tx_len) && (tx->pkt_data != NULL))
|
{
|
// memcpy(ind->tx_data, tx->pkt_data, tx->pkt_len);
|
}
|
|
// Send the message
|
WsfMsgSend(ranging_cb.handle_id, ind);
|
}
|
else
|
{
|
LOG_WARNING(TRACE_MODULE_APP, "memory is not enough for UWB_PKT_TX_DONE_IND_T\r\n");
|
}
|
}
|
|
#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
|
#pragma clang diagnostic push
|
#pragma clang diagnostic ignored "-Wcast-qual"
|
#endif
|
|
void uwb_pkt_rx_done_ind(const struct MAC_HW_REPORT_T *rx, enum RANGING_STAGE_T stage, uint8_t slot_idx)
|
{
|
// send an indication to application
|
// struct UWB_PKT_RX_DONE_IND_T *ind = WsfMsgAlloc(sizeof(struct UWB_PKT_RX_DONE_IND_T) + rx->pkt_len);
|
struct UWB_PKT_RX_DONE_IND_T *ind = WsfMsgAlloc(sizeof(struct UWB_PKT_RX_DONE_IND_T));
|
|
if (ind != NULL)
|
{
|
ind->hdr.event = UWB_PKT_RX_DONE_MSG;
|
ind->ranging_stage = (uint8_t)stage;
|
|
ind->block_idx = ranging_env.block_idx;
|
ind->round_idx = ranging_env.round_idx;
|
ind->sts_idx = ranging_env.sts_idx;
|
ind->slot_idx = slot_idx;
|
ind->status = rx->err_code;
|
// ind->rssi = rx->rssi;
|
ind->rssi = correct_rssi(rx->rssi);
|
ind->snr = correct_snr(rx->snr);
|
|
ind->rx_len = rx->pkt_len;
|
|
if ((rx->pkt_len) && (rx->pkt_data != NULL))
|
{
|
// memcpy(ind->rx_data, rx->pkt_data, rx->pkt_len);
|
}
|
|
#if RANGING_FOM_FILTER_EN
|
if (ranging_debug_csi_en_get())
|
{
|
uint8_t frame_idx = 0;
|
|
if (ind->ranging_stage == RANGING_POLL)
|
{
|
frame_idx = 1;
|
}
|
else if (ind->ranging_stage == RANGING_FINAL)
|
{
|
frame_idx = 2;
|
}
|
else if (ind->ranging_stage == RANGING_FINAL_DATA)
|
{
|
frame_idx = 3;
|
}
|
|
debug_csi.antenna = ranging_env.main_ant_id[0];
|
debug_csi.frame_idx = frame_idx;
|
|
uint32_t val = REG_READ(0x40003050);
|
debug_csi.frame[frame_idx].rf_gain = (val & 0x07);
|
debug_csi.frame[frame_idx].bb_gain = ((val >> 3) & 0x1f);
|
debug_csi.frame[frame_idx].bd_cnt = phy_bd_cnt_get();
|
debug_csi.frame[frame_idx].sfd_cnt = phy_sfd_cnt_get();
|
debug_csi.frame[frame_idx].error_code = ind->status;
|
|
if (rx->err_code != 0x0830)
|
{
|
debug_csi.frame[frame_idx].rssi = ind->rssi;
|
debug_csi.frame[frame_idx].snr = ind->snr;
|
debug_csi.frame[frame_idx].channel_power = REG_READ(0x40002064);
|
debug_csi.frame[frame_idx].noise_power = REG_READ(0x40002078);
|
debug_csi.frame[frame_idx].freq_offset = phy_freq_offset_get();
|
|
dump_preamble_cir(frame_idx, 128);
|
|
if ((frame_idx == 1) || (frame_idx == 2))
|
{
|
dump_sts_cir(frame_idx);
|
}
|
}
|
|
// prepare to report ranging failure message
|
if (rx->err_code != UWB_RX_OK)
|
{
|
debug_csi.session_id = ranging_env.session_id;
|
debug_csi.sts_index = ind->sts_idx;
|
debug_csi.ranging_status = 0xFF;
|
debug_csi.block_index = (uint16_t)ind->block_idx;
|
debug_csi.round_index = ind->round_idx;
|
}
|
}
|
#endif
|
|
// Send the message
|
WsfMsgSend(ranging_cb.handle_id, ind);
|
}
|
else
|
{
|
LOG_WARNING(TRACE_MODULE_APP, "memory is not enough for UWB_PKT_RX_DONE_IND_T\r\n");
|
}
|
}
|
#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
|
#pragma clang diagnostic pop
|
#endif
|
|
void ranging_report(struct RANGING_REPORT_IND_T *report, uint16_t block_idx, uint16_t round_idx, uint32_t sts_idx)
|
{
|
struct RANGING_REPORT_IND_T *ind;
|
|
if ((ind = WsfMsgAlloc(sizeof(struct RANGING_REPORT_IND_T))) != NULL)
|
{
|
ind->hdr.event = RANGING_REPORT_MSG;
|
|
#if RANGING_FOM_FILTER_EN
|
if (ranging_debug_csi_en_get())
|
{
|
debug_csi.session_id = ranging_env.session_id;
|
|
debug_csi.sts_index = sts_idx;
|
debug_csi.block_index = block_idx;
|
debug_csi.round_index = round_idx;
|
|
debug_csi.ranging_status = report->status;
|
}
|
#endif
|
|
ind->block_idx = block_idx;
|
memcpy(&ind->Tround1, &report->Tround1, 4 * sizeof(int64_t) + 2);
|
|
// Send the message
|
WsfMsgSend(ranging_cb.handle_id, ind);
|
}
|
else
|
{
|
LOG_WARNING(TRACE_MODULE_APP, "memory is not enough for RANGING_REPORT_MSG\r\n");
|
}
|
}
|
|
void ranging_req_update_keys(uint32_t sts_idx)
|
{
|
struct RANGING_UPD_KEYS_T *upd_keys;
|
|
if ((upd_keys = WsfMsgAlloc(sizeof(struct RANGING_UPD_KEYS_T))) != NULL)
|
{
|
upd_keys->hdr.event = RANGING_UPDATE_KEYS;
|
upd_keys->sts_idx = sts_idx;
|
|
// Send the message
|
WsfMsgSend(ranging_cb.handle_id, upd_keys);
|
}
|
else
|
{
|
LOG_WARNING(TRACE_MODULE_APP, "memory is not enough for RANGING_UPDATE_KEYS\r\n");
|
}
|
}
|
|
void ranging_monitor_start(uint32_t time_ms)
|
{
|
WsfTimerStartMs(&ranging_cb.daemon_timer, time_ms, WSF_TIMER_ONE_SHOT);
|
}
|
|
void ranging_monitor_stop(void)
|
{
|
WsfTimerStop(&ranging_cb.daemon_timer);
|
}
|
