/*
 * Copyright (c) 2019-2025 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_uwb.h"
#include "mk_reset.h"
#include "mk_clock.h"
#include "mk_trace.h"
#include "mk_misc.h"
#include "mk_aes.h"
#include "mk_lsp.h"
#include "mk_power.h"
#if RANGING_EN
#include "lib_ranging.h"
#endif

#include "board.h"

#ifndef RSSI_EN
#define RSSI_EN (0)
#endif

// single tone power range: -20dBm ~ 10dBm, step: 0.5dB (+/-0.2dB); tx power = -20dBm + 0.5 * power_level (+/- 1.5dB).
static const uint8_t power_table_CH9[][5] = {
    {0x0, 0x0, 0x0, 0x2, 0x2}, {0x0, 0x0, 0x0, 0x2, 0x2}, {0x0, 0x0, 0x0, 0x2, 0x2}, {0x0, 0x0, 0x0, 0x2, 0x2}, {0x0, 0x3, 0x0, 0x2, 0x2},
    {0x1, 0x0, 0x0, 0x2, 0x2}, {0x1, 0x2, 0x0, 0x2, 0x2}, {0x0, 0x5, 0x1, 0x2, 0x2}, {0x2, 0x2, 0x0, 0x2, 0x2}, {0x3, 0x1, 0x0, 0x2, 0x2},
    {0x2, 0x1, 0x1, 0x2, 0x2}, {0x2, 0x4, 0x1, 0x2, 0x2}, {0x1, 0x2, 0x2, 0x2, 0x2}, {0x1, 0x4, 0x2, 0x2, 0x2}, {0x3, 0x4, 0x1, 0x2, 0x2},
    {0x2, 0x2, 0x2, 0x2, 0x2}, {0x2, 0x5, 0x2, 0x2, 0x2}, {0x0, 0x2, 0x7, 0x2, 0x2}, {0x0, 0x4, 0x7, 0x2, 0x2}, {0x0, 0x6, 0x7, 0x2, 0x2},
    {0x1, 0x2, 0x7, 0x2, 0x2}, {0x1, 0x4, 0x7, 0x2, 0x2}, {0x1, 0x6, 0x7, 0x2, 0x2}, {0x2, 0x3, 0x7, 0x2, 0x2}, {0x2, 0x5, 0x7, 0x2, 0x2},
    {0x3, 0x2, 0x7, 0x2, 0x2}, {0x3, 0x4, 0x7, 0x2, 0x2}, {0x4, 0x2, 0x7, 0x2, 0x2}, {0x4, 0x4, 0x7, 0x2, 0x2}, {0x4, 0x6, 0x7, 0x2, 0x2},
    {0x5, 0x1, 0x7, 0x2, 0x2}, {0x5, 0x3, 0x7, 0x2, 0x2}, {0x5, 0x5, 0x7, 0x2, 0x2}, {0x6, 0x2, 0x7, 0x2, 0x2}, {0x6, 0x4, 0x7, 0x2, 0x2},
    {0x6, 0x6, 0x7, 0x2, 0x2}, {0x7, 0x4, 0x7, 0x2, 0x2}, {0x8, 0x1, 0x7, 0x2, 0x2}, {0x9, 0x0, 0x7, 0x2, 0x2}, {0x9, 0x1, 0x7, 0x2, 0x2},
    {0x9, 0x3, 0x7, 0x2, 0x2}, {0xa, 0x0, 0x7, 0x2, 0x2}, {0xa, 0x2, 0x7, 0x2, 0x2}, {0xb, 0x0, 0x7, 0x2, 0x2}, {0xb, 0x2, 0x7, 0x2, 0x2},
    {0xc, 0x0, 0x7, 0x2, 0x2}, {0xb, 0x6, 0x7, 0x2, 0x2}, {0xc, 0x4, 0x7, 0x2, 0x2}, {0xc, 0x6, 0x7, 0x2, 0x2}, {0xd, 0x3, 0x7, 0x2, 0x3},
    {0xc, 0x0, 0x7, 0x3, 0x3}, {0xc, 0x2, 0x7, 0x3, 0x3}, {0xc, 0x5, 0x7, 0x3, 0x3}, {0xc, 0x7, 0x7, 0x3, 0x3}, {0xd, 0x5, 0x7, 0x3, 0x3},
    {0xd, 0x7, 0x7, 0x3, 0x3}, {0xd, 0x7, 0x7, 0x3, 0x3}, {0xd, 0x7, 0x7, 0x3, 0x3}, {0xd, 0x7, 0x7, 0x3, 0x3}, {0xd, 0x7, 0x7, 0x3, 0x3},
    {0xd, 0x7, 0x7, 0x3, 0x3},
};

static const uint8_t power_table_CH5[][5] = {
    {0x0, 0x0, 0x0, 0x2, 0x2}, {0x0, 0x0, 0x0, 0x2, 0x2}, {0x0, 0x0, 0x0, 0x2, 0x2}, {0x0, 0x0, 0x0, 0x2, 0x2}, {0x0, 0x0, 0x0, 0x2, 0x2},
    {0x0, 0x0, 0x0, 0x2, 0x2}, {0x0, 0x1, 0x0, 0x2, 0x2}, {0x0, 0x4, 0x0, 0x2, 0x2}, {0x0, 0x7, 0x0, 0x2, 0x2}, {0x1, 0x1, 0x0, 0x2, 0x2},
    {0x1, 0x4, 0x0, 0x2, 0x2}, {0x1, 0x7, 0x0, 0x2, 0x2}, {0x2, 0x3, 0x0, 0x2, 0x2}, {0x1, 0x4, 0x1, 0x2, 0x2}, {0x1, 0x6, 0x1, 0x2, 0x2},
    {0x3, 0x5, 0x0, 0x2, 0x2}, {0x2, 0x4, 0x1, 0x2, 0x2}, {0x3, 0x1, 0x1, 0x2, 0x2}, {0x3, 0x3, 0x1, 0x2, 0x2}, {0x4, 0x1, 0x1, 0x2, 0x2},
    {0x4, 0x4, 0x1, 0x2, 0x2}, {0x2, 0x6, 0x2, 0x2, 0x2}, {0x3, 0x2, 0x2, 0x2, 0x2}, {0x1, 0x6, 0x3, 0x2, 0x2}, {0x2, 0x2, 0x3, 0x2, 0x2},
    {0x1, 0x2, 0x7, 0x2, 0x2}, {0x1, 0x4, 0x7, 0x2, 0x2}, {0x3, 0x3, 0x3, 0x2, 0x2}, {0x2, 0x4, 0x4, 0x2, 0x2}, {0x2, 0x5, 0x7, 0x2, 0x2},
    {0x3, 0x1, 0x7, 0x2, 0x2}, {0x3, 0x5, 0x4, 0x2, 0x2}, {0x4, 0x1, 0x7, 0x2, 0x2}, {0x4, 0x3, 0x7, 0x2, 0x2}, {0x5, 0x6, 0x3, 0x2, 0x2},
    {0x5, 0x0, 0x7, 0x2, 0x2}, {0x5, 0x2, 0x7, 0x2, 0x2}, {0x5, 0x4, 0x7, 0x2, 0x2}, {0x6, 0x0, 0x7, 0x2, 0x2}, {0x6, 0x2, 0x7, 0x2, 0x2},
    {0x6, 0x5, 0x7, 0x2, 0x2}, {0x6, 0x7, 0x7, 0x2, 0x2}, {0x7, 0x4, 0x7, 0x2, 0x2}, {0x7, 0x7, 0x4, 0x2, 0x2}, {0x8, 0x2, 0x7, 0x2, 0x2},
    {0x8, 0x5, 0x7, 0x2, 0x2}, {0x9, 0x1, 0x7, 0x2, 0x2}, {0x9, 0x4, 0x7, 0x2, 0x2}, {0xa, 0x0, 0x7, 0x2, 0x2}, {0xa, 0x3, 0x7, 0x2, 0x2},
    {0xa, 0x6, 0x7, 0x2, 0x2}, {0xb, 0x2, 0x7, 0x2, 0x2}, {0xb, 0x5, 0x7, 0x2, 0x2}, {0xc, 0x1, 0x7, 0x2, 0x2}, {0xc, 0x5, 0x7, 0x2, 0x2},
    {0xd, 0x2, 0x7, 0x2, 0x2}, {0xc, 0x1, 0x7, 0x3, 0x3}, {0xc, 0x6, 0x7, 0x3, 0x3}, {0xd, 0x2, 0x7, 0x3, 0x3}, {0xe, 0x2, 0x7, 0x3, 0x3},
    {0xe, 0x7, 0x7, 0x3, 0x3},
};

static const uint8_t power_table_CH2[][5] = {
    {0x0, 0x7, 0x0, 0x2, 0x2}, {0x1, 0x0, 0x0, 0x2, 0x2}, {0x0, 0x4, 0x1, 0x2, 0x2}, {0x0, 0x7, 0x1, 0x2, 0x2}, {0x2, 0x2, 0x0, 0x2, 0x2},
    {0x1, 0x1, 0x1, 0x2, 0x2}, {0x1, 0x6, 0x1, 0x2, 0x2}, {0x3, 0x5, 0x0, 0x2, 0x2}, {0x4, 0x2, 0x0, 0x2, 0x2}, {0x2, 0x6, 0x1, 0x2, 0x2},
    {0x3, 0x0, 0x1, 0x2, 0x2}, {0x3, 0x5, 0x1, 0x2, 0x2}, {0x5, 0x3, 0x0, 0x2, 0x2}, {0x4, 0x5, 0x1, 0x2, 0x2}, {0x2, 0x4, 0x2, 0x2, 0x2},
    {0x2, 0x7, 0x2, 0x2, 0x2}, {0x5, 0x3, 0x1, 0x2, 0x2}, {0x5, 0x6, 0x1, 0x2, 0x2}, {0x6, 0x0, 0x1, 0x2, 0x2}, {0x6, 0x3, 0x1, 0x2, 0x2},
    {0x2, 0x5, 0x3, 0x2, 0x2}, {0x2, 0x6, 0x3, 0x2, 0x2}, {0x1, 0x6, 0x4, 0x2, 0x2}, {0x1, 0x6, 0x7, 0x2, 0x2}, {0x2, 0x3, 0x4, 0x2, 0x2},
    {0x2, 0x3, 0x7, 0x2, 0x2}, {0x2, 0x6, 0x7, 0x2, 0x2}, {0x3, 0x2, 0x7, 0x2, 0x2}, {0x3, 0x5, 0x7, 0x2, 0x2}, {0x3, 0x7, 0x7, 0x2, 0x2},
    {0x4, 0x7, 0x4, 0x2, 0x2}, {0x5, 0x1, 0x4, 0x2, 0x2}, {0x5, 0x1, 0x7, 0x2, 0x2}, {0x5, 0x3, 0x7, 0x2, 0x2}, {0x5, 0x6, 0x7, 0x2, 0x2},
    {0x6, 0x2, 0x7, 0x2, 0x2}, {0x6, 0x5, 0x7, 0x2, 0x2}, {0x7, 0x1, 0x7, 0x2, 0x2}, {0x7, 0x4, 0x7, 0x2, 0x2}, {0x8, 0x1, 0x4, 0x2, 0x2},
    {0x8, 0x2, 0x7, 0x2, 0x2}, {0x8, 0x5, 0x7, 0x2, 0x2}, {0x9, 0x3, 0x4, 0x2, 0x2}, {0x9, 0x4, 0x7, 0x2, 0x2}, {0xa, 0x0, 0x7, 0x2, 0x2},
    {0xa, 0x2, 0x7, 0x2, 0x2}, {0xa, 0x5, 0x7, 0x2, 0x2}, {0xa, 0x7, 0x7, 0x2, 0x2}, {0xb, 0x3, 0x7, 0x2, 0x2}, {0xb, 0x6, 0x7, 0x2, 0x2},
    {0xc, 0x1, 0x7, 0x2, 0x2}, {0xc, 0x4, 0x7, 0x2, 0x2}, {0xd, 0x1, 0x7, 0x2, 0x2}, {0xe, 0x0, 0x7, 0x2, 0x2}, {0xe, 0x4, 0x7, 0x2, 0x2},
    {0xf, 0x1, 0x7, 0x2, 0x2}, {0xf, 0x5, 0x7, 0x2, 0x2}, {0xe, 0x4, 0x7, 0x3, 0x3}, {0xf, 0x3, 0x7, 0x3, 0x3}, {0xf, 0x7, 0x7, 0x3, 0x3},
    {0xf, 0x7, 0x7, 0x3, 0x3},
};

static const uint8_t power_table_CH12[][5] = {
    {0x0, 0x3, 0x2, 0x1, 0x0}, {0x0, 0x5, 0x2, 0x1, 0x0}, {0x0, 0x1, 0x3, 0x1, 0x0}, {0x0, 0x3, 0x3, 0x1, 0x0}, {0x1, 0x0, 0x3, 0x1, 0x0},
    {0x1, 0x2, 0x3, 0x1, 0x0}, {0x2, 0x0, 0x3, 0x1, 0x0}, {0x2, 0x2, 0x3, 0x1, 0x0}, {0x3, 0x2, 0x3, 0x1, 0x0}, {0x3, 0x4, 0x3, 0x1, 0x0},
    {0x4, 0x2, 0x3, 0x1, 0x0}, {0x0, 0x0, 0x3, 0x2, 0x0}, {0x0, 0x3, 0x3, 0x2, 0x0}, {0x0, 0x6, 0x3, 0x2, 0x0}, {0x1, 0x0, 0x3, 0x2, 0x0},
    {0x1, 0x3, 0x3, 0x2, 0x0}, {0x1, 0x6, 0x3, 0x2, 0x0}, {0x2, 0x1, 0x3, 0x2, 0x0}, {0x2, 0x4, 0x3, 0x2, 0x0}, {0x2, 0x7, 0x3, 0x2, 0x0},
    {0x3, 0x3, 0x3, 0x2, 0x0}, {0x0, 0x1, 0x7, 0x3, 0x0}, {0x0, 0x6, 0x7, 0x3, 0x0}, {0x1, 0x0, 0x4, 0x3, 0x0}, {0x1, 0x0, 0x7, 0x3, 0x0},
    {0x1, 0x4, 0x7, 0x3, 0x0}, {0x1, 0x7, 0x7, 0x3, 0x0}, {0x2, 0x0, 0x7, 0x3, 0x0}, {0x2, 0x4, 0x7, 0x3, 0x0}, {0x2, 0x7, 0x7, 0x3, 0x0},
    {0x3, 0x2, 0x7, 0x3, 0x0}, {0x3, 0x6, 0x7, 0x3, 0x0}, {0x4, 0x2, 0x7, 0x3, 0x0}, {0x4, 0x6, 0x7, 0x3, 0x0}, {0x5, 0x1, 0x4, 0x3, 0x0},
    {0x5, 0x2, 0x7, 0x3, 0x0}, {0x5, 0x6, 0x7, 0x3, 0x0}, {0x6, 0x0, 0x7, 0x3, 0x0}, {0x6, 0x4, 0x7, 0x3, 0x0}, {0x6, 0x7, 0x7, 0x3, 0x0},
    {0x7, 0x3, 0x7, 0x3, 0x0}, {0x7, 0x7, 0x7, 0x3, 0x0}, {0x8, 0x2, 0x7, 0x3, 0x0}, {0x8, 0x6, 0x7, 0x3, 0x0}, {0x9, 0x2, 0x7, 0x3, 0x0},
    {0x9, 0x6, 0x7, 0x3, 0x0}, {0xa, 0x2, 0x7, 0x3, 0x0}, {0xa, 0x7, 0x7, 0x3, 0x0}, {0xb, 0x5, 0x7, 0x3, 0x0}, {0xc, 0x3, 0x7, 0x3, 0x0},
    {0xc, 0x7, 0x7, 0x3, 0x0}, {0xd, 0x7, 0x7, 0x3, 0x0}, {0xe, 0x7, 0x7, 0x3, 0x0}, {0xe, 0x6, 0x7, 0x4, 0x0}, {0xe, 0x7, 0x7, 0x4, 0x3},
    {0xe, 0x7, 0x7, 0x4, 0x3}, {0xe, 0x7, 0x7, 0x4, 0x3}, {0xe, 0x7, 0x7, 0x4, 0x3}, {0xe, 0x7, 0x7, 0x4, 0x3}, {0xe, 0x7, 0x7, 0x4, 0x3},
    {0xe, 0x7, 0x7, 0x4, 0x3},
};

// DK G-001
static const int8_t uwb_power_table_CH2[] = {
    -29, -28, -28, -27, -27, -26, -26, -25, -25, -24, -24, -23, -23, -22, -22, -22, -21, -20, -20, -19, -19, -19, -18, -18, -17, -17, -16, -16, -15, -15,
    -14, -14, -14, -13, -13, -12, -11, -11, -10, -10, -9,  -9,  -8,  -8,  -8,  -7,  -7,  -6,  -6,  -5,  -5,  -4,  -4,  -3,  -2,  -2,  -1,  -1,  0,   0,
};

static const int8_t uwb_power_table_CH5[] = {
    -25, -25, -25, -25, -25, -25, -25, -25, -24, -24, -23, -22, -22, -22, -21, -21, -20, -20, -19, -19, -18, -18, -17, -17, -17, -16, -15, -15, -14, -14,
    -13, -13, -12, -12, -12, -11, -10, -10, -9,  -9,  -8,  -8,  -7,  -7,  -6,  -6,  -5,  -5,  -4,  -3,  -3,  -2,  -2,  -2,  -1,  0,   0,   1,   1,   2,
};

static const int8_t uwb_power_table_CH9[] = {
    -27, -27, -27, -27, -27, -26, -26, -25, -25, -24, -23, -23, -22, -22, -22, -21, -21, -20, -20, -19, -19, -18, -18, -17, -17, -16, -16, -15, -15, -14,
    -14, -13, -13, -12, -12, -11, -10, -10, -9,  -9,  -8,  -8,  -7,  -6,  -6,  -5,  -5,  -4,  -4,  -3,  -3,  -2,  -2,  -2,  -1,  -1,  -1,  -1,  -1,  -1,
};

static const uint8_t (*power_table)[5] = power_table_CH9;

extern int16_t ranging_ant_delays_get(uint8_t ant_idx);
__WEAK int16_t ranging_ant_delays_get(uint8_t ant_idx)
{
    return 0;
}

extern void ranging_ant_delays_set(uint8_t ant_idx, int16_t delay_rstu);
__WEAK void ranging_ant_delays_set(uint8_t ant_idx, int16_t delay_rstu)
{
}

extern int pdoa_ant_delays_get(int16_t *delays, uint8_t rx_ant_num);
__WEAK int pdoa_ant_delays_get(int16_t *delays, uint8_t rx_ant_num)
{
    return 0;
}

extern int pdoa_ant_delays_set(int16_t *delays, uint8_t rx_ant_num);
__WEAK int pdoa_ant_delays_set(int16_t *delays, uint8_t rx_ant_num)
{
    return 0;
}

extern int pdoa_ant_space_set(int16_t ant_space);
__WEAK int pdoa_ant_space_set(int16_t ant_space)
{
    return 0;
}

extern int pdoa_gain_set(int16_t *gain, uint8_t rx_ant_num);
__WEAK int pdoa_gain_set(int16_t *gain, uint8_t rx_ant_num)
{
    return 0;
}

extern int pdoa_angle_offset_set(int16_t *angle_offset);
__WEAK int pdoa_angle_offset_set(int16_t *angle_offset)
{
    return 0;
}

extern void sts_lsp_store(void);
__WEAK void sts_lsp_store(void)
{
}

int uwb_open(void)
{
    reset_module(RESET_MODULE_DPRX);
    reset_module(RESET_MODULE_DPTX);

    aes_open(AES_ID0, NULL);

#ifdef UWB_AUDIO_EN
#ifdef CELL_PHONE_EN
    mac_init(IRQ_PRIORITY_HIGH, 6, 1, PHY_PAYLOAD_LEN_MAX);
#else
    mac_init(IRQ_PRIORITY_HIGH, 1, 6, PHY_PAYLOAD_LEN_MAX);
#endif
#else
    lsp_open(0, NULL);
    mac_init(IRQ_PRIORITY_HIGH, 3, 3, PHY_PAYLOAD_LEN_MAX);
#endif

#ifdef XIP_EN
    mac_ifs_set(PHY_LIFS_PERIOD_MACCLK_LONG, PHY_SIFS_PERIOD_MACCLK_LONG);
#endif

#if RSSI_EN
    mac_rssi_calculation_en(1);
#endif

    phy_init(IRQ_PRIORITY_NONE);

    phy_timer_open(1, IRQ_PRIORITY_HIGH);

    LOG_INFO(TRACE_MODULE_DRIVER, "uwb_open\r\n");
    return 0;
}

int uwb_close(void)
{
    power_off_radio();
#ifndef UWB_AUDIO_EN
    lsp_close();
#endif
    aes_close(AES_ID0);

    clock_disable(CLOCK_MAC);
    mac_timer_close();
    phy_timer_close();

    return 0;
}

void uwb_rx_antenna_open(enum UWB_RX_ANT_T antenna_idx)
{
    if (antenna_idx == UWB_RX_ANT_ALL)
    {
        // lna scan en
        REG_WRITE(0x40000628, 0x00);
    }
    else
    {
        // lna scan dis
        REG_WRITE(0x40000628, (1 << 6) | rx_ant_code[antenna_idx & 0x03]);
    }
}

void uwb_channel_switch(uint8_t ch_num)
{
    // CH_NUM
    uint32_t value = REG_READ(0x40000404) & (~0xFU);
    REG_WRITE(0x40000404, value | ch_num);

    // LO calibrate
    REG_WRITE(0x40006000, 0x80000400);

    while ((REG_READ(0x40006004) & 0x00000008) == 0)
    {
    }

    if (ch_num >= 10)
    {
#if HIGH_PERFORMANCE_MODE_EN
        REG_WRITE(0x40000600, 0x05000500); // rx  LNA BM/CTANK  port1  port0
        REG_WRITE(0x40000604, 0x05000500); // rx  LNA BM/CTANK  port3  port2
#else
        REG_WRITE(0x40000600, 0x04000400); // rx  LNA BM/CTANK  port1  port0
        REG_WRITE(0x40000604, 0x04000400); // rx  LNA BM/CTANK  port3  port2
#endif
    }
    else if (ch_num >= 5)
    {
#if HIGH_PERFORMANCE_MODE_EN
        REG_WRITE(0x40000600, 0x05030503); // rx  LNA BM/CTANK  port1  port0
        REG_WRITE(0x40000604, 0x05000500); // rx  LNA BM/CTANK  port3  port2
#else
        REG_WRITE(0x40000600, 0x04030403); // rx  LNA BM/CTANK  port1  port0
        REG_WRITE(0x40000604, 0x04000400); // rx  LNA BM/CTANK  port3  port2
#endif
    }
    else
    {
        // low band
#if HIGH_PERFORMANCE_MODE_EN
        REG_WRITE(0x40000600, 0x050f050f); // rx  LNA BM/CTANK  port1  port0
        REG_WRITE(0x40000604, 0x050f050f); // rx  LNA BM/CTANK  port3  port2
#else
        REG_WRITE(0x40000600, 0x040f040f); // rx  LNA BM/CTANK  port1  port0
        REG_WRITE(0x40000604, 0x040f040f); // rx  LNA BM/CTANK  port3  port2
#endif
    }

    if (ch_num == 12)
    {
        power_table = power_table_CH12;
    }
    else if (ch_num >= 9)
    {
        power_table = power_table_CH9;
    }
    else if (ch_num >= 5)
    {
        power_table = power_table_CH5;
    }
    else
    {
        // low band
        power_table = power_table_CH2;
    }
}

// 0x0 - Symmetrical Root-Raised-Cosine Pulse
// 0x1 - Precursor-Free Pulse (FiRa pulse)
// 0x2 - Precursor-Free Pulse
void uwb_pulse_shape_set(uint8_t pulse_shape)
{
    uint32_t value = REG_READ(0x40000404) & ~(0x3U << 16);
    if (pulse_shape == 0)
    {
        REG_WRITE(0x40000404, value | (0x01 << 16));
    }
    else
    {
        REG_WRITE(0x40000404, value);
    }
}

// 0x0 - 2ns(500M), 0x3 - 0.92ns(900M), 0x2 - 0.75ns(1.3G)
void uwb_pulse_width_set(uint8_t width)
{
    uint32_t val = REG_READ(0x40000404);
    val = val & ~(0x3U << 16);
    val |= (uint32_t)((width & 0x3) << 16);
    REG_WRITE(0x40000404, val);
}

void uwb_tx_power_set(uint8_t tx_power_level)
{
    uint32_t value;
    // PA
    value = REG_READ(0x400060c4) & ~((0x7fU << 7) | (0x7fU << 15) | (1U << 22));
    uint32_t pa_code = ((uint32_t)(power_table[tx_power_level][0] & 0xF) << 3) | (power_table[tx_power_level][1] & 0x7);
    REG_WRITE(0x400060c4, value | (pa_code << 15) | (1U << 14) | (pa_code << 7) | (1U << 6));

    // vgate
    // REG_WRITE(0x400060c8, 0xa013277d);

    value = REG_READ(0x400060c8) & ~((0x7U << 3) | (1U << 2));
    uint32_t vgate_code = power_table[tx_power_level][2] & 0x7;
    REG_WRITE(0x400060c8, value | (vgate_code << 3) | (1U << 2));

    // TXDAC config
    value = REG_READ(0x40000610) & ~(0x1fU);
    uint32_t txdac_cfg = ((power_table[tx_power_level][3] & 0x7U) << 2) | (power_table[tx_power_level][4] & 0x3);
    REG_WRITE(0x40000610, value | (txdac_cfg));

    // dac dc offset
    REG_WRITE(0x4000602c, 0x000aa001);
    REG_WRITE(0x4000602c, 0x000aa000);
    delay_us(100);
}

int8_t uwb_tx_power_get(uint8_t ch_num, uint8_t tx_power_level)
{
    int8_t uwb_power;
    if (ch_num >= 9)
    {
        uwb_power = uwb_power_table_CH9[tx_power_level];
    }
    else if (ch_num >= 5)
    {
        uwb_power = uwb_power_table_CH5[tx_power_level];
    }
    else
    {
        uwb_power = uwb_power_table_CH2[tx_power_level];
    }
    return uwb_power;
}

static void uwb_tx_init(uint8_t tx_power_level)
{
    // TX tank
    REG_WRITE(0x400060a0, 0x003ea812);
    REG_WRITE(0x400060a4, 0x5a9bd79d);
    REG_WRITE(0x400060a8, 0x49bd79d5);
    REG_WRITE(0x400060ac, 0x00000015);

    uint32_t value = REG_READ(0x40000234);
    REG_WRITE(0x40000234, value | 0x40);

    value = REG_READ(0x40000614) & ~(0x7U << 12);
    REG_WRITE(0x40000614, value | (2 << 12));

    uwb_tx_power_set(tx_power_level);

    LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_DRIVER, "uwb_tx_init, power level %d\r\n", tx_power_level);
}

static void uwb_rx_init(void)
{
    REG_WRITE(0x40006054, 0x00820820);
    REG_WRITE(0x40006058, 0x00820820);
    REG_WRITE(0x4000605c, 0x00820820);
    REG_WRITE(0x40006060, 0x00820820);
    REG_WRITE(0x40006050, 0x000FD502);
    REG_WRITE(0x40006090, 0x00550202);
    REG_WRITE(0x40006098, 0x00540001);
    REG_WRITE(0x40006098, 0x00540000);
    delay_us(100);
    REG_WRITE(0x40006090, 0x00540001);
    REG_WRITE(0x40006090, 0x00540000);
    delay_us(100);
    // Bypass LPF_DAC1 calibration
    REG_WRITE(0x40006090, 0x00540004);

#if 0
    // fix agc gain
    uint32_t filter_gain = 25;
    uint32_t lna_gain = 5;
    uint32_t gain = (1U << 31) | ((filter_gain & 0x1f) << 16) | (lna_gain & 0x7);
    REG_WRITE(0x40000700, gain);
#endif

    LOG_INFO(TRACE_NO_REPORT_HOST | TRACE_MODULE_DRIVER, "uwb_rx_init\r\n");
}

void uwb_calibration_params_set(uint8_t ch_num)
{
    LOG_INFO(TRACE_MODULE_DRIVER, "Vdd core %u Loopback time %u\r\n", board_param.vdd_core_vol, board_param.loopback_time);

    uint8_t ch_idx = CALIB_CH(ch_num);
    for (uint8_t i = 0; i < 4; i++)
    {
        ranging_ant_delays_set(i, board_param.ant_delays[ch_idx][i] + board_param.loopback_time);
    }
    LOG_INFO(TRACE_MODULE_DRIVER, "ToF ant delays %d %d %d %d\r\n", board_param.ant_delays[ch_idx][0], board_param.ant_delays[ch_idx][1],
             board_param.ant_delays[ch_idx][2], board_param.ant_delays[ch_idx][3]);

    pdoa_ant_delays_set(&board_param.pdoa_delays[ch_idx][0], 4);
    LOG_INFO(TRACE_MODULE_DRIVER, "PDoA delays %d %d %d %d\r\n", board_param.pdoa_delays[ch_idx][0], board_param.pdoa_delays[ch_idx][1],
             board_param.pdoa_delays[ch_idx][2], board_param.pdoa_delays[ch_idx][3]);

    pdoa_gain_set(&board_param.pdoa_gains[ch_idx][0], 4);
    LOG_INFO(TRACE_MODULE_DRIVER, "PDoA gains %d %d %d %d\r\n", board_param.pdoa_gains[ch_idx][0], board_param.pdoa_gains[ch_idx][1],
             board_param.pdoa_gains[ch_idx][2], board_param.pdoa_gains[ch_idx][3]);

    pdoa_ant_space_set(board_param.pdoa_ant_space);
    LOG_INFO(TRACE_MODULE_DRIVER, "PDoA ant space %d\r\n", board_param.pdoa_ant_space);

    pdoa_angle_offset_set(board_param.pdoa_offsets);
    LOG_INFO(TRACE_MODULE_DRIVER, "PDoA angle offsets %d %d\r\n", board_param.pdoa_offsets[0], board_param.pdoa_offsets[1]);
}

void uwb_configure(uint8_t mode, uint8_t tx_power_level, struct UWB_CONFIG_T *ppdu_params)
{
    // RF config
    uwb_channel_switch(ppdu_params->ch_num);
    // Pulse shape
    uwb_pulse_shape_set(ppdu_params->pulse_shape);
    // Antenna
    uwb_rx_antenna_open((enum UWB_RX_ANT_T)ppdu_params->rx_main_ant);
    LOG_INFO(TRACE_MODULE_DRIVER, "Open ANT%d Pulseshape %d\r\n", ppdu_params->rx_main_ant & 0x03, ppdu_params->pulse_shape);

    // Configure PHY parameters
    phy_params_configure(mode, HIGH_PERFORMANCE_MODE_EN, ppdu_params);

    // PHY config
    if (mode & PHY_TX)
    {
        uwb_tx_init(tx_power_level);
        phy_tx_params_set((void *)ppdu_params->phy_params_container);
    }

    if (mode & PHY_RX)
    {
        uwb_rx_init();
        phy_rx_params_set((void *)ppdu_params->phy_params_container);
    }

    // MAC config
    mac_crc_mode_configure(ppdu_params->fcs_type);
}

/*
scheduled_mode
0: Immediately
1: Defer to TX
2: CSMA-CA TX
*/
int uwb_tx(uint8_t *pkt_data, uint16_t pkt_len, uint8_t scheduled_mode, uint32_t target_time)
{
    int ret = 0;

    if (scheduled_mode == TX_MODE_CSMACA)
    {
        // Power on radio
        power_on_radio(1, 1);
        ret = mac_tx(EVT_MODE_MAC_FIX_PHY_ASAP, target_time, 0, pkt_data, pkt_len);
    }
    else if (scheduled_mode == TX_MODE_DEFER)
    {
        power_on_radio(1, 0);
        ret = mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, 0, pkt_data, pkt_len);
    }
    else
    {
        power_on_radio(1, 0);
        ret = mac_tx(EVT_MODE_MAC_PHY_ASAP, 0, 0, pkt_data, pkt_len);
    }
    mac_start();

    // LOG_INFO(TRACE_MODULE_DRIVER, "UWB TX %d\r\n", pkt_len);
    return ret;
}

int uwb_rx(uint8_t scheduled_mode, uint32_t target_time, uint32_t timeout)
{
    int ret = 0;

    sts_lsp_store();

    // Power on radio
    power_on_radio(0, 1);

    uint32_t lock = int_lock();
    if (scheduled_mode == RX_MODE_DEFER)
    {
        ret = mac_rx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, timeout);
    }
    else
    {
        ret = mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, timeout);
    }
    mac_start();
    int_unlock(lock);

    // LOG_INFO(TRACE_MODULE_DRIVER, "UWB RX %d\r\n", timeout_us);
    return ret;
}

/*
scheduled_mode
0: Immediately
1: Defer to TX
*/
int uwb_loopback(uint8_t *pkt_data, uint16_t pkt_len, uint8_t scheduled_mode, uint32_t target_time)
{
    int ret = 0;

    // Enable loopback function
    mac_loopback_mode_set(1);

#if 0
    // Disable RX AGC, and use register to configure the gain of LNA and filter;
    uint8_t rf_gain = 3;
    uint8_t bb_gain = 0;
    uint32_t val = (1U << 31) | ((bb_gain & 0x1FU) << 16) | (rf_gain & 0x7U);
    REG_WRITE(0x40000700, val);

    val = REG_READ(0x40003040);
    // Disable enAGC
    val &= 0xFFFFFFFE;
    REG_WRITE(0x40003040, val);
#endif

    // Power on radio
    power_on_radio(1, 1);

    uint32_t lock = int_lock();

    uint32_t target = REG_READ(0x40000418) + US_TO_PHY_TIMER_COUNT(100);
    // Rx Timeout
    REG_WRITE(0x40000408, target + US_TO_PHY_TIMER_COUNT(500));

    if (scheduled_mode == TX_MODE_DEFER)
    {
        ret = mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target_time, 0, pkt_data, pkt_len);
    }
    else
    {
        ret = mac_tx(EVT_MODE_MAC_PHY_ASAP, 0, 0, pkt_data, pkt_len);
    }
    mac_start();

    int_unlock(lock);

    // LOG_INFO(TRACE_MODULE_DRIVER, "UWB Loopback %d\r\n", pkt_len);
    return ret;
}

// cannot work at scheduled mode
void uwb_rx_force_off(bool int_rpt_dis)
{
    while ((REG_READ(0x40000400) & 0x0f) == 6)
    {
    }

    uint32_t lock = int_lock();
    if ((REG_READ(0x40000400) & 0x0f) == 7)
    {
        uint32_t ts = phy_timer_count_get() + 250; /* 2us timeout */
        REG_WRITE(0x40000408, ts);                 /* cfg the rx timeout */
        if (int_rpt_dis)
        {
            mac_current_rx_report_discard();
        }
    }
    int_unlock(lock);
}

void uwb_tx_carrier_only(uint8_t on, uint8_t ch_num, uint8_t tx_power_level)
{
    static uint32_t pwr_reg_value = 0;

    if (on)
    {
        power_on_radio(1, 0);

        // start
        uwb_tx_init(tx_power_level);
        uwb_channel_switch(ch_num);

        // Enable datapath TX clock
        SYSCON->SYS_CMU |= (1U << CLOCK_TX);

        pwr_reg_value = REG_READ(0x4000002C);
        REG_WRITE(0x4000002C, 0x42700000);
        REG_WRITE(0x40000704, 0x80000011);
    }
    else
    {
        // stop
        REG_WRITE(0x40000704, 0x0);
        if (pwr_reg_value != 0)
        {
            REG_WRITE(0x4000002C, pwr_reg_value);
        }

        // Disable datapath TX clock
        SYSCON->SYS_CMU &= ~(1U << CLOCK_TX);

        power_off_radio();
    }
}

static uint8_t flag_uwb_sw_trx_start = 0;

void uwb_blocking_tx_start(const uint8_t *pkt_data, uint16_t pkt_len, uint32_t timeout_ms)
{
    flag_uwb_sw_trx_start = 1;
    LOG_INFO(TRACE_MODULE_DRIVER, "uwb_blocking_tx_start\r\n");

    // Disable MAC interrupt
    NVIC_DisableIRQ(MAC_IRQn);

    uwb_rx(RX_MODE_IMMEDIATE, 0, MS_TO_PHY_TIMER_COUNT(10));
    delay_us(20);

    // MAC_MODE_CONTROL
    REG_WRITE(0x5000A004, 0x00000009);
    REG_WRITE(0x5000A004, 0x0000000D);

    phy_tx_payload(pkt_data, pkt_len);

    // MAC_SOFT_MODE_CFG
    uint32_t value = REG_READ(0x5000A12C);
    uint16_t tx_buf_start = (TX_PAYLOAD_ADDR - 0x02080000) >> 2;
    REG_WRITE(0x5000A12C, (value & 0xffe00000) | (uint32_t)((pkt_len) << 11) | tx_buf_start);

    // SM 0 - immediate TRX, 1 - deferred TXR
    REG_READ(0x40000434);

    // TRIGGER_TIMING
    REG_READ(0x40000424);

    // mask tx int
    // value = REG_READ(0x5000A020) | (1 << 21) | (0x1f << 15);
    value = ~(4U << 15);
    REG_WRITE(0x5000A020, value);

    // Enable TX
    // MAC_SOFT_MODE_TRIG
    REG_WRITE(0x5000A130, 0x1);

    uint32_t elapsed_time = 0;
    uint32_t count = 0;
    uint32_t start = sys_timer_get();

    while (flag_uwb_sw_trx_start)
    {
        while (sys_timer_get() - start < 624) // 10us
        {
        }

        // Trigger TX
        REG_WRITE(0x40000430, 0x1);
        start = sys_timer_get();

        if (timeout_ms)
        {
            count++;
            if (count >= 100)
            {
                count = 0;
                elapsed_time++;
                if (elapsed_time >= timeout_ms)
                {
                    flag_uwb_sw_trx_start = 0;
                }
            }
        }
    }

#if 0
    // wait TX done
    while ((REG_READ(0x5000A024) & (1 << 17)) == 0)
    {
        delay_us(100);
    }

    // Clear done flag
    REG_WRITE(0x5000A028, 0xFFFFFFFF);
#endif

    LOG_INFO(TRACE_MODULE_DRIVER, "uwb_blocking_tx_start exit\r\n");

    power_off_radio();
    mac_restart();
}

void uwb_blocking_rx_start(uint32_t timeout_ms, drv_callback_t callback)
{
    flag_uwb_sw_trx_start = 1;
    LOG_INFO(TRACE_MODULE_DRIVER, "uwb_blocking_rx_start\r\n");

    // Disable MAC interrupt
    NVIC_DisableIRQ(MAC_IRQn);

    power_on_radio(0, 1);

    // MAC_MODE_CONTROL - clear MAC (bug in software mode)
    REG_WRITE(0x5000A004, 0x00000009);
    REG_WRITE(0x5000A004, 0x0000000D);

    // MAC_SOFT_MODE_CFG
    uint32_t value = REG_READ(0x5000A12C);
    uint16_t rx_buf_start = (RX_PAYLOAD_ADDR - 0x02080000) >> 2;
    value = value & ~(0x7ffU << 21);
    REG_WRITE(0x5000A12C, value | (uint32_t)(rx_buf_start << 21));

    // SM 0 - immediate TRX, 1 - deferred TXR
    REG_READ(0x40000434);

    // TRIGGER_TIMING
    REG_READ(0x40000424);

    // LOG_INFO(TRACE_MODULE_DRIVER, "MAC S %x\r\n", REG_READ(0x5000A024));

    // mask rx int
    // value = REG_READ(0x5000A020) | (1 << 21) | (0x1f << 15);
    value = ~(2U << 15);
    REG_WRITE(0x5000A020, value);

    // Enable RX
    // MAC_SOFT_MODE_TRIG
    REG_WRITE(0x5000A130, 0x2);

    uint32_t elapsed_time = 0;
    uint32_t start;

    while (flag_uwb_sw_trx_start)
    {
        start = sys_timer_get();
        // RX_TIMEOUT_CNT - 50ms
        value = REG_READ(0x40000418);
        REG_WRITE(0x40000408, value + MS_TO_PHY_TIMER_COUNT(50));

        // Trigger RX
        REG_WRITE(0x40000430, 0x2);

        // wait RX done
        // wait MAC RX done
        // while ((REG_READ(0x5000A024) & (1 << 16)) == 0)
        // wait PHY RX done
        while ((REG_READ(0x40002038) & (1 << 26)) == 0)
        {
            delay_us(10);
            if (sys_timer_get() - start > __MS_TO_TICKS(51))
            {
                LOG_INFO(TRACE_MODULE_DRIVER, "RX-Done wasn't set\r\n");
                break;
            }
        }

        // Clear done flag
        REG_WRITE(0x5000A028, 0xFFFFFFFF);

        if (callback)
        {
            // TODO: report RX packet
        }

        if (timeout_ms)
        {
            elapsed_time += __TICKS_TO_MS(sys_timer_get() - start);
            if (elapsed_time >= timeout_ms)
            {
                flag_uwb_sw_trx_start = 0;
            }
        }
    }

    LOG_INFO(TRACE_MODULE_DRIVER, "uwb_blocking_rx_start exit\r\n");

    power_off_radio();
    mac_restart();
}

void uwb_blocking_trx_stop(void)
{
    flag_uwb_sw_trx_start = 0;
    LOG_INFO(TRACE_MODULE_DRIVER, "uwb_blocking_trx_stop\r\n");
}

#if RANGING_EN
static volatile uint16_t loopback_num = 300;
static volatile uint16_t loopback_count = 0;

static volatile uint16_t loopback_rx_timestamp_frac = 0;
static volatile int16_t loopback_time_min = 32767;
static volatile int16_t loopback_time_max = -32768;
static volatile int32_t loopback_time_sum = 0;
static volatile uint8_t loopback_cal_done = 0;

static void uwb_loopback_tx_process(struct MAC_HW_REPORT_T *tx_report)
{
    int64_t tx_timestamp = 0;
    uint16_t tx_timestamp_frac = 0;
    // uint32_t tx_timestamp_int = 0;
    int16_t loopback_time = 0;

    if ((tx_report->err_code == UWB_TX_OK) && (loopback_rx_timestamp_frac != 0x8000))
    {
        tx_timestamp = ranging_tx_time(tx_report->timestamp);
        tx_timestamp_frac = tx_timestamp & 0x1ff;
        // tx_timestamp_int = (uint32_t)(tx_timestamp >> 9);

        loopback_time = (int16_t)(loopback_rx_timestamp_frac - tx_timestamp_frac);

        if (loopback_time > loopback_time_max)
        {
            loopback_time_max = loopback_time;
        }
        else if (loopback_time < loopback_time_min)
        {
            loopback_time_min = loopback_time;
        }

        loopback_count++;
        if (loopback_count >= loopback_num)
        {
            // Check later
            loopback_cal_done = 1;
        }
        loopback_time_sum += loopback_time;
        // LOG_INFO(TRACE_MODULE_APP, "Loopback time $%d;\r\n", loopback_time);

        // uint32_t val = REG_READ(0x40003050);
        // uint8_t rf_gain = (val & 0x07);
        // uint8_t bb_gain = ((val >> 3) & 0x1f);
        // LOG_INFO(TRACE_MODULE_APP, "rf_gain %u bb_gain %u\r\n", rf_gain, bb_gain);
    }
}

static void uwb_loopback_rx_process(struct MAC_HW_REPORT_T *rx_report)
{
    power_off_radio();
    int64_t rx_timestamp = 0;
    // uint32_t rx_timestamp_int = 0;

    if (rx_report->err_code == UWB_RX_OK)
    {
        rx_timestamp = ranging_rx_time(rx_report);
        loopback_rx_timestamp_frac = rx_timestamp & 0x1ff;
        // rx_timestamp_int = (uint32_t)(rx_timestamp >> 9);
    }
    else
    {
        // Loopback error
        loopback_rx_timestamp_frac = 0x8000;
    }
}

void uwb_loopback_time_update(uint16_t count, struct UWB_CONFIG_T *ppdu_params)
{
    uint8_t tx_payload[20];
    uint16_t tx_len = 20;

    for (uint8_t i = 0; i < tx_len; i++)
    {
        tx_payload[i] = 0x55;
    }

    // high Tx power will cause saturation
    uwb_configure(PHY_TX | PHY_RX, 10, ppdu_params);
    // init ranging lib for timestamp calculation
    ranging_lib_init(ppdu_params->sts_pkt_cfg, CE_AUX_CH_PWR_NLOS_FOM);

    mac_callback_t bak_tx_cb;
    mac_callback_t bak_rx_cb;

    uint32_t lock = int_lock();
    mac_get_process_handler(&bak_tx_cb, &bak_rx_cb);
    mac_register_process_handler(uwb_loopback_tx_process, uwb_loopback_rx_process);
    int_unlock(lock);

    loopback_num = count;
    loopback_count = 0;

    loopback_cal_done = 0;
    loopback_time_sum = 0;
    loopback_time_min = 32767;
    loopback_time_max = -32768;

    do
    {
        if (ppdu_params->sts_pkt_cfg == STS_PKT_CFG_3)
        {
            uwb_loopback(NULL, 0, 0, 0);
        }
        else
        {
            uwb_loopback(tx_payload, tx_len, 0, 0);
        }

        delay_us(1800);

        // Check the MAC busy state
        while (mac_is_busy())
        {
        }

    } while (loopback_cal_done == 0);

    board_param.loopback_time = (int16_t)(loopback_time_sum / loopback_num);

    LOG_INFO(TRACE_MODULE_APP, "Loopback time [%d, %d] %d \r\n", loopback_time_min, loopback_time_max, board_param.loopback_time);

    // Restore MAC callback
    mac_register_process_handler(bak_tx_cb, bak_rx_cb);
}

void uwb_loopback_calibration(uint8_t wr_nvm_en, uint16_t count, struct UWB_CONFIG_T *ppdu_params)
{
    uint8_t tx_payload[20];
    uint16_t tx_len = 20;

    for (uint8_t i = 0; i < tx_len; i++)
    {
        tx_payload[i] = 0x55;
    }

    // high Tx power will cause saturation
    uwb_configure(PHY_TX | PHY_RX, 10, ppdu_params);
    // init ranging lib for timestamp calculation
    ranging_lib_init(ppdu_params->sts_pkt_cfg, CE_AUX_CH_PWR_NLOS_FOM);

    mac_callback_t bak_tx_cb;
    mac_callback_t bak_rx_cb;

    uint32_t lock = int_lock();
    mac_get_process_handler(&bak_tx_cb, &bak_rx_cb);
    mac_register_process_handler(uwb_loopback_tx_process, uwb_loopback_rx_process);
    int_unlock(lock);

    uint16_t loopback_time_range[3] = {0xffff, 0xffff, 0xffff};
    int16_t loopback_time_avg[3];
    loopback_num = count;

    for (uint8_t j = 0; j < 3; j++)
    {
        // VDD_core voltage ranging [1, 2, 3]
        board_param.vdd_core_vol = j + 1;
        loopback_cal_done = 0;
        loopback_count = 0;
        loopback_time_sum = 0;
        loopback_time_min = 32767;
        loopback_time_max = -32768;

        do
        {
            if (ppdu_params->sts_pkt_cfg == STS_PKT_CFG_3)
            {
                uwb_loopback(NULL, 0, 0, 0);
            }
            else
            {
                uwb_loopback(tx_payload, tx_len, 0, 0);
            }

            delay_us(1800);

            // Check the MAC busy state
            while (mac_is_busy())
            {
            }

        } while (loopback_cal_done == 0);

        loopback_time_range[j] = (uint16_t)(loopback_time_max - loopback_time_min);
        loopback_time_avg[j] = (int16_t)(loopback_time_sum / loopback_num);
#if 0
        // Set a condition to speed up calibration
        if (loopback_time_range[j] < 30)
        {
            break;
        }
#endif
    }

    uint8_t idx = 0;
    if (loopback_time_range[2] > loopback_time_range[1])
    {
        if (loopback_time_range[0] > loopback_time_range[1])
        {
            // 2 > 0 > 1
            idx = 1;
        }
        else
        {
            // 2 > 1 >= 0
            idx = 0;
        }
    }
    else
    {
        if (loopback_time_range[2] > loopback_time_range[0])
        {
            // 1 >= 2 > 0
            idx = 0;
        }
        else
        {
            // 1 >= 0 >= 2
            idx = 2;
        }
    }
    board_param.vdd_core_vol = idx + 1;
    board_param.loopback_time = loopback_time_avg[idx];

    LOG_INFO(TRACE_MODULE_APP, "Loopback %u %u %u [%d]%d\r\n", loopback_time_range[0], loopback_time_range[1], loopback_time_range[2], board_param.vdd_core_vol,
             board_param.loopback_time);

    // Restore MAC callback
    mac_register_process_handler(bak_tx_cb, bak_rx_cb);
    // Write NVM
    if (wr_nvm_en)
    {
        board_calibration_param_write(BOARD_VDD_CORE_VOL, &board_param.vdd_core_vol, sizeof(board_param.vdd_core_vol));
        board_calibration_param_write(BOARD_LOOPBACK_TIME, (uint8_t *)&board_param.loopback_time, sizeof(board_param.loopback_time));
    }
}
#else
void uwb_loopback_calibration(uint8_t wr_nvm_en, uint16_t count, struct UWB_CONFIG_T *ppdu_params)
{
    UNUSED(wr_nvm_en);
    UNUSED(count);
    UNUSED(ppdu_params);
}
#endif