/* * 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 "mk_aes.h" #include "mk_power.h" #include "lib_aoa.h" #include "lib_ranging.h" #include "uwb_data_transfer.h" #include "uwb_data.h" struct RANGING_ENV_T ranging_env; uint8_t fira_uwb_tx_buf[PHY_PAYLOAD_LEN_MAX]; static struct DATA_TRANSFER_CB_T data_transfer_cb; static struct UWB_OP_T op = { .session_configure = data_transfer_configure, .session_start = data_transfer_start, .session_stop = data_transfer_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 data_transfer_tx_process(struct MAC_HW_REPORT_T *tx_report); static void data_transfer_rx_process(struct MAC_HW_REPORT_T *rx_report); void app_session_init(void); //------------------------------------------------------------------------------ int data_transfer_init(uint8_t handle_id) { /* store handler ID */ data_transfer_cb.handle_id = handle_id; /* init rx queue */ WSF_QUEUE_INIT(&data_transfer_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 data_transfer_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(data_transfer_tx_process, data_transfer_rx_process); uwbs_handler_init(&op); } void data_transfer_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]); 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); ranging_env.slots_per_block = (uint16_t)(uwb_app_config.session_param.ranging_interval / RSTU_TO_MS(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 = ranging_env.slot_interval * uwb_app_config.session_param.slots_per_round; 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); // uwb_dm_config(&mdsdu_tx, &mdsdu_rx); LOG_INFO(TRACE_MODULE_FIRA, "slot duration %d slots per round %d ranging interval %d\r\n", uwb_app_config.session_param.slot_duration, uwb_app_config.session_param.slots_per_round, uwb_app_config.session_param.ranging_interval); } void data_transfer_start(void) { ranging_env.enable = 1; ranging_env.lost_cnt = 0xFF; ranging_env.anchor_point = phy_timer_count_get(); ranging_env.slot_idx = 0; ranging_env.is_hopping = uwb_app_config.session_param.hopping_mode; enum DEV_ROLE_T role = uwb_app_config.session_param.device_role; if ((role == DEV_ROLE_INITIATOR) || (role == DEV_ROLE_GATE_CONTROLLER)) { uwb_app_config.ranging_stage = RANGING_DTPCM; phy_timer_target_set(ranging_env.anchor_point + ranging_env.ranging_period - ranging_env.uwb_period_prefetch_time, session_timer_callback); power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN); } else { ranging_env.is_hopping = 0; ranging_env.stride_length = 0; ranging_env.round_offset_in_block = 0; ranging_env.next_round_index = 0; power_on_radio(0, 1); uwb_app_config.ranging_stage = RANGING_SYNC; phy_sts_pkt_cfg_set(SP0); ranging_update_slot_index(ranging_env.slot_idx); 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, "Ranging start, role %d\r\n", uwb_app_config.session_param.device_role); } void data_transfer_stop(void) { ranging_env.enable = 0; LOG_INFO(TRACE_MODULE_FIRA, "Ranging stop\r\n"); } #if 0 /* bitmap for 1 responder slot idx: 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 ... TX bitmap: 1 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 ... RX bitmap: 0 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 ... bitmap for 2 responders slot idx: 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 ... TX bitmap: 1 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 ... RX bitmap: 0 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 ... bitmap for 3 responders slot idx: 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 ... TX bitmap: 1 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 ... RX bitmap: 0 0 1 1 1 0 1 1 1 0 1 1 1 0 1 1 1 ... */ void uwb_slot_bitmap_generate(uint8_t *bits, uint8_t dev_idx, uint8_t dev_num) { ASSERT(bits, "Input bits is null"); uint8_t offset = dev_idx; uint8_t size = uwb_slot_bitmap_size_get(); while (offset < size * 8) { bits[offset >> 3] |= (uint8_t)(1 << (offset & 0x7)); offset += dev_num; } } #else void uwb_slot_bitmap_generate(uint8_t *bits, uint8_t dev_idx, uint8_t dev_num) { ASSERT(bits, "Input bits is null"); uint8_t size = uwb_slot_bitmap_size_get(); uint8_t offset = 0; int bitValue; while (offset < size * 8) { if (dev_idx == 0) { bitValue = ((offset + 1) % 3 == 0) ? 0 : 1; bits[offset / 8] |= (bitValue << (offset % 8)); } else { bitValue = ((offset + 1) % 3 == 0) ? 1 : 0; bits[offset / 8] |= (bitValue << (offset % 8)); } offset++; } } #endif uint8_t uwb_dm_rx_responder_idx(uint8_t slot_idx, uint8_t dev_num) { ASSERT(slot_idx > 1, "RX slot idx is wrong"); uint8_t dev_idx = (slot_idx - 2) % (dev_num + 1); return dev_idx; } void uwb_pkt_tx_done_ind(const struct MAC_HW_REPORT_T *tx, enum RANGING_STAGE_T stage, uint8_t slot_idx) { #if DM_PRINT_PAYLOAD_EN struct UWB_PKT_TX_DONE_IND_T *ind = WsfMsgAlloc(sizeof(struct UWB_PKT_TX_DONE_IND_T) + tx->pkt_len); #else struct UWB_PKT_TX_DONE_IND_T *ind = WsfMsgAlloc(sizeof(struct UWB_PKT_TX_DONE_IND_T)); #endif 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; #if DM_PRINT_PAYLOAD_EN if ((ind->tx_len) && (tx->pkt_data != NULL)) { memcpy(ind->tx_data, tx->pkt_data, tx->pkt_len); } #endif // Send the message WsfMsgSend(data_transfer_cb.handle_id, ind); } else { LOG_WARNING(TRACE_MODULE_UWB, "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 uint16_t 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 #if DM_PRINT_PAYLOAD_EN struct UWB_PKT_RX_DONE_IND_T *ind = WsfMsgAlloc(sizeof(struct UWB_PKT_RX_DONE_IND_T) + rx->pkt_len); #else struct UWB_PKT_RX_DONE_IND_T *ind = WsfMsgAlloc(sizeof(struct UWB_PKT_RX_DONE_IND_T)); #endif 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->rssi = rx->rssi; ind->snr = rx->snr; if (rx->err_code == UWB_RX_OK) { #if 1 ind->rx_len = rx->pkt_len; #if DM_PRINT_PAYLOAD_EN if ((ind->rx_len) && (rx->pkt_data != NULL)) { memcpy(ind->rx_data, rx->pkt_data, rx->pkt_len); } #endif #else // Decrypt packet ind->rx_len = fira_packet_decrypt(rx->pkt_data, ind->rx_data, rx->pkt_len, slot_idx); ind->status |= (ind->rx_len ? 0 : UWB_MAC_DEC_ERR); #endif } // Send the message WsfMsgSend(data_transfer_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; } } #if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) #pragma clang diagnostic pop #endif void session_timer_callback(void *dev, uint32_t time) { // LOG_INFO(TRACE_MODULE_FIRA, "PHY timer slot %d\r\n", ranging_env.slot_idx); // board_led_on(BOARD_LED_2); if (uwb_app_config.session_param.ranging_round_usage == DATA_TRANSFER_PHASE) { data_transfer_phy_timer_callback(dev, time); } // board_led_off(BOARD_LED_2); } enum RANGING_STAGE_T session_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_1); enum RANGING_STAGE_T stage = RANGING_IDLE; if (uwb_app_config.session_param.ranging_round_usage == DATA_TRANSFER_PHASE) { stage = data_transfer_fsm(ind); } // board_led_off(BOARD_LED_1); return stage; } static void data_transfer_tx_process(struct MAC_HW_REPORT_T *tx_report) { session_fsm(tx_report); } static void data_transfer_rx_process(struct MAC_HW_REPORT_T *rx_report) { session_fsm(rx_report); } #if UWB_SERIAL_DATA_TRANSFER_EN static uint8_t pkt_header_buff[2]; static uint8_t data_buff[SERIAL_MESSAGE_MAX_LEN]; static struct RING_BUFFER_T tx_cb; static struct RING_BUFFER_T rx_cb; static void uwb_serial_cb_init(struct RING_BUFFER_T *tx, struct RING_BUFFER_T *rx) { for (int i = 0; i < UWB_SERIAL_DATA_POOL_SIZE; i++) { tx->buffer[i].flag = true; rx->buffer[i].flag = true; } tx->head = 0; tx->tail = 0; tx->check = 0; rx->head = 0; rx->tail = 0; rx->check = 0; return; } static bool uwb_serial_cb_is_full(struct RING_BUFFER_T *cb) { if (cb->buffer[cb->tail].flag == false && ((cb->tail + 1) % (UWB_SERIAL_DATA_POOL_SIZE) == cb->head)) { LOG_WARNING(TRACE_NO_OPTION | TRACE_MODULE_APP, "tx_cb_is_full\r\n"); return true; } return false; } static struct UWB_TX_MSG_t *uwb_serial_cb_read(struct RING_BUFFER_T *cb) { if (cb->buffer[cb->check].flag == false) { return &cb->buffer[cb->check]; } return NULL; } static void uwb_serial_cb_write(struct RING_BUFFER_T *cb, uint8_t *value, uint16_t length) { if (uwb_serial_cb_is_full(cb)) { LOG_INFO(TRACE_MODULE_APP, "uwb serial data fifo full\r\n"); return; } if (cb->buffer[cb->tail].flag == true) { cb->buffer[cb->tail].flag = false; cb->buffer[cb->tail].length = length; memcpy(&cb->buffer[cb->tail].msg, value, cb->buffer[cb->tail].length); #if 0 LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "serial write FIFO[%d] MSG\r\n", cb->tail); for (uint8_t i = 0; i < cb->buffer[cb->tail].length; i++) { LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "%02x ", cb->buffer[cb->tail].msg[i]); } LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "\r\n"); #endif } else { LOG_INFO(TRACE_MODULE_APP, "uwb fifo block number %d\r\n", cb->tail); } cb->tail = (cb->tail + 1) % UWB_SERIAL_DATA_POOL_SIZE; } void uwb_serial_tx_msg_check(void) { struct UWB_TX_MSG_t *uwb_tx_msg = uwb_serial_cb_read(&tx_cb); if (uwb_tx_msg == NULL) { return; } if (uwb_dm_tx_is_busy() == 1) { return; } if (uwb_tx_msg->flag == false) { struct DM_MDSDU_T mdsdu_tx; mdsdu_tx.mac_addr = ranging_responder_addr_get(0); mdsdu_tx.len = uwb_tx_msg->length; mdsdu_tx.data = uwb_tx_msg->msg; mdsdu_tx.ready = 1; uwb_dm_tx_pkt(&mdsdu_tx); tx_cb.buffer[tx_cb.check].flag = true; tx_cb.check = (tx_cb.check + 1) % UWB_SERIAL_DATA_POOL_SIZE; return; } return; } void uwb_serial_rx_msg_update(struct DM_MDSDU_T *rx) { rx_cb.buffer[rx_cb.check].length = rx->len; rx_cb.buffer[rx_cb.check].flag = false; rx_cb.check = (rx_cb.check + 1) % UWB_SERIAL_DATA_POOL_SIZE; uwb_dm_rx_config(rx_cb.buffer[rx_cb.check].msg); return; } void uwb_serial_rx_msg_check(void) { if (rx_cb.buffer[rx_cb.tail].flag == false) { #if 0 LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "serial read FIFO[%d] MSG\r\n", rx_cb.tail); for (uint8_t i = 0; i < rx_cb.buffer[rx_cb.tail].length; i++) { LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "%02x ", rx_cb.buffer[rx_cb.tail].msg[i]); } LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "\r\n"); #endif int ret = DRV_OK; ret = uart_send(UART_ID1, rx_cb.buffer[rx_cb.tail].msg, rx_cb.buffer[rx_cb.tail].length, 0); if (ret != DRV_OK) { LOG_INFO(TRACE_MODULE_APP, "UART ERROR 0x%02x\r\n", ret); } else { rx_cb.buffer[rx_cb.tail].flag = true; rx_cb.tail = (rx_cb.tail + 1) % UWB_SERIAL_DATA_POOL_SIZE; } } return; } static void serial_receive_callback(void *dev, uint32_t err_code) { uint32_t lock = int_lock(); volatile static uint16_t tmp_read_idx = 0; if (tmp_read_idx == 0) { uint16_t sum_bytes = (uint16_t)((pkt_header_buff[1] << 8) + (pkt_header_buff[0])); ASSERT(sum_bytes <= SERIAL_MESSAGE_MAX_LEN, "Frame length is over range %u", sum_bytes); tmp_read_idx = sum_bytes; uart_receive(UART_ID1, data_buff, sum_bytes, serial_receive_callback); } else { #if 0 LOG_INFO(TRACE_MODULE_APP, "serial recv bytes [%d]\r\n",tmp_read_idx); for (uint8_t i = 0; i < tmp_read_idx; i++) { LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "%02x ", data_buff[i]); } LOG_INFO(TRACE_NO_OPTION | TRACE_MODULE_APP, "\r\n"); #endif if (tmp_read_idx <= MDSDU_MTU_MAX) { uwb_serial_cb_write(&tx_cb, data_buff, tmp_read_idx); } else { while (tmp_read_idx >= MDSDU_MTU_MAX) { uwb_serial_cb_write(&tx_cb, data_buff, MDSDU_MTU_MAX); tmp_read_idx -= MDSDU_MTU_MAX; } uwb_serial_cb_write(&tx_cb, &data_buff[MDSDU_MTU_MAX], tmp_read_idx); } tmp_read_idx = 0; } if (tmp_read_idx == 0) { uart_receive(UART_ID1, pkt_header_buff, sizeof(pkt_header_buff), serial_receive_callback); } int_unlock(lock); } void uwb_serial_data_transfer_init(void) { LOG_INFO(TRACE_MODULE_APP, "UART input data format: Length(2B) + Data \r\n"); struct UART_CFG_T serialuwb_cfg = { .parity = UART_PARITY_NONE, .stop = UART_STOP_BITS_1, .data = UART_DATA_BITS_8, .flow = UART_FLOW_CONTROL_NONE, .rx_level = UART_RXFIFO_CHAR_1, .tx_level = UART_TXFIFO_EMPTY, .baud = BAUD_921600, .dma_en = true, .int_rx = false, .int_tx = false, }; uart_open(UART_ID1, &serialuwb_cfg); uwb_serial_cb_init(&tx_cb, &rx_cb); uwb_dm_rx_config(rx_cb.buffer[rx_cb.head].msg); uart_receive(UART_ID1, pkt_header_buff, sizeof(pkt_header_buff), serial_receive_callback); return; } #else #include "mk_timer.h" static uint8_t tx_buff[1024]; static uint8_t rx_buff[1024]; static uint16_t length = 977; static void data_gen(uint16_t len) { uint16_t offset = 0; while (len) { for (uint8_t i = 0; i < 255; i++) { if (len == 0) { break; } tx_buff[offset++] = i; len--; } } } void uwb_data_transfer_send_test(void) { struct DM_MDSDU_T mdsdu_tx; mdsdu_tx.mac_addr = ranging_responder_addr_get(0); mdsdu_tx.len = length; mdsdu_tx.data = tx_buff; mdsdu_tx.ready = 1; uwb_dm_tx_pkt(&mdsdu_tx); return; } static void timer_callback(void *dev, uint32_t time) { uwb_data_transfer_send_test(); } void uwb_data_transfer_init_test(void) { if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR) { data_gen(length); struct TIMER_CFG_T timer_cfg = { .extin_type = TIMER_EXTIN_NONE, .load = 0x7A12, .int_en = true, .callback = timer_callback, }; timer_open(TIMER_ID0, &timer_cfg); } uwb_dm_rx_config(rx_buff); return; } #endif