//***********************************************************************************************************
//! @file        cli_protocol.c
//! @brief       Реализация протокола CLI с адресацией (board_id)
//! @version     v1.2.1
//! @date        20.04.2026
//!
//! @details     Протокол предназначен для тестирования и диагностики встроенного ПО устройств,
//!              подключенных к ПК через UART (COM-порт). ПК выступает мастером, устройство — подчинённым.
//!              Команды передаются в текстовом виде, данные кодируются в HEX.
//!              
//!              Расширение протокола: новые команды регистрируются через cli_register_command().
//!
//!              Максимальный размер поля данных: 64 байта.
//!
//! @note        Для работы необходимо наличие LMCAL, ring_buffer, а также tsp_dev_info из модуля tcp.h.
//***********************************************************************************************************

#include "cli_protocol.h"
#include "ring_buffer.h"
#include "lmcal.h"
#include "tcp.h"
#include "fpga_cmn.h"
#include "flash_mem.h"
#include "submodule.h"
#include <string.h>
#include <stdio.h>

// Включаем заголовок для доступа к регистрам USART (только для ожидания TC)
#include "gd32f4xx_usart.h"

//************************************ Локальные константы **************************************************
#define CLI_CMD_PREFIX          'T'     // Префикс команды
#define CLI_RESPONSE_PREFIX     't'     // Префикс успешного ответа
#define CLI_ERROR_PREFIX        'e'     // Префикс ошибки
#define CLI_MSG_TERMINATOR      '\r'    // Символ конца сообщения

// Макрос для преобразования ASCII HEX-символа в 4-битное значение (0-15), 0xFF при ошибке
#define HEX_CHAR_TO_NIBBLE(c)  (((c) >= '0' && (c) <= '9') ? ((c) - '0') :      \
                                ((c) >= 'A' && (c) <= 'F') ? ((c) - 'A' + 10) : \
                                ((c) >= 'a' && (c) <= 'f') ? ((c) - 'a' + 10) : 0xFF)

//************************************ Локальные переменные *************************************************
static ringbuf_t fifo_rx;                           // Кольцевой буфер приёма байт из UART
static u8 fifo_rx_buf[CLI_CMD_BUFFER_SIZE];         // Память для кольцевого буфера

static char cli_cmd_buffer[CLI_CMD_BUFFER_SIZE];    // Буфер для накопления строки команды
static u16 cli_cmd_index = 0;                       // Текущая длина строки в буфере

static bool cli_connected = false;                  // Флаг активного соединения (после успешной T00)
static u32 cli_last_activity = 0;                   // Время последней активности (тики)
static u32 (*cli_get_tick)(void) = NULL;            // Счётчик 

static cli_device_info_t cli_device_info;           // Информация об устройстве (имя, серийный номер, версия, board_id)

// Пароль хранится как байтовый массив с завершающим нулём (для удобства отладки)
static u8 cli_password[CLI_PASSWORD_MAX_LEN + 1];
static u8 cli_password_len = 0;                     // Фактическая длина пароля в байтах

// Таблица обработчиков команд. Индекс = код команды (0..255)
static cli_cmd_handler_t cmd_handlers[CLI_MAX_COMMANDS];

//************************************ Прототипы локальных функций ******************************************
static inline void byte_to_hex(u8 byte, char* hex);
static u8 hex_to_byte(const char* hex);
static void safe_strncpy(char* dst, const char* src, size_t dst_size);
static void send_response(u8 cmd, const u8* data, u8 len);
static void send_error(u8 cmd, u8 err);
static void uart_wait_tx_done(void);
static void handle_cmd_open(const u8* data, u8 len);
static void handle_cmd_close(const u8* data, u8 len);
static void handle_cmd_ping(const u8* data, u8 len);
static void handle_cmd_get_info(const u8* data, u8 len);
static void handle_cmd_read_mem(const u8* data, u8 len);
static void handle_cmd_write_mem(const u8* data, u8 len);
static void parse_and_execute(void);
static void uart_rx_cb(void);
static void register_builtin_commands(void);

//-----------------------------------------------------------------------------------------------------------
// Преобразование байта в два HEX-символа (старший и младший полубайт)
//-----------------------------------------------------------------------------------------------------------
static inline void byte_to_hex(u8 byte, char* hex)
{
    const char* d = "0123456789ABCDEF";
    hex[0] = d[(byte >> 4) & 0x0F];
    hex[1] = d[byte & 0x0F];
}

//-----------------------------------------------------------------------------------------------------------
// Преобразование двух HEX-символов в байт. Возвращает 0xFF при ошибке.
//-----------------------------------------------------------------------------------------------------------
static u8 hex_to_byte(const char* hex)
{
    u8 h = HEX_CHAR_TO_NIBBLE(hex[0]);
    u8 l = HEX_CHAR_TO_NIBBLE(hex[1]);
    return (h == 0xFF || l == 0xFF) ? 0xFF : (h << 4) | l;
}

//-----------------------------------------------------------------------------------------------------------
// Безопасное копирование строки с гарантированным завершающим нулём.
// Аналог strncpy, но всегда добавляет '\0' в конце буфера.
//-----------------------------------------------------------------------------------------------------------
static void safe_strncpy(char* dst, const char* src, size_t dst_size)
{
    if (dst == NULL || src == NULL || dst_size == 0) return;
    strncpy(dst, src, dst_size - 1);
    dst[dst_size - 1] = '\0';
}

//-----------------------------------------------------------------------------------------------------------
// Отправка строки через UART (публичная обёртка)
//-----------------------------------------------------------------------------------------------------------
void cli_send_string(const char* str)
{
    if (!str) return;
    while (*str) {
        lmcal_uart_write(CLI_UART_CHANNEL, (u16)*str++);
    }
    // Ждём завершения передачи, чтобы не смешивать вывод
    uart_wait_tx_done();
}

//-----------------------------------------------------------------------------------------------------------
// Ожидание завершения передачи UART (флаг TC)
// Используем прямой доступ к регистрам, т.к. LMCAL не предоставляет такой функции.
//-----------------------------------------------------------------------------------------------------------
static void uart_wait_tx_done(void)
{
    #if CLI_UART_CHANNEL == LMCAL_UART_CHANNEL0
        #define CLI_USART USART5
    #elif CLI_UART_CHANNEL == LMCAL_UART_CHANNEL1
        #define CLI_USART USART0
    #else
        #define CLI_USART USART0
    #endif
    volatile u32 timeout = 100000;
    while (usart_flag_get(CLI_USART, USART_FLAG_TC) == RESET && --timeout);

}

//-----------------------------------------------------------------------------------------------------------
// Формирование и отправка успешного ответа на команду.
// Формат: t[cmd][board_id][data...]\r
//-----------------------------------------------------------------------------------------------------------
static void send_response(u8 cmd, const u8* data, u8 len)
{
    u8 buf[CLI_CMD_BUFFER_SIZE];
    u16 idx = 0;
    buf[idx++] = CLI_RESPONSE_PREFIX;                       // 't'
    byte_to_hex(cmd, (char*)&buf[idx]); idx += 2;          // код команды
    byte_to_hex(cli_device_info.board_id, (char*)&buf[idx]); idx += 2; // board_id
    for (u8 i = 0; i < len; i++) {
        byte_to_hex(data[i], (char*)&buf[idx]);             // данные в HEX
        idx += 2;
    }
    buf[idx++] = CLI_MSG_TERMINATOR;                        // '\r'
    for (u16 i = 0; i < idx; i++) {
        lmcal_uart_write(CLI_UART_CHANNEL, buf[i]);
    }
    uart_wait_tx_done();   // гарантируем, что ответ отправлен полностью
}

//-----------------------------------------------------------------------------------------------------------
// Формирование и отправка сообщения об ошибке.
// Формат: e[cmd][board_id][err]\r
//-----------------------------------------------------------------------------------------------------------
static void send_error(u8 cmd, u8 err)
{
    u8 buf[8];
    u16 idx = 0;
    buf[idx++] = CLI_ERROR_PREFIX;                          // 'e'
    byte_to_hex(cmd, (char*)&buf[idx]); idx += 2;
    byte_to_hex(cli_device_info.board_id, (char*)&buf[idx]); idx += 2;
    byte_to_hex(err, (char*)&buf[idx]); idx += 2;           // код ошибки
    buf[idx++] = CLI_MSG_TERMINATOR;
    for (u16 i = 0; i < idx; i++) {
        lmcal_uart_write(CLI_UART_CHANNEL, buf[i]);
    }
    uart_wait_tx_done();
}

//-----------------------------------------------------------------------------------------------------------
// Обработчик команды открытия соединения (CLI_CMD_OPEN = 0x00)
// Данные могут содержать пароль. Если пароль задан, проверяем его.
//-----------------------------------------------------------------------------------------------------------
static void handle_cmd_open(const u8* data, u8 len)
{
    (void)len;

    if (cli_connected) {
        send_error(CLI_CMD_OPEN, CLI_ERR_INTERNAL);
        return;
    }
    
    if (len != cli_password_len || memcmp(data, cli_password, len) != 0) {
        send_error(CLI_CMD_OPEN, CLI_ERR_INTERNAL);
        return;
    }

    cli_connected = true;
    if (cli_get_tick) cli_last_activity = cli_get_tick();
    send_response(CLI_CMD_OPEN, NULL, 0);
}

//-----------------------------------------------------------------------------------------------------------
// Обработчик команды закрытия соединения (CLI_CMD_CLOSE = 0x01)
//-----------------------------------------------------------------------------------------------------------
static void handle_cmd_close(const u8* data, u8 len)
{
    (void)data; (void)len;
    cli_connected = false;
    send_response(CLI_CMD_CLOSE, NULL, 0);
    cli_cmd_index = 0;
    memset(cli_cmd_buffer, 0, sizeof(cli_cmd_buffer));
}

//-----------------------------------------------------------------------------------------------------------
// Обработчик команды проверки соединения (CLI_CMD_PING = 0x02)
// Возвращает эхо переданных данных.
//-----------------------------------------------------------------------------------------------------------
static void handle_cmd_ping(const u8* data, u8 len)
{
    if (!cli_connected) {
        send_error(CLI_CMD_PING, CLI_ERR_INTERNAL);
        return;
    }
    send_response(CLI_CMD_PING, data, len);
}

//-----------------------------------------------------------------------------------------------------------
// Обработчик команды получения информации об устройстве (CLI_CMD_GET_INFO = 0x0A)
// Формат ответа: board_id (1 байт), длина имени, имя, длина серийного, серийный,
//                длина версии, версия (ASCII).
//-----------------------------------------------------------------------------------------------------------
static void handle_cmd_get_info(const u8* data, u8 len)
{
    (void)data; (void)len;
    if (!cli_connected) {
        send_error(CLI_CMD_GET_INFO, CLI_ERR_INTERNAL);
        return;
    }
    u8 buf[CLI_DATA_MAX_SIZE];
    u16 idx = 0;
    u8 str_len;

    buf[idx++] = cli_device_info.board_id;

    str_len = strlen(cli_device_info.name);
    if (str_len > 31) str_len = 31;
    buf[idx++] = str_len;
    memcpy(&buf[idx], cli_device_info.name, str_len);
    idx += str_len;

    str_len = strlen(cli_device_info.serial);
    if (str_len > 15) str_len = 15;
    buf[idx++] = str_len;
    memcpy(&buf[idx], cli_device_info.serial, str_len);
    idx += str_len;

    char ver[16];
    snprintf(ver, sizeof(ver), "%lu", tsp_dev_info.mcu_ver);
    str_len = strlen(ver);
    if (str_len > 15) str_len = 15;
    buf[idx++] = str_len;
    memcpy(&buf[idx], ver, str_len);
    idx += str_len;

    send_response(CLI_CMD_GET_INFO, buf, idx);
}

//-----------------------------------------------------------------------------------------------------------
// Обработчик команды чтения памяти (CLI_CMD_READ_MEM = 0x14)
// Данные: 4 байта адреса (big-endian), 1 байт размера (1-64)
// Исправлена проверка границ: теперь учитывается размер
//-----------------------------------------------------------------------------------------------------------
static void handle_cmd_read_mem(const u8* data, u8 len)
{
    if (!cli_connected) {
        send_error(CLI_CMD_READ_MEM, CLI_ERR_INTERNAL);
        return;
    }
    if (len != 5) {
        send_error(CLI_CMD_READ_MEM, CLI_ERR_INVALID_ARG);
        return;
    }
    
    u32 addr = 0;
    for (u8 i = 0; i < 4; i++) addr = (addr << 8) | data[i];
    u8 size = data[4];
    
    if (size == 0 || size > CLI_DATA_MAX_SIZE) {
        send_error(CLI_CMD_READ_MEM, CLI_ERR_INVALID_ARG);
        return;
    }

    // Проверка на переполнение адреса
    u32 end_addr = addr + size;
    if (end_addr < addr) {
        send_error(CLI_CMD_READ_MEM, CLI_ERR_INTERNAL);
        return;
    }

    // Проверка допустимых диапазонов (GD32F450: Flash 2MB, SRAM 192KB)
    bool valid = false;
    
    if ((addr >= 0x08000000) && (end_addr <= 0x081FFFFF)) {
        valid = true;   // Flash (банк 0 + банк 1 = 2 МБ)
    } else if ((addr >= 0x20000000) && (end_addr <= 0x2002FFFF)) {
        valid = true;   // SRAM (192 КБ)
    }

    if (valid) {
        u8 buf[CLI_DATA_MAX_SIZE];
        for (u8 i = 0; i < size; i++) {
            buf[i] = *(volatile u8*)(addr + i);
        }
        send_response(CLI_CMD_READ_MEM, buf, size);
    } else {
        printf("READ_MEM error: addr=0x%08X, size=%d, end=0x%08X\n", addr, size, end_addr);
        send_error(CLI_CMD_READ_MEM, CLI_ERR_INTERNAL);
    }
}

//-----------------------------------------------------------------------------------------------------------
// Обработчик команды записи памяти (CLI_CMD_WRITE_MEM = 0x15)
// Данные: 4 байта адреса, затем записываемые байты (1-64)
// Запись разрешена только в SRAM (0x20000000-0x2001FFFF) с учётом размера
//-----------------------------------------------------------------------------------------------------------
static void handle_cmd_write_mem(const u8* data, u8 len)
{
    if (!cli_connected) {
        send_error(CLI_CMD_WRITE_MEM, CLI_ERR_INTERNAL);
        return;
    }
    if (len < 5) {
        send_error(CLI_CMD_WRITE_MEM, CLI_ERR_INVALID_ARG);
        return;
    }
    
    u32 addr = 0;
    for (u8 i = 0; i < 4; i++) addr = (addr << 8) | data[i];
    u8 size = len - 4;
    
    if (size > CLI_DATA_MAX_SIZE) {
        send_error(CLI_CMD_WRITE_MEM, CLI_ERR_INVALID_ARG);
        return;
    }

    // Проверка на переполнение
    u32 end_addr = addr + size;
    if (end_addr < addr) {
        send_error(CLI_CMD_WRITE_MEM, CLI_ERR_INTERNAL);
        return;
    }

    // Запись разрешена только в SRAM
    if ((addr >= 0x20000000) && (end_addr <= 0x2002FFFF)) {
        for (u8 i = 0; i < size; i++) {
            *(volatile u8*)(addr + i) = data[4 + i];
        }
        send_response(CLI_CMD_WRITE_MEM, NULL, 0);
    } else {
        printf("WRITE_MEM error: addr=0x%08X, size=%d, end=0x%08X\n", addr, size, end_addr);
        send_error(CLI_CMD_WRITE_MEM, CLI_ERR_INTERNAL);
    }
}

//-----------------------------------------------------------------------------------------------------------
// Разбор принятой строки и вызов соответствующего обработчика.
//-----------------------------------------------------------------------------------------------------------
static void parse_and_execute(void)
{
    char* buf = cli_cmd_buffer;
    u16 len = cli_cmd_index;

    if (len < 6) { cli_cmd_index = 0; return; }
    if (buf[0] != CLI_CMD_PREFIX) { cli_cmd_index = 0; return; }
    if (buf[len - 1] != CLI_MSG_TERMINATOR) return;

    u8 cmd = hex_to_byte(&buf[1]);
    if (cmd == 0xFF) { cli_cmd_index = 0; return; }

    u8 board_id = hex_to_byte(&buf[3]);
    if (board_id == 0xFF) { cli_cmd_index = 0; return; }
    if (board_id != cli_device_info.board_id) { cli_cmd_index = 0; return; }

    u16 data_start = 5;
    u16 chars = len - 1 - data_start;
    u8 data[CLI_DATA_MAX_SIZE + 4];
    u8 data_len = 0;
    if (chars > 0) {
        if (chars % 2 != 0) { send_error(cmd, CLI_ERR_INVALID_ARG); cli_cmd_index = 0; return; }
        data_len = chars / 2;
        if (data_len > CLI_DATA_MAX_SIZE + 4) { send_error(cmd, CLI_ERR_INVALID_ARG); cli_cmd_index = 0; return; }
        for (u8 i = 0; i < data_len; i++) {
            data[i] = hex_to_byte(&buf[data_start + i * 2]);
            if (data[i] == 0xFF) { send_error(cmd, CLI_ERR_INVALID_ARG); cli_cmd_index = 0; return; }
        }
    }

    if (cli_get_tick) cli_last_activity = cli_get_tick();

    if (cmd_handlers[cmd] != NULL) {
        cmd_handlers[cmd](data, data_len);
    } else {
        send_error(cmd, CLI_ERR_UNKNOWN_CMD);
    }
    cli_cmd_index = 0;
}

//-----------------------------------------------------------------------------------------------------------
// Колбек прерывания по приёму байта из UART.
//-----------------------------------------------------------------------------------------------------------
static void uart_rx_cb(void)
{
    u16 rx = lmcal_uart_read(CLI_UART_CHANNEL);
    u8 ch = (u8)(rx & 0xFF);
    rb_add_item(&fifo_rx, &ch);
}

//-----------------------------------------------------------------------------------------------------------
// Регистрация встроенных команд протокола.
//-----------------------------------------------------------------------------------------------------------
static void register_builtin_commands(void)
{
    const cli_command_t builtin[] = {
        { CLI_CMD_OPEN,      handle_cmd_open },
        { CLI_CMD_CLOSE,     handle_cmd_close },
        { CLI_CMD_PING,      handle_cmd_ping },
        { CLI_CMD_GET_INFO,  handle_cmd_get_info },
        { CLI_CMD_READ_MEM,  handle_cmd_read_mem },
        { CLI_CMD_WRITE_MEM, handle_cmd_write_mem },
    };
    for (size_t i = 0; i < sizeof(builtin)/sizeof(builtin[0]); i++) {
        cli_register_command(&builtin[i]);
    }
}

//************************************ Публичные функции ****************************************************

//-----------------------------------------------------------------------------------------------------------
fun_res_t cli_protocol_init(void)
{
    fifo_rx.item_size = sizeof(u8);
    rb_init(&fifo_rx, fifo_rx_buf, sizeof(fifo_rx_buf));
    rb_clear(&fifo_rx);
    lmcal_uart_set_callback(CLI_UART_CHANNEL, CB_INT_RX_CALLBACK, uart_rx_cb);

    cli_cmd_index = 0;
    cli_connected = false;
    cli_last_activity = 0;
    memset(cli_cmd_buffer, 0, sizeof(cli_cmd_buffer));

    safe_strncpy(cli_device_info.name, "CLI Device", sizeof(cli_device_info.name));
    safe_strncpy(cli_device_info.serial, "00000000", sizeof(cli_device_info.serial));
    safe_strncpy(cli_device_info.version, "1.0.0", sizeof(cli_device_info.version));
    cli_device_info.board_id = 0;

    memset(cmd_handlers, 0, sizeof(cmd_handlers));
    register_builtin_commands();

    const u8 default_pass[] = CLI_DEFAULT_PASSWORD_BYTES;
    cli_protocol_set_password(default_pass, sizeof(default_pass));

    cli_send_string("\r\n********** LTA PROTOCOL ENABLED **********\r\n");
    cli_send_string("|>  T000131333537           - open with password '1357' (hex bytes 0x31,0x33,0x35,0x37)\r\n");
    cli_send_string("|>  T0101                           - close connection\r\n");
    cli_send_string("|>  T0201AABB                 - ping with data 0xAA,0xBB\r\n");
    cli_send_string("|>  T0A01                          - get device info\r\n");
    cli_send_string("|>  T14010800000004       - read 4 bytes from 0x08000000\r\n");
    cli_send_string("|>  T14012000000004       - read 4 bytes from 0x20000000\r\n");
    cli_send_string("|>  T150120000000AABB - write 0xAA,0xBB to SRAM 0x20000000\r\n");
    cli_send_string("******************************************************\r\n");
    cli_send_string("|> ");
    return ERR_OK;
}

//-----------------------------------------------------------------------------------------------------------
fun_res_t cli_protocol_deinit(void)
{
    lmcal_uart_set_callback(CLI_UART_CHANNEL, CB_INT_RX_CALLBACK, NULL);
    cli_connected = false;
    return ERR_OK;
}

//-----------------------------------------------------------------------------------------------------------
void cli_protocol_process(void)
{
    static enum {
        WAIT_CMD,       // Ожидание первого символа команды
        RECEIVING,      // Приём символов команды
        EXPECT_NL       // Ожидание \n после \r
    } state = WAIT_CMD;

    while (rb_get_items_qty(&fifo_rx) > 0) {
        u8 ch;
        if (rb_get_item(&fifo_rx, &ch) != RB_RES_OK) continue;

        switch (state) {
            case WAIT_CMD:
                if (ch == ' ' || ch == '\t' || ch == '\r' || ch == '\n') continue;
                cli_cmd_index = 0;
                cli_cmd_buffer[cli_cmd_index++] = ch;
                state = RECEIVING;
                break;
            case RECEIVING:
                if (ch == '\r' || ch == '\n') {

                    while (cli_cmd_index > 0 && (cli_cmd_buffer[cli_cmd_index - 1] == ' ' 
                        || cli_cmd_buffer[cli_cmd_index - 1] == '\t')) 
                            {
                                cli_cmd_index--;
                            }

                    if (cli_cmd_index > 0) {
                        cli_cmd_buffer[cli_cmd_index] = CLI_MSG_TERMINATOR;
                        cli_cmd_index++;
                        parse_and_execute();
                        cli_send_string("|> ");
                    }
                    state = EXPECT_NL;
                } else {
                    if (cli_cmd_index < CLI_CMD_BUFFER_SIZE - 1) {
                        cli_cmd_buffer[cli_cmd_index++] = ch;
                    } else {
                        cli_cmd_index = 0;
                        state = WAIT_CMD;
                    }
                }
                break;
            case EXPECT_NL:
                if (ch == '\n') {
                    state = WAIT_CMD;
                } else {
                    cli_cmd_index = 0;
                    cli_cmd_buffer[cli_cmd_index++] = ch;
                    state = RECEIVING;
                }
                break;
        }
    }

    // Таймаут соединения (работает только если cli_get_tick != NULL)
    if (cli_connected && cli_get_tick) {
        if ((cli_get_tick() - cli_last_activity) > CLI_CONNECTION_TIMEOUT_MS) {
            cli_connected = false;
        }
    }
}

//-----------------------------------------------------------------------------------------------------------
fun_res_t cli_protocol_set_device_info(const char* name, const char* serial, const char* ver, u8 board_id)
{
    if (name)   safe_strncpy(cli_device_info.name, name, sizeof(cli_device_info.name));
    if (serial) safe_strncpy(cli_device_info.serial, serial, sizeof(cli_device_info.serial));
    if (ver)    safe_strncpy(cli_device_info.version, ver, sizeof(cli_device_info.version));
    cli_device_info.board_id = board_id;
    return ERR_OK;
}

//-----------------------------------------------------------------------------------------------------------
fun_res_t cli_protocol_set_board_id(u8 board_id)
{
    cli_device_info.board_id = board_id;
    return ERR_OK;
}

//-----------------------------------------------------------------------------------------------------------
u8 cli_protocol_get_board_id(void)
{
    return cli_device_info.board_id;
}

//-----------------------------------------------------------------------------------------------------------
void cli_protocol_set_password(const u8* pass, u8 len)
{
    if (len > CLI_PASSWORD_MAX_LEN) len = CLI_PASSWORD_MAX_LEN;
    memcpy(cli_password, pass, len);
    cli_password_len = len;
    cli_password[len] = '\0';
}

//-----------------------------------------------------------------------------------------------------------
bool cli_protocol_verify_password(const u8* pass, u8 len)
{
    return (len == cli_password_len) && (memcmp(pass, cli_password, len) == 0);
}

//-----------------------------------------------------------------------------------------------------------
fun_res_t cli_register_command(const cli_command_t* cmd)
{
    if (cmd == NULL || cmd->handler == NULL) {
        return ERR_INVALID_ARGUMENT;
    }
    if ((int)cmd->code >= CLI_MAX_COMMANDS) {
        return ERR_INVALID_ARGUMENT;
    }
    if (cmd_handlers[cmd->code] != NULL) {
        return ERR_RUNTIME_ERROR;
    }
    cmd_handlers[cmd->code] = cmd->handler;
    return ERR_OK;
}
