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