安邦手环GPS删除部分无用数据和修改4G波特率9600出厂测试固件

chen

2024-11-08 cc432b761c884a0bd8e9d83db0a4e26109fc08b1

 keil/include/drivers/mk_trace.c

对比新文件
@@ -0,0 +1,1857 @@
/*
 * 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_reset.h"
#include "mk_io.h"
#include "mk_misc.h"

#include "stdio.h"
#include "string.h"
#include "stdlib.h"

#if TRACE_EN

#ifndef BACKTRACE_EN
#define BACKTRACE_EN (1)
#endif

#ifndef TRACE_BUF_SIZE
#define TRACE_BUF_SIZE 2048
#endif

#if UART_CONSOLE_EN
/*
|-------|---------|----------| ---------- | -------------------------------------- |
|       | Header  | Preamble | 1B         | CMD = 0xF1, ACK = 0xF2                 |
|       |         | -------- | ---------- | -------------------------------------- |
|       |         | Length   | 2B         | The length of the payload              |
|       | ------- | -------- | ---------- | -------------------------------------- |
| Frame |         | ID       | 2B         | Payload - Identification of the frame  |
|       |         | -------- | ---------- | -------------------------------------- |
|       | Payload | Data     | Length - 4 | Payload - Data of the frame            |
|       |         | -------- | ---------- | -------------------------------------- |
|       |         | CRC      | 2B         | Payload - CRC16 of the whole frame     |
| ----- | ------- | -------- | ---------- | -------------------------------------- |
*/
#include "crc.h"
#include "mk_uwb.h"
#include "uwb_api.h"
#define CONSOLE_BUF_SIZE 1024

#define CONSOLE_RX_HEADER 0
#define CONSOLE_RX_PAYLOAD 1

#define CONSOLE_CMD 0xF1

#define POS_PREAMBLE 0
#define POS_LENGTH 1
#define POS_ID 3
#define POS_DATA 5

#define LEN_HEADER 3
#define LEN_ID 2
#define LEN_CRC 2

enum ID_TYPE_T
{
    // F1 04 00 - 00 00 F6 5F
    ID_RANGING_STOP = 0x0000,
    // F1 04 00 - 01 00 C7 6C
    ID_RANGING_START = 0x0001,
    // F1 07 00 - 02 00 28 00 08 F6 F9
    ID_PHY_PARAMS_SET = 0x0002,

    ID_TYPE_MAX,
};

static void console_receive_callback(void *dev, uint32_t err_code);

typedef uint8_t (*console_cmd_function_t)(void const *param, uint16_t const len);

#ifndef UWB_UCI_TEST_EN
extern void app_ranging_report_callback(void *report);
#endif

// Element of a cmd handler table.
struct console_cmd_handler
{
    // ID of the cmd.
    uint16_t id;
    // Pointer to the handler function for the cmd id above.
    console_cmd_function_t function;
};

static uint8_t console_ranging_stop(void const *param, uint16_t const len)
{
    uwbapi_session_stop(uwb_app_config.session_id);
    return 1;
}

static uint8_t console_ranging_start(void const *param, uint16_t const len)
{
#ifndef UWB_UCI_TEST_EN
    uwbapi_session_start(uwb_app_config.session_id, app_ranging_report_callback);
#else
    uwbapi_session_start(uwb_app_config.session_id, NULL);
#endif
    return 1;
}

static uint8_t console_phy_params_set(void const *param, uint16_t const len)
{
    uint8_t ret = 0;
    if (len == sizeof(struct PHY_USER_CONFIG_T))
    {
        phy_loop_params_configure((const struct PHY_USER_CONFIG_T *)param, (uint8_t)len, 1);
        ret = 1;
    }
    else
    {
        LOG_ERROR(TRACE_MODULE_DRIVER, "PHY params length is wrong\r\n");
    }

    return ret;
}

static const struct console_cmd_handler console_cmd_handler_list[ID_TYPE_MAX] = {
    {ID_RANGING_STOP, (console_cmd_function_t)console_ranging_stop},
    {ID_RANGING_START, (console_cmd_function_t)console_ranging_start},
    {ID_PHY_PARAMS_SET, (console_cmd_function_t)console_phy_params_set},
};
#endif

extern uint32_t __INITIAL_SP;
#if BACKTRACE_EN
void HardFault_Handler(void) __attribute__((naked));
#else
__NO_RETURN void HardFault_Handler(void);
#endif
__NO_RETURN void trace_exception_handler(void *regs);
__NO_RETURN void trace_end(void);

#ifndef TRACE_LVL_CONFIG_0
#define TRACE_LVL_CONFIG_0 (0x44444444)
#endif

#ifndef TRACE_LVL_CONFIG_1
#define TRACE_LVL_CONFIG_1 (0x00044444)
#endif

const static uint8_t trace_level_cfg[TRACE_MODULE_NUM] = {
    // MAC
    (TRACE_LVL_CONFIG_0 >> (4 * TRACE_MODULE_MAC)) & 0xf,
    // PHY
    (TRACE_LVL_CONFIG_0 >> (4 * TRACE_MODULE_PHY)) & 0xf,
    // DRIVER
    (TRACE_LVL_CONFIG_0 >> (4 * TRACE_MODULE_DRIVER)) & 0xf,
    // APP
    (TRACE_LVL_CONFIG_0 >> (4 * TRACE_MODULE_APP)) & 0xf,
    // UWB
    (TRACE_LVL_CONFIG_0 >> (4 * TRACE_MODULE_UWB)) & 0xf,
    // UCI
    (TRACE_LVL_CONFIG_0 >> (4 * TRACE_MODULE_UCI)) & 0xf,
    // TEST
    (TRACE_LVL_CONFIG_0 >> (4 * TRACE_MODULE_TEST)) & 0xf,
    // BOOT
    (TRACE_LVL_CONFIG_0 >> (4 * TRACE_MODULE_BOOT)) & 0xf,
    // OS
    (TRACE_LVL_CONFIG_1 >> (4 * (TRACE_MODULE_OS - 8))) & 0xf,
    // FIRA
    (TRACE_LVL_CONFIG_1 >> (4 * (TRACE_MODULE_FIRA - 8))) & 0xf,
    // CCC
    (TRACE_LVL_CONFIG_1 >> (4 * (TRACE_MODULE_CCC - 8))) & 0xf,
    // SE
    (TRACE_LVL_CONFIG_1 >> (4 * (TRACE_MODULE_SE - 8))) & 0xf,
    // SCP03
    (TRACE_LVL_CONFIG_1 >> (4 * (TRACE_MODULE_SCP03 - 8))) & 0xf,
};

const static char *module_name[TRACE_MODULE_NUM] = {"[MAC]",  "[PHY]", "[DRV]",  "[APP]", "[UWB]", "[UCI]",  "[TEST]",
        "[BOOT]", "[OS]",  "[FIRA]", "[CCC]", "[SE]",  "[SCP03]"
                                                   };

const static char *level_tag[TRACE_LEVEL_NUM] = {
    "[ERROR]",
    "[WARN]",
    "[INFO]",
    "[VERBOSE]",
};

struct TRACE_HANDLE_T
{
    enum TRACE_PORT_T port;
    char tx_buf[TRACE_BUF_SIZE];
#if UART_CONSOLE_EN
    uint8_t rx_buf[CONSOLE_BUF_SIZE];
#endif
    uint32_t guard;
    uint16_t wptr;
    uint16_t rptr;
    uint16_t send;
    uint8_t busy;
    uint8_t sending;
    uint8_t enabled;
#if UART_CONSOLE_EN
    uint8_t console_fsm;
    uint8_t reserved[2];
#else
    uint8_t reserved[3];
#endif
};

struct TRACE_ASSERT_INF_T
{
    const char *FILE;
    const char *FUNC;
    uint32_t LINE;
    uint32_t SP;
    uint32_t LR;
    uint32_t xPSR;
    uint32_t PRIMASK;
    uint32_t CONTROL;
    uint32_t MSP;
    uint32_t PSP;
    uint32_t ID;
};

struct TRACE_EXCEPTION_INF_T
{
    uint32_t REGS[13];
    uint32_t SP;
    uint32_t LR;
    uint32_t PC;
    uint32_t xPSR;
    uint32_t PRIMASK;
    uint32_t CONTROL;
    uint32_t ICSR;
    uint32_t AIRCR;
    uint32_t SCR;
    uint32_t CCR;
    uint32_t MSP;
    uint32_t PSP;
    uint32_t EXC_RETURN;
    uint32_t ID;
};

static struct TRACE_HANDLE_T trace_handle;
static TRACE_CRASH_DUMP_CB_T trace_crash_dump_cb_list[1];

static void trace_sending(void);

int trace_open(enum TRACE_PORT_T port, enum UART_BAUD_T baud_rate)
{
    int ret = 0;
    if (port >= TRACE_PORT_NUM)
    {
        return -1;
    }

    trace_handle.guard = 0xc0ffc0ff;
    trace_handle.wptr = 0;
    trace_handle.rptr = 0;
    trace_handle.busy = false;
    trace_handle.sending = false;
    trace_handle.send = 0;
    trace_handle.port = port;
    trace_handle.enabled = 1;

    if ((trace_handle.port == TRACE_PORT_UART0) || (trace_handle.port == TRACE_PORT_UART1))
    {
        enum UART_DEV_T trace_uart = (trace_handle.port == TRACE_PORT_UART0) ? UART_ID0 : UART_ID1;
        struct UART_CFG_T trace_uart_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_rate,
#ifdef TRACE_BLOCKING
                  .dma_en = false,
#else
                  .dma_en = true,
#endif
                   .int_rx = false,
                    .int_tx = false
        };

        ret = uart_open(trace_uart, &trace_uart_cfg);

#if UART_CONSOLE_EN
        // 1. Header
        trace_handle.console_fsm = CONSOLE_RX_HEADER;
        uart_receive(trace_uart, &trace_handle.rx_buf[0], LEN_HEADER, console_receive_callback);
#endif
    }
    else if (trace_handle.port == TRACE_PORT_SPI0)
    {
    }

    return ret;
}

int trace_close(void)
{
    int ret = 0;
    trace_handle.enabled = 0;
    if ((trace_handle.port == TRACE_PORT_UART0) || (trace_handle.port == TRACE_PORT_UART1))
    {
        enum UART_DEV_T trace_uart = (trace_handle.port == TRACE_PORT_UART0) ? UART_ID0 : UART_ID1;
        ret = uart_close(trace_uart);
    }
    else if (trace_handle.port == TRACE_PORT_SPI0)
    {
    }
    return ret;
}

int trace_crash_dump_cb_register(TRACE_CRASH_DUMP_CB_T cb)
{
    for (uint32_t i = 0; i < ARRAY_SIZE(trace_crash_dump_cb_list); i++)
    {
        if (trace_crash_dump_cb_list[i] == NULL)
        {
            trace_crash_dump_cb_list[i] = cb;
            return 0;
        }
    }

    return -1;
}

static void trace_sending_continue(void *dev, uint32_t err_code)
{
    uint32_t lock = int_lock();
    if (err_code & (UART_ERR_MODEM | UART_ERR_LINE))
    {
        // uart error
        LOG_ERROR(TRACE_MODULE_DRIVER, "UART Error %x\r\n", err_code);
    }
    else
    {
        trace_handle.rptr += trace_handle.send;
        if (trace_handle.rptr >= TRACE_BUF_SIZE)
        {
            trace_handle.rptr -= TRACE_BUF_SIZE;
        }

        // sent done
        trace_handle.sending = false;

        // continue to send rest
        trace_sending();
    }

    int_unlock(lock);
}

static void trace_sending(void)
{
    uint32_t lock = int_lock();

    if (trace_handle.wptr != trace_handle.rptr)
    {
        trace_handle.sending = true;

        if (trace_handle.wptr > trace_handle.rptr)
        {
            trace_handle.send = trace_handle.wptr - trace_handle.rptr;
        }
        else
        {
            trace_handle.send = TRACE_BUF_SIZE - trace_handle.rptr;
        }

        if (trace_handle.send > (TRACE_BUF_SIZE >> 1))
        {
            trace_handle.send = (TRACE_BUF_SIZE >> 1);
        }

        uint8_t *tx_buf = (uint8_t *)&trace_handle.tx_buf[trace_handle.rptr];
        uint32_t tx_len = trace_handle.send;

        if ((trace_handle.port == TRACE_PORT_UART0) || (trace_handle.port == TRACE_PORT_UART1))
        {
            enum UART_DEV_T trace_uart = (trace_handle.port == TRACE_PORT_UART0) ? UART_ID0 : UART_ID1;

            if (DRV_OK != uart_send(trace_uart, tx_buf, tx_len, trace_sending_continue))
            {
                // uart busy or error
                trace_handle.sending = false;
            }
        }
        else if (trace_handle.port == TRACE_PORT_SPI0)
        {
        }
    }
    else
    {
        trace_handle.send = 0;
    }
    int_unlock(lock);
}

int trace_buf_is_empty(void)
{
    uint32_t lock = int_lock();
    int ret = (trace_handle.wptr == trace_handle.rptr ? 1 : 0);
    int_unlock(lock);
    return ret;
}

int trace_output(const char *buf, int len)
{
    int ret = 0;
    int free_len;
    int size;

    if (len <= 0)
    {
        return -1;
    }

    uint32_t lock = int_lock();

    if (trace_handle.busy == false)
    {
        trace_handle.busy = true;

        if (trace_handle.wptr >= trace_handle.rptr)
        {
            free_len = TRACE_BUF_SIZE - trace_handle.wptr + trace_handle.rptr;
        }
        else
        {
            free_len = trace_handle.rptr - trace_handle.wptr;
        }

        if (free_len < len)
        {
            ret = -1;
            // ASSERT(0, "TRACE buffer is not enough free %d need %d", free_len, len);

            uint16_t target;
            if (trace_handle.sending)
            {
                target = trace_handle.rptr + trace_handle.send;
                if (target >= TRACE_BUF_SIZE)
                {
                    target -= TRACE_BUF_SIZE;
                }
            }
            else
            {
                target = trace_handle.rptr;
            }

            uint16_t discard;
            if (trace_handle.wptr >= target)
            {
                discard = trace_handle.wptr - target;
            }
            else
            {
                discard = TRACE_BUF_SIZE - target + trace_handle.wptr;
            }
            trace_handle.wptr = target;
            trace_handle.busy = false;
            LOG_ERROR(TRACE_MODULE_DRIVER | TRACE_NO_OPTION, "\r\nTrace buffer is full, discards %u bytes\r\n", discard);
        }
        else
        {
            // copy data to trace buffer
            size = TRACE_BUF_SIZE - trace_handle.wptr;
            if (size >= len)
            {
                size = len;
            }
            memcpy(&trace_handle.tx_buf[trace_handle.wptr], &buf[0], (uint32_t)size);
            if (size < len)
            {
                memcpy(&trace_handle.tx_buf[0], &buf[size], (uint32_t)(len - size));
            }
            trace_handle.wptr += len;
            if (trace_handle.wptr >= TRACE_BUF_SIZE)
            {
                trace_handle.wptr -= TRACE_BUF_SIZE;
            }

            if (trace_handle.sending == false)
            {
                // sending right away
                trace_sending();
            }

            ret = len;
            trace_handle.busy = false;
        }
    }
    else
    {
        ASSERT(0, "TRACE BUSY");
    }

    int_unlock(lock);

    return ret;
}

static int trace_output_blocking(const char *buf, int len)
{
    if ((trace_handle.port == TRACE_PORT_UART0) || (trace_handle.port == TRACE_PORT_UART1))
    {
        UART_TypeDef *trace_uart = (trace_handle.port == TRACE_PORT_UART0) ? UART0 : UART1;

        // polling
        while (len)
        {
            if (trace_uart->STATUS & UART_STATUS_TFNF_MSK)
            {
                trace_uart->TX_DATA = *buf++;
                len--;
            }
        }
    }
    else if (trace_handle.port == TRACE_PORT_SPI0)
    {
    }

    return 0;
}

#if UART_CONSOLE_EN
static void console_receive_callback(void *dev, uint32_t err_code)
{
    if (err_code & (UART_ERR_MODEM | UART_ERR_LINE))
    {
        // uart error
        LOG_ERROR(TRACE_MODULE_DRIVER, "Console Error %x\r\n", err_code);
    }
    else
    {
        uint16_t payload_len = 0;
        enum UART_DEV_T trace_uart = (trace_handle.port == TRACE_PORT_UART0) ? UART_ID0 : UART_ID1;
        switch (trace_handle.console_fsm)
        {
        case CONSOLE_RX_HEADER:
        {
            if (trace_handle.rx_buf[POS_PREAMBLE] == CONSOLE_CMD)
            {
                payload_len = (uint16_t)(trace_handle.rx_buf[POS_LENGTH + 1] << 8) + trace_handle.rx_buf[POS_LENGTH];
                trace_handle.console_fsm = CONSOLE_RX_PAYLOAD;
                uart_receive(trace_uart, &trace_handle.rx_buf[POS_ID], payload_len, console_receive_callback);
            }
            else if (trace_handle.rx_buf[1] == CONSOLE_CMD)
            {
                trace_handle.rx_buf[POS_PREAMBLE] = CONSOLE_CMD;
                trace_handle.rx_buf[1] = trace_handle.rx_buf[2];
                uart_receive(trace_uart, &trace_handle.rx_buf[2], 1, console_receive_callback);
            }
            else if (trace_handle.rx_buf[2] == CONSOLE_CMD)
            {
                trace_handle.rx_buf[POS_PREAMBLE] = CONSOLE_CMD;
                uart_receive(trace_uart, &trace_handle.rx_buf[1], 2, console_receive_callback);
            }
            else
            {
                uart_receive(trace_uart, &trace_handle.rx_buf[POS_PREAMBLE], LEN_HEADER, console_receive_callback);
            }
            break;
        }

        case CONSOLE_RX_PAYLOAD:
        {
            // check crc
            payload_len = (uint16_t)(trace_handle.rx_buf[POS_LENGTH + 1] << 8) + trace_handle.rx_buf[POS_LENGTH];
            uint16_t crc_frame =
                (uint16_t)(trace_handle.rx_buf[payload_len + LEN_HEADER - LEN_CRC + 1] << 8) + trace_handle.rx_buf[payload_len + LEN_HEADER - LEN_CRC];
            uint16_t crc_cal = crc16(&trace_handle.rx_buf[0], payload_len + LEN_HEADER - LEN_CRC);

            if (crc_frame == crc_cal)
            {
                uint16_t id = (uint16_t)(trace_handle.rx_buf[POS_ID + 1] << 8) + trace_handle.rx_buf[POS_ID];

                // packet process
                if (console_cmd_handler_list[id].function)
                {
                    console_cmd_handler_list[id].function(&trace_handle.rx_buf[POS_DATA], payload_len - LEN_CRC - LEN_ID);
                }
                else
                {
                    LOG_ERROR(TRACE_MODULE_DRIVER, "Console CMD ID %04x Function is Null\r\n", id);
                }
            }
            else
            {
                LOG_ERROR(TRACE_MODULE_DRIVER, "Console CRC Error %04x %04x\r\n", crc_cal, crc_frame);
            }
            trace_handle.console_fsm = CONSOLE_RX_HEADER;
            uart_receive(trace_uart, &trace_handle.rx_buf[0], LEN_HEADER, console_receive_callback);
            break;
        }

        default:
            break;
        }
    }
}
#endif

#if TRACE_STD_LIB_EN == 0

#define FORMAT_FLAG_LEFT_JUSTIFY (1u << 0)
#define FORMAT_FLAG_PAD_ZERO (1u << 1)
#define FORMAT_FLAG_PRINT_SIGN (1u << 2)
#define FORMAT_FLAG_ALTERNATE (1u << 3)

typedef struct
{
    char *pBuffer;
    unsigned BufferSize;
    unsigned Cnt;

    int ReturnValue;

} MK_PRINTF_DESC_T;

static void _StoreChar(MK_PRINTF_DESC_T *p, char c)
{
    unsigned Cnt;

    Cnt = p->Cnt;
    if ((Cnt + 1u) <= p->BufferSize)
    {
        *(p->pBuffer + Cnt) = c;
        p->Cnt = Cnt + 1u;
        p->ReturnValue++;
    }
    //
    // Write part of string, when the buffer is full
    //
}

static void _PrintUnsigned(MK_PRINTF_DESC_T *pBufferDesc, unsigned v, unsigned Base, unsigned NumDigits, unsigned FieldWidth, unsigned FormatFlags)
{
    static const char _aV2C[16] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
    unsigned Div;
    unsigned Digit;
    unsigned Number;
    unsigned Width;
    char c;

    Number = v;
    Digit = 1u;
    //
    // Get actual field width
    //
    Width = 1u;
    while (Number >= Base)
    {
        Number = (Number / Base);
        Width++;
    }
    if (NumDigits > Width)
    {
        Width = NumDigits;
    }
    //
    // Print leading chars if necessary
    //
    if ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0u)
    {
        if (FieldWidth != 0u)
        {
            if (((FormatFlags & FORMAT_FLAG_PAD_ZERO) == FORMAT_FLAG_PAD_ZERO) && (NumDigits == 0u))
            {
                c = '0';
            }
            else
            {
                c = ' ';
            }
            while ((FieldWidth != 0u) && (Width < FieldWidth))
            {
                FieldWidth--;
                _StoreChar(pBufferDesc, c);
                if (pBufferDesc->ReturnValue < 0)
                {
                    break;
                }
            }
        }
    }
    if (pBufferDesc->ReturnValue >= 0)
    {
        //
        // Compute Digit.
        // Loop until Digit has the value of the highest digit required.
        // Example: If the output is 345 (Base 10), loop 2 times until Digit is 100.
        //
        while (1)
        {
            if (NumDigits > 1u)
            {   // User specified a min number of digits to print? => Make sure we loop at least that often, before
                // checking anything else (> 1 check avoids problems with NumDigits being signed / unsigned)
                NumDigits--;
            }
            else
            {
                Div = v / Digit;
                if (Div < Base)
                {   // Is our divider big enough to extract the highest digit from value? => Done
                    break;
                }
            }
            Digit *= Base;
        }
        //
        // Output digits
        //
        do
        {
            Div = v / Digit;
            v -= Div * Digit;
            _StoreChar(pBufferDesc, _aV2C[Div]);
            if (pBufferDesc->ReturnValue < 0)
            {
                break;
            }
            Digit /= Base;
        } while (Digit);
        //
        // Print trailing spaces if necessary
        //
        if ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == FORMAT_FLAG_LEFT_JUSTIFY)
        {
            if (FieldWidth != 0u)
            {
                while ((FieldWidth != 0u) && (Width < FieldWidth))
                {
                    FieldWidth--;
                    _StoreChar(pBufferDesc, ' ');
                    if (pBufferDesc->ReturnValue < 0)
                    {
                        break;
                    }
                }
            }
        }
    }
}

static void _PrintInt(MK_PRINTF_DESC_T *pBufferDesc, int v, unsigned Base, unsigned NumDigits, unsigned FieldWidth, unsigned FormatFlags)
{
    unsigned Width;
    int Number;

    Number = (v < 0) ? -v : v;

    //
    // Get actual field width
    //
    Width = 1u;
    while (Number >= (int)Base)
    {
        Number = (Number / (int)Base);
        Width++;
    }
    if (NumDigits > Width)
    {
        Width = NumDigits;
    }
    if ((FieldWidth > 0u) && ((v < 0) || ((FormatFlags & FORMAT_FLAG_PRINT_SIGN) == FORMAT_FLAG_PRINT_SIGN)))
    {
        FieldWidth--;
    }

    //
    // Print leading spaces if necessary
    //
    if ((((FormatFlags & FORMAT_FLAG_PAD_ZERO) == 0u) || (NumDigits != 0u)) && ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0u))
    {
        if (FieldWidth != 0u)
        {
            while ((FieldWidth != 0u) && (Width < FieldWidth))
            {
                FieldWidth--;
                _StoreChar(pBufferDesc, ' ');
                if (pBufferDesc->ReturnValue < 0)
                {
                    break;
                }
            }
        }
    }
    //
    // Print sign if necessary
    //
    if (pBufferDesc->ReturnValue >= 0)
    {
        if (v < 0)
        {
            v = -v;
            _StoreChar(pBufferDesc, '-');
        }
        else if ((FormatFlags & FORMAT_FLAG_PRINT_SIGN) == FORMAT_FLAG_PRINT_SIGN)
        {
            _StoreChar(pBufferDesc, '+');
        }
        else
        {
        }
        if (pBufferDesc->ReturnValue >= 0)
        {
            //
            // Print leading zeros if necessary
            //
            if (((FormatFlags & FORMAT_FLAG_PAD_ZERO) == FORMAT_FLAG_PAD_ZERO) && ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0u) && (NumDigits == 0u))
            {
                if (FieldWidth != 0u)
                {
                    while ((FieldWidth != 0u) && (Width < FieldWidth))
                    {
                        FieldWidth--;
                        _StoreChar(pBufferDesc, '0');
                        if (pBufferDesc->ReturnValue < 0)
                        {
                            break;
                        }
                    }
                }
            }
            if (pBufferDesc->ReturnValue >= 0)
            {
                //
                // Print number without sign
                //
                _PrintUnsigned(pBufferDesc, (unsigned)v, Base, NumDigits, FieldWidth, FormatFlags);
            }
        }
    }
}

int trace_format(char *buf, unsigned int size, const char *fmt, va_list args)
{
    char c;
    int v;
    unsigned NumDigits;
    unsigned FormatFlags;
    unsigned FieldWidth;

    MK_PRINTF_DESC_T BufferDesc;
    BufferDesc.pBuffer = buf;
    BufferDesc.BufferSize = size;
    BufferDesc.Cnt = 0u;
    BufferDesc.ReturnValue = 0;

    do
    {
        c = *fmt;
        fmt++;
        if (c == 0u)
        {
            break;
        }
        if (c == '%')
        {
            //
            // Filter out flags
            //
            FormatFlags = 0u;
            v = 1;
            do
            {
                c = *fmt;
                switch (c)
                {
                case '-':
                    FormatFlags |= FORMAT_FLAG_LEFT_JUSTIFY;
                    fmt++;
                    break;
                case '0':
                    FormatFlags |= FORMAT_FLAG_PAD_ZERO;
                    fmt++;
                    break;
                case '+':
                    FormatFlags |= FORMAT_FLAG_PRINT_SIGN;
                    fmt++;
                    break;
                case '#':
                    FormatFlags |= FORMAT_FLAG_ALTERNATE;
                    fmt++;
                    break;
                default:
                    v = 0;
                    break;
                }
            } while (v);
            //
            // filter out field with
            //
            FieldWidth = 0u;
            do
            {
                c = *fmt;
                if ((c < '0') || (c > '9'))
                {
                    break;
                }
                fmt++;
                FieldWidth = (FieldWidth * 10u) + ((unsigned)c - '0');
            } while (1);

            //
            // Filter out precision (number of digits to display)
            //
            NumDigits = 0u;
            c = *fmt;
            if (c == '.')
            {
                fmt++;
                do
                {
                    c = *fmt;
                    if ((c < '0') || (c > '9'))
                    {
                        break;
                    }
                    fmt++;
                    NumDigits = NumDigits * 10u + ((unsigned)c - '0');
                } while (1);
            }
            //
            // Filter out length modifier
            //
            c = *fmt;
            do
            {
                if ((c == 'l') || (c == 'h'))
                {
                    fmt++;
                    c = *fmt;
                }
                else
                {
                    break;
                }
            } while (1);
            //
            // Handle specifiers
            //
            switch (c)
            {
            case 'c':
            {
                char c0;
                v = va_arg(args, int);
                c0 = (char)v;
                _StoreChar(&BufferDesc, c0);
                break;
            }
            case 'd':
                v = va_arg(args, int);
                _PrintInt(&BufferDesc, v, 10u, NumDigits, FieldWidth, FormatFlags);
                break;
            case 'u':
                v = va_arg(args, int);
                _PrintUnsigned(&BufferDesc, (unsigned)v, 10u, NumDigits, FieldWidth, FormatFlags);
                break;
            case 'x':
            case 'X':
                v = va_arg(args, int);
                _PrintUnsigned(&BufferDesc, (unsigned)v, 16u, NumDigits, FieldWidth, FormatFlags);
                break;
            case 's':
            {
                const char *s = va_arg(args, const char *);
                do
                {
                    c = *s;
                    s++;
                    if (c == '\0')
                    {
                        break;
                    }
                    _StoreChar(&BufferDesc, c);
                } while (BufferDesc.ReturnValue >= 0);
            }
            break;
            case 'f':
            case 'F':
            {
                float fv = (float)va_arg(args, double);
                if (fv < 0)
                    _StoreChar(&BufferDesc, '-');
                v = abs((int)fv);
                _PrintInt(&BufferDesc, v, 10u, NumDigits, FieldWidth, FormatFlags);
                _StoreChar(&BufferDesc, '.');
                v = abs((int)(fv * 1000000));
                v = v % 1000000;
                _PrintInt(&BufferDesc, v, 10u, 6, FieldWidth, FormatFlags);
            }
            break;
            case 'p':
                v = va_arg(args, int);
                _PrintUnsigned(&BufferDesc, (unsigned)v, 16u, 8u, 8u, 0u);
                break;
            case '%':
                _StoreChar(&BufferDesc, '%');
                break;
            default:
                break;
            }
            fmt++;
        }
        else
        {
            _StoreChar(&BufferDesc, c);
        }
    } while (BufferDesc.ReturnValue >= 0);

    if (BufferDesc.ReturnValue > 0)
    {
        //
        // Write remaining data, if any
        //
    }
    return BufferDesc.ReturnValue;
}

#else

__attribute__((__format__(__printf__, 3, 0))) int trace_format(char *buf, unsigned int size, const char *fmt, va_list args)
{
    return vsnprintf(buf, size, fmt, args);
}

#endif

int trace_printf(uint16_t module, uint8_t level, const char *fmt, ...)
{
    if (trace_handle.enabled == 0)
    {
        return 0;
    }
    char buf[256] = {0};
    uint32_t pre_len = 0;
    int len = 0;
    uint8_t mod_idx = (module & 0xff);

    if (mod_idx >= TRACE_MODULE_NUM)
    {
        return -1;
    }
    if ((level >= TRACE_LEVEL_NUM) || (level == 0))
    {
        return -1;
    }
    if (level > trace_level_cfg[mod_idx])
    {
        return 0;
    }

    // uint32_t lock = int_lock();
    if ((module & TRACE_NO_TIMESTAMP) == 0)
    {
        mk_snprintf(&buf[pre_len], sizeof(buf) - pre_len, "%10u | ", sys_tick_us());
        pre_len = strlen(buf);
    }
    if ((module & TRACE_NO_MODULE_NAME) == 0)
    {
        mk_snprintf(&buf[pre_len], sizeof(buf) - pre_len, "%s", module_name[mod_idx]);
        pre_len = strlen(buf);
    }
    if ((module & TRACE_NO_LEVEL_TAG) == 0)
    {
        mk_snprintf(&buf[pre_len], sizeof(buf) - pre_len, "%s", level_tag[level - 1]);
        pre_len = strlen(buf);
    }

    va_list argp;
    va_start(argp, fmt);
    len = trace_format(&buf[pre_len], sizeof(buf) - pre_len, fmt, argp);
    va_end(argp);

    // int_unlock(lock);
    return trace_output(buf, (int)pre_len + len);
}

#if TRACE_OVER_UCI_EN
int uci_print_enable = 0;

static unsigned short checksum_string(const char *addr, int count)
{
    long sum = 0;

    // Take original data as 16 digits, add it with 32 digits, so can carry save
    while (count > 1)
    {
        sum += (*addr) | *(addr + 1) << 8;
        addr += 2;
        count -= 2;
    }

    /* Add left-over byte, if any */
    if (count > 0)
    {
        sum += *(unsigned const char *)addr;
    }
    /* Fold 32-bit sum to 16 bits */
    while (sum >> 16)
    {
        sum = (sum & 0xffff) + (sum >> 16);
    }
    return (unsigned short)~sum;
}

extern void uci_vendor_log_upload(uint16_t len, const uint8_t *data);

void trace_upto_host(uint16_t module, uint8_t level, const char *fmt, ...)
{
    if (uci_print_enable == 0)
    {
        // TRACE(TRACE_MODULE_APP, TRACE_LEVEL_INFO, "uci proxy not start.\r\n");
        return;
    }

    uint8_t mod_idx = (module & 0xff);

    if (mod_idx >= TRACE_MODULE_NUM)
    {
        return;
    }
    if ((level >= TRACE_LEVEL_NUM) || (level == 0))
    {
        return;
    }
    if (level > trace_level_cfg[mod_idx])
    {
        return;
    }

    if ((module & TRACE_NO_OPTION) || (module & TRACE_NO_REPORT_HOST))
    {
        return;
    }

    int it = 0;
    double dd = 0;
    char const *p;
    // checksum
    unsigned short stringCsum;
    va_list aq;
    va_start(aq, fmt);
    p = fmt;

    char var_count = 0; // To control the length of data sent, only support up to 4 input variables, can be expanded
    char dataBits = 0;  // Two bits represent a input variable's length, so the max is 4 bytes, can be expanded[0-0,1-1,2-2,3-4]
    uint8_t j = 0;
    uint8_t recdBuf[256];

    stringCsum = checksum_string(fmt, (int)strlen(fmt));

    // TRACE(TRACE_MODULE_FIRA, TRACE_LEVEL_ERROR, "%s cksum=%x len=%d\r\n", fmt, stringCsum, strlen(fmt));

    for (j = 0; j < sizeof(unsigned short); j++)
    {
        *(recdBuf + j) = (stringCsum >> (8 * j) & 0xff);
    }

    *(recdBuf + 2) = 0;
    j = 3;

    while (*p != '\0')
    {   // fill begin at recdBuf[3]
        if (*p != '%')
        {
            p++;
            continue;
        }

        if ((*(p + 1) == '0') && ((*(p + 2) > '0') && (*(p + 2) < '9'))) /* skip %0* */
        {
            p += 2;
        }

        switch (*++p)
        {
        case 's':
        {
            recdBuf[j++] = 0;
            dataBits |= 1 << 2 * var_count++;
        }
        break;
        case 'c':
        case 'd':
        {
            it = va_arg(aq, int);
            if (it > 0x7FFF)
            {
                *(recdBuf + j++) = 0xff;
                *(recdBuf + j++) = 0x7f;
                dataBits |= 2 << 2 * var_count++;
            }

            if (it < 256)
            {   // 1byte
                recdBuf[j++] = (char)it;
                dataBits |= 1 << 2 * var_count++;
            }
            else
            {   // 2byte
                *(recdBuf + j++) = it & 0xff;
                *(recdBuf + j++) = it >> 8 & 0xff;
                dataBits |= 2 << 2 * var_count++;
            }
        }
        break;
        case 'x':
        case 'X':
        case 'u':
        {
            it = va_arg(aq, int);
            if (it > 0xFFFF)
            {
                *(recdBuf + j++) = 0xff;
                *(recdBuf + j++) = 0xff;
                dataBits |= 2 << 2 * var_count++;
            }

            if (it < 0x100) //<0xFF
            {   // 1byte
                recdBuf[j++] = (char)it;
                dataBits |= 1 << 2 * var_count++;
            }
            else
            {   // 2byte
                *(recdBuf + j++) = it & 0xff;
                *(recdBuf + j++) = it >> 8 & 0xff;
                dataBits |= 2 << 2 * var_count++;
            }
        }
        break;
        case 'f':
        {
            dd = va_arg(aq, double);
            if (dd > 3.4e38)
            {
                *(recdBuf + j++) = 0xff;
                *(recdBuf + j++) = 0xff;
                *(recdBuf + j++) = 0xff;
                *(recdBuf + j++) = 0xff;
                dataBits |= 3 << 2 * var_count++;
            }
            else
            {
                *(recdBuf + j++) = (unsigned int)dd & 0xff;
                *(recdBuf + j++) = (unsigned int)dd >> 8 & 0xff;
                *(recdBuf + j++) = (unsigned int)dd >> 16 & 0xff;
                *(recdBuf + j++) = (unsigned int)dd >> 24 & 0xff;
                dataBits |= 3 << 2 * var_count++;
            }
        }
        break;
        }
        p++;
    }
    va_end(aq);

    if (var_count > 4)
    {
        // TRACE(TRACE_MODULE_APP, TRACE_LEVEL_INFO, "We only support for 4 var input.\r\n");
        return;
    }

    *(recdBuf + 2) = dataBits;
    uci_vendor_log_upload((uint16_t)j, (uint8_t *)recdBuf);
}
#endif

__attribute__((__format__(__printf__, 3, 0))) int mk_snprintf(char *buf, unsigned int size, const char *fmt, ...)
{
    va_list argp;
    int i = 0;
    va_start(argp, fmt);
    i = trace_format(buf, size, fmt, argp);
    va_end(argp);

    return i;
}

__attribute__((__format__(__printf__, 3, 0))) int trace_dump(uint16_t module, uint8_t level, const char *fmt, uint32_t size, const void *data, uint32_t count)
{
    if (trace_handle.enabled == 0)
    {
        return 0;
    }
    char buf[256] = {0};
    int len = 0;

    if (module >= TRACE_MODULE_NUM)
    {
        return -1;
    }
    if ((level >= TRACE_LEVEL_NUM) || (level == 0))
    {
        return -1;
    }
    if (level > trace_level_cfg[module])
    {
        return 0;
    }

    if (size == sizeof(uint32_t))
    {
        for (uint32_t i = 0; i < count && (uint32_t)len < sizeof(buf); i++)
        {
            len += mk_snprintf(&buf[len], sizeof(buf) - (uint32_t)len, fmt, *(const uint32_t *)((const uint32_t *)data + i));
        }
    }
    else if (size == sizeof(uint16_t))
    {
        for (uint32_t i = 0; i < count && (uint32_t)len < sizeof(buf); i++)
        {
            len += mk_snprintf(&buf[len], sizeof(buf) - (uint32_t)len, fmt, *(const uint16_t *)((const uint16_t *)data + i));
        }
    }
    else if (size == sizeof(uint8_t))
    {
        for (uint32_t i = 0; i < count && (uint32_t)len < sizeof(buf); i++)
        {
            len += mk_snprintf(&buf[len], sizeof(buf) - (uint32_t)len, fmt, *(const uint8_t *)((const uint8_t *)data + i));
        }
    }

    ASSERT((uint32_t)len <= sizeof(buf), "len %d", len);

    return trace_output(buf, len);
}

#if BACKTRACE_EN
static int trace_address_executable(uint32_t addr)
{
    // check thumb flag
    if ((addr & 0x01) == 0)
    {
        return 0;
    }

    // check location
    if (CODE_BASE < addr && addr < CODE_BASE + SRAM_SIZE)
    {
        return 1;
    }
    if (ROM_BASE < addr && addr < ROM_BASE + ROM_SIZE)
    {
        return 1;
    }

    return 0;
}

static void trace_print_backtrace(uint32_t addr, uint32_t depth, uint32_t count)
{
    uint32_t *stack = (uint32_t *)addr;
    char buf[20] = {0};
    int len;

    if (addr & 0x03)
    {
        return;
    }

    len = mk_snprintf(buf, sizeof(buf), TRACE_NEW_LINE "Backtrace: " TRACE_NEW_LINE);
    trace_output_blocking(buf, len);

    for (uint32_t i = 0, j = 0; i < depth && j < count; i++)
    {
        if (trace_address_executable(stack[i]))
        {
            len = mk_snprintf(buf, sizeof(buf), "%08X" TRACE_NEW_LINE, stack[i]);
            trace_output_blocking(buf, len);
            j++;
        }
    }
}

static void trace_crash_dump_callback(void)
{
    for (uint32_t i = 0; i < ARRAY_SIZE(trace_crash_dump_cb_list); i++)
    {
        if (trace_crash_dump_cb_list[i])
        {
            trace_crash_dump_cb_list[i]();
        }
    }
}
#endif

__NO_RETURN void trace_end(void)
{
    trace_output_blocking("Crashed!!!\r\n", 12);
#if TRACE_REBOOT_EN
    // reboot
    reset_module(RESET_MODULE_REBOOT);
#endif
    // open SWD
    // io_pin_mux_set(IO_PIN_15, IO_FUNC0);
    // io_pin_mux_set(IO_PIN_16, IO_FUNC0);
    // trace_output_blocking("SWD open\r\n", 10);

    do
    {
        __NOP();
        __NOP();
        __NOP();
        __NOP();
    } while (1);
}

#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wuninitialized"
#endif

#if BACKTRACE_EN
void trace_flush(void)
{
    if ((trace_handle.port == TRACE_PORT_UART0) || (trace_handle.port == TRACE_PORT_UART1))
    {
        enum UART_DEV_T trace_uart = (trace_handle.port == TRACE_PORT_UART0) ? UART_ID0 : UART_ID1;
        while (uart_tx_in_progress(trace_uart))
        {
        }
    }
    else if (trace_handle.port == TRACE_PORT_SPI0)
    {
    }

    uint32_t lock = int_lock();

         while (trace_handle.wptr != trace_handle.rptr)
    {
        if (trace_handle.wptr > trace_handle.rptr)
        {
            trace_handle.send = trace_handle.wptr - trace_handle.rptr;
        }
        else
        {
            trace_handle.send = TRACE_BUF_SIZE - trace_handle.rptr;
        }

        uint8_t *tx_buf = (uint8_t *)&trace_handle.tx_buf[trace_handle.rptr];
        uint32_t tx_len = trace_handle.send;
        trace_output_blocking((const char *)tx_buf, (int)tx_len);

        trace_handle.rptr += trace_handle.send;
        if (trace_handle.rptr >= TRACE_BUF_SIZE)
        {
            trace_handle.rptr -= TRACE_BUF_SIZE;
        }
    }

    // sent done
    trace_handle.sending = false;
    int_unlock(lock);
}

static const char trace_new_line[] = TRACE_NEW_LINE;

static void _trace_assert_dump(uint32_t primask, uint32_t sp, uint32_t lr, const char *file, const char *func, uint32_t line, const char *fmt)
{
    struct TRACE_ASSERT_INF_T assert_inf;

    assert_inf.SP = sp;
    assert_inf.LR = lr;

    assert_inf.ID = TRACE_ASSERT_ID;
    assert_inf.FILE = file;
    assert_inf.FUNC = func;
    assert_inf.LINE = line;
    assert_inf.xPSR = __get_xPSR();
    assert_inf.MSP = __get_MSP();
    assert_inf.PSP = __get_PSP();
    assert_inf.PRIMASK = primask;
    assert_inf.CONTROL = __get_CONTROL();

    // Store assert_inf in memory (optional)
    //*(volatile uint32_t *)SRAM_BASE = (uint32_t)&assert_inf;
    trace_flush();

    static const char desc_file[] = "FILE    : ";
    static const char desc_func[] = "FUNCTION: ";
    static const char desc_line[] = "LINE    : ";

    int len = 0;

    static const char separate_line[] = "----------------------------------------" TRACE_NEW_LINE;
    trace_output_blocking(separate_line, sizeof(separate_line) - 1);
    // format output string
    trace_output_blocking(fmt, (int)strlen(fmt));
    trace_output_blocking(trace_new_line, sizeof(trace_new_line) - 1);
    trace_output_blocking(separate_line, sizeof(separate_line) - 1);

    // Timestamp
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "%10d | " TRACE_NEW_LINE, sys_tick_us());

    // Title and PC
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, TRACE_NEW_LINE "### ASSERT @ 0x%08X ###" TRACE_NEW_LINE,
                       (assert_inf.LR & ~1U) - 4);
    trace_output_blocking(trace_handle.tx_buf, len);

    // File
    trace_output_blocking(desc_file, sizeof(desc_file) - 1);
    trace_output_blocking(file, (int)strlen(file));
    trace_output_blocking(trace_new_line, sizeof(trace_new_line) - 1);
    // Func
    trace_output_blocking(desc_func, sizeof(desc_func) - 1);
    trace_output_blocking(func, (int)strlen(func));
    trace_output_blocking(trace_new_line, sizeof(trace_new_line) - 1);
    // Line
    trace_output_blocking(desc_line, sizeof(desc_func) - 1);
    len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), "%d", line);
    trace_output_blocking(trace_handle.tx_buf, len);
    trace_output_blocking(trace_new_line, sizeof(trace_new_line) - 1);

    // SP, LR
    len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), TRACE_NEW_LINE "SP=%08X" TRACE_NEW_LINE, assert_inf.SP);
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "LR=%08X" TRACE_NEW_LINE, assert_inf.LR);
    trace_output_blocking(trace_handle.tx_buf, len);

    // xPSR
    len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), "xPSR=%08X" TRACE_NEW_LINE, assert_inf.xPSR);
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "N=%d, Z=%d, C=%d, V=%d" TRACE_NEW_LINE,
                       ((assert_inf.xPSR >> 31) & 0x1), ((assert_inf.xPSR >> 30) & 0x1), ((assert_inf.xPSR >> 29) & 0x1), ((assert_inf.xPSR >> 28) & 0x1));
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "IPSR=%08X" TRACE_NEW_LINE, assert_inf.xPSR & 0x3F);
    trace_output_blocking(trace_handle.tx_buf, len);

    // PRIMASK, CONTROL
    len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), "PRIMASK=%08X" TRACE_NEW_LINE, assert_inf.PRIMASK);
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "CONTROL=%08X" TRACE_NEW_LINE, assert_inf.CONTROL);
    trace_output_blocking(trace_handle.tx_buf, len);

    // MSP, PSP
    len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), "MSP=%08X" TRACE_NEW_LINE, assert_inf.MSP);
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "PSP=%08X" TRACE_NEW_LINE, assert_inf.PSP);
    trace_output_blocking(trace_handle.tx_buf, len);

    // Backtrace
    uint32_t stack_base = (uint32_t)(&__INITIAL_SP);
    trace_print_backtrace(assert_inf.SP, MIN(TRACE_BACKTRACE_DEPTH, stack_base - assert_inf.SP) >> 2, TRACE_BACKTRACE_COUNT);
}
#endif

void __NO_RETURN trace_assert_dump(const char *file, const char *func, uint32_t line, const char *fmt, ...)
{
    int_lock();
#if BACKTRACE_EN
    uint32_t primask = __get_PRIMASK();
    register uint32_t sp;
    register uint32_t lr;

#if defined(__CC_ARM)

#else
__ASM volatile("mov %0, sp" : "=r"(sp));
__ASM volatile("mov %0, lr" : "=r"(lr));
#endif

    // Parameters
    va_list argp;
    va_start(argp, fmt);
    int len = trace_format(&trace_handle.tx_buf[0], sizeof(trace_handle.tx_buf), fmt, argp);
    va_end(argp);
    trace_handle.tx_buf[len] = 0;

    _trace_assert_dump(primask, sp, lr, file, func, line, &trace_handle.tx_buf[0]);

    // User registered callback
    trace_crash_dump_callback();
#endif
    // Delay for printing
    trace_end();
    // while(1);
}

#if BACKTRACE_EN
__NO_RETURN void trace_exception_handler(void *regs)
{
    struct TRACE_EXCEPTION_INF_T *inf = regs;

    inf->ID = TRACE_EXCEPTION_ID;

    // Store inf in memory (optional)
    //*(volatile uint32_t *)SRAM_BASE = (uint32_t)&inf;
    trace_flush();

    int len = 0;
    uint32_t value;
    uint32_t *stack;

    // Timestamp
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "%10d | " TRACE_NEW_LINE, sys_tick_us());

    // Title
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len,
                       TRACE_NEW_LINE TRACE_NEW_LINE "### EXCEPTION %d ###" TRACE_NEW_LINE, inf->ICSR & 0x3F);
    trace_output_blocking(trace_handle.tx_buf, len);
    // PC
    len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), "PC=%08X", inf->PC);
    // Exception number
    value = inf->xPSR & 0x3F;
    if (value == 0)
    {
        len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, ", ThreadMode" TRACE_NEW_LINE);
    }
    else
    {
        len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, ", ExceptionNumber=%d" TRACE_NEW_LINE, value);
    }
    trace_output_blocking(trace_handle.tx_buf, len);

    // R0 ~ R12
    for (int i = 0; i < 13; i++)
    {
        len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), "R%-2d=%08X" TRACE_NEW_LINE, i, inf->REGS[i]);
        trace_output_blocking(trace_handle.tx_buf, len);
    }
    // SP, LR
    len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), "SP=%08X" TRACE_NEW_LINE, inf->SP);
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "LR=%08X" TRACE_NEW_LINE, inf->LR);
    trace_output_blocking(trace_handle.tx_buf, len);

    // xPSR
    len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), "xPSR=%08X" TRACE_NEW_LINE, inf->xPSR);
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "N=%d, Z=%d, C=%d, V=%d" TRACE_NEW_LINE,
                       ((inf->xPSR >> 31) & 0x1), ((inf->xPSR >> 30) & 0x1), ((inf->xPSR >> 29) & 0x1), ((inf->xPSR >> 28) & 0x1));
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "IPSR=%08X" TRACE_NEW_LINE, inf->xPSR & 0x3F);
    trace_output_blocking(trace_handle.tx_buf, len);

    // PRIMASK, CONTROL
    len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), "PRIMASK=%08X" TRACE_NEW_LINE, inf->PRIMASK);
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "CONTROL=%08X" TRACE_NEW_LINE, inf->CONTROL);
    trace_output_blocking(trace_handle.tx_buf, len);

    // MSP, PSP, EXC_RETURN
    if (inf->EXC_RETURN & 0x04)
    {
        len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), "MSP=%08X" TRACE_NEW_LINE, inf->PSP);
        len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "PSP=%08X" TRACE_NEW_LINE, inf->MSP);
    }
    else
    {
        len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), "MSP=%08X" TRACE_NEW_LINE, inf->MSP);
        len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "PSP=%08X" TRACE_NEW_LINE, inf->PSP);
    }
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "EXC_RETURN=%08X" TRACE_NEW_LINE, inf->EXC_RETURN);
    trace_output_blocking(trace_handle.tx_buf, len);

    // ICSR, AIRCR, SCR, CCR
    len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), "ICSR=%08X" TRACE_NEW_LINE, inf->ICSR);
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "AIRCR=%08X" TRACE_NEW_LINE, inf->AIRCR);
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "SCR=%08X" TRACE_NEW_LINE, inf->SCR);
    len += mk_snprintf(&trace_handle.tx_buf[len], sizeof(trace_handle.tx_buf) - (uint32_t)len, "CCR=%08X" TRACE_NEW_LINE TRACE_NEW_LINE, inf->CCR);
    trace_output_blocking(trace_handle.tx_buf, len);

    stack = (uint32_t *)(inf->SP & ~0x3U);
    uint32_t stack_base = (uint32_t)(&__INITIAL_SP);
    for (uint32_t i = 0; (i < TRACE_STACK_DUMP_LEN) && ((i * 4 + inf->SP) < stack_base); i += 4)
    {
        len = mk_snprintf(trace_handle.tx_buf, sizeof(trace_handle.tx_buf), "%08X: %08X %08X %08X %08X" TRACE_NEW_LINE, (uint32_t)&stack[i], stack[i],
                          stack[i + 1], stack[i + 2], stack[i + 3]);
        trace_output_blocking(trace_handle.tx_buf, len);
    }

    // Backtrace
    trace_print_backtrace(inf->SP, MIN(TRACE_BACKTRACE_DEPTH, (stack_base - inf->SP)) >> 2, TRACE_BACKTRACE_COUNT);

    // User registered callback
    trace_crash_dump_callback();

    // Delay for printing

    trace_end();
}
#endif

/*----------------------------------------------------------------------------
  Default Handler for Exceptions / Interrupts
 *----------------------------------------------------------------------------*/
void HardFault_Handler(void)
{
#if BACKTRACE_EN
    __ASM volatile(
        "movs r1, #0x04 \n"
        "mov r0, lr \n"
        "tst r0, r1 \n"
        "beq label_1 \n"

        // Using PSP
        "mrs r3, psp \n"
        "mrs r2, msp \n"
        "b label_2 \n"

        "label_1: \n"
        // Using MSP
        "mrs r3, msp \n"
        "mrs r2, psp \n"

        "label_2: \n"
        // Allocate space for r0-r15,psr...
        "sub sp, #27*4 \n"

        // Save r4-r7
        "add r0, sp, #4*4 \n"
        "stm r0!, {r4-r7} \n"

        // Save r8 - r11
        "mov r4, r8 \n"
        "mov r5, r9 \n"
        "mov r6, r10 \n"
        "mov r7, r11 \n"
        "add r0, sp, #8*4 \n"
        "stm r0!, {r4-r7} \n"

        // Save r0-r3
        "mov r0, r3 \n"
        "ldm r0!, {r4-r7} \n"
        "mov r0, sp \n"
        "stm r0!, {r4-r7} \n"

        // Save r12
        "ldr r0, [r3, #4*4] \n"
        "str r0, [sp, #12*4] \n"
        // Save sp
        "movs r0, #8*4 \n"
        "add r0, r0, r3 \n"
        "str r0, [sp, #13*4] \n"
        // Save lr
        "ldr r0, [r3, #5*4] \n"
        "str r0, [sp, #14*4] \n"
        // Save pc
        "ldr r0, [r3, #6*4] \n"
        "str r0, [sp, #15*4] \n"
        // Save PSR
        "ldr r0, [r3, #7*4] \n"
        "str r0, [sp, #16*4] \n"
        // Save primask
        "mrs r0, primask \n"
        "str r0, [sp, #17*4] \n"
        // Save control
        "mrs r0, control \n"
        "str r0, [sp, #18*4] \n"
        // Save ICSR
        "ldr r1, =0xE000ED04 \n"
        "ldr r0, [r1] \n"
        "str r0, [sp, #19*4] \n"
        // Save AIRCR
        "ldr r1, =0xE000ED0C \n"
        "ldr r0, [r1] \n"
        "str r0, [sp, #20*4] \n"
        // Save SCR
        "ldr r1, =0xE000ED10 \n"
        "ldr r0, [r1] \n"
        "str r0, [sp, #21*4] \n"
        // Save CCR
        "ldr r1, =0xE000ED14 \n"
        "ldr r0, [r1] \n"
        "str r0, [sp, #22*4] \n"
        // Save msp, psp
        "str r3, [sp, #23*4] \n"
        "str r2, [sp, #24*4] \n"
        // Save exc_return
        "mov r0, lr \n"
        "str r0, [sp, #25*4] \n"

        // Invoke the fault handler
        "mov r0, sp \n"
        "bl trace_exception_handler \n");
#else
    trace_end();
    // while(1);
#endif
}

#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
#pragma clang diagnostic pop
#endif

#else
__NO_RETURN void HardFault_Handler(void);

void HardFault_Handler(void)
{
#if TRACE_REBOOT_EN
    // reboot
    reset_module(RESET_MODULE_REBOOT);
#endif
    // open SWD
    // io_pin_mux_set(IO_PIN_15, IO_FUNC0);
    // io_pin_mux_set(IO_PIN_16, IO_FUNC0);

    do
    {
        __NOP();
        __NOP();
        __NOP();
        __NOP();
    } while (1);
}

int trace_printf(uint16_t module, uint8_t level, const char *fmt, ...)
{
    return 0;
}

int trace_dump(uint16_t module, uint8_t level, const char *fmt, uint32_t size, const void *data, uint32_t count)
{
    return 0;
}

void __NO_RETURN trace_assert_dump(const char *file, const char *func, uint32_t line, const char *fmt, ...)
{
    HardFault_Handler();
}

void trace_flush(void)
{
}

#endif