337 lines
9.5 KiB
C
337 lines
9.5 KiB
C
/*==============================================================================
|
|
* parser.c - BLE command parser / dispatcher
|
|
*
|
|
* Common module that parses binary command packets received over BLE and
|
|
* dispatches them to the appropriate handler from the command table.
|
|
*
|
|
* Key point: this file does not know which Cmd_* functions exist.
|
|
* cmd_table.c injects the table pointer at boot via dr_parser_init().
|
|
*
|
|
* Packet layout: [TAG 4B] [data N] [CRC16 2B (optional)]
|
|
* - TAG : 4-char ASCII identifier (e.g. "mid?")
|
|
* - data : Big-Endian uint16 or ASCII
|
|
* - CRC16: present when g_plat.crc_check is true; trailing 2 bytes (LE)
|
|
*
|
|
* Flow:
|
|
* 1) dr_cmd_parser() - external entry point
|
|
* 2) dr_parse_cmd() - length / CRC verification + TAG/data split
|
|
* 3) dr_cmd_dispatch() - walks the command table -> calls handler
|
|
*
|
|
* Error responses (all formatted as [err_tag 4B] [echo cmd_tag 4B] = 8 bytes):
|
|
* - rxs: Too short (< 4B, or < 7B with CRC enabled)
|
|
* - rxc: CRC fail
|
|
* - rxd: Disabled command
|
|
* - rxn: NULL handler
|
|
* - rxx: Unknown command
|
|
*============================================================================*/
|
|
|
|
#include "parser.h"
|
|
#include <string.h>
|
|
|
|
|
|
/*------------------------------------------------------------------------------
|
|
* Globals (declared extern in header)
|
|
*----------------------------------------------------------------------------*/
|
|
dr_platform_if_t g_plat = { 0, 0, 0 };
|
|
bool g_log_enable = false;
|
|
|
|
|
|
/*------------------------------------------------------------------------------
|
|
* Internal state - command table pointer injected via dr_parser_init()
|
|
*----------------------------------------------------------------------------*/
|
|
static const CmdEntry *m_cmd_table = NULL;
|
|
static uint16_t m_cmd_count = 0;
|
|
|
|
|
|
/*==============================================================================
|
|
* Internal utilities (static)
|
|
*============================================================================*/
|
|
|
|
/* Copy first 4 bytes of buffer into a null-terminated TAG string. */
|
|
static void dr_copy_tag(const uint8_t *buf, char *tag_out)
|
|
{
|
|
tag_out[0] = (char)buf[0];
|
|
tag_out[1] = (char)buf[1];
|
|
tag_out[2] = (char)buf[2];
|
|
tag_out[3] = (char)buf[3];
|
|
tag_out[4] = '\0';
|
|
}
|
|
|
|
/* Compare two 4-char TAGs (byte-by-byte, faster than memcmp here). */
|
|
static bool dr_tag_eq(const char *tag, const char *key4)
|
|
{
|
|
return (tag[0] == key4[0] &&
|
|
tag[1] == key4[1] &&
|
|
tag[2] == key4[2] &&
|
|
tag[3] == key4[3]);
|
|
}
|
|
|
|
|
|
/*==============================================================================
|
|
* Public utilities - used by handlers
|
|
*============================================================================*/
|
|
|
|
bool dr_get_u16(const ParsedCmd *cmd, uint8_t word_index, uint16_t *out)
|
|
{
|
|
uint8_t pos = (uint8_t)(word_index * 2);
|
|
if (cmd->data_len < (uint8_t)(pos + 2))
|
|
{
|
|
return false;
|
|
}
|
|
*out = (uint16_t)((uint16_t)cmd->data[pos] << 8) | (uint16_t)cmd->data[pos + 1];
|
|
return true;
|
|
}
|
|
|
|
void dr_get_ascii(const ParsedCmd *cmd, uint8_t offset, char *out, uint8_t max_len)
|
|
{
|
|
uint8_t i;
|
|
uint8_t remain;
|
|
|
|
if (offset >= cmd->data_len)
|
|
{
|
|
out[0] = '\0';
|
|
return;
|
|
}
|
|
|
|
remain = (uint8_t)(cmd->data_len - offset);
|
|
if (remain > max_len)
|
|
{
|
|
remain = max_len;
|
|
}
|
|
|
|
for (i = 0; i < remain; i++)
|
|
{
|
|
out[i] = (char)cmd->data[offset + i];
|
|
}
|
|
out[remain] = '\0';
|
|
}
|
|
|
|
|
|
/*==============================================================================
|
|
* CRC16 (Nordic SDK CRC-CCITT variant)
|
|
*============================================================================*/
|
|
|
|
uint16_t dr_crc16_compute(const uint8_t *p_data, uint32_t size, const uint16_t *p_crc)
|
|
{
|
|
uint32_t i;
|
|
uint16_t crc = (p_crc == NULL) ? 0xFFFF : *p_crc;
|
|
|
|
for (i = 0; i < size; i++)
|
|
{
|
|
crc = (uint8_t)(crc >> 8) | (crc << 8);
|
|
crc ^= p_data[i];
|
|
crc ^= (uint8_t)(crc & 0xFF) >> 4;
|
|
crc ^= (crc << 8) << 4;
|
|
crc ^= ((crc & 0xFF) << 4) << 1;
|
|
}
|
|
return crc;
|
|
}
|
|
|
|
static bool dr_crc16_check(const uint8_t *p_data, uint32_t data_len, uint16_t expected_crc)
|
|
{
|
|
return (dr_crc16_compute(p_data, data_len, NULL) == expected_crc);
|
|
}
|
|
|
|
/* Extract trailing 2 bytes (Little-Endian) as expected CRC and verify. */
|
|
static bool dr_crc16_check_packet(const uint8_t *packet, uint32_t packet_len)
|
|
{
|
|
uint16_t expected_crc;
|
|
uint32_t data_len;
|
|
|
|
if (packet_len < 2)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
data_len = packet_len - 2;
|
|
expected_crc = (uint16_t)packet[packet_len - 2]
|
|
| ((uint16_t)packet[packet_len - 1] << 8);
|
|
|
|
return dr_crc16_check(packet, data_len, expected_crc);
|
|
}
|
|
|
|
|
|
/*==============================================================================
|
|
* Error response helper
|
|
*
|
|
* Packet: [err_tag 4B] [echo cmd_tag 4B] = 8 bytes = 4 words
|
|
* CRC16 is appended automatically by the tx_bin layer.
|
|
*============================================================================*/
|
|
static void dr_send_error(const char *err_tag, const char *cmd_tag)
|
|
{
|
|
if (g_plat.tx_bin)
|
|
{
|
|
uint8_t err_buf[8];
|
|
memcpy(&err_buf[0], err_tag, 4);
|
|
memcpy(&err_buf[4], cmd_tag, 4);
|
|
g_plat.tx_bin(err_buf, 4);
|
|
}
|
|
}
|
|
|
|
|
|
/*==============================================================================
|
|
* Packet parser - raw buffer -> ParsedCmd
|
|
*
|
|
* Error responses (rxs: / rxc:) are emitted directly from this function.
|
|
*============================================================================*/
|
|
static bool dr_parse_cmd(const uint8_t *buffer, uint8_t length, ParsedCmd *out)
|
|
{
|
|
uint8_t data_len;
|
|
|
|
/* Less than 4 bytes -> TAG cannot be identified */
|
|
if (length < 4)
|
|
{
|
|
dr_send_error("rxs:", "????");
|
|
if (g_plat.log && g_log_enable)
|
|
{
|
|
g_plat.log("[parser] too short (%u bytes) -> rxs:\r\n", length);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/* Extract TAG first (used in error echoes below) */
|
|
dr_copy_tag(buffer, out->tag);
|
|
|
|
/* CRC verification */
|
|
if (g_plat.crc_check)
|
|
{
|
|
if (length < 7)
|
|
{
|
|
dr_send_error("rxs:", out->tag);
|
|
if (g_plat.log && g_log_enable)
|
|
{
|
|
g_plat.log("[parser] CRC enabled but too short (%u) -> rxs:\r\n", length);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
if (!dr_crc16_check_packet(buffer, length))
|
|
{
|
|
dr_send_error("rxc:", out->tag);
|
|
if (g_plat.log && g_log_enable)
|
|
{
|
|
g_plat.log("[parser] CRC mismatch '%s' -> rxc:\r\n", out->tag);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
data_len = (uint8_t)(length - 4 - 2); /* strip TAG and CRC */
|
|
}
|
|
else
|
|
{
|
|
data_len = (uint8_t)(length - 4);
|
|
}
|
|
|
|
if (data_len > DR_MAX_DATA)
|
|
{
|
|
data_len = DR_MAX_DATA;
|
|
}
|
|
|
|
if (data_len > 0)
|
|
{
|
|
memcpy(out->data, buffer + 4, data_len);
|
|
}
|
|
out->data_len = data_len;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
/*==============================================================================
|
|
* Command dispatcher
|
|
*
|
|
* Lower-cases the parsed TAG and walks the command table to find a matching
|
|
* handler. Emits the matching error response on miss / disabled / NULL handler.
|
|
*============================================================================*/
|
|
static int dr_cmd_dispatch(const ParsedCmd *cmd)
|
|
{
|
|
uint16_t i;
|
|
char tag_lower[5];
|
|
|
|
if (m_cmd_table == NULL)
|
|
{
|
|
dr_send_error("rxn:", cmd->tag);
|
|
if (g_plat.log)
|
|
{
|
|
g_plat.log("[parser] table not initialized -> rxn:\n");
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/* Case-insensitive matching */
|
|
for (i = 0; i < 4 && cmd->tag[i]; i++)
|
|
{
|
|
tag_lower[i] = (cmd->tag[i] >= 'A' && cmd->tag[i] <= 'Z') ? (cmd->tag[i] + 32) : cmd->tag[i];
|
|
}
|
|
tag_lower[i] = '\0';
|
|
|
|
for (i = 0; i < m_cmd_count; i++)
|
|
{
|
|
if (dr_tag_eq(tag_lower, m_cmd_table[i].tag))
|
|
{
|
|
|
|
if (!m_cmd_table[i].enabled)
|
|
{
|
|
dr_send_error("rxd:", cmd->tag);
|
|
if (g_plat.log && g_log_enable)
|
|
{
|
|
g_plat.log("Command '%s' disabled -> rxd:\n", cmd->tag);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
if (m_cmd_table[i].handler == NULL)
|
|
{
|
|
dr_send_error("rxn:", cmd->tag);
|
|
if (g_plat.log)
|
|
{
|
|
g_plat.log("[parser] NULL handler for '%s' -> rxn:\n", cmd->tag);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
return m_cmd_table[i].handler(cmd);
|
|
}
|
|
}
|
|
|
|
/* TAG not found in table */
|
|
dr_send_error("rxx:", cmd->tag);
|
|
if (g_plat.log && g_log_enable)
|
|
{
|
|
g_plat.log("Unknown TAG '%s' -> rxx:\n", cmd->tag);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
/*==============================================================================
|
|
* Public API
|
|
*============================================================================*/
|
|
|
|
/* Called by cmd_table.c at boot to inject the command table. */
|
|
void dr_parser_init(const CmdEntry *table, uint16_t count)
|
|
{
|
|
m_cmd_table = table;
|
|
m_cmd_count = count;
|
|
}
|
|
|
|
/* Entry point - called from BLE NUS / UART receive callbacks.
|
|
*
|
|
* Returns:
|
|
* 1 = command processed successfully
|
|
* 0 = unknown TAG / disabled command (error response already sent)
|
|
* -1 = parse failure (rxs: / rxc: error response already sent)
|
|
*/
|
|
int dr_cmd_parser(const uint8_t *buf, uint8_t len)
|
|
{
|
|
ParsedCmd cmd;
|
|
|
|
if (!dr_parse_cmd(buf, len, &cmd))
|
|
{
|
|
if (g_plat.log) g_plat.log("[PARSER] PARSE FAIL\r\n");
|
|
return -1;
|
|
}
|
|
|
|
return dr_cmd_dispatch(&cmd);
|
|
}
|