87 lines
2.4 KiB
C
87 lines
2.4 KiB
C
/*******************************************************************************
|
|
* @file parser.c
|
|
* @brief BLE command parser
|
|
*
|
|
* Packet format : [TAG 4B] [DATA NB] [CRC16 2B]
|
|
******************************************************************************/
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#include "parser.h"
|
|
#include "cmd_common.h"
|
|
#include "cmd_table.h"
|
|
#include "debug_print.h"
|
|
|
|
/* 파서 엔트리 */
|
|
int ble_cmd_dispatch(const uint8_t *buf, uint16_t len)
|
|
{
|
|
//DBG_CORE("[PARSER] enter len=%u\r\n", len);
|
|
//DBG_CORE("[CMD] RX len=%u\r\n", len);
|
|
|
|
char raw_tag[5] = { '?', '?', '?', '?', '\0' };
|
|
for (uint16_t i = 0; i < len && i < 4U; i++)
|
|
{
|
|
raw_tag[i] = (char)buf[i];
|
|
}
|
|
|
|
if (len < 7U)
|
|
{
|
|
DBG_ERR("[CMD] Too short (%u)\r\n", len);
|
|
if (cmd_send_response_tag_echo("rxs:", raw_tag) != 0)
|
|
{
|
|
DBG_ERR("[CMD] rxs tx failed\r\n");
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
uint16_t calc_crc = cmd_crc16_compute(buf, len - 2U);
|
|
uint16_t recv_crc = (uint16_t)(buf[len - 2U]) | ((uint16_t)(buf[len - 1U]) << 8);
|
|
|
|
if (calc_crc != recv_crc)
|
|
{
|
|
DBG_ERR("[CMD] CRC fail tag=%s calc=0x%04X recv=0x%04X\r\n", raw_tag, calc_crc, recv_crc);
|
|
if (cmd_send_response_tag_echo("rxc:", raw_tag) != 0)
|
|
{
|
|
DBG_ERR("[CMD] rxc tx failed\r\n");
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
char tag[5] =
|
|
{
|
|
cmd_ascii_to_lower((char)buf[0]),
|
|
cmd_ascii_to_lower((char)buf[1]),
|
|
cmd_ascii_to_lower((char)buf[2]),
|
|
cmd_ascii_to_lower((char)buf[3]),
|
|
'\0'
|
|
};
|
|
|
|
const uint8_t *data = buf + 4;
|
|
uint8_t data_len = (uint8_t)(len - 4U - 2U);
|
|
|
|
for (int i = 0; i < cmd_table_size; i++)
|
|
{
|
|
if (memcmp(tag, cmd_table[i].tag, 4) == 0)
|
|
{
|
|
if (cmd_table[i].handler == NULL)
|
|
{
|
|
DBG_ERR("[CMD] Null handler: %s\r\n", cmd_table[i].tag);
|
|
if (cmd_send_response_tag_echo("rxn:", raw_tag) != 0)
|
|
{
|
|
DBG_ERR("[CMD] rxn tx failed\r\n");
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
return cmd_table[i].handler(data, data_len);
|
|
}
|
|
}
|
|
|
|
DBG_ERR("[CMD] Unknown: raw=%s normalized=%s\r\n", raw_tag, tag);
|
|
if (cmd_send_response_tag_echo("rxx:", raw_tag) != 0)
|
|
{
|
|
DBG_ERR("[CMD] rxx tx failed\r\n");
|
|
}
|
|
return 0;
|
|
}
|