/* * 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 "wsf_buf.h" #include "lib_aoa.h" #include "lib_ranging.h" #include "uwb_data_transfer.h" #include "uwb_data.h" #if ((UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_0) || (UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_1) || \ (UWB_DATA_TRANSFER_MODE == DATA_TRANSFER_TEST_MODE)) #if UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_0 // Serial port receives characters. static uint8_t ch; #endif #if ((UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_0) || (UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_1)) #define SERIAL_NUM UART_ID0 // Serial TX Data Buffer static uint8_t serial_tx_msg[MDSDU_MTU_MAX * UWB_SERIAL_DATA_POOL_SIZE]; // Serial RX Data Buffer static uint8_t serial_rx_msg[MDSDU_MTU_MAX * UWB_SERIAL_DATA_POOL_SIZE]; #endif // Serial TX/RX Data Buffer Memory management functions static void initMemoryPool(MemoryPool *pool, uint8_t *data); static void updateMemory_allocate_block(MemoryPool *pool, MemoryBlock *block); static void *allocateMemory(MemoryPool *pool, uint16_t size); static bool mergeFreeBlocks(MemoryPool *pool, uint8_t *prt, uint16_t length); static bool freeMemoryPool(MemoryPool *pool, MemoryBlock *block); static MemoryPool g_tx_pool; static MemoryPool g_rx_pool; static bool block_send_flag = false; #endif // UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_0 or SERIAL_DATA_TRANSFER_MODE_1 or DATA_TRANSFER_TEST_MODE 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_initiator_addr_set = ranging_initiator_addr_set, .session_controller_addr_set = ranging_controller_addr_set, .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"); } uint16_t data_transfer_send(uint8_t *data, uint16_t length) { struct DM_MDSDU_T mdsdu_tx; uint16_t status = 0; mdsdu_tx.mac_addr = ranging_responder_addr_get(0); mdsdu_tx.len = length; mdsdu_tx.data = data; mdsdu_tx.ready = 1; status = uwb_dm_tx_pkt(&mdsdu_tx); return status; } #if 1 /* 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 struct UWB_PKT_RX_DONE_IND_T *ind = WsfMsgAlloc(sizeof(struct UWB_PKT_RX_DONE_IND_T) + rx->pkt_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->rssi = rx->rssi; ind->rssi = correct_rssi(rx->rssi); ind->snr = correct_snr(rx->snr); if (rx->err_code == UWB_RX_OK) { #if 1 ind->rx_len = rx->pkt_len; if ((ind->rx_len) && (rx->pkt_data != NULL)) { memcpy(ind->rx_data, rx->pkt_data, rx->pkt_len); } #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_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_0) || (UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_1) || \ (UWB_DATA_TRANSFER_MODE == DATA_TRANSFER_TEST_MODE)) #if UWB_DATA_TRANSFER_MODE == 1 static uint8_t pkt_header_buff[2]; static uint8_t data_buff[SERIAL_MESSAGE_MAX_LEN]; #endif /**************************************************************************** * Private Functions ****************************************************************************/ /*****************************************************************************/ #ifdef MEMORY_DEBUG void memory_check(void) { #if 0 MemoryBlock *current_free = g_tx_pool.free; int i = 0; while(current_free != NULL) { LOG_INFO(TRACE_MODULE_APP, "free block[%d] start%p, end %p size %d\r\n", i, (void*)current_free->start,(void*)(current_free->start+current_free->size), current_free->size); i++; current_free = current_free->next; } i = 0; MemoryBlock *current_allocate = g_tx_pool.allocated; while(current_allocate != NULL) { LOG_INFO(TRACE_MODULE_APP, "allocate block[%d] start %p, end %p size %d free %d\r\n", i, (void*)current_allocate->start,(void*)(current_allocate->start+current_allocate->size), current_allocate->size, current_allocate->is_free); i++; current_allocate = current_allocate->next; } #endif MemoryBlock *current_free = g_tx_pool.free; int i = 0; while (current_free != NULL) { i++; current_free = current_free->next; } LOG_INFO(TRACE_MODULE_APP, "free block[%d]\r\n", i); i = 0; MemoryBlock *current_allocate = g_tx_pool.allocated; MemoryBlock *tail = NULL; while (current_allocate != NULL) { i++; tail = current_allocate; current_allocate = current_allocate->next; } if (tail != g_tx_pool.allocated->tail) { LOG_INFO(TRACE_MODULE_APP, "tail != g_tx_pool.allocated->tail\r\n"); } LOG_INFO(TRACE_MODULE_APP, "allocate block[%d]\r\n", i); return; } #endif #if ((UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_0) || (UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_1) || \ (UWB_DATA_TRANSFER_MODE == DATA_TRANSFER_TEST_MODE)) /**************************************************************************** * Name: initMemoryPool * * Description: Initialize the UWB receive/transmit Circular Buffer. * * Input Parameters: * pool - * data - * * Returned Value: * NULL * ****************************************************************************/ static void initMemoryPool(MemoryPool *pool, uint8_t *data) { if (data == NULL || pool == NULL) { return; } LOG_INFO(TRACE_MODULE_APP, "Init Memory POOL 0x%p\r\n", (void *)data); pool->memoryPool = data; pool->allocated = NULL; pool->free = (MemoryBlock *)WsfBufAlloc(sizeof(MemoryBlock)); if (pool->free == NULL) { WsfBufFree(pool); pool = NULL; return; } pool->free->start = pool->memoryPool; pool->free->size = MDSDU_MTU_MAX * UWB_SERIAL_DATA_POOL_SIZE; pool->free->is_free = 1; pool->free->next = NULL; pool->free->prev = NULL; pool->free->tail = pool->free; return; } /**************************************************************************** * Name: updateMemory_allocate_block * * Description: Add allocated memory to the memory management chain table * * Input Parameters: * pool - * block - * * Returned Value: * NULL * ****************************************************************************/ static void updateMemory_allocate_block(MemoryPool *pool, MemoryBlock *block) { if (pool == NULL || block == NULL) { LOG_INFO(TRACE_MODULE_APP, "%s Input parameter error\r\n", __func__); return; } if (pool->allocated == NULL) { block->next = NULL; block->prev = NULL; block->tail = block; pool->allocated = block; } else { MemoryBlock *tail = pool->allocated->tail; tail->next = block; block->next = NULL; block->prev = tail; pool->allocated->tail = block; } return; } /**************************************************************************** * Name: allocateMemory * * Description: Allocation of memory * * Input Parameters: * pool - * size - * * Returned Value: * NULL * ****************************************************************************/ static void *allocateMemory(MemoryPool *pool, uint16_t size) { uint32_t lock = int_lock(); void *prt = NULL; MemoryBlock *current = pool->free; while (current != NULL) { if (current->is_free && current->size >= size) { MemoryBlock *allocateblock = (MemoryBlock *)WsfBufAlloc(sizeof(MemoryBlock)); if (allocateblock == NULL) { return NULL; } allocateblock->is_free = 0; allocateblock->size = size; allocateblock->start = current->start; updateMemory_allocate_block(pool, allocateblock); uint16_t free_size = current->size - size; current->start = current->start + size * sizeof(uint8_t); current->size = free_size; current->is_free = 1; prt = (void *)allocateblock->start; break; } current = current->next; } int_unlock(lock); return prt; } /**************************************************************************** * Name: mergeFreeBlocks * * Description: Merge neighboring memory blocks. * * Input Parameters: * pool - * prt - * length - * * Returned Value: * NULL * ****************************************************************************/ static bool mergeFreeBlocks(MemoryPool *pool, uint8_t *prt, uint16_t length) { MemoryBlock *current = pool->free; uint8_t *start_addr = prt; uint8_t *end_addr = prt + length; bool ret = false; while (current != NULL) { if (current->start == (end_addr)) { current->start = start_addr; current->size = length + current->size; current->is_free = 1; ret = true; break; } else if (start_addr == (current->start + current->size)) { current->size = length + current->size; current->is_free = 1; ret = true; break; } current = current->next; } if (current != NULL && current->prev != NULL) { if ((current->prev->start + current->prev->size) == current->start) { current->prev->size = current->prev->size + current->size; current->prev->is_free = 1; current->prev->next = current->next; current->next->prev = current->prev; if (pool->free->tail == current) { pool->free->tail = current->prev; } WsfBufFree(current); current = NULL; } if ((current->start + current->size) == current->prev->start) { current->prev->start = current->start; current->prev->size = current->prev->size + current->size; current->prev->is_free = 1; current->prev->next = current->next; current->next->prev = current->prev; if (pool->free->tail == current) { pool->free->tail = current->prev; } WsfBufFree(current); current = NULL; } } if (current != NULL && current->next != NULL) { if (((current->start + current->size) == current->next->start)) { current->size = current->size + current->next->size; current->is_free = 1; if (pool->free->tail == current->next) { pool->free->tail = current; } current->next = current->next->next; current->next->next->prev = current; WsfBufFree(current->next); current->next = NULL; } if ((current->next->start + current->next->size) == current->start) { current->start = current->next->start; current->size = current->size + current->next->size; current->is_free = 1; if (pool->free->tail == current->next) { pool->free->tail = current; } current->next = current->next->next; current->next->next->prev = current; WsfBufFree(current->next); current->next = NULL; } } return ret; } /**************************************************************************** * Name: freeMemoryPool * * Description: free memory blocks. * * Input Parameters: * pool - * prt - * length - * * Returned Value: * NULL * ****************************************************************************/ static bool freeMemoryPool(MemoryPool *pool, MemoryBlock *block) { uint32_t lock = int_lock(); if (pool == NULL || block == NULL) { LOG_INFO(TRACE_MODULE_APP, "%s Input parameter error\r\n", __func__); return false; } MemoryBlock *current_allocate = block; MemoryBlock *next = current_allocate->next; MemoryBlock *prev = current_allocate->prev; uint16_t block_len = current_allocate->size; uint8_t *prt = current_allocate->start; if (next == NULL && prev == NULL) { pool->allocated = NULL; } else if (prev == NULL) { next->tail = pool->allocated->tail; pool->allocated = next; pool->allocated->prev = NULL; } else { prev->next = next; next->prev = prev; if (pool->allocated->tail == current_allocate) { pool->allocated->tail = prev; } else if (pool->allocated->tail == pool->allocated) { pool->allocated->tail = NULL; } } WsfBufFree(current_allocate); current_allocate = NULL; bool flag = mergeFreeBlocks(pool, prt, block_len); if (flag == false) { MemoryBlock *newfreeblock = (MemoryBlock *)WsfBufAlloc(sizeof(MemoryBlock)); if (newfreeblock == NULL) { return false; } newfreeblock->start = prt; newfreeblock->size = block_len; newfreeblock->is_free = 1; newfreeblock->next = NULL; newfreeblock->prev = pool->free->tail; pool->free->tail->next = newfreeblock; pool->free->tail = newfreeblock; } int_unlock(lock); return true; } /*****************************************************************************/ /**************************************************************************** * Name: uwb_serial_cb_init * * Description: * Initialize UWB transmit/receive Circular Buffer. ****************************************************************************/ void uwb_cb_init(uint8_t *tx_cb, uint8_t *rx_cb) { if (tx_cb == NULL || rx_cb == NULL) { LOG_INFO(TRACE_MODULE_APP, "\r\n Initialize UWB transmit/receive Circular Buffer Error\r\n"); return; } initMemoryPool(&g_tx_pool, tx_cb); initMemoryPool(&g_rx_pool, rx_cb); return; } /**************************************************************************** * Name: uwb_serial_cb_read * * Description: Accessing the UWB receive/transmit Circular Buffer. * * Input Parameters: * cb - UWB transmit/receive Circular Buffers * * Returned Value: * Returns the first valid data segment of the UWB receive/transmit Circular Buffer. * ****************************************************************************/ struct MemoryBlock *uwb_transmit_cb_read(void) { MemoryBlock *current_allocate = g_tx_pool.allocated; MemoryBlock *next = NULL; if (current_allocate == NULL) { return NULL; } while (current_allocate != NULL) { if (current_allocate->is_free == 1) { next = current_allocate->next; bool ret = freeMemoryPool(&g_tx_pool, current_allocate); if (ret == false) { LOG_INFO(TRACE_MODULE_APP, "free Memory address %p fail\r\n", (void *)current_allocate->start); } current_allocate = next; } else { break; } } return current_allocate; } struct MemoryBlock *uwb_receive_cb_read(void) { MemoryBlock *current_allocate = g_rx_pool.allocated; if (current_allocate == NULL) { return NULL; } if (current_allocate->is_free == 1) { return NULL; } if (current_allocate->next != NULL) { LOG_INFO(TRACE_MODULE_APP, "Next full\r\n"); } return current_allocate; } /**************************************************************************** * Name: uwb_serial_cb_write * * Description: Write the data received from the serial port to the UWB buffer * and wait for UWB transmission. * * Input Parameters: * cb - UWB transmit/receive Circular Buffers * value - Waiting for data to be sent from UWB * length - Length of data waiting to be sent by the UWB * * Returned Value: * NULL * ****************************************************************************/ void uwb_transmit_cb_write(uint16_t length) { uint8_t *ptr = (uint8_t *)allocateMemory(&g_tx_pool, length); if (ptr == NULL) { LOG_INFO(TRACE_MODULE_APP, "uwb transmit Cache full\r\n"); return; }; return; } void uwb_receive_cb_write(const uint8_t *value, uint16_t length) { uint8_t *ptr = (uint8_t *)allocateMemory(&g_rx_pool, length); if (ptr == NULL) { LOG_INFO(TRACE_MODULE_APP, "uw breceive Cache full\r\n"); return; } memcpy(ptr, value, length); return; } #if UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_1 /**************************************************************************** * Name: serial_receive_callback * * Description: * Serial port receive data callback function ****************************************************************************/ 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(SERIAL_NUM, pkt_header_buff, sizeof(pkt_header_buff), serial_receive_callback); } int_unlock(lock); } #elif UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_0 /**************************************************************************** * Name: serial_receive_callback * * Description: * Serial port receive data callback function * This function will receive a single character from the uart module and * append it to a string. The string will be be sent over UWB when the * last character received was a 'new line' '\n' (hex 0x0A) or if the * string has reached the maximum data length. ****************************************************************************/ static void serial_receive_callback(void *dev, uint32_t err_code) { uint32_t lock = int_lock(); static uint16_t index = 0; g_tx_pool.free->start[index] = ch; index++; if ((g_tx_pool.free->start[index - 1] == '\n') || (g_tx_pool.free->start[index - 1] == '\r')) { // When the last character received is 'new line' '\n' (hex 0x0A) // then it is not transmitted if (index > 1) { // Write complete string to UWB transmit buffer uwb_transmit_cb_write(index); } index = 0; } else if (index == MDSDU_MTU_MAX) { // Write complete string to UWB transmit buffer uwb_transmit_cb_write(index); index = 0; } uart_receive(SERIAL_NUM, &ch, 1, serial_receive_callback); int_unlock(lock); return; } #endif //#if UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_1 /**************************************************************************** * Public Functions ****************************************************************************/ #if (UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_0 || UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_1) /**************************************************************************** * Name: uwb_serial_tx_msg_check * * Description: * Check if there is data to be sent in the UWB transmit buffer, * if so, send it. ****************************************************************************/ void uwb_serial_tx_msg_check(void) { if (block_send_flag == true) { return; } MemoryBlock *current_send_block = uwb_transmit_cb_read(); if (current_send_block == NULL) { return; } if (uwb_dm_tx_is_busy() == 1) { return; } current_send_block->is_free = 1; struct DM_MDSDU_T mdsdu_tx; mdsdu_tx.mac_addr = ranging_responder_addr_get(0); mdsdu_tx.len = current_send_block->size; mdsdu_tx.data = current_send_block->start; mdsdu_tx.ready = 1; uwb_dm_tx_pkt(&mdsdu_tx); block_send_flag = true; return; } /**************************************************************************** * Name: uwb_serial_rx_msg_check * * Description: * Check if there is data in the UWB receive data buffer and send it * through the serial port if there is. ****************************************************************************/ void uwb_serial_rx_msg_check(void) { int ret = DRV_OK; MemoryBlock *block = uwb_receive_cb_read(); if (block == NULL) { return; } ret = uart_send(SERIAL_NUM, block->start, block->size, 0); if (ret == DRV_OK) { freeMemoryPool(&g_rx_pool, block); } return; } void uwb_serial_data_transfer_tx_done(void) { block_send_flag = false; // LOG_INFO(TRACE_MODULE_APP, "block_send_flag %d \r\n",block_send_flag); } #endif //(UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_0 || UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_1) #endif // UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_0 or SERIAL_DATA_TRANSFER_MODE_1 or DATA_TRANSFER_TEST_MODE /**************************************************************************** * Name: uwb_serial_data_transfer_init * * Description: * UWB Serial Data Transfer passthrough initialization * Mode 0, packet has no data format, serial data is terminated by '\r\n'. * Mode 1, packet has packet format 2 bytes data length and payload. * * Input Parameters: * NULL * * Returned Value: * NULL * ****************************************************************************/ void uwb_serial_data_transfer_init(void) { #if UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_1 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(SERIAL_NUM, &serialuwb_cfg); uwb_serial_cb_init(&tx_cb, &rx_cb); uwb_dm_rx_config(rx_cb.buffer[rx_cb.head].msg); uart_receive(SERIAL_NUM, pkt_header_buff, sizeof(pkt_header_buff), serial_receive_callback); #elif UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_0 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 = false, .int_rx = true, .int_tx = true, }; uart_open(SERIAL_NUM, &serialuwb_cfg); uwb_cb_init(serial_tx_msg, serial_rx_msg); uwb_dm_rx_config(g_rx_pool.free->start); uart_receive(SERIAL_NUM, &ch, 1, serial_receive_callback); #endif return; } #elif UWB_DATA_TRANSFER_MODE == DATA_TRANSFER_TEST_MODE #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; } void uwb_data_transfer_init_test(void) { data_gen(length); uwb_dm_rx_config(rx_buff); return; } #endif //#if ((UWB_DATA_TRANSFER_MODE == SERIAL_DATA_TRANSFER_MODE_0 || SERIAL_DATA_TRANSFER_MODE_1 || DATA_TRANSFER_TEST_MODE))