/*
|
* 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_aes.h"
|
#include "mk_misc.h"
|
|
#include "uwb_audio.h"
|
#include "board.h"
|
|
extern void uwb_audio_buf_update(uint8_t idx);
|
|
struct RANGING_ENV_T ranging_env;
|
uint8_t fira_uwb_tx_buf[PHY_PAYLOAD_LEN_MAX];
|
|
static struct UWB_AUDIO_CB_T uwb_audio_cb;
|
|
static struct UWB_OP_T op = {
|
.session_configure = uwb_audio_configure,
|
.session_start = uwb_audio_start,
|
.session_stop = uwb_audio_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_dynamic_update_responder_list = NULL,
|
.session_set_ccc_ursk = NULL,
|
.vendor_session_configure = NULL,
|
.vendor_session_start = NULL,
|
.vendor_session_stop = NULL,
|
};
|
|
static void uwb_audio_tx_process(struct MAC_HW_REPORT_T *tx_report);
|
static void uwb_audio_rx_process(struct MAC_HW_REPORT_T *rx_report);
|
void app_session_init(void);
|
|
static uint8_t *uwb_audio_tx_msg_buf[2][UWB_AUDIO_TX_SLOT_NUM];
|
static uint16_t uwb_audio_tx_msg_len[2][UWB_AUDIO_TX_SLOT_NUM];
|
|
UWB_AUDIO_SDU_T uwb_audio_buf[2][UWB_AUDIO_TX_SLOT_NUM];
|
|
static uint8_t uwb_audio_tx_flag[2] = {0};
|
static uint8_t uwb_audio_rx_flag[2] = {0};
|
|
uint8_t *mac_tx_buf_get(uint8_t idx);
|
|
extern uint8_t *mac_tx_buf_get(uint8_t idx);
|
extern uint8_t mac_repeat_rx(enum EVT_MODE_T mode, uint32_t tgt_time, uint8_t em_idx, uint32_t timeout);
|
extern uint8_t mac_repeat_tx(enum EVT_MODE_T mode, uint32_t tgt_time, uint8_t em_idx, uint8_t *pkt_data, uint16_t pkt_len);
|
|
int uwb_audio_init(uint8_t handle_id)
|
{
|
/* store handler ID */
|
uwb_audio_cb.handle_id = handle_id;
|
|
/* init rx queue */
|
WSF_QUEUE_INIT(&uwb_audio_cb.msg_queue);
|
|
for (uint8_t i = 0; i < UWB_AUDIO_TX_SLOT_NUM; i++)
|
{
|
uwb_audio_tx_msg_buf[0][i] = mac_tx_buf_get(i);
|
uwb_audio_tx_msg_buf[1][i] = mac_tx_buf_get(UWB_AUDIO_TX_SLOT_NUM + i);
|
|
uwb_audio_buf[0][i].data = uwb_audio_tx_msg_buf[0][i] + UWB_AUDIO_DATA_OFFSET;
|
uwb_audio_buf[1][i].data = uwb_audio_tx_msg_buf[1][i] + UWB_AUDIO_DATA_OFFSET;
|
}
|
|
return 0;
|
}
|
|
int uwb_audio_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(uwb_audio_tx_process, uwb_audio_rx_process);
|
|
uwbs_handler_init(&op);
|
}
|
|
void uwb_audio_configure(void)
|
{
|
fira_keys_generate();
|
|
aes_update_key(AES_ID0, &fira_key.devPayKey.ukey.keyByte[0]);
|
mac_update_ccm_key((uint32_t *)&fira_key.devPayKey.ukey.keyWord[0]);
|
mac_update_key_table(KEY_IDX_0, &fira_key.devPayKey.ukey.keyWord[0]);
|
|
uwb_app_config.ranging_stage = RANGING_IDLE;
|
|
ranging_env.uwb_period_prefetch_time = UWB_PERIOD_PREFETCH_TIME;
|
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;
|
ranging_env.uwb_rx_open_in_advance_wakeup = UWB_RX_OPEN_IN_ADVANCE;
|
ranging_env.uwb_rx_window_wakeup = UWB_RX_WINDOW;
|
ranging_env.ranging_period = MS_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.ranging_interval) / 4;
|
|
// ranging_env.slots_per_block = (uint16_t)(uwb_app_config.session_param.ranging_interval * 1000 / RSTU_TO_US(uwb_app_config.session_param.slot_duration));
|
// ASSERT(ranging_env.slots_per_block >= uwb_app_config.session_param.slots_per_round, "Ranging block parameters configure wrong");
|
|
ranging_env.slot_interval = RSTU_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.slot_duration);
|
ranging_env.round_duration = UWB_AUDIO_TX_SLOT_NUM * ranging_env.slot_interval + RSTU_TO_PHY_TIMER_COUNT(UWB_AUDIO_ACK_SLOT_DURATION);
|
|
ranging_env.slots_per_block = (uint16_t)(ranging_env.ranging_period / ranging_env.slot_interval);
|
|
ranging_env.phy_sts_index = fira_key.phyStsIdxInit - ranging_env.slots_per_block;
|
ranging_env.enable = 0;
|
|
ranging_env.slot_idx = 0;
|
ranging_env.tof = 0;
|
|
ranging_env.range_data.ranging_type = 0x1; // TWR (SS-TWR, DS-TWR)
|
ranging_env.range_data.ranging_interval = uwb_app_config.session_param.ranging_interval;
|
ranging_env.range_data.mac_addr_mode = uwbs_mac_addr_mode_get();
|
ranging_env.range_data.session_id = uwb_app_config.session_id;
|
ranging_env.stride_length = uwb_app_config.session_param.stride_length;
|
ranging_env.round_offset_in_block = 0;
|
ranging_env.next_round_index = 0;
|
|
uwbs_configure(PHY_TX | PHY_RX, uwb_app_config.session_param.tx_power_level);
|
|
LOG_INFO(TRACE_MODULE_FIRA, "slot duration %dus ranging interval %fms slots per round %d slots per block %d\r\n",
|
RSTU_TO_US(uwb_app_config.session_param.slot_duration), uwb_app_config.session_param.ranging_interval / 4.0,
|
uwb_app_config.session_param.slots_per_round, ranging_env.slots_per_block);
|
}
|
|
void uwb_audio_start(void)
|
{
|
ranging_env.enable = 1;
|
ranging_env.lost_cnt = 0x0;
|
ranging_env.anchor_point = phy_timer_count_get();
|
ranging_env.slot_idx = 0;
|
|
phy_sts_pkt_cfg_set(SP0);
|
|
enum DEV_ROLE_T role = uwb_app_config.session_param.device_role;
|
if (role == DEV_ROLE_INITIATOR)
|
{
|
uwb_app_config.ranging_stage = RANGING_DM_TX;
|
uwb_app_config.session_param.responder_slot_idx = 1;
|
phy_timer_target_set(ranging_env.anchor_point + ranging_env.ranging_period - UWB_PERIOD_PREFETCH_TIME, uwb_audio_phy_timer_callback);
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN);
|
}
|
else
|
{
|
uwb_app_config.ranging_stage = RANGING_SYNC;
|
ranging_update_slot_index(ranging_env.slot_idx);
|
power_on_radio(0, 1);
|
mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, ranging_env.ranging_period);
|
mac_start();
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
|
}
|
|
ranging_env.count = 0;
|
ranging_env.count_last = 0;
|
|
LOG_INFO(TRACE_MODULE_FIRA, "Session start, role %d\r\n", uwb_app_config.session_param.device_role);
|
}
|
|
void uwb_audio_stop(void)
|
{
|
ranging_env.enable = 0;
|
LOG_INFO(TRACE_MODULE_FIRA, "Ranging stop\r\n");
|
}
|
|
static void uwb_audio_tx_done_ind(const struct MAC_HW_REPORT_T *tx, enum RANGING_STAGE_T stage, uint8_t slot_idx)
|
{
|
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->ranging_stage = (uint8_t)stage;
|
ind->slot_idx = slot_idx;
|
ind->status = tx->err_code;
|
ind->tx_len = tx->pkt_len;
|
|
// Send the message
|
WsfMsgSend(uwb_audio_cb.handle_id, ind);
|
}
|
else
|
{
|
LOG_WARNING(TRACE_MODULE_UWB, "memory is not enough for UWB_PKT_TX_DONE_IND_T\r\n");
|
}
|
}
|
|
static uint16_t uwb_audio_rx_done_ind(const struct MAC_HW_REPORT_T *rx, enum RANGING_STAGE_T stage, uint8_t slot_idx)
|
{
|
// send an indication to application
|
const uint16_t rx_len = (uwb_app_config.session_param.device_type == DEV_TYPE_CONTROLLER) ? rx->pkt_len : UWB_AUDIO_DATA_OFFSET;
|
struct UWB_PKT_RX_DONE_IND_T *ind = WsfMsgAlloc(sizeof(struct UWB_PKT_RX_DONE_IND_T) + rx_len);
|
|
if (ind != NULL)
|
{
|
ind->hdr.event = UWB_PKT_RX_DONE_MSG;
|
ind->ranging_stage = (uint8_t)stage;
|
ind->slot_idx = slot_idx;
|
ind->status = rx->err_code;
|
ind->rx_len = rx->pkt_len;
|
|
if (rx->err_code == UWB_RX_OK)
|
{
|
if (rx_len)
|
{
|
memcpy(ind->rx_data, rx->pkt_data, rx_len);
|
}
|
else
|
{
|
// SPI transmit
|
}
|
}
|
|
// Send the message
|
WsfMsgSend(uwb_audio_cb.handle_id, ind);
|
return ind->status;
|
}
|
else
|
{
|
LOG_WARNING(TRACE_MODULE_UWB, "memory is not enough for UWB_PKT_RX_DONE_IND_T\r\n");
|
return 0xFFFF;
|
}
|
}
|
|
static void uwb_audio_audio_pkt_construct(uint16_t mac_addr, UWB_AUDIO_SDU_T *sdu)
|
{
|
uint8_t *input = uwb_audio_tx_msg_buf[uwb_app_config.session_param.responder_slot_idx][sdu->slot_idx];
|
uint16_t input_len = 0;
|
|
/*
|
Frame Type : 0 - 2
|
Security Enable : 3
|
Frame Pending : 4
|
AR : 5
|
PAND ID Compression : 6
|
Reserved : 7
|
Sequence Number Suppression : 8
|
IE Present : 9
|
Destination Addressing Mode : 10 - 11
|
Frame Version : 12 - 13
|
Source Addressing Mode : 14 - 15
|
*/
|
|
uint16_t frame_control = (2 << 12) | (2 << 10) | (1 << 8) | (1 << 6) | (1 << 0);
|
input[input_len++] = frame_control & 0xff;
|
input[input_len++] = (frame_control >> 8) & 0xff;
|
|
input[input_len++] = mac_addr & 0xff;
|
input[input_len++] = (mac_addr >> 8) & 0xff;
|
|
// Payload
|
input[input_len++] = sdu->slot_idx;
|
input[input_len++] = sdu->len & 0xFF;
|
input[input_len++] = (sdu->len >> 8) & 0xFF;
|
|
// Audio data is filled by uwb_audio_buf_update()
|
input_len += sdu->len;
|
|
uwb_audio_tx_msg_len[uwb_app_config.session_param.responder_slot_idx][sdu->slot_idx] = input_len;
|
}
|
|
static void uwb_audio_ack_pkt_construct(uint16_t mac_addr, uint8_t slot_idx)
|
{
|
uint8_t *input = uwb_audio_tx_msg_buf[uwb_app_config.session_param.responder_slot_idx][0];
|
uint16_t input_len = 0;
|
|
/*
|
Frame Type : 0 - 2
|
Security Enable : 3
|
Frame Pending : 4
|
AR : 5
|
PAND ID Compression : 6
|
Reserved : 7
|
Sequence Number Suppression : 8
|
IE Present : 9
|
Destination Addressing Mode : 10 - 11
|
Frame Version : 12 - 13
|
Source Addressing Mode : 14 - 15
|
*/
|
|
uint16_t frame_control = (2 << 12) | (2 << 10) | (1 << 8) | (1 << 6) | (1 << 0);
|
input[input_len++] = frame_control & 0xff;
|
input[input_len++] = (frame_control >> 8) & 0xff;
|
|
input[input_len++] = mac_addr & 0xff;
|
input[input_len++] = (mac_addr >> 8) & 0xff;
|
|
// Payload
|
input[input_len++] = slot_idx;
|
|
// RX packets status
|
input[input_len++] = uwb_audio_rx_flag[uwb_app_config.session_param.responder_slot_idx];
|
|
uwb_audio_tx_msg_len[uwb_app_config.session_param.responder_slot_idx][0] = input_len;
|
}
|
|
static uint8_t uwb_audio_audio_pkt_process(uint8_t *pkt_data, uint16_t pkt_len)
|
{
|
uint8_t slot_idx = pkt_data[UWB_AUDIO_SLOT_IDX_OFFSET];
|
// LOG_INFO(TRACE_MODULE_FIRA, "slot index %d\r\n", slot_idx);
|
|
uwb_audio_buf[uwb_app_config.session_param.responder_slot_idx][slot_idx].len = READ_SHORT(&pkt_data[UWB_AUDIO_DATA_LEN_OFFSET]);
|
uwb_audio_rx_flag[uwb_app_config.session_param.responder_slot_idx] |= (1 << slot_idx);
|
|
return slot_idx;
|
}
|
|
static uint8_t uwb_audio_ack_pkt_process(uint8_t *pkt_data, uint16_t pkt_len)
|
{
|
uint8_t slot_idx = pkt_data[UWB_AUDIO_SLOT_IDX_OFFSET];
|
// LOG_INFO(TRACE_MODULE_FIRA, "slot index %d\r\n", slot_idx);
|
|
// ACK
|
uwb_audio_rx_flag[uwb_app_config.session_param.responder_slot_idx] = pkt_data[5];
|
// LOG_INFO(TRACE_MODULE_FIRA, "%d\r\n", pkt_data[5]);
|
|
return slot_idx;
|
}
|
|
static uint32_t uwb_audio_target_time(uint8_t slot)
|
{
|
uint32_t round_idx = slot / (UWB_AUDIO_TX_SLOT_NUM + UWB_AUDIO_ACK_SLOT_NUM);
|
uint8_t slot_offset = slot % (UWB_AUDIO_TX_SLOT_NUM + UWB_AUDIO_ACK_SLOT_NUM);
|
|
uint32_t target = ranging_env.anchor_point + (ranging_env.round_duration * round_idx) + (slot_offset * ranging_env.slot_interval);
|
|
return target;
|
}
|
|
#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
|
#pragma clang diagnostic push
|
#pragma clang diagnostic ignored "-Wunreachable-code-break"
|
#pragma clang diagnostic ignored "-Wswitch-enum"
|
#endif
|
|
void uwb_audio_phy_timer_callback(void *dev, uint32_t time)
|
{
|
// LOG_INFO(TRACE_MODULE_FIRA, "FSM %02x slot %d\r\n", uwb_app_config.ranging_stage, ranging_env.slot_idx);
|
// board_led_on(BOARD_LED_1);
|
uint32_t target_time;
|
uint8_t responder_idx = uwb_app_config.session_param.responder_slot_idx;
|
|
if (ranging_env.enable)
|
{
|
if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR)
|
{
|
switch (uwb_app_config.ranging_stage)
|
{
|
case RANGING_DM_TX:
|
{
|
if (ranging_env.repeat)
|
{
|
for (uint8_t i = 0; i < UWB_AUDIO_TX_SLOT_NUM; i++)
|
{
|
uwb_audio_buf[responder_idx][i].ready = 1;
|
uwb_audio_tx_flag[responder_idx] |= (1 << i);
|
}
|
}
|
else
|
{
|
ranging_env.anchor_point += ranging_env.ranging_period;
|
ranging_env.phy_sts_index += uwb_app_config.session_param.slots_per_round;
|
|
uwb_app_config.session_param.responder_slot_idx = !uwb_app_config.session_param.responder_slot_idx;
|
responder_idx = uwb_app_config.session_param.responder_slot_idx;
|
uwb_audio_buf_update(responder_idx);
|
|
for (uint8_t i = 0; i < UWB_AUDIO_TX_SLOT_NUM; i++)
|
{
|
if (uwb_audio_buf[responder_idx][i].ready)
|
{
|
uwb_audio_audio_pkt_construct(0xffff, &uwb_audio_buf[responder_idx][i]);
|
uwb_audio_tx_flag[responder_idx] |= (1 << i);
|
}
|
else
|
{
|
uwb_audio_tx_flag[responder_idx] &= ~(1 << i);
|
}
|
}
|
}
|
|
// send DM
|
if (uwb_audio_tx_flag[responder_idx] == 0)
|
{
|
uwb_audio_fsm(NULL);
|
goto _exit;
|
}
|
else
|
{
|
power_on_radio(1, 0);
|
|
for (uint8_t i = 0; i < UWB_AUDIO_TX_SLOT_NUM; i++)
|
{
|
if (uwb_audio_tx_flag[responder_idx] & (1 << i))
|
{
|
target_time = uwb_audio_target_time(ranging_env.slot_idx + i);
|
|
uint8_t *tx = uwb_audio_tx_msg_buf[responder_idx][i];
|
// board_led_on(BOARD_LED_1);
|
// mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, 0, tx, uwb_audio_tx_msg_len[responder_idx][i]);
|
uint8_t em_idx = responder_idx * UWB_AUDIO_TX_SLOT_NUM + i;
|
mac_repeat_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, em_idx, tx, uwb_audio_tx_msg_len[responder_idx][i]);
|
// board_led_off(BOARD_LED_1);
|
}
|
}
|
}
|
}
|
break;
|
|
case RANGING_DM_RX:
|
{
|
// receive DM
|
target_time = uwb_audio_target_time(ranging_env.slot_idx) - UWB_RX_OPEN_IN_ADVANCE + UWB_RX_GUARD_TIME;
|
power_on_radio(0, 1);
|
mac_rx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, UWB_ACK_RX_WINDOW);
|
}
|
break;
|
|
default:
|
ASSERT(0, "UNEXPECTED STAGE\r\n");
|
break;
|
}
|
}
|
else if (uwb_app_config.session_param.device_role == DEV_ROLE_RESPONDER)
|
{
|
switch (uwb_app_config.ranging_stage)
|
{
|
case RANGING_SYNC:
|
{
|
// mac_update_key_table(KEY_IDX_0, &fira_key.devPayKey.ukey.keyWord[0]);
|
power_on_radio(0, 1);
|
mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, ranging_env.ranging_period);
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
|
}
|
break;
|
|
case RANGING_DM_RX:
|
{
|
// mac_update_key_table(KEY_IDX_0, &fira_key.devPayKey.ukey.keyWord[0]);
|
|
if (ranging_env.repeat)
|
{
|
}
|
else
|
{
|
ranging_env.anchor_point += ranging_env.ranging_period;
|
uwb_app_config.session_param.responder_slot_idx = !uwb_app_config.session_param.responder_slot_idx;
|
responder_idx = uwb_app_config.session_param.responder_slot_idx;
|
}
|
uwb_audio_rx_flag[responder_idx] = 0;
|
|
// receive DM
|
power_on_radio(0, 1);
|
|
for (uint8_t i = 0; i < UWB_AUDIO_TX_SLOT_NUM; i++)
|
{
|
target_time = uwb_audio_target_time(ranging_env.slot_idx + i) - UWB_RX_OPEN_IN_ADVANCE;
|
// mac_rx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, UWB_RX_WINDOW);
|
uint8_t em_idx = responder_idx * UWB_AUDIO_TX_SLOT_NUM + i;
|
mac_repeat_rx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, em_idx, UWB_RX_WINDOW);
|
}
|
}
|
break;
|
|
case RANGING_DM_TX:
|
{
|
// send DM
|
uint16_t mac_addr = uwbs_peer_short_addr_get();
|
power_on_radio(1, 0);
|
|
uwb_audio_ack_pkt_construct(mac_addr, ranging_env.slot_idx);
|
target_time = uwb_audio_target_time(ranging_env.slot_idx) + UWB_RX_GUARD_TIME;
|
mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, 0, uwb_audio_tx_msg_buf[responder_idx][0], uwb_audio_tx_msg_len[responder_idx][0]);
|
}
|
break;
|
|
default:
|
ASSERT(0, "UNEXPECTED STAGE\r\n");
|
break;
|
}
|
}
|
|
mac_start();
|
}
|
else
|
{
|
power_mode_clear(POWER_UNIT_APP);
|
}
|
|
_exit:
|
// board_led_off(BOARD_LED_1);
|
return;
|
}
|
|
enum RANGING_STAGE_T uwb_audio_fsm(const struct MAC_HW_REPORT_T *ind)
|
{
|
// LOG_INFO(TRACE_MODULE_FIRA, "FSM %02x slot %d\r\n", uwb_app_config.ranging_stage, ranging_env.slot_idx);
|
// board_led_on(BOARD_LED_2);
|
uint16_t status = 0;
|
uint8_t responder_idx = uwb_app_config.session_param.responder_slot_idx;
|
|
if (ranging_env.enable)
|
{
|
ranging_env.count++;
|
uint32_t target_time = ranging_env.anchor_point;
|
|
if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR)
|
{
|
switch (uwb_app_config.ranging_stage)
|
{
|
case RANGING_DM_TX:
|
{
|
if (uwb_audio_tx_flag[responder_idx])
|
{
|
uint8_t offset = search_byte_right_one(uwb_audio_tx_flag[responder_idx]) - 1;
|
// process tx-DM packet
|
uwb_audio_tx_done_ind(ind, uwb_app_config.ranging_stage, ranging_env.slot_idx + offset);
|
uwb_audio_buf[responder_idx][offset].ready = 0;
|
|
uwb_audio_tx_flag[responder_idx] &= ~(1 << offset);
|
if (uwb_audio_tx_flag[responder_idx] == 0)
|
{
|
#if UWB_UDP_MODE_EN
|
ranging_env.slot_idx = 0;
|
target_time += ranging_env.ranging_period;
|
#else
|
// TX done - add a half slot as guard time for TX/RX switching
|
ranging_env.slot_idx += UWB_AUDIO_TX_SLOT_NUM;
|
uwb_app_config.ranging_stage = RANGING_DM_RX;
|
target_time = uwb_audio_target_time(ranging_env.slot_idx) + UWB_RX_GUARD_TIME;
|
#endif
|
}
|
}
|
else
|
{
|
// skip RX
|
ranging_env.slot_idx = 0;
|
target_time += ranging_env.ranging_period;
|
}
|
}
|
break;
|
|
case RANGING_DM_RX:
|
{
|
ranging_env.repeat = 0;
|
// process rx-DM packet
|
status = uwb_audio_rx_done_ind(ind, uwb_app_config.ranging_stage, ranging_env.slot_idx);
|
if ((status == UWB_RX_OK) && (uwb_audio_ack_pkt_process(ind->pkt_data, ind->pkt_len) == ranging_env.slot_idx))
|
{
|
if (uwb_audio_rx_flag[responder_idx] != ((1 << UWB_AUDIO_TX_SLOT_NUM) - 1))
|
{
|
ranging_env.repeat = 1;
|
}
|
}
|
|
ranging_env.slot_idx++;
|
if (ranging_env.repeat)
|
{
|
if (ranging_env.slot_idx + uwb_app_config.session_param.slots_per_round >= ranging_env.slots_per_block)
|
{
|
ranging_env.slot_idx = 0;
|
target_time += ranging_env.ranging_period;
|
ranging_env.repeat = 0;
|
}
|
else
|
{
|
target_time = uwb_audio_target_time(ranging_env.slot_idx);
|
}
|
}
|
else
|
{
|
ranging_env.slot_idx = 0;
|
target_time += ranging_env.ranging_period;
|
ranging_env.repeat = 0;
|
}
|
uwb_app_config.ranging_stage = RANGING_DM_TX;
|
}
|
break;
|
|
default:
|
ASSERT(0, "UNEXPECTED STAGE\r\n");
|
break;
|
}
|
}
|
else if (uwb_app_config.session_param.device_role == DEV_ROLE_RESPONDER)
|
{
|
switch (uwb_app_config.ranging_stage)
|
{
|
case RANGING_SYNC:
|
{
|
// process rx-DM packet
|
status = uwb_audio_rx_done_ind(ind, uwb_app_config.ranging_stage, ranging_env.slot_idx);
|
if ((status == UWB_RX_OK) && (uwb_audio_audio_pkt_process(ind->pkt_data, ind->pkt_len) == ranging_env.slot_idx))
|
{
|
// update anchor point
|
ranging_env.anchor_point = ind->timestamp - phy_shr_duration();
|
|
ranging_env.lost_cnt = 0;
|
ranging_env.slot_idx = 0;
|
target_time = ranging_env.anchor_point + ranging_env.ranging_period;
|
uwb_app_config.ranging_stage = RANGING_DM_RX;
|
}
|
else
|
{
|
// receive DM again
|
mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, ranging_env.ranging_period);
|
mac_start();
|
return uwb_app_config.ranging_stage;
|
}
|
}
|
break;
|
|
case RANGING_DM_RX:
|
{
|
// process rx-DM packet
|
status = uwb_audio_rx_done_ind(ind, uwb_app_config.ranging_stage, ranging_env.slot_idx);
|
if ((status == UWB_RX_OK) &&
|
(uwb_audio_audio_pkt_process(ind->pkt_data, ind->pkt_len) == (ranging_env.slot_idx % (UWB_AUDIO_TX_SLOT_NUM + UWB_AUDIO_ACK_SLOT_NUM))))
|
{
|
// update anchor point
|
if (ranging_env.slot_idx == 0)
|
{
|
ranging_env.anchor_point = ind->timestamp - phy_shr_duration();
|
target_time = ranging_env.anchor_point;
|
}
|
ranging_env.lost_cnt = 0;
|
}
|
else
|
{
|
ranging_env.lost_cnt++;
|
}
|
|
ranging_env.slot_idx++;
|
if ((ranging_env.slot_idx % (UWB_AUDIO_TX_SLOT_NUM + UWB_AUDIO_ACK_SLOT_NUM)) == UWB_AUDIO_TX_SLOT_NUM)
|
{
|
if (ranging_env.lost_cnt > 10)
|
{
|
ranging_env.repeat = 0;
|
ranging_env.slot_idx = 0;
|
uwb_app_config.ranging_stage = RANGING_SYNC;
|
target_time = ranging_env.anchor_point + ranging_env.ranging_period;
|
}
|
else
|
{
|
#if UWB_UDP_MODE_EN
|
ranging_env.slot_idx = 0;
|
target_time = ranging_env.anchor_point + ranging_env.ranging_period;
|
#else
|
// Add a half slot as guard time for RX/TX switching
|
target_time = uwb_audio_target_time(ranging_env.slot_idx) + UWB_RX_GUARD_TIME;
|
uwb_app_config.ranging_stage = RANGING_DM_TX;
|
#endif
|
}
|
}
|
}
|
break;
|
|
case RANGING_DM_TX:
|
{
|
// process tx-DM packet
|
uwb_audio_tx_done_ind(ind, uwb_app_config.ranging_stage, ranging_env.slot_idx);
|
|
if (uwb_audio_rx_flag[responder_idx] == ((1 << UWB_AUDIO_TX_SLOT_NUM) - 1))
|
{
|
ranging_env.repeat = 0;
|
}
|
else
|
{
|
ranging_env.repeat = 1;
|
}
|
|
ranging_env.slot_idx++;
|
if (ranging_env.repeat)
|
{
|
if (ranging_env.slot_idx + uwb_app_config.session_param.slots_per_round >= ranging_env.slots_per_block)
|
{
|
ranging_env.slot_idx = 0;
|
target_time += ranging_env.ranging_period;
|
ranging_env.repeat = 0;
|
}
|
else
|
{
|
target_time = uwb_audio_target_time(ranging_env.slot_idx);
|
}
|
}
|
else
|
{
|
ranging_env.slot_idx = 0;
|
target_time += ranging_env.ranging_period;
|
}
|
uwb_app_config.ranging_stage = RANGING_DM_RX;
|
}
|
break;
|
|
default:
|
ASSERT(0, "UNEXPECTED STAGE\r\n");
|
break;
|
}
|
}
|
|
if (target_time != ranging_env.anchor_point)
|
{
|
power_off_radio();
|
|
if ((ranging_env.slot_idx % (UWB_AUDIO_TX_SLOT_NUM + UWB_AUDIO_ACK_SLOT_NUM)) == 0)
|
{
|
// for 3 packets construction
|
phy_timer_target_set(target_time - UWB_PERIOD_PREFETCH_TIME, uwb_audio_phy_timer_callback);
|
}
|
else
|
{
|
// for 1 packet construction
|
phy_timer_target_set(target_time - UWB_EVT_PREFETCH_TIME, uwb_audio_phy_timer_callback);
|
}
|
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN);
|
}
|
}
|
else
|
{
|
power_off_radio();
|
power_mode_clear(POWER_UNIT_APP);
|
}
|
|
// board_led_off(BOARD_LED_2);
|
|
return uwb_app_config.ranging_stage;
|
}
|
#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
|
#pragma clang diagnostic pop
|
#endif
|
|
static void uwb_audio_tx_process(struct MAC_HW_REPORT_T *tx_report)
|
{
|
uwb_audio_fsm(tx_report);
|
}
|
|
static void uwb_audio_rx_process(struct MAC_HW_REPORT_T *rx_report)
|
{
|
uwb_audio_fsm(rx_report);
|
}
|
