/*
|
* 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))
|
