/*
|
* 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_power.h"
|
#include "mk_uwb.h"
|
#include "mk_clock.h"
|
#include "uwb_api.h"
|
#include "ul_tdoa.h"
|
#include "lib_ranging.h"
|
#include "lib_aoa.h"
|
#include "mk_trng.h"
|
#include <stdlib.h>
|
|
struct UL_TDOA_ENV_T ul_tdoa_env;
|
|
static struct UL_TDOA_CB_T ul_tdoa_cb;
|
|
static struct UWB_OP_T op = {
|
.session_configure = ul_tdoa_configure,
|
.session_start = ul_tdoa_start,
|
.session_stop = ul_tdoa_stop,
|
.session_local_addr_set = uwbs_local_short_addr_set,
|
.session_peer_addr_set = NULL,
|
.session_responder_addr_add = NULL,
|
.session_responder_list_clr = NULL,
|
.session_dynamic_update_responder_list = NULL,
|
.session_set_ccc_ursk = NULL,
|
};
|
|
static void ul_tdoa_timer_callback(void *dev, uint32_t time);
|
|
static uint8_t tx_msg[127] = {0};
|
static uint16_t tx_msg_len = 0;
|
|
static void ul_tdoa_tx_process(struct MAC_HW_REPORT_T *tx_report);
|
static void ul_tdoa_rx_process(struct MAC_HW_REPORT_T *rx_report);
|
void app_session_init(void);
|
|
int ul_tdoa_init(uint8_t handle_id)
|
{
|
/* store handler ID */
|
ul_tdoa_cb.handle_id = handle_id;
|
|
/* init rx queue */
|
WSF_QUEUE_INIT(&ul_tdoa_cb.msg_queue);
|
|
LOG_INFO(TRACE_MODULE_APP, "Ranging lib version: %s\r\n", MK8000_get_rangelib_version());
|
LOG_INFO(TRACE_MODULE_APP, "AoA lib version: %s\r\n", MK8000_get_aoalib_version());
|
return 0;
|
}
|
|
int ul_tdoa_deinit(void)
|
{
|
return 0;
|
}
|
|
// 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(ul_tdoa_tx_process, ul_tdoa_rx_process);
|
|
uwbs_handler_init(&op);
|
}
|
|
void ul_tdoa_configure(void)
|
{
|
ul_tdoa_env.stage = UL_TDOA_IDLE;
|
ul_tdoa_env.ranging_period = MS_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.ranging_interval);
|
ul_tdoa_env.random_window = MS_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.ul_tdoa_random_window);
|
ul_tdoa_env.sequence_num = 0;
|
ul_tdoa_env.lost_cnt = 0;
|
|
uint32_t seed = 0;
|
|
trng_open();
|
trng_get(&seed, 1, NULL);
|
trng_close();
|
|
srand(seed);
|
LOG_INFO(TRACE_MODULE_APP, "Random seed %u\r\n", seed);
|
|
phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
|
uwbs_configure(PHY_TX | PHY_RX, uwb_app_config.session_param.tx_power_level);
|
|
aoa_param_config();
|
}
|
|
static uint32_t ul_tdoa_tx_offset_get(void)
|
{
|
uint32_t tx_offset_ms = (uint32_t)rand() % uwb_app_config.session_param.ul_tdoa_random_window;
|
if (tx_offset_ms < 1)
|
{
|
tx_offset_ms = 1;
|
}
|
return MS_TO_PHY_TIMER_COUNT(tx_offset_ms);
|
}
|
|
static void ul_tdoa_pkt_construct(enum OWR_MESSAGE_TYPE_T type, int64_t tx_time)
|
{
|
uint8_t input[160];
|
uint8_t input_len = 0;
|
|
/*
|
Bits:
|
0-2 Frame Type
|
3 Long Frame Control
|
4-5 Destination Addressing mode
|
6-7 Source Addressing mode
|
8 PAN ID Present
|
9 Security Enabled
|
10 Sequence Number Suppression
|
11 Frame Pending
|
12-13 Frame Version
|
14 Ack Request
|
15 IE Present
|
*/
|
uint16_t frame_control = (1 << 15) | (1 << 10) | (1 << 9) | (2 << 6) | (1 << 3) | (5 << 0);
|
|
input[input_len++] = frame_control & 0xff;
|
input[input_len++] = (frame_control >> 8) & 0xff;
|
|
uint16_t mac_addr = uwbs_local_short_addr_get();
|
input[input_len++] = mac_addr & 0xff;
|
input[input_len++] = (mac_addr >> 8) & 0xff;
|
|
// Auxiliary Security Header
|
uint8_t sec_lvl = 6;
|
uint8_t sec_control = (1 << 5) | (uint8_t)(sec_lvl << 0);
|
|
input[input_len++] = sec_control;
|
|
uint32_t phy_sts_index = ul_tdoa_env.sequence_num;
|
uint32_t session_id = uwb_app_config.session_id;
|
|
// Header IE - Measurement Report Message Type 1
|
uint16_t header_ie = (0x0 << 15) | (0x0 << 7) | (19 << 0);
|
uint32_t vendor_oui = 0x5a18ff;
|
input[input_len++] = header_ie & 0xff;
|
input[input_len++] = (header_ie >> 8) & 0xff;
|
input[input_len++] = vendor_oui & 0xff;
|
input[input_len++] = (vendor_oui >> 8) & 0xff;
|
input[input_len++] = (vendor_oui >> 16) & 0xff;
|
input[input_len++] = 0x08;
|
input[input_len++] = 0x08;
|
input[input_len++] = 0x08;
|
input[input_len++] = 0x08;
|
input[input_len++] = 0x08;
|
input[input_len++] = 0x08;
|
input[input_len++] = 0x08;
|
input[input_len++] = 0x08;
|
input[input_len++] = session_id & 0xff;
|
input[input_len++] = (session_id >> 8) & 0xff;
|
input[input_len++] = (session_id >> 16) & 0xff;
|
input[input_len++] = (session_id >> 24) & 0xff;
|
input[input_len++] = phy_sts_index & 0xff;
|
input[input_len++] = (phy_sts_index >> 8) & 0xff;
|
input[input_len++] = (phy_sts_index >> 16) & 0xff;
|
input[input_len++] = (phy_sts_index >> 24) & 0xff;
|
|
header_ie = (0x7e << 7) | (0 << 0); // HT1 IE
|
input[input_len++] = header_ie & 0xff;
|
input[input_len++] = (header_ie >> 8) & 0xff;
|
|
// Payload IE
|
uint8_t payload_content_len = 16;
|
uint16_t payload_ie = (1 << 15) | (0x2 << 11) | (payload_content_len);
|
|
input[input_len++] = payload_ie & 0xff;
|
input[input_len++] = (payload_ie >> 8) & 0xff;
|
|
input[input_len++] = vendor_oui & 0xff;
|
input[input_len++] = (vendor_oui >> 8) & 0xff;
|
input[input_len++] = (vendor_oui >> 16) & 0xff;
|
|
// OWR message type || UWB message ID
|
uint8_t uwb_message_id = (uint8_t)(type << 4) | 0x7;
|
input[input_len++] = uwb_message_id;
|
|
// Reserved (4) || TX timestamp present (2) || Device ID Present (2)
|
uint8_t message_control = (uint8_t)(2 << 2) | 1;
|
input[input_len++] = message_control;
|
|
uint32_t frame_number = ul_tdoa_env.sequence_num;
|
input[input_len++] = frame_number & 0xff;
|
input[input_len++] = (frame_number >> 8) & 0xff;
|
input[input_len++] = (frame_number >> 16) & 0xff;
|
input[input_len++] = (frame_number >> 24) & 0xff;
|
|
uint8_t *device_id = &uwb_app_config.session_param.ul_tdoa_device_id[1];
|
input[input_len++] = device_id[0];
|
input[input_len++] = device_id[1];
|
|
input[input_len++] = tx_time & 0xff;
|
input[input_len++] = (tx_time >> 8) & 0xff;
|
input[input_len++] = (tx_time >> 16) & 0xff;
|
input[input_len++] = (tx_time >> 24) & 0xff;
|
input[input_len++] = (tx_time >> 32) & 0xff;
|
input[input_len++] = (tx_time >> 40) & 0xff;
|
input[input_len++] = (tx_time >> 48) & 0xff;
|
input[input_len++] = (tx_time >> 56) & 0xff;
|
|
memcpy(&tx_msg[0], &input[0], input_len);
|
tx_msg_len = input_len;
|
}
|
|
void ul_tdoa_start(void)
|
{
|
ul_tdoa_env.anchor_point = phy_timer_count_get();
|
ul_tdoa_env.tx_offset = ul_tdoa_tx_offset_get();
|
|
enum DEV_ROLE_T role = uwb_app_config.session_param.device_role;
|
if (role == DEV_ROLE_UT_SYNC_ANCHOR)
|
{
|
uint32_t curr_count = phy_timer_count_get();
|
uint32_t count = (uint32_t)(ul_tdoa_env.anchor_point + ul_tdoa_env.tx_offset - curr_count);
|
if (count > ul_tdoa_env.random_window)
|
{
|
count = ul_tdoa_env.random_window;
|
}
|
if (count > MS_TO_PHY_TIMER_COUNT(2))
|
{
|
power_on_radio(0, 1);
|
ul_tdoa_env.stage = UL_TDOA_LISTEN;
|
mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, count - MS_TO_PHY_TIMER_COUNT(2));
|
mac_start();
|
}
|
else
|
{
|
ul_tdoa_env.stage = UL_TDOA_TX_SYNC;
|
phy_timer_target_set(ul_tdoa_env.anchor_point + ul_tdoa_env.tx_offset - UWB_PERIOD_PREFETCH_TIME, ul_tdoa_timer_callback);
|
}
|
}
|
else if (role == DEV_ROLE_UT_TAG)
|
{
|
ul_tdoa_env.stage = UL_TDOA_TX_BLINK;
|
phy_timer_target_set(ul_tdoa_env.anchor_point + ul_tdoa_env.tx_offset - UWB_PERIOD_PREFETCH_TIME, ul_tdoa_timer_callback);
|
}
|
else
|
{
|
power_on_radio(0, 1);
|
ul_tdoa_env.stage = UL_TDOA_LISTEN;
|
mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, ul_tdoa_env.random_window);
|
mac_start();
|
}
|
|
ul_tdoa_env.enable = 1;
|
LOG_INFO(TRACE_MODULE_APP, "UL-TDoA start, role %d\r\n", role);
|
}
|
|
void ul_tdoa_stop(void)
|
{
|
ul_tdoa_env.enable = 0;
|
LOG_INFO(TRACE_MODULE_APP, "UL-TDoA stop\r\n");
|
}
|
|
static void ul_tdoa_timer_callback(void *dev, uint32_t time)
|
{
|
ul_tdoa_env.sequence_num++;
|
|
// Calculate tx timestamp
|
int64_t tx_time = ranging_tx_time(ul_tdoa_env.anchor_point + ul_tdoa_env.tx_offset + phy_shr_duration());
|
enum OWR_MESSAGE_TYPE_T type = (uwb_app_config.session_param.device_role == DEV_ROLE_UT_SYNC_ANCHOR ? OWR_SYNV_UTM : OWR_BLINK_UTM);
|
|
ul_tdoa_pkt_construct(type, tx_time);
|
power_on_radio(1, 0);
|
mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, ul_tdoa_env.anchor_point + ul_tdoa_env.tx_offset, 0, tx_msg, tx_msg_len);
|
mac_start();
|
}
|
|
void ul_tdoa_process(const struct MAC_HW_REPORT_T *ind)
|
{
|
enum DEV_ROLE_T role = uwb_app_config.session_param.device_role;
|
// Power off radio
|
power_off_radio();
|
|
if (role == DEV_ROLE_UT_SYNC_ANCHOR)
|
{
|
if (ul_tdoa_env.stage == UL_TDOA_LISTEN)
|
{
|
if (ind->err_code == UWB_RX_OK)
|
{
|
int64_t timestamp = ranging_rx_time(ind);
|
ul_tdoa_rx_ind(ind->err_code, ind->pkt_data, ind->pkt_len, timestamp);
|
}
|
}
|
else
|
{
|
// TX sync done - update next tx offset
|
ul_tdoa_env.tx_offset = ul_tdoa_tx_offset_get();
|
ul_tdoa_env.anchor_point += ul_tdoa_env.ranging_period;
|
LOG_INFO(TRACE_MODULE_APP, "UL-TDoA TX Sync %u\r\n", ul_tdoa_env.sequence_num);
|
}
|
|
uint32_t curr_count = phy_timer_count_get();
|
uint32_t count = (uint32_t)(ul_tdoa_env.anchor_point + ul_tdoa_env.tx_offset - curr_count);
|
if (count > ul_tdoa_env.random_window)
|
{
|
count = ul_tdoa_env.random_window;
|
}
|
|
if (count > MS_TO_PHY_TIMER_COUNT(2))
|
{
|
ul_tdoa_env.stage = UL_TDOA_LISTEN;
|
power_on_radio(0, 1);
|
mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, count - MS_TO_PHY_TIMER_COUNT(2));
|
mac_start();
|
}
|
else
|
{
|
ul_tdoa_env.stage = UL_TDOA_TX_SYNC;
|
phy_timer_target_set(ul_tdoa_env.anchor_point + ul_tdoa_env.tx_offset - UWB_PERIOD_PREFETCH_TIME, ul_tdoa_timer_callback);
|
}
|
}
|
else if (role == DEV_ROLE_UT_TAG)
|
{
|
// TX blink done - update next tx offset
|
ul_tdoa_env.tx_offset = ul_tdoa_tx_offset_get();
|
ul_tdoa_env.anchor_point += ul_tdoa_env.ranging_period;
|
|
phy_timer_target_set(ul_tdoa_env.anchor_point + ul_tdoa_env.tx_offset - UWB_PERIOD_PREFETCH_TIME, ul_tdoa_timer_callback);
|
|
LOG_INFO(TRACE_MODULE_APP, "UL-TDoA TX Blink %u\r\n", ul_tdoa_env.sequence_num);
|
}
|
else
|
{
|
if (ind->err_code == UWB_RX_OK)
|
{
|
int64_t timestamp = ranging_rx_time(ind);
|
ul_tdoa_rx_ind(ind->err_code, ind->pkt_data, ind->pkt_len, timestamp);
|
}
|
|
power_on_radio(0, 1);
|
mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, ul_tdoa_env.random_window);
|
mac_start();
|
}
|
}
|
|
void ul_tdoa_rx_ind(uint16_t status, const uint8_t *data, uint16_t len, int64_t timestamp)
|
{
|
struct UL_TDOA_RX_IND_T *ind;
|
|
if ((ind = WsfMsgAlloc(sizeof(struct UL_TDOA_RX_IND_T) + len)) != NULL)
|
{
|
ind->hdr.event = UL_TDOA_RX_IND_MSG;
|
ind->status = status;
|
ind->rx_len = len;
|
ind->rx_timestamp = timestamp;
|
if (data != NULL)
|
{
|
memcpy(ind->rx_data, data, len);
|
}
|
|
// Send the message
|
WsfMsgSend(ul_tdoa_cb.handle_id, ind);
|
}
|
}
|
|
static void ul_tdoa_tx_process(struct MAC_HW_REPORT_T *tx_report)
|
{
|
ul_tdoa_process(tx_report);
|
}
|
|
static void ul_tdoa_rx_process(struct MAC_HW_REPORT_T *rx_report)
|
{
|
ul_tdoa_process(rx_report);
|
}
|
