From db557305da53536b93a373b8cb8233aae62773b6 Mon Sep 17 00:00:00 2001 From: ttwards <12411711@mail.sustech.edu.cn> Date: Mon, 6 Jul 2026 17:51:19 +0800 Subject: [PATCH 1/6] refactor(motor): split controller API header --- Documents/api-reference.md | 4 + Documents/modules.md | 1 + Documents/motor/api.md | 3 + include/zephyr/drivers/motor.h | 556 +------------------- include/zephyr/drivers/motor/controller.h | 587 ++++++++++++++++++++++ 5 files changed, 596 insertions(+), 555 deletions(-) create mode 100644 include/zephyr/drivers/motor/controller.h diff --git a/Documents/api-reference.md b/Documents/api-reference.md index 9495838..128cc22 100644 --- a/Documents/api-reference.md +++ b/Documents/api-reference.md @@ -10,6 +10,8 @@ 头文件：`include/zephyr/drivers/motor.h` Motor API 将不同厂商的旋转电机统一为 `set/get/control` 三类应用操作。 +控制器类型、参数、选择与内置计算辅助位于 +`include/zephyr/drivers/motor/controller.h`；`motor.h` 会继续包含该头文件以保持兼容。 #### 类型 @@ -55,6 +57,8 @@ Motor API 将不同厂商的旋转电机统一为 `set/get/control` 三类应用 #### 控制器函数 +头文件：`include/zephyr/drivers/motor/controller.h` + | 接口 | 说明 | | --- | --- | | `motor_controller_update(data, cfg, input, output)` | 执行一次控制器更新。 | diff --git a/Documents/modules.md b/Documents/modules.md index 9354fd3..1a443ea 100644 --- a/Documents/modules.md +++ b/Documents/modules.md @@ -39,6 +39,7 @@ Zephyr 设备模型、devicetree 与 Kconfig 组合这些模块。 路径： - `include/zephyr/drivers/motor.h` +- `include/zephyr/drivers/motor/controller.h` - `drivers/motor/` - `dts/bindings/motor/` - `dts/bindings/motor-controller/` diff --git a/Documents/motor/api.md b/Documents/motor/api.md index 8b52f05..3dca971 100644 --- a/Documents/motor/api.md +++ b/Documents/motor/api.md @@ -1,5 +1,8 @@ # 电机驱动 API +应用入口头文件是 `include/zephyr/drivers/motor.h`。控制器类型、参数、选择函数和内置计算辅助 +位于 `include/zephyr/drivers/motor/controller.h`；`motor.h` 会继续包含该头文件以保持既有代码兼容。 + 电机接口分成三层语义： - `motor_setpoint_t`: 用户下发的目标设定。 diff --git a/include/zephyr/drivers/motor.h b/include/zephyr/drivers/motor.h index 9690a08..336ea9e 100644 --- a/include/zephyr/drivers/motor.h +++ b/include/zephyr/drivers/motor.h @@ -33,6 +33,7 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -50,57 +51,6 @@ extern "C" { #define RAD2DEG(x) 57.2957795131f * x #endif -#define MOTOR_CONTROLLER_ID_AUTO UINT8_MAX -#define MOTOR_CONTROLLER_MAX 4 -#define MOTOR_CONTROLLER_PARAM_MAX 2 - -/** - * @brief 电机工作模式枚举 - * - * MIT: MIT模式 - * PV: 位置-速度控制 - * VO: 单控制量控制 - */ -enum motor_mode { - MIT = 0, - PV = 1, - VO = 2, -}; - -enum motor_target { - MOTOR_TARGET_NONE = 0, - MOTOR_TARGET_TORQUE = 1, - MOTOR_TARGET_SPEED = 2, - MOTOR_TARGET_POSITION = 3, -}; - -enum motor_controller_select { - MOTOR_CONTROLLER_DEFAULT = 0, - MOTOR_CONTROLLER_BY_ID = 1, -}; - -enum motor_state_flags { - MOTOR_STATE_POSITION = BIT(0), - MOTOR_STATE_SPEED = BIT(1), - MOTOR_STATE_TORQUE = BIT(2), - MOTOR_STATE_CURRENT = BIT(3), - MOTOR_STATE_TEMPERATURE = BIT(4), -}; - -enum motor_output_type { - MOTOR_OUTPUT_NONE = 0, - MOTOR_OUTPUT_TORQUE = 1, - MOTOR_OUTPUT_CURRENT = 2, - MOTOR_OUTPUT_SPEED = 3, - MOTOR_OUTPUT_POSITION = 4, - MOTOR_OUTPUT_NATIVE = 5, -}; - -enum motor_controller_output_field { - MOTOR_CONTROLLER_OUTPUT_VALUE = BIT(0), - MOTOR_CONTROLLER_OUTPUT_SPEED = BIT(1), -}; - /** * @brief 电机控制命令枚举 */ @@ -112,132 +62,6 @@ enum motor_cmd { CLEAR_ERROR, }; -typedef float (*motor_slip_cb_t)(const struct device *dev); - -struct motor_controller_config; -struct motor_controller_data; -struct motor_controller_input; -struct motor_controller_output; - -struct motor_controller_info { - uint8_t id; - enum motor_mode mode; - enum motor_target target; - enum motor_output_type output; - uint32_t required_states; - char name[32]; -}; - -typedef struct motor_controller_info motor_controller_info_t; - -struct motor_controller_params { - float k_p; - float k_i; - float k_d; - float integral_limit; - float output_limit; - float output_offset; - float detri_lpf; -}; - -typedef int (*motor_controller_update_t)(struct motor_controller_data *data, - const struct motor_controller_config *cfg, - const struct motor_controller_input *input, - struct motor_controller_output *output); -typedef int (*motor_controller_get_params_t)(const struct motor_controller_config *cfg, - uint8_t index, struct motor_controller_params *params); -typedef void (*motor_controller_reset_t)(struct motor_controller_data *data); - -struct motor_controller_api { - motor_controller_update_t update; - motor_controller_get_params_t get_params; - motor_controller_reset_t reset; -}; - -struct motor_controller_config { - motor_controller_info_t info; - const struct motor_controller_api *api; - uint8_t param_count; - struct motor_controller_params params[MOTOR_CONTROLLER_PARAM_MAX]; -}; - -struct motor_controller_stage_data { - float err_integral; - float err_prev; - float err_derivate; - uint32_t prev_time; -}; - -struct motor_controller_data { - struct motor_controller_stage_data stages[MOTOR_CONTROLLER_PARAM_MAX]; -}; - -struct motor_driver_config { - /** Physical device */ - const struct device *phy; - /** motor ID */ - uint8_t id; - /** CAN TX ID */ - int tx_id; - /** CAN RX ID */ - int rx_id; - /** Configured controllers */ - struct motor_controller_config controllers[MOTOR_CONTROLLER_MAX]; -}; - -struct motor_setpoint { - float angle; - float rpm; - float torque; - - float speed_limit[2]; - float torque_limit[2]; - - enum motor_mode mode; - enum motor_target target; - enum motor_controller_select controller_select; - uint8_t controller_id; -}; - -typedef struct motor_setpoint motor_setpoint_t; - -struct motor_status { - float angle; - float rpm; - float torque; - float temperature; - float sum_angle; - - float speed_limit[2]; - float torque_limit[2]; - - enum motor_mode mode; - enum motor_target target; - uint8_t controller_id; - /* Communication reachability, independent from motor enable state. */ - bool online; - /* Actual motor enable state when feedback reports it; otherwise requested enable state. */ - bool enabled; - int error; -}; - -typedef struct motor_status motor_status_t; - -struct motor_controller_input { - motor_status_t status; - motor_setpoint_t setpoint; - float position_error; - bool has_position_error; - uint32_t timestamp; -}; - -struct motor_controller_output { - enum motor_output_type type; - uint32_t fields; - float value; - float speed; -}; - struct motor_link_state { /* Communication reachability, updated by replies or periodic reports/timeouts. */ bool online; @@ -264,206 +88,6 @@ struct motor_driver_data { struct motor_controller_data controllers[MOTOR_CONTROLLER_MAX]; }; -static inline float motor_controller_clamp(float value, float min, float max) -{ - if (value > max) { - return max; - } - if (value < min) { - return min; - } - return value; -} - -static inline float motor_controller_stage_update(struct motor_controller_stage_data *data, - const struct motor_controller_params *params, - float error, uint32_t timestamp) -{ - float output; - - if (data == NULL || params == NULL) { - return 0; - } - if (data->prev_time == 0 || timestamp == 0) { - data->prev_time = timestamp; - data->err_prev = error; - output = isnan(params->k_p) ? params->output_offset - : params->k_p * error + params->output_offset; - if (params->output_limit != 0) { - output = motor_controller_clamp(output, -params->output_limit, - params->output_limit); - } - return output; - } - - float delta_us = k_cyc_to_us_near32(timestamp - data->prev_time); - if (delta_us < 1.0f) { - return 0; - } - - if (!isnan(params->k_i)) { - data->err_integral += error * delta_us / 1000000.0f; - if (params->integral_limit != 0) { - data->err_integral = - motor_controller_clamp(data->err_integral, -params->integral_limit, - params->integral_limit); - } - } - - float raw_derivate = (error - data->err_prev) / delta_us * 1000000.0f; - if (isnan(params->detri_lpf)) { - data->err_derivate = raw_derivate; - } else { - data->err_derivate = params->detri_lpf * data->err_derivate + - (1.0f - params->detri_lpf) * raw_derivate; - } - - output = params->output_offset; - if (!isnan(params->k_p)) { - output += params->k_p * error; - } - if (!isnan(params->k_i)) { - output += params->k_i * data->err_integral; - } - if (!isnan(params->k_d)) { - output += params->k_d * data->err_derivate; - } - if (params->output_limit != 0) { - output = - motor_controller_clamp(output, -params->output_limit, params->output_limit); - } - - data->err_prev = error; - data->prev_time = timestamp; - - return output; -} - -static inline int motor_builtin_controller_update(struct motor_controller_data *data, - const struct motor_controller_config *cfg, - const struct motor_controller_input *input, - struct motor_controller_output *output) -{ - float target_speed; - float target_torque; - - if (data == NULL || cfg == NULL || input == NULL || output == NULL) { - return -EINVAL; - } - - *output = (struct motor_controller_output){ - .type = cfg->info.output, - .fields = 0, - .value = 0, - .speed = 0, - }; - - if (cfg->info.mode == PV && cfg->info.target == MOTOR_TARGET_POSITION) { - if (cfg->param_count < 2) { - return -EINVAL; - } - float position_error = input->has_position_error - ? input->position_error - : input->setpoint.angle - input->status.sum_angle; - - target_speed = motor_controller_stage_update(&data->stages[0], &cfg->params[0], - position_error, input->timestamp); - if (input->setpoint.speed_limit[0] < input->setpoint.speed_limit[1]) { - target_speed = - motor_controller_clamp(target_speed, input->setpoint.speed_limit[0], - input->setpoint.speed_limit[1]); - } - target_torque = motor_controller_stage_update(&data->stages[1], &cfg->params[1], - target_speed - input->status.rpm, - input->timestamp); - target_torque += input->setpoint.torque; - output->type = MOTOR_OUTPUT_TORQUE; - output->fields = MOTOR_CONTROLLER_OUTPUT_VALUE | MOTOR_CONTROLLER_OUTPUT_SPEED; - output->value = target_torque; - output->speed = target_speed; - return 0; - } - - if (cfg->info.mode == VO && cfg->info.target == MOTOR_TARGET_SPEED) { - if (cfg->param_count < 1) { - return -EINVAL; - } - target_torque = motor_controller_stage_update( - &data->stages[0], &cfg->params[0], input->setpoint.rpm - input->status.rpm, - input->timestamp); - target_torque += input->setpoint.torque; - output->type = MOTOR_OUTPUT_TORQUE; - output->fields = MOTOR_CONTROLLER_OUTPUT_VALUE; - output->value = target_torque; - return 0; - } - - if (cfg->info.mode == VO && cfg->info.target == MOTOR_TARGET_TORQUE) { - output->type = MOTOR_OUTPUT_TORQUE; - output->fields = MOTOR_CONTROLLER_OUTPUT_VALUE; - output->value = input->setpoint.torque; - return 0; - } - - return -ENOTSUP; -} - -static inline int motor_builtin_controller_get_params(const struct motor_controller_config *cfg, - uint8_t index, - struct motor_controller_params *params) -{ - if (cfg == NULL || params == NULL || index >= cfg->param_count || - index >= MOTOR_CONTROLLER_PARAM_MAX) { - return -EINVAL; - } - - *params = cfg->params[index]; - return 0; -} - -static inline void motor_builtin_controller_reset(struct motor_controller_data *data) -{ - if (data != NULL) { - memset(data, 0, sizeof(*data)); - } -} - -static const struct motor_controller_api motor_builtin_controller_api = { - .update = motor_builtin_controller_update, - .get_params = motor_builtin_controller_get_params, - .reset = motor_builtin_controller_reset, -}; - -static inline int motor_controller_update(struct motor_controller_data *data, - const struct motor_controller_config *cfg, - const struct motor_controller_input *input, - struct motor_controller_output *output) -{ - if (cfg == NULL || cfg->api == NULL || cfg->api->update == NULL) { - return -ENOSYS; - } - - return cfg->api->update(data, cfg, input, output); -} - -static inline int motor_controller_get_params(const struct motor_controller_config *cfg, - uint8_t index, struct motor_controller_params *params) -{ - if (cfg == NULL || cfg->api == NULL || cfg->api->get_params == NULL) { - return -ENOSYS; - } - - return cfg->api->get_params(cfg, index, params); -} - -static inline void motor_controller_reset(struct motor_controller_data *data, - const struct motor_controller_config *cfg) -{ - if (cfg != NULL && cfg->api != NULL && cfg->api->reset != NULL) { - cfg->api->reset(data); - } -} - #define motor_set_angle(dev, _angle) \ motor_set(dev, &(motor_setpoint_t){ \ .angle = _angle, .mode = PV, .target = MOTOR_TARGET_POSITION}) @@ -603,186 +227,8 @@ static inline void z_impl_motor_control(const struct device *dev, enum motor_cmd api->motor_control(dev, cmd); } -static inline bool motor_controller_info_valid(const motor_controller_info_t *info) -{ - return info != NULL && info->name[0] != '\0'; -} - -static inline int motor_get_controller_count(const struct device *dev) -{ - const struct motor_driver_config *cfg = dev->config; - int count = 0; - - for (int i = 0; i < ARRAY_SIZE(cfg->controllers); i++) { - if (!motor_controller_info_valid(&cfg->controllers[i].info)) { - break; - } - count++; - } - - return count; -} - -static inline int motor_get_controller_info(const struct device *dev, uint8_t index, - motor_controller_info_t *info) -{ - const struct motor_driver_config *cfg = dev->config; - - if (info == NULL || index >= ARRAY_SIZE(cfg->controllers) || - !motor_controller_info_valid(&cfg->controllers[index].info)) { - return -EINVAL; - } - - *info = cfg->controllers[index].info; - return 0; -} - -static inline int motor_resolve_controller(const struct device *dev, motor_setpoint_t *setpoint, - motor_controller_info_t *info) -{ - if (setpoint == NULL) { - return -EINVAL; - } - if (setpoint->target == MOTOR_TARGET_NONE) { - return 0; - } - - if (setpoint->controller_select == MOTOR_CONTROLLER_BY_ID) { - motor_controller_info_t selected = {0}; - motor_controller_info_t *selected_info = info != NULL ? info : &selected; - int ret = motor_get_controller_info(dev, setpoint->controller_id, selected_info); - - if (ret < 0) { - return ret; - } - if (selected_info->mode != setpoint->mode || - selected_info->target != setpoint->target) { - return -EINVAL; - } - return 0; - } - - for (uint8_t i = 0; i < motor_get_controller_count(dev); i++) { - motor_controller_info_t candidate = {0}; - - if (motor_get_controller_info(dev, i, &candidate) < 0) { - continue; - } - if (candidate.mode == setpoint->mode && candidate.target == setpoint->target) { - setpoint->controller_id = i; - if (info != NULL) { - *info = candidate; - } - return 0; - } - } - - return -ENOTSUP; -} - #define DT_GET_CANPHY(node_id) DEVICE_DT_GET(DT_PHANDLE(node_id, can_channel)) -#define MOTOR_DT_CONTROLLER_PARAM_VALUE(node_id, prop, fallback_prop, default_value) \ - COND_CODE_1(DT_NODE_HAS_PROP(node_id, prop), (DT_STRING_UNQUOTED(node_id, prop)), \ - (DT_STRING_UNQUOTED_OR(node_id, fallback_prop, default_value))) - -#define MOTOR_DT_CONTROLLER_PARAMS(node_id, kp_prop, ki_prop, kd_prop, i_prop, out_prop, lpf_prop, \ - offset_prop, i_fallback, out_fallback, lpf_fallback, \ - offset_fallback) \ - { \ - .k_p = DT_STRING_UNQUOTED_OR(node_id, kp_prop, 0), \ - .integral_limit = MOTOR_DT_CONTROLLER_PARAM_VALUE(node_id, i_prop, i_fallback, 0), \ - .output_limit = \ - MOTOR_DT_CONTROLLER_PARAM_VALUE(node_id, out_prop, out_fallback, 0), \ - .detri_lpf = \ - MOTOR_DT_CONTROLLER_PARAM_VALUE(node_id, lpf_prop, lpf_fallback, NAN), \ - .k_i = DT_STRING_UNQUOTED_OR(node_id, ki_prop, NAN), \ - .k_d = DT_STRING_UNQUOTED_OR(node_id, kd_prop, NAN), \ - .output_offset = \ - MOTOR_DT_CONTROLLER_PARAM_VALUE(node_id, offset_prop, offset_fallback, 0), \ - } - -#define MOTOR_DT_SINGLE_CONTROLLER_PARAMS(node_id) \ - MOTOR_DT_CONTROLLER_PARAMS(node_id, k_p, k_i, k_d, i_max, out_max, detri_lpf, offset, \ - i_max, out_max, detri_lpf, offset) - -#define MOTOR_DT_PV_CONTROLLER_PARAMS(node_id) \ - MOTOR_DT_CONTROLLER_PARAMS(node_id, pos_k_p, pos_k_i, pos_k_d, pos_i_max, pos_out_max, \ - pos_detri_lpf, pos_offset, i_max, out_max, detri_lpf, offset), \ - MOTOR_DT_CONTROLLER_PARAMS(node_id, vel_k_p, vel_k_i, vel_k_d, vel_i_max, \ - vel_out_max, vel_detri_lpf, vel_offset, i_max, out_max, \ - detri_lpf, offset) - -#define MOTOR_DT_MIT_CONTROLLER_PARAMS(node_id) \ - { \ - .k_p = DT_STRING_UNQUOTED_OR(node_id, k_p, 0), \ - .integral_limit = DT_STRING_UNQUOTED_OR(node_id, i_max, 0), \ - .output_limit = DT_STRING_UNQUOTED_OR(node_id, out_max, 0), \ - .detri_lpf = DT_STRING_UNQUOTED_OR(node_id, detri_lpf, NAN), \ - .k_i = DT_STRING_UNQUOTED_OR(node_id, k_i, NAN), \ - .k_d = DT_STRING_UNQUOTED_OR(node_id, k_d, NAN), \ - .output_offset = DT_STRING_UNQUOTED_OR(node_id, offset, 0), \ - } - -#define MOTOR_DT_CONTROLLER_MODE(node_id) \ - COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_mit), (MIT), \ - (COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_pv), (PV), (VO)))) - -#define MOTOR_DT_CONTROLLER_TARGET(node_id) \ - COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_vo), \ - ((enum motor_target)(DT_ENUM_IDX_OR(node_id, target, 1) + 1)), \ - (MOTOR_TARGET_POSITION)) - -#define MOTOR_DT_CONTROLLER_OUTPUT(node_id) \ - COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_mit), (MOTOR_OUTPUT_NATIVE), \ - (MOTOR_OUTPUT_TORQUE)) - -#define MOTOR_DT_CONTROLLER_REQUIRED_STATES(node_id) \ - COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_pv), \ - (MOTOR_STATE_POSITION | MOTOR_STATE_SPEED), \ - (COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_vo), \ - (COND_CODE_1(DT_ENUM_IDX_OR(node_id, target, 1), \ - (MOTOR_STATE_SPEED), (MOTOR_STATE_TORQUE))), \ - (0)))) - -#define MOTOR_DT_CONTROLLER_PARAM_COUNT(node_id) \ - COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_pv), (2), \ - (COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_mit), (1), (1)))) - -#define MOTOR_DT_CONTROLLER_PARAM_CONFIGS(node_id) \ - COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_pv), \ - (MOTOR_DT_PV_CONTROLLER_PARAMS(node_id)), \ - (COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_mit), \ - (MOTOR_DT_MIT_CONTROLLER_PARAMS(node_id)), \ - (MOTOR_DT_SINGLE_CONTROLLER_PARAMS(node_id))))) - -#define MOTOR_DT_CONTROLLER_CONFIG_BY_IDX(node_id, prop, idx) \ - { \ - .info = \ - { \ - .id = idx, \ - .mode = MOTOR_DT_CONTROLLER_MODE( \ - DT_PROP_BY_IDX(node_id, prop, idx)), \ - .target = MOTOR_DT_CONTROLLER_TARGET( \ - DT_PROP_BY_IDX(node_id, prop, idx)), \ - .output = MOTOR_DT_CONTROLLER_OUTPUT( \ - DT_PROP_BY_IDX(node_id, prop, idx)), \ - .required_states = MOTOR_DT_CONTROLLER_REQUIRED_STATES( \ - DT_PROP_BY_IDX(node_id, prop, idx)), \ - .name = DT_NODE_FULL_NAME(DT_PROP_BY_IDX(node_id, prop, idx)), \ - }, \ - .api = &motor_builtin_controller_api, \ - .param_count = \ - MOTOR_DT_CONTROLLER_PARAM_COUNT(DT_PROP_BY_IDX(node_id, prop, idx)), \ - .params = {MOTOR_DT_CONTROLLER_PARAM_CONFIGS(DT_PROP_BY_IDX(node_id, prop, idx))}, \ - } - -#define MOTOR_DT_CONTROLLER_CONFIGS(node_id) \ - COND_CODE_1(DT_NODE_HAS_PROP(node_id, controllers), \ - (DT_FOREACH_PROP_ELEM_SEP(node_id, controllers, \ - MOTOR_DT_CONTROLLER_CONFIG_BY_IDX, (, ))), \ - ()) - #define MOTOR_DT_DRIVER_CONFIG_GET(node_id) \ { \ .phy = (const struct device *)DT_GET_CANPHY(node_id), \ diff --git a/include/zephyr/drivers/motor/controller.h b/include/zephyr/drivers/motor/controller.h new file mode 100644 index 0000000..d2ec89a --- /dev/null +++ b/include/zephyr/drivers/motor/controller.h @@ -0,0 +1,587 @@ +/* + * Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @file + * @brief Motor controller profiles and helpers + */ + +#ifndef ZEPHYR_INCLUDE_DRIVERS_MOTOR_CONTROLLER_H_ +#define ZEPHYR_INCLUDE_DRIVERS_MOTOR_CONTROLLER_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define MOTOR_CONTROLLER_ID_AUTO UINT8_MAX +#define MOTOR_CONTROLLER_MAX 4 +#define MOTOR_CONTROLLER_PARAM_MAX 2 + +/** + * @brief 电机工作模式枚举 + * + * MIT: MIT模式 + * PV: 位置-速度控制 + * VO: 单控制量控制 + */ +enum motor_mode { + MIT = 0, + PV = 1, + VO = 2, +}; + +enum motor_target { + MOTOR_TARGET_NONE = 0, + MOTOR_TARGET_TORQUE = 1, + MOTOR_TARGET_SPEED = 2, + MOTOR_TARGET_POSITION = 3, +}; + +enum motor_controller_select { + MOTOR_CONTROLLER_DEFAULT = 0, + MOTOR_CONTROLLER_BY_ID = 1, +}; + +enum motor_state_flags { + MOTOR_STATE_POSITION = BIT(0), + MOTOR_STATE_SPEED = BIT(1), + MOTOR_STATE_TORQUE = BIT(2), + MOTOR_STATE_CURRENT = BIT(3), + MOTOR_STATE_TEMPERATURE = BIT(4), +}; + +enum motor_output_type { + MOTOR_OUTPUT_NONE = 0, + MOTOR_OUTPUT_TORQUE = 1, + MOTOR_OUTPUT_CURRENT = 2, + MOTOR_OUTPUT_SPEED = 3, + MOTOR_OUTPUT_POSITION = 4, + MOTOR_OUTPUT_NATIVE = 5, +}; + +enum motor_controller_output_field { + MOTOR_CONTROLLER_OUTPUT_VALUE = BIT(0), + MOTOR_CONTROLLER_OUTPUT_SPEED = BIT(1), +}; + +struct motor_controller_config; +struct motor_controller_data; +struct motor_controller_input; +struct motor_controller_output; + +struct motor_controller_info { + uint8_t id; + enum motor_mode mode; + enum motor_target target; + enum motor_output_type output; + uint32_t required_states; + char name[32]; +}; + +typedef struct motor_controller_info motor_controller_info_t; + +struct motor_controller_params { + float k_p; + float k_i; + float k_d; + float integral_limit; + float output_limit; + float output_offset; + float detri_lpf; +}; + +typedef int (*motor_controller_update_t)(struct motor_controller_data *data, + const struct motor_controller_config *cfg, + const struct motor_controller_input *input, + struct motor_controller_output *output); +typedef int (*motor_controller_get_params_t)(const struct motor_controller_config *cfg, + uint8_t index, struct motor_controller_params *params); +typedef void (*motor_controller_reset_t)(struct motor_controller_data *data); + +struct motor_controller_api { + motor_controller_update_t update; + motor_controller_get_params_t get_params; + motor_controller_reset_t reset; +}; + +struct motor_controller_config { + motor_controller_info_t info; + const struct motor_controller_api *api; + uint8_t param_count; + struct motor_controller_params params[MOTOR_CONTROLLER_PARAM_MAX]; +}; + +struct motor_controller_stage_data { + float err_integral; + float err_prev; + float err_derivate; + uint32_t prev_time; +}; + +struct motor_controller_data { + struct motor_controller_stage_data stages[MOTOR_CONTROLLER_PARAM_MAX]; +}; + +struct motor_driver_config { + /** Physical device */ + const struct device *phy; + /** motor ID */ + uint8_t id; + /** CAN TX ID */ + int tx_id; + /** CAN RX ID */ + int rx_id; + /** Configured controllers */ + struct motor_controller_config controllers[MOTOR_CONTROLLER_MAX]; +}; + +struct motor_setpoint { + float angle; + float rpm; + float torque; + + float speed_limit[2]; + float torque_limit[2]; + + enum motor_mode mode; + enum motor_target target; + enum motor_controller_select controller_select; + uint8_t controller_id; +}; + +typedef struct motor_setpoint motor_setpoint_t; + +struct motor_status { + float angle; + float rpm; + float torque; + float temperature; + float sum_angle; + + float speed_limit[2]; + float torque_limit[2]; + + enum motor_mode mode; + enum motor_target target; + uint8_t controller_id; + /* Communication reachability, independent from motor enable state. */ + bool online; + /* Actual motor enable state when feedback reports it; otherwise requested enable state. */ + bool enabled; + int error; +}; + +typedef struct motor_status motor_status_t; + +struct motor_controller_input { + motor_status_t status; + motor_setpoint_t setpoint; + float position_error; + bool has_position_error; + uint32_t timestamp; +}; + +struct motor_controller_output { + enum motor_output_type type; + uint32_t fields; + float value; + float speed; +}; + +static inline float motor_controller_clamp(float value, float min, float max) +{ + if (value > max) { + return max; + } + if (value < min) { + return min; + } + return value; +} + +static inline float motor_controller_stage_update(struct motor_controller_stage_data *data, + const struct motor_controller_params *params, + float error, uint32_t timestamp) +{ + float output; + + if (data == NULL || params == NULL) { + return 0; + } + if (data->prev_time == 0 || timestamp == 0) { + data->prev_time = timestamp; + data->err_prev = error; + output = isnan(params->k_p) ? params->output_offset + : params->k_p * error + params->output_offset; + if (params->output_limit != 0) { + output = motor_controller_clamp(output, -params->output_limit, + params->output_limit); + } + return output; + } + + float delta_us = k_cyc_to_us_near32(timestamp - data->prev_time); + if (delta_us < 1.0f) { + return 0; + } + + if (!isnan(params->k_i)) { + data->err_integral += error * delta_us / 1000000.0f; + if (params->integral_limit != 0) { + data->err_integral = + motor_controller_clamp(data->err_integral, -params->integral_limit, + params->integral_limit); + } + } + + float raw_derivate = (error - data->err_prev) / delta_us * 1000000.0f; + if (isnan(params->detri_lpf)) { + data->err_derivate = raw_derivate; + } else { + data->err_derivate = params->detri_lpf * data->err_derivate + + (1.0f - params->detri_lpf) * raw_derivate; + } + + output = params->output_offset; + if (!isnan(params->k_p)) { + output += params->k_p * error; + } + if (!isnan(params->k_i)) { + output += params->k_i * data->err_integral; + } + if (!isnan(params->k_d)) { + output += params->k_d * data->err_derivate; + } + if (params->output_limit != 0) { + output = + motor_controller_clamp(output, -params->output_limit, params->output_limit); + } + + data->err_prev = error; + data->prev_time = timestamp; + + return output; +} + +static inline int motor_builtin_controller_update(struct motor_controller_data *data, + const struct motor_controller_config *cfg, + const struct motor_controller_input *input, + struct motor_controller_output *output) +{ + float target_speed; + float target_torque; + + if (data == NULL || cfg == NULL || input == NULL || output == NULL) { + return -EINVAL; + } + + *output = (struct motor_controller_output){ + .type = cfg->info.output, + .fields = 0, + .value = 0, + .speed = 0, + }; + + if (cfg->info.mode == PV && cfg->info.target == MOTOR_TARGET_POSITION) { + if (cfg->param_count < 2) { + return -EINVAL; + } + float position_error = input->has_position_error + ? input->position_error + : input->setpoint.angle - input->status.sum_angle; + + target_speed = motor_controller_stage_update(&data->stages[0], &cfg->params[0], + position_error, input->timestamp); + if (input->setpoint.speed_limit[0] < input->setpoint.speed_limit[1]) { + target_speed = + motor_controller_clamp(target_speed, input->setpoint.speed_limit[0], + input->setpoint.speed_limit[1]); + } + target_torque = motor_controller_stage_update(&data->stages[1], &cfg->params[1], + target_speed - input->status.rpm, + input->timestamp); + target_torque += input->setpoint.torque; + output->type = MOTOR_OUTPUT_TORQUE; + output->fields = MOTOR_CONTROLLER_OUTPUT_VALUE | MOTOR_CONTROLLER_OUTPUT_SPEED; + output->value = target_torque; + output->speed = target_speed; + return 0; + } + + if (cfg->info.mode == VO && cfg->info.target == MOTOR_TARGET_SPEED) { + if (cfg->param_count < 1) { + return -EINVAL; + } + target_torque = motor_controller_stage_update( + &data->stages[0], &cfg->params[0], input->setpoint.rpm - input->status.rpm, + input->timestamp); + target_torque += input->setpoint.torque; + output->type = MOTOR_OUTPUT_TORQUE; + output->fields = MOTOR_CONTROLLER_OUTPUT_VALUE; + output->value = target_torque; + return 0; + } + + if (cfg->info.mode == VO && cfg->info.target == MOTOR_TARGET_TORQUE) { + output->type = MOTOR_OUTPUT_TORQUE; + output->fields = MOTOR_CONTROLLER_OUTPUT_VALUE; + output->value = input->setpoint.torque; + return 0; + } + + return -ENOTSUP; +} + +static inline int motor_builtin_controller_get_params(const struct motor_controller_config *cfg, + uint8_t index, + struct motor_controller_params *params) +{ + if (cfg == NULL || params == NULL || index >= cfg->param_count || + index >= MOTOR_CONTROLLER_PARAM_MAX) { + return -EINVAL; + } + + *params = cfg->params[index]; + return 0; +} + +static inline void motor_builtin_controller_reset(struct motor_controller_data *data) +{ + if (data != NULL) { + memset(data, 0, sizeof(*data)); + } +} + +static const struct motor_controller_api motor_builtin_controller_api = { + .update = motor_builtin_controller_update, + .get_params = motor_builtin_controller_get_params, + .reset = motor_builtin_controller_reset, +}; + +static inline int motor_controller_update(struct motor_controller_data *data, + const struct motor_controller_config *cfg, + const struct motor_controller_input *input, + struct motor_controller_output *output) +{ + if (cfg == NULL || cfg->api == NULL || cfg->api->update == NULL) { + return -ENOSYS; + } + + return cfg->api->update(data, cfg, input, output); +} + +static inline int motor_controller_get_params(const struct motor_controller_config *cfg, + uint8_t index, struct motor_controller_params *params) +{ + if (cfg == NULL || cfg->api == NULL || cfg->api->get_params == NULL) { + return -ENOSYS; + } + + return cfg->api->get_params(cfg, index, params); +} + +static inline void motor_controller_reset(struct motor_controller_data *data, + const struct motor_controller_config *cfg) +{ + if (cfg != NULL && cfg->api != NULL && cfg->api->reset != NULL) { + cfg->api->reset(data); + } +} + +static inline bool motor_controller_info_valid(const motor_controller_info_t *info) +{ + return info != NULL && info->name[0] != '\0'; +} + +static inline int motor_get_controller_count(const struct device *dev) +{ + const struct motor_driver_config *cfg = dev->config; + int count = 0; + + for (int i = 0; i < ARRAY_SIZE(cfg->controllers); i++) { + if (!motor_controller_info_valid(&cfg->controllers[i].info)) { + break; + } + count++; + } + + return count; +} + +static inline int motor_get_controller_info(const struct device *dev, uint8_t index, + motor_controller_info_t *info) +{ + const struct motor_driver_config *cfg = dev->config; + + if (info == NULL || index >= ARRAY_SIZE(cfg->controllers) || + !motor_controller_info_valid(&cfg->controllers[index].info)) { + return -EINVAL; + } + + *info = cfg->controllers[index].info; + return 0; +} + +static inline int motor_resolve_controller(const struct device *dev, motor_setpoint_t *setpoint, + motor_controller_info_t *info) +{ + if (setpoint == NULL) { + return -EINVAL; + } + if (setpoint->target == MOTOR_TARGET_NONE) { + return 0; + } + + if (setpoint->controller_select == MOTOR_CONTROLLER_BY_ID) { + motor_controller_info_t selected = {0}; + motor_controller_info_t *selected_info = info != NULL ? info : &selected; + int ret = motor_get_controller_info(dev, setpoint->controller_id, selected_info); + + if (ret < 0) { + return ret; + } + if (selected_info->mode != setpoint->mode || + selected_info->target != setpoint->target) { + return -EINVAL; + } + return 0; + } + + for (uint8_t i = 0; i < motor_get_controller_count(dev); i++) { + motor_controller_info_t candidate = {0}; + + if (motor_get_controller_info(dev, i, &candidate) < 0) { + continue; + } + if (candidate.mode == setpoint->mode && candidate.target == setpoint->target) { + setpoint->controller_id = i; + if (info != NULL) { + *info = candidate; + } + return 0; + } + } + + return -ENOTSUP; +} + +#define MOTOR_DT_CONTROLLER_PARAM_VALUE(node_id, prop, fallback_prop, default_value) \ + COND_CODE_1(DT_NODE_HAS_PROP(node_id, prop), (DT_STRING_UNQUOTED(node_id, prop)), \ + (DT_STRING_UNQUOTED_OR(node_id, fallback_prop, default_value))) + +#define MOTOR_DT_CONTROLLER_PARAMS(node_id, kp_prop, ki_prop, kd_prop, i_prop, out_prop, lpf_prop, \ + offset_prop, i_fallback, out_fallback, lpf_fallback, \ + offset_fallback) \ + { \ + .k_p = DT_STRING_UNQUOTED_OR(node_id, kp_prop, 0), \ + .integral_limit = MOTOR_DT_CONTROLLER_PARAM_VALUE(node_id, i_prop, i_fallback, 0), \ + .output_limit = \ + MOTOR_DT_CONTROLLER_PARAM_VALUE(node_id, out_prop, out_fallback, 0), \ + .detri_lpf = \ + MOTOR_DT_CONTROLLER_PARAM_VALUE(node_id, lpf_prop, lpf_fallback, NAN), \ + .k_i = DT_STRING_UNQUOTED_OR(node_id, ki_prop, NAN), \ + .k_d = DT_STRING_UNQUOTED_OR(node_id, kd_prop, NAN), \ + .output_offset = \ + MOTOR_DT_CONTROLLER_PARAM_VALUE(node_id, offset_prop, offset_fallback, 0), \ + } + +#define MOTOR_DT_SINGLE_CONTROLLER_PARAMS(node_id) \ + MOTOR_DT_CONTROLLER_PARAMS(node_id, k_p, k_i, k_d, i_max, out_max, detri_lpf, offset, \ + i_max, out_max, detri_lpf, offset) + +#define MOTOR_DT_PV_CONTROLLER_PARAMS(node_id) \ + MOTOR_DT_CONTROLLER_PARAMS(node_id, pos_k_p, pos_k_i, pos_k_d, pos_i_max, pos_out_max, \ + pos_detri_lpf, pos_offset, i_max, out_max, detri_lpf, offset), \ + MOTOR_DT_CONTROLLER_PARAMS(node_id, vel_k_p, vel_k_i, vel_k_d, vel_i_max, \ + vel_out_max, vel_detri_lpf, vel_offset, i_max, out_max, \ + detri_lpf, offset) + +#define MOTOR_DT_MIT_CONTROLLER_PARAMS(node_id) \ + { \ + .k_p = DT_STRING_UNQUOTED_OR(node_id, k_p, 0), \ + .integral_limit = DT_STRING_UNQUOTED_OR(node_id, i_max, 0), \ + .output_limit = DT_STRING_UNQUOTED_OR(node_id, out_max, 0), \ + .detri_lpf = DT_STRING_UNQUOTED_OR(node_id, detri_lpf, NAN), \ + .k_i = DT_STRING_UNQUOTED_OR(node_id, k_i, NAN), \ + .k_d = DT_STRING_UNQUOTED_OR(node_id, k_d, NAN), \ + .output_offset = DT_STRING_UNQUOTED_OR(node_id, offset, 0), \ + } + +#define MOTOR_DT_CONTROLLER_MODE(node_id) \ + COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_mit), (MIT), \ + (COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_pv), (PV), (VO)))) + +#define MOTOR_DT_CONTROLLER_TARGET(node_id) \ + COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_vo), \ + ((enum motor_target)(DT_ENUM_IDX_OR(node_id, target, 1) + 1)), \ + (MOTOR_TARGET_POSITION)) + +#define MOTOR_DT_CONTROLLER_OUTPUT(node_id) \ + COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_mit), (MOTOR_OUTPUT_NATIVE), \ + (MOTOR_OUTPUT_TORQUE)) + +#define MOTOR_DT_CONTROLLER_REQUIRED_STATES(node_id) \ + COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_pv), \ + (MOTOR_STATE_POSITION | MOTOR_STATE_SPEED), \ + (COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_vo), \ + (COND_CODE_1(DT_ENUM_IDX_OR(node_id, target, 1), \ + (MOTOR_STATE_SPEED), (MOTOR_STATE_TORQUE))), \ + (0)))) + +#define MOTOR_DT_CONTROLLER_PARAM_COUNT(node_id) \ + COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_pv), (2), \ + (COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_mit), (1), (1)))) + +#define MOTOR_DT_CONTROLLER_PARAM_CONFIGS(node_id) \ + COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_pv), \ + (MOTOR_DT_PV_CONTROLLER_PARAMS(node_id)), \ + (COND_CODE_1(DT_NODE_HAS_COMPAT(node_id, motor_controller_mit), \ + (MOTOR_DT_MIT_CONTROLLER_PARAMS(node_id)), \ + (MOTOR_DT_SINGLE_CONTROLLER_PARAMS(node_id))))) + +#define MOTOR_DT_CONTROLLER_CONFIG_BY_IDX(node_id, prop, idx) \ + { \ + .info = \ + { \ + .id = idx, \ + .mode = MOTOR_DT_CONTROLLER_MODE( \ + DT_PROP_BY_IDX(node_id, prop, idx)), \ + .target = MOTOR_DT_CONTROLLER_TARGET( \ + DT_PROP_BY_IDX(node_id, prop, idx)), \ + .output = MOTOR_DT_CONTROLLER_OUTPUT( \ + DT_PROP_BY_IDX(node_id, prop, idx)), \ + .required_states = MOTOR_DT_CONTROLLER_REQUIRED_STATES( \ + DT_PROP_BY_IDX(node_id, prop, idx)), \ + .name = DT_NODE_FULL_NAME(DT_PROP_BY_IDX(node_id, prop, idx)), \ + }, \ + .api = &motor_builtin_controller_api, \ + .param_count = \ + MOTOR_DT_CONTROLLER_PARAM_COUNT(DT_PROP_BY_IDX(node_id, prop, idx)), \ + .params = {MOTOR_DT_CONTROLLER_PARAM_CONFIGS(DT_PROP_BY_IDX(node_id, prop, idx))}, \ + } + +#define MOTOR_DT_CONTROLLER_CONFIGS(node_id) \ + COND_CODE_1(DT_NODE_HAS_PROP(node_id, controllers), \ + (DT_FOREACH_PROP_ELEM_SEP(node_id, controllers, \ + MOTOR_DT_CONTROLLER_CONFIG_BY_IDX, (, ))), \ + ()) + +#ifdef __cplusplus +} +#endif + +#endif /* ZEPHYR_INCLUDE_DRIVERS_MOTOR_CONTROLLER_H_ */ From 8587568a03bee6f63a092b1604f3eb22e3dcf0aa Mon Sep 17 00:00:00 2001 From: ttwards <12411711@mail.sustech.edu.cn> Date: Wed, 1 Jul 2026 22:43:08 +0800 Subject: [PATCH 2/6] fix(communication): align ARES transport callbacks Replace the old USB send-with-lock transport hook with TX completion callbacks, add transport capability metadata, and keep USB/UART callback semantics aligned for Dual protocol sends. Validation: git diff --check; west build -b robomaster_board_c samples/communication/ares_communication --pristine; west build -b dm_mc02 samples/communication/mqttlite_usb --pristine; west twister -T tests/native_sim/mqttlite --platform native_sim/native/64 --inline-logs; west twister -T tests/native_sim/module_smoke --platform native_sim/native/64 --inline-logs. --- .../updates/ares_usb_transport_update.md | 148 ++++++++++++++++++ include/ares/interface/ares_interface.h | 20 ++- include/ares/interface/uart/uart.h | 17 +- include/ares/interface/usb/usb_bulk.h | 10 +- include/ares/protocol/dual/dual_protocol.h | 31 +--- lib/ares/interface/ares_interface.h | 20 ++- lib/ares/interface/uart/uart.c | 82 +++++++++- lib/ares/interface/usb/usb_bulk.c | 142 ++++++++++------- lib/ares/interface/usb/usb_bulk.h | 10 +- lib/ares/protocol/dual/dual_protocol.c | 68 ++++++-- 10 files changed, 434 insertions(+), 114 deletions(-) create mode 100644 Documents/updates/ares_usb_transport_update.md diff --git a/Documents/updates/ares_usb_transport_update.md b/Documents/updates/ares_usb_transport_update.md new file mode 100644 index 0000000..14f5c7b --- /dev/null +++ b/Documents/updates/ares_usb_transport_update.md @@ -0,0 +1,148 @@ +# ARES USB Transport Update + +Date: 2026-07-01 +Board under test: dm_mc02 / STM32H723 +USB device: ARES Bulk Interface Async, full-speed USB, VID:PID 1209:0001 + +## Summary + +This update fixes the ARES USB bulk transport and dual protocol reconnect path. The +main symptom was that the USB endpoints could still receive host frames while the +protocol state stayed offline or the reply path stayed permanently busy after a +host disconnect/reconnect. + +No benchmark-only firmware hooks are part of this update. The final validation was +done on the normal `samples/communication/ares_communication` application. + +## Root Cause + +The old USB path treated `USBD_STATE_CONFIGURED` as the main source of truth for +the protocol `CONNECTED` event. In practice, the Zephyr USB class `enable()` +callback is the reliable point where the interface has been enabled and the first +bulk OUT transfer can be submitted. + +There was also a second reconnect bug in the dual protocol TX path. The old +implementation used mutex-style send ownership. After a host disconnect, an IN +transfer could be left without a normal completion path. The board continued to +receive and parse FUNC frames, but the matching reply path stayed marked busy, so +later FUNC replies were suppressed. + +## Old Architecture + +- USB protocol connection was driven mostly by USB device state messages. +- Bulk OUT submission and protocol online state were loosely coupled. +- Reply and sync sends used a lock-like ownership model. +- Disconnect cleanup cleared the backup queue, but did not clear all in-flight + FUNC/SYNC send state. +- Reconnecting the host could leave the device in a split-brain state: + - OUT endpoint and parser were active. + - Protocol online/reply state could remain stale. + - Host could send frames but receive no replies. + +## New Architecture + +- USB class `enable()` first marks the interface enabled, then submits the initial + bulk OUT buffer. +- The protocol receives `ARES_PROTOCOL_EVENT_CONNECTED` only after that OUT + submission succeeds. +- A dedicated USB connected bit deduplicates `CONNECTED` and `DISCONNECTED` + events from class callbacks and USB state messages. +- USB class `disable()`, reset, and suspend all flow through the same disconnect + state helper. +- The ARES interface API now exposes TX completion callbacks and simple transport + capabilities. +- USB bulk stores the TX completion callback in `net_buf` user data and calls it + from IN completion. +- Dual protocol FUNC/SYNC sends use atomic `DUAL_TX_IN_FLIGHT` state that is + cleared by TX completion. +- Offline/reconnect cleanup now clears: + - FUNC in-flight state. + - SYNC in-flight state. + - FUNC reply backup queue. + - FUNC backup count. + +The intended state model is: + +1. USB class enable succeeds. +2. Initial OUT buffer is queued. +3. Protocol is marked connected. +4. Protocol sends use TX completion to release in-flight state. +5. Disconnect/reconnect cleanup always resets stale TX state before going online + again. + +## Files Changed + +- `include/ares/interface/ares_interface.h` +- `lib/ares/interface/ares_interface.h` +- `include/ares/interface/usb/usb_bulk.h` +- `lib/ares/interface/usb/usb_bulk.h` +- `include/ares/protocol/dual/dual_protocol.h` +- `lib/ares/interface/usb/usb_bulk.c` +- `lib/ares/protocol/dual/dual_protocol.c` + +## Validation + +Build: + +```sh +west build -d build +``` + +Flash: + +```sh +west flash -d build --no-rebuild --runner openocd --config mambo/boards/damiao/dm_mc02/support/openocd.cfg +``` + +Startup log after reset showed the expected class-driven connection path: + +```text +ares_usb_bulk_async: Enable ARES Bulk interface +dual_protocol: dual_protocol Connection established due to event. +``` + +Three consecutive host reconnect tests completed successfully. Before this fix, +later runs could drop to `replies=0` while the board still logged incoming +`func_cb` calls. + +```text +Run 1: sent=45949 replies=45449 send_fps=9189.8 reply_fps_over_send=9089.8 +Run 2: sent=46238 replies=45819 send_fps=9247.6 reply_fps_over_send=9163.8 +Run 3: sent=46039 replies=45626 send_fps=9207.8 reply_fps_over_send=9125.2 +``` + +## Performance Data + +These numbers measure the normal ARES protocol path, not raw USB bulk maximum +throughput. The tested application also still emits serial logs and periodic +application traffic. + +Best observed exec/reply rate: + +- Host-to-device exec send rate: 9247.6 frames/s. +- Device-to-host reply rate during send window: 9163.8 frames/s. +- Exec frame payload on the wire: 18 bytes. +- Reply frame payload on the wire: 10 bytes. +- Approximate effective protocol payload throughput: + - OUT: 162.6 KiB/s. + - IN replies: 89.5 KiB/s. + - Combined: 252 KiB/s, about 2.06 Mbit/s. + +Round-trip latency for single `exec -> reply` calls: + +- Typical RTT: 0.28 ms to 0.36 ms. +- Average RTT: 0.30 ms to 0.39 ms. +- p95 RTT: 0.44 ms to 0.64 ms. +- p99 RTT: 0.56 ms to 1.12 ms. +- Observed max RTT: 1.4 ms to 3.8 ms. + +For application budgeting, `1 ms` is a conservative round-trip target for the +current full-speed USB ARES protocol path. + +## Notes + +- The board enumerated as full-speed USB during the test. +- The temporary USB bulk benchmark and UART diagnostic sample directories were + deleted and are not part of this update. +- Raw USB bulk throughput can be higher than the protocol numbers above, but the + protocol numbers are the relevant data for current ARES exec/reply behavior. diff --git a/include/ares/interface/ares_interface.h b/include/ares/interface/ares_interface.h index 0d651cd..c04c020 100644 --- a/include/ares/interface/ares_interface.h +++ b/include/ares/interface/ares_interface.h @@ -5,6 +5,7 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -13,6 +14,19 @@ extern "C" { struct AresProtocol; struct AresInterface; +typedef void (*ares_interface_tx_done_cb_t)(struct AresInterface *interface, + struct net_buf *buf, int status, + void *user_data); + +enum AresInterfaceCaps { + ARES_INTERFACE_CAP_STREAM = BIT(0), + ARES_INTERFACE_CAP_PACKET = BIT(1), + ARES_INTERFACE_CAP_ZERO_COPY = BIT(2), + ARES_INTERFACE_CAP_TX_COMPLETE = BIT(3), + ARES_INTERFACE_CAP_TX_QUEUE = BIT(4), + ARES_INTERFACE_CAP_ETHERNET_FRAME = BIT(5), +}; + /** * @brief API that an interface must implement. * @@ -22,13 +36,15 @@ struct AresInterface; */ struct AresInterfaceAPI { int (*send)(struct AresInterface *interface, struct net_buf *buf); - int (*send_with_lock)(struct AresInterface *interface, struct net_buf *buf, - struct k_mutex *mutex); + int (*send_with_callback)(struct AresInterface *interface, struct net_buf *buf, + ares_interface_tx_done_cb_t cb, void *user_data); int (*send_raw)(struct AresInterface *interface, uint8_t *data, uint16_t len); int (*connect)(struct AresInterface *interface); int (*disconnect)(struct AresInterface *interface); bool (*is_connected)(struct AresInterface *interface); + uint32_t (*caps)(struct AresInterface *interface); + size_t (*mtu)(struct AresInterface *interface); struct net_buf *(*alloc_buf)(struct AresInterface *interface); struct net_buf *(*alloc_buf_with_data)(struct AresInterface *interface, void *data, diff --git a/include/ares/interface/uart/uart.h b/include/ares/interface/uart/uart.h index 7b3c0a2..0638310 100644 --- a/include/ares/interface/uart/uart.h +++ b/include/ares/interface/uart/uart.h @@ -16,8 +16,12 @@ int ares_uart_init(struct AresInterface *interface); int ares_uart_send(struct AresInterface *interface, struct net_buf *buf); +int ares_uart_send_with_callback(struct AresInterface *interface, struct net_buf *buf, + ares_interface_tx_done_cb_t cb, void *user_data); int ares_uart_send_raw(struct AresInterface *interface, uint8_t *data, uint16_t len); struct net_buf *ares_uart_interface_alloc_buf(struct AresInterface *interface); +uint32_t ares_uart_caps(struct AresInterface *interface); +size_t ares_uart_mtu(struct AresInterface *interface); void ares_uart_init_dev(struct AresInterface *interface, const struct device *uart_dev); @@ -41,21 +45,24 @@ struct AresUartInterface { struct k_sem sem; struct k_thread thread; - k_thread_stack_t thread_stack[ARES_UART_PROCESSING_THREAD_STACK_SIZE]; + K_KERNEL_STACK_MEMBER(thread_stack, ARES_UART_PROCESSING_THREAD_STACK_SIZE); struct k_msgq tx_msgq; // 发送消息队列 char __aligned(4) tx_msgq_buffer[sizeof(struct net_buf *) * ARES_UART_TX_QUEUE_SIZE]; - struct k_sem tx_sem; // 用于发送流控制的信号量 - struct net_buf *current_tx_buf; // 指向当前正在发送的buf - struct k_thread tx_thread; // 发送线程的句柄 - k_thread_stack_t tx_thread_stack[ARES_UART_TX_THREAD_STACK_SIZE]; // 发送线程的栈 + struct k_sem tx_sem; // 用于发送流控制的信号量 + struct net_buf *current_tx_buf; // 指向当前正在发送的buf + struct k_thread tx_thread; // 发送线程的句柄 + K_KERNEL_STACK_MEMBER(tx_thread_stack, ARES_UART_TX_THREAD_STACK_SIZE); // 发送线程的栈 }; #define ARES_UART_INTERFACE_DEFINE(Interface_name) \ struct AresInterfaceAPI ares_uart_interface_api = { \ .init = ares_uart_init, \ .send = ares_uart_send, \ + .send_with_callback = ares_uart_send_with_callback, \ .send_raw = ares_uart_send_raw, \ + .caps = ares_uart_caps, \ + .mtu = ares_uart_mtu, \ .alloc_buf = ares_uart_interface_alloc_buf, \ }; \ struct AresUartInterface Internal_##Interface_name = {NULL}; \ diff --git a/include/ares/interface/usb/usb_bulk.h b/include/ares/interface/usb/usb_bulk.h index caa557d..925a85b 100644 --- a/include/ares/interface/usb/usb_bulk.h +++ b/include/ares/interface/usb/usb_bulk.h @@ -13,8 +13,10 @@ int ares_usbd_write(struct AresInterface *interface, struct net_buf *buf); struct net_buf *ares_interface_alloc_buf(struct AresInterface *interface); struct net_buf *ares_interface_alloc_buf_with_data(struct AresInterface *interface, void *data, size_t len); -int ares_usbd_write_with_lock(struct AresInterface *interface, struct net_buf *buf, - struct k_mutex *mutex); +int ares_usbd_write_with_callback(struct AresInterface *interface, struct net_buf *buf, + ares_interface_tx_done_cb_t cb, void *user_data); +uint32_t ares_usbd_caps(struct AresInterface *interface); +size_t ares_usbd_mtu(struct AresInterface *interface); struct AresBulkInterface { struct AresInterface *interface; @@ -32,7 +34,9 @@ struct AresBulkInterface { struct AresInterfaceAPI ares_bulk_interface_api = { \ .init = ares_usbd_init, \ .send = ares_usbd_write, \ - .send_with_lock = ares_usbd_write_with_lock, \ + .send_with_callback = ares_usbd_write_with_callback, \ + .caps = ares_usbd_caps, \ + .mtu = ares_usbd_mtu, \ .alloc_buf = ares_interface_alloc_buf, \ .alloc_buf_with_data = ares_interface_alloc_buf_with_data, \ }; \ diff --git a/include/ares/protocol/dual/dual_protocol.h b/include/ares/protocol/dual/dual_protocol.h index f45a63a..4869c0e 100644 --- a/include/ares/protocol/dual/dual_protocol.h +++ b/include/ares/protocol/dual/dual_protocol.h @@ -94,6 +94,8 @@ enum frame_type { #define SYNC_PACK_STATUS_WRITE BIT(1) #define SYNC_PACK_STATUS_DONE BIT(2) +#define DUAL_TX_IN_FLIGHT 0 + #define GET_8BITS(buf, n_byte) (*(uint8_t *)(buf + n_byte)) #define GET_16BITS(buf, n_byte) (*(uint16_t *)(buf + n_byte)) #define GET_32BITS(buf, n_byte) (*(uint32_t *)(buf + n_byte)) @@ -116,7 +118,7 @@ struct sync_pack { dual_trans_cb_t cb; uint8_t *buf; - struct k_mutex mutex; + atomic_t tx_state; }; struct id_mapping { @@ -127,7 +129,7 @@ struct id_mapping { uint32_t arg3; uint16_t req_id; - struct k_mutex mutex; + atomic_t tx_state; __aligned(4) uint8_t buf[REPL_FRAME_LENGTH + 2]; // +2 for potential CRC16 }; @@ -207,19 +209,6 @@ int dual_sync_flush(struct AresProtocol *protocol, sync_table_t *pack); }; \ struct dual_protocol_data Protocol_name##_data = { \ .name = #Protocol_name, \ - .heart_beat_timer = {0}, \ - .err_frame_mutex = {0}, \ - .func_cnt = 0, \ - .sync_cnt = 0, \ - .func_tx_bckup_msgq = {0}, \ - .online = false, \ - .func_tx_bckup_cnt = 0, \ - .state = PARSER_STATE_IDLE, \ - .current_frame_type = FRAME_TYPE_UNKNOWN, \ - .rx_buffer_pos = 0, \ - .expected_frame_length = 0, \ - .header_value = 0, \ - .crc_enabled = false, \ }; \ struct AresProtocol Protocol_name = { \ .name = #Protocol_name, \ @@ -236,18 +225,6 @@ int dual_sync_flush(struct AresProtocol *protocol, sync_table_t *pack); }; \ struct dual_protocol_data Protocol_name##_data = { \ .name = #Protocol_name, \ - .heart_beat_timer = {0}, \ - .err_frame_mutex = {0}, \ - .func_cnt = 0, \ - .sync_cnt = 0, \ - .func_tx_bckup_msgq = {0}, \ - .online = false, \ - .func_tx_bckup_cnt = 0, \ - .state = PARSER_STATE_IDLE, \ - .current_frame_type = FRAME_TYPE_UNKNOWN, \ - .rx_buffer_pos = 0, \ - .expected_frame_length = 0, \ - .header_value = 0, \ .crc_enabled = true, \ }; \ struct AresProtocol Protocol_name = { \ diff --git a/lib/ares/interface/ares_interface.h b/lib/ares/interface/ares_interface.h index 0d651cd..c04c020 100644 --- a/lib/ares/interface/ares_interface.h +++ b/lib/ares/interface/ares_interface.h @@ -5,6 +5,7 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -13,6 +14,19 @@ extern "C" { struct AresProtocol; struct AresInterface; +typedef void (*ares_interface_tx_done_cb_t)(struct AresInterface *interface, + struct net_buf *buf, int status, + void *user_data); + +enum AresInterfaceCaps { + ARES_INTERFACE_CAP_STREAM = BIT(0), + ARES_INTERFACE_CAP_PACKET = BIT(1), + ARES_INTERFACE_CAP_ZERO_COPY = BIT(2), + ARES_INTERFACE_CAP_TX_COMPLETE = BIT(3), + ARES_INTERFACE_CAP_TX_QUEUE = BIT(4), + ARES_INTERFACE_CAP_ETHERNET_FRAME = BIT(5), +}; + /** * @brief API that an interface must implement. * @@ -22,13 +36,15 @@ struct AresInterface; */ struct AresInterfaceAPI { int (*send)(struct AresInterface *interface, struct net_buf *buf); - int (*send_with_lock)(struct AresInterface *interface, struct net_buf *buf, - struct k_mutex *mutex); + int (*send_with_callback)(struct AresInterface *interface, struct net_buf *buf, + ares_interface_tx_done_cb_t cb, void *user_data); int (*send_raw)(struct AresInterface *interface, uint8_t *data, uint16_t len); int (*connect)(struct AresInterface *interface); int (*disconnect)(struct AresInterface *interface); bool (*is_connected)(struct AresInterface *interface); + uint32_t (*caps)(struct AresInterface *interface); + size_t (*mtu)(struct AresInterface *interface); struct net_buf *(*alloc_buf)(struct AresInterface *interface); struct net_buf *(*alloc_buf_with_data)(struct AresInterface *interface, void *data, diff --git a/lib/ares/interface/uart/uart.c b/lib/ares/interface/uart/uart.c index f44674d..322df76 100755 --- a/lib/ares/interface/uart/uart.c +++ b/lib/ares/interface/uart/uart.c @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -13,6 +14,7 @@ #include "ares/interface/uart/uart.h" #include "zephyr/sys/ring_buffer.h" #include +#include LOG_MODULE_REGISTER(ares_uart, LOG_LEVEL_INF); // 日志级别可以按需调整 @@ -21,8 +23,30 @@ LOG_MODULE_REGISTER(ares_uart, LOG_LEVEL_INF); // 日志级别可以按需调整 #define ARES_UART_PROCESSING_THREAD_PRIORITY K_PRIO_PREEMPT(5) #define ARES_UART_RX_INACTIVE_TIMEOUT 10 -NET_BUF_POOL_DEFINE(uart_net_buf_pool, 16, 128, 4, NULL); // 增加缓冲区数量和大小 -K_MEM_SLAB_DEFINE(uart_rx_slab, ARES_UART_BLOCK_SIZE, 8, 4); +struct ares_uart_tx_meta { + ares_interface_tx_done_cb_t tx_done; + void *tx_user_data; +}; + +NET_BUF_POOL_DEFINE(uart_net_buf_pool, 16, 128, sizeof(struct ares_uart_tx_meta), + NULL); // 增加缓冲区数量和大小 +K_MEM_SLAB_DEFINE_IN_SECT(uart_rx_slab, __nocache, ARES_UART_BLOCK_SIZE, 8, 4); + +static uint8_t __nocache __aligned(32) uart_tx_dma_buf[MAX_FRAME_PAYLOAD_SIZE]; + +static void ares_uart_complete_tx(struct AresInterface *interface, struct net_buf *buf, int status) +{ + struct ares_uart_tx_meta *meta; + + if (buf == NULL) { + return; + } + + meta = net_buf_user_data(buf); + if (meta && meta->tx_done) { + meta->tx_done(interface, buf, status, meta->tx_user_data); + } +} /** * @brief 发送一个数据包到队列中 @@ -34,11 +58,24 @@ K_MEM_SLAB_DEFINE(uart_rx_slab, ARES_UART_BLOCK_SIZE, 8, 4); * @return 0 on success, -ENOMEM if the queue is full. */ int ares_uart_send(struct AresInterface *interface, struct net_buf *buf) +{ + return ares_uart_send_with_callback(interface, buf, NULL, NULL); +} + +int ares_uart_send_with_callback(struct AresInterface *interface, struct net_buf *buf, + ares_interface_tx_done_cb_t cb, void *user_data) { struct AresUartInterface *uart_if = interface->priv_data; + struct ares_uart_tx_meta *meta = net_buf_user_data(buf); + + if (meta) { + meta->tx_done = cb; + meta->tx_user_data = user_data; + } if (k_msgq_put(&uart_if->tx_msgq, &buf, K_NO_WAIT) != 0) { LOG_ERR("TX message queue is full. Dropping packet!"); + ares_uart_complete_tx(interface, buf, -ENOMEM); net_buf_unref(buf); return -ENOMEM; } @@ -56,14 +93,37 @@ int ares_uart_send_raw(struct AresInterface *interface, uint8_t *data, uint16_t LOG_ERR("uart_tx failed with error %d", err); return -EIO; } + return 0; } struct net_buf *ares_uart_interface_alloc_buf(struct AresInterface *interface) { struct net_buf *buf = net_buf_alloc(&uart_net_buf_pool, K_NO_WAIT); + if (buf) { + struct ares_uart_tx_meta *meta = net_buf_user_data(buf); + if (meta) { + meta->tx_done = NULL; + meta->tx_user_data = NULL; + } + } return buf; } +uint32_t ares_uart_caps(struct AresInterface *interface) +{ + ARG_UNUSED(interface); + + return ARES_INTERFACE_CAP_STREAM | ARES_INTERFACE_CAP_TX_COMPLETE | + ARES_INTERFACE_CAP_TX_QUEUE; +} + +size_t ares_uart_mtu(struct AresInterface *interface) +{ + ARG_UNUSED(interface); + + return MAX_FRAME_PAYLOAD_SIZE; +} + /** * @brief (新增) 发送线程入口函数 * @@ -80,11 +140,20 @@ static void ares_uart_tx_thread_entry(void *p1, void *p2, void *p3) while (1) { k_msgq_get(&uart_if->tx_msgq, &buf, K_FOREVER); k_sem_take(&uart_if->tx_sem, K_FOREVER); + if (buf->len > sizeof(uart_tx_dma_buf)) { + LOG_ERR("UART frame too large for DMA buffer: %u", buf->len); + ares_uart_complete_tx(interface, buf, -EMSGSIZE); + net_buf_unref(buf); + k_sem_give(&uart_if->tx_sem); + continue; + } + memcpy(uart_tx_dma_buf, buf->data, buf->len); uart_if->current_tx_buf = buf; - err = uart_tx(uart_if->uart_dev, buf->data, buf->len, SYS_FOREVER_US); + err = uart_tx(uart_if->uart_dev, uart_tx_dma_buf, buf->len, SYS_FOREVER_US); if (err != 0) { LOG_ERR("uart_tx failed with error %d, even with flow control!", err); k_sem_give(&uart_if->tx_sem); + ares_uart_complete_tx(interface, uart_if->current_tx_buf, -EIO); net_buf_unref(uart_if->current_tx_buf); uart_if->current_tx_buf = NULL; } @@ -101,6 +170,7 @@ static void uart_callback(const struct device *dev, struct uart_event *evt, void /* --- TX 事件处理 --- */ case UART_TX_DONE: if (uart_if->current_tx_buf != NULL) { + ares_uart_complete_tx(uart_if->interface, uart_if->current_tx_buf, 0); net_buf_unref(uart_if->current_tx_buf); uart_if->current_tx_buf = NULL; } @@ -110,6 +180,8 @@ static void uart_callback(const struct device *dev, struct uart_event *evt, void case UART_TX_ABORTED: LOG_WRN("UART TX aborted"); if (uart_if->current_tx_buf != NULL) { + ares_uart_complete_tx(uart_if->interface, uart_if->current_tx_buf, + -ECANCELED); net_buf_unref(uart_if->current_tx_buf); uart_if->current_tx_buf = NULL; } @@ -178,6 +250,7 @@ void ares_uart_thread_entry(void *p1, void *p2, void *p3) void ares_uart_init_dev(struct AresInterface *interface, const struct device *uart_dev) { struct AresUartInterface *uart_if = interface->priv_data; + uart_if->interface = interface; uart_if->uart_dev = (struct device *)uart_dev; } @@ -185,6 +258,7 @@ void ares_uart_init_dev(struct AresInterface *interface, const struct device *ua int ares_uart_init(struct AresInterface *interface) { struct AresUartInterface *uart_if = interface->priv_data; + uart_if->interface = interface; if (!device_is_ready(uart_if->uart_dev)) { LOG_ERR("UART device not ready"); @@ -215,7 +289,6 @@ int ares_uart_init(struct AresInterface *interface) (void *)interface, NULL, NULL, ARES_UART_PROCESSING_THREAD_PRIORITY, 0, K_NO_WAIT); k_thread_name_set(&uart_if->thread, "ares_uart_rx"); - k_thread_start(&uart_if->thread); /* --- 新增: 初始化 TX 部分 --- */ uart_if->current_tx_buf = NULL; @@ -228,7 +301,6 @@ int ares_uart_init(struct AresInterface *interface) (void *)interface, NULL, NULL, ARES_UART_PROCESSING_THREAD_PRIORITY, 0, K_NO_WAIT); k_thread_name_set(&uart_if->tx_thread, "ares_uart_tx"); - k_thread_start(&uart_if->tx_thread); LOG_INF("Ares UART Interface initialized with TX queue."); return 0; diff --git a/lib/ares/interface/usb/usb_bulk.c b/lib/ares/interface/usb/usb_bulk.c index e2ca82f..a5d30cc 100644 --- a/lib/ares/interface/usb/usb_bulk.c +++ b/lib/ares/interface/usb/usb_bulk.c @@ -46,6 +46,7 @@ static struct AresInterface *ares_interface; #define ARES_IF_FUNCTION_ENABLED 0 #define ARES_IF_FUNCTION_OUT_ENGAGED 1 #define ARES_IF_FUNCTION_IN_ENGAGED 2 +#define ARES_IF_FUNCTION_CONNECTED 3 /* === USB Device and Descriptor Definitions === */ @@ -68,6 +69,7 @@ USBD_CONFIGURATION_DEFINE(ares_hs_config, attributes, 250, &hs_cfg_desc); // Forward declaration struct usbd_class_data *ares_if_class_data; +static void ares_usbd_protocol_event(enum AresProtocolEvent event); struct ares_if_desc { struct usb_if_descriptor if0; @@ -85,6 +87,38 @@ struct ares_if_data { atomic_t state; }; +struct ares_udc_buf_info { + struct udc_buf_info udc_buf_info; + ares_interface_tx_done_cb_t tx_done; + void *tx_user_data; +}; + +static void ares_usbd_set_connected(struct usbd_class_data *const c_data, bool connected) +{ + struct ares_if_data *data; + + if (c_data == NULL) { + return; + } + + data = usbd_class_get_private(c_data); + if (data == NULL) { + return; + } + + if (connected) { + if (!atomic_test_bit(&data->state, ARES_IF_FUNCTION_ENABLED)) { + return; + } + + if (!atomic_test_and_set_bit(&data->state, ARES_IF_FUNCTION_CONNECTED)) { + ares_usbd_protocol_event(ARES_PROTOCOL_EVENT_CONNECTED); + } + } else if (atomic_test_and_clear_bit(&data->state, ARES_IF_FUNCTION_CONNECTED)) { + ares_usbd_protocol_event(ARES_PROTOCOL_EVENT_DISCONNECTED); + } +} + static uint8_t ares_if_get_bulk_out(struct usbd_class_data *const c_data) { struct ares_if_data *data = usbd_class_get_private(c_data); @@ -167,6 +201,10 @@ static int ares_if_request_handler(struct usbd_class_data *const c_data, struct if (ep == ares_if_get_bulk_in(c_data)) { atomic_clear_bit(&data->state, ARES_IF_FUNCTION_IN_ENGAGED); + struct ares_udc_buf_info *buf_info = net_buf_user_data(buf); + if (buf_info && buf_info->tx_done) { + buf_info->tx_done(ares_interface, buf, err_code, buf_info->tx_user_data); + } net_buf_unref(buf); /* @@ -262,13 +300,19 @@ static void *ares_if_get_desc(struct usbd_class_data *const c_data, const enum u static void ares_if_enable(struct usbd_class_data *const c_data) { struct ares_if_data *data = usbd_class_get_private(c_data); + int err; + LOG_INF("Enable ARES Bulk interface"); if (atomic_test_and_set_bit(&data->state, ARES_IF_FUNCTION_ENABLED)) { return; } - if (ares_if_submit_bulk_out(c_data) != 0) { + err = ares_if_submit_bulk_out(c_data); + if (err != 0) { LOG_ERR("Failed to submit initial bulk OUT request on enable"); + return; } + + ares_usbd_set_connected(c_data, true); } static void ares_if_disable(struct usbd_class_data *const c_data) @@ -277,6 +321,8 @@ static void ares_if_disable(struct usbd_class_data *const c_data) struct net_buf *buf; LOG_INF("Disable ARES Bulk interface"); + ares_usbd_set_connected(c_data, false); + atomic_clear_bit(&data->state, ARES_IF_FUNCTION_ENABLED); atomic_clear_bit(&data->state, ARES_IF_FUNCTION_IN_ENGAGED); atomic_clear_bit(&data->state, ARES_IF_FUNCTION_OUT_ENGAGED); @@ -337,11 +383,6 @@ static void ares_processing_thread_entry(void *p1, void *p2, void *p3) /* === Public API and Setup Function === */ -struct ares_udc_buf_info { - struct udc_buf_info udc_buf_info; - struct k_mutex *mutex; -} __attribute__((packed)); - static const char *const ares_usbd_bulk_first_blocklist[] = { "ares_if_0", NULL, @@ -350,18 +391,14 @@ static const char *const ares_usbd_bulk_first_blocklist[] = { /* To send data, we can allocate from the general-purpose system pool */ void buf_cb_unlock(struct net_buf *buf) { - struct ares_udc_buf_info *buf_info = net_buf_user_data(buf); - if (buf_info->mutex) { - LOG_DBG("Unlocking mutex %p", buf_info->mutex); - k_mutex_unlock(buf_info->mutex); - } LOG_DBG("Destroying buffer %p", buf); net_buf_destroy(buf); } UDC_BUF_POOL_DEFINE(tx_pool, 8, 512, sizeof(struct ares_udc_buf_info), buf_cb_unlock); -int ares_usbd_write(struct AresInterface *interface, struct net_buf *buf) +static int ares_usbd_write_common(struct AresInterface *interface, struct net_buf *buf, + ares_interface_tx_done_cb_t cb, void *user_data) { struct usbd_class_data *c_data = ares_if_class_data; struct ares_if_data *data = NULL; @@ -388,7 +425,8 @@ int ares_usbd_write(struct AresInterface *interface, struct net_buf *buf) struct ares_udc_buf_info *buf_info = net_buf_user_data(buf); if (buf_info) { - buf_info->mutex = NULL; + buf_info->tx_done = cb; + buf_info->tx_user_data = user_data; buf_info->udc_buf_info.ep = ares_if_get_bulk_in(c_data); } @@ -397,6 +435,9 @@ int ares_usbd_write(struct AresInterface *interface, struct net_buf *buf) atomic_clear_bit(&data->state, ARES_IF_FUNCTION_IN_ENGAGED); LOG_ERR("Enqueue error %d", err); clear_exit: + if (cb) { + cb(interface, buf, err, user_data); + } net_buf_unref(buf); } else { LOG_DBG("Enqueueing buffer %p", buf); @@ -404,48 +445,35 @@ int ares_usbd_write(struct AresInterface *interface, struct net_buf *buf) return err; } -int ares_usbd_write_with_lock(struct AresInterface *interface, struct net_buf *buf, - struct k_mutex *mutex) +int ares_usbd_write(struct AresInterface *interface, struct net_buf *buf) { - struct usbd_class_data *c_data = ares_if_class_data; - struct ares_if_data *data = NULL; - int err; + return ares_usbd_write_common(interface, buf, NULL, NULL); +} - if (c_data == NULL) { - err = -ENODEV; - goto clear_exit; - } +int ares_usbd_write_with_callback(struct AresInterface *interface, struct net_buf *buf, + ares_interface_tx_done_cb_t cb, void *user_data) +{ + return ares_usbd_write_common(interface, buf, cb, user_data); +} - data = usbd_class_get_private(c_data); - if (data == NULL) { - err = -EINVAL; - goto clear_exit; - } - if (!atomic_test_bit(&data->state, ARES_IF_FUNCTION_ENABLED)) { - err = -EPERM; - goto clear_exit; - } - if (atomic_test_and_set_bit(&data->state, ARES_IF_FUNCTION_IN_ENGAGED)) { - err = -EBUSY; - goto clear_exit; - } +uint32_t ares_usbd_caps(struct AresInterface *interface) +{ + ARG_UNUSED(interface); - struct ares_udc_buf_info *buf_info = net_buf_user_data(buf); - if (buf_info) { - buf_info->mutex = mutex; - buf_info->udc_buf_info.ep = ares_if_get_bulk_in(c_data); - } + return ARES_INTERFACE_CAP_PACKET | ARES_INTERFACE_CAP_ZERO_COPY | + ARES_INTERFACE_CAP_TX_COMPLETE; +} - err = usbd_ep_enqueue(c_data, buf); - if (err) { - atomic_clear_bit(&data->state, ARES_IF_FUNCTION_IN_ENGAGED); - LOG_DBG("locked Enqueue error %d", err); -clear_exit: - net_buf_unref(buf); - } else { - LOG_DBG("locked Enqueueing buffer %p", buf); +size_t ares_usbd_mtu(struct AresInterface *interface) +{ + struct usbd_class_data *c_data = ares_if_class_data; + + ARG_UNUSED(interface); + + if (c_data == NULL) { + return 64U; } - return err; + return ares_if_get_mps(c_data); } struct net_buf *ares_interface_alloc_buf(struct AresInterface *interface) @@ -454,7 +482,8 @@ struct net_buf *ares_interface_alloc_buf(struct AresInterface *interface) if (buf) { struct ares_udc_buf_info *buf_info = net_buf_user_data(buf); if (buf_info) { - buf_info->mutex = NULL; + buf_info->tx_done = NULL; + buf_info->tx_user_data = NULL; } } return buf; @@ -467,7 +496,8 @@ struct net_buf *ares_interface_alloc_buf_with_data(struct AresInterface *interfa if (buf) { struct ares_udc_buf_info *buf_info = net_buf_user_data(buf); if (buf_info) { - buf_info->mutex = NULL; + buf_info->tx_done = NULL; + buf_info->tx_user_data = NULL; } } return buf; @@ -516,16 +546,16 @@ static void ares_usbd_msg_cb(struct usbd_context *const usbd_ctx, const struct u { switch (msg->type) { case USBD_STATE_CONFIGURED: - LOG_INF("USB device configured"); - ares_usbd_protocol_event(ARES_PROTOCOL_EVENT_CONNECTED); + // LOG_INF("USB device configured"); + ares_usbd_set_connected(ares_if_class_data, true); break; case USBD_MSG_RESET: - LOG_INF("USB device reset"); - ares_usbd_protocol_event(ARES_PROTOCOL_EVENT_DISCONNECTED); + // LOG_INF("USB device reset"); + ares_usbd_set_connected(ares_if_class_data, false); break; case USBD_MSG_SUSPEND: LOG_INF("USB device suspended"); - ares_usbd_protocol_event(ARES_PROTOCOL_EVENT_DISCONNECTED); + ares_usbd_set_connected(ares_if_class_data, false); break; default: break; diff --git a/lib/ares/interface/usb/usb_bulk.h b/lib/ares/interface/usb/usb_bulk.h index caa557d..925a85b 100644 --- a/lib/ares/interface/usb/usb_bulk.h +++ b/lib/ares/interface/usb/usb_bulk.h @@ -13,8 +13,10 @@ int ares_usbd_write(struct AresInterface *interface, struct net_buf *buf); struct net_buf *ares_interface_alloc_buf(struct AresInterface *interface); struct net_buf *ares_interface_alloc_buf_with_data(struct AresInterface *interface, void *data, size_t len); -int ares_usbd_write_with_lock(struct AresInterface *interface, struct net_buf *buf, - struct k_mutex *mutex); +int ares_usbd_write_with_callback(struct AresInterface *interface, struct net_buf *buf, + ares_interface_tx_done_cb_t cb, void *user_data); +uint32_t ares_usbd_caps(struct AresInterface *interface); +size_t ares_usbd_mtu(struct AresInterface *interface); struct AresBulkInterface { struct AresInterface *interface; @@ -32,7 +34,9 @@ struct AresBulkInterface { struct AresInterfaceAPI ares_bulk_interface_api = { \ .init = ares_usbd_init, \ .send = ares_usbd_write, \ - .send_with_lock = ares_usbd_write_with_lock, \ + .send_with_callback = ares_usbd_write_with_callback, \ + .caps = ares_usbd_caps, \ + .mtu = ares_usbd_mtu, \ .alloc_buf = ares_interface_alloc_buf, \ .alloc_buf_with_data = ares_interface_alloc_buf_with_data, \ }; \ diff --git a/lib/ares/protocol/dual/dual_protocol.c b/lib/ares/protocol/dual/dual_protocol.c index f5f1a8a..3e493c5 100755 --- a/lib/ares/protocol/dual/dual_protocol.c +++ b/lib/ares/protocol/dual/dual_protocol.c @@ -86,14 +86,40 @@ struct net_buf *alloc_and_add_data(struct AresProtocol *protocol, uint8_t *data, } } -int send_try_lock(struct AresProtocol *protocol, struct net_buf *buf, struct k_mutex *mutex) +static int send_with_tx_done(struct AresProtocol *protocol, struct net_buf *buf, + ares_interface_tx_done_cb_t cb, void *user_data) { - if (protocol->interface->api->send_with_lock && mutex) { - return protocol->interface->api->send_with_lock(protocol->interface, buf, mutex); - } else { - k_mutex_unlock(mutex); + if (buf == NULL) { + return -ENOMEM; + } + + if (cb != NULL) { + if (protocol->interface->api->send_with_callback == NULL) { + net_buf_unref(buf); + return -ENOTSUP; + } + return protocol->interface->api->send_with_callback(protocol->interface, buf, cb, + user_data); + } + + if (protocol->interface->api->send) { return protocol->interface->api->send(protocol->interface, buf); } + + net_buf_unref(buf); + return -ENOTSUP; +} + +static void dual_clear_tx_in_flight(struct AresInterface *interface, struct net_buf *buf, + int status, void *user_data) +{ + atomic_t *tx_state = user_data; + + ARG_UNUSED(interface); + ARG_UNUSED(buf); + ARG_UNUSED(status); + + atomic_clear_bit(tx_state, DUAL_TX_IN_FLIGHT); } static void error_handle(struct AresProtocol *protocol, uint16_t req_id, uint16_t error) @@ -204,9 +230,19 @@ static void usb_offline_clean(struct AresProtocol *protocol) { struct tx_msg_bck msg; struct dual_protocol_data *data = protocol->priv_data; + + for (uint8_t i = 0; i < data->func_cnt; i++) { + atomic_clear_bit(&data->func_table[i].tx_state, DUAL_TX_IN_FLIGHT); + } + + for (uint8_t i = 0; i < data->sync_cnt; i++) { + atomic_clear_bit(&data->sync_table[i].tx_state, DUAL_TX_IN_FLIGHT); + } + while (k_msgq_num_used_get(&data->func_tx_bckup_msgq) > 0) { k_msgq_get(&data->func_tx_bckup_msgq, &msg, K_NO_WAIT); } + data->func_tx_bckup_cnt = 0; } static void usb_trans_heart_beat(struct k_timer *timer) @@ -312,11 +348,12 @@ static void parse_func(struct AresProtocol *protocol, func_table_t *map) k_msgq_put(&data->func_tx_bckup_msgq, &msg, K_NO_WAIT); data->func_tx_bckup_cnt++; - uint8_t *repl_frame = map->buf; - if (k_mutex_lock(&map->mutex, K_NO_WAIT) != 0) { - LOG_DBG("%s Failed to lock FUNC frame mutex.", data->name); + if (atomic_test_and_set_bit(&map->tx_state, DUAL_TX_IN_FLIGHT)) { + LOG_DBG("%s FUNC reply frame is still in flight.", data->name); return; } + + uint8_t *repl_frame = map->buf; GET_16BITS(repl_frame, REPL_HEAD_IDX) = REPL_FRAME_HEAD; GET_16BITS(repl_frame, REPL_FUNC_ID_IDX) = map->id; GET_32BITS(repl_frame, REPL_RET_IDX) = (uint32_t)ret; @@ -330,9 +367,10 @@ static void parse_func(struct AresProtocol *protocol, func_table_t *map) int err = 0; struct net_buf *buf = alloc_and_add_data(protocol, repl_frame, frame_len); - err = send_try_lock(protocol, buf, &map->mutex); + err = send_with_tx_done(protocol, buf, dual_clear_tx_in_flight, &map->tx_state); if (err != 0) { + atomic_clear_bit(&map->tx_state, DUAL_TX_IN_FLIGHT); LOG_ERR("%s Failed to send REPLY frame.", data->name); return; } @@ -392,6 +430,9 @@ int dual_sync_flush(struct AresProtocol *protocol, sync_table_t *pack) LOG_ERR("%s Sync pack is NULL.", data->name); return -EINVAL; } + if (atomic_test_and_set_bit(&pack->tx_state, DUAL_TX_IN_FLIGHT)) { + return -EBUSY; + } memcpy(pack->buf + SYNC_DATA_IDX, pack->data, pack->len); size_t frame_len = pack->len + SYNC_FRAME_LENGTH_OFFSET; @@ -403,9 +444,10 @@ int dual_sync_flush(struct AresProtocol *protocol, sync_table_t *pack) // LOG_HEXDUMP_DBG(pack->buf, frame_len, "Sync data to send:"); struct net_buf *buf = alloc_and_add_data(protocol, pack->buf, frame_len); - int ret = send_try_lock(protocol, buf, &pack->mutex); + int ret = send_with_tx_done(protocol, buf, dual_clear_tx_in_flight, &pack->tx_state); if (ret != 0) { + atomic_clear_bit(&pack->tx_state, DUAL_TX_IN_FLIGHT); // LOG_ERR("Failed to send SYNC frame. %d", ret); return -EBUSY; } @@ -463,6 +505,7 @@ sync_table_t *dual_sync_add(struct AresProtocol *protocol, uint16_t ID, uint8_t data->sync_table[data->sync_cnt].ID = ID; data->sync_table[data->sync_cnt].len = len; data->sync_table[data->sync_cnt].cb = cb; + atomic_set(&data->sync_table[data->sync_cnt].tx_state, 0); size_t buf_size = len + SYNC_FRAME_LENGTH_OFFSET; if (data->crc_enabled) { @@ -471,12 +514,12 @@ sync_table_t *dual_sync_add(struct AresProtocol *protocol, uint16_t ID, uint8_t data->sync_table[data->sync_cnt].buf = k_heap_aligned_alloc(&dual_protocol_heap, 4, buf_size, K_NO_WAIT); - memset(data->sync_table[data->sync_cnt].buf, 0, buf_size); if (data->sync_table[data->sync_cnt].buf == NULL) { LOG_ERR("%s Failed to allocate memory for SYNC frame. Count: %d", data->name, data->sync_cnt); return NULL; } + memset(data->sync_table[data->sync_cnt].buf, 0, buf_size); GET_16BITS(data->sync_table[data->sync_cnt].buf, SYNC_HEAD_IDX) = SYNC_FRAME_HEAD; GET_16BITS(data->sync_table[data->sync_cnt].buf, SYNC_ID_IDX) = ID; data->sync_cnt++; @@ -502,6 +545,7 @@ void dual_func_add(struct AresProtocol *protocol, uint16_t id, dual_trans_func_t } data->func_table[data->func_cnt].id = id; data->func_table[data->func_cnt].cb = cb; + atomic_set(&data->func_table[data->func_cnt].tx_state, 0); data->func_cnt++; } @@ -806,12 +850,14 @@ void ares_dual_protocol_event(struct AresProtocol *protocol, enum AresProtocolEv if (event == ARES_PROTOCOL_EVENT_CONNECTED) { LOG_INF("%s Connection established due to event.", data->name); k_msleep(600); + usb_offline_clean(protocol); data->online = true; // 重置状态机状态 reset_parser_state(data); } else if (event == ARES_PROTOCOL_EVENT_DISCONNECTED) { LOG_INF("%s Connection lost due to event.", data->name); data->online = false; + usb_offline_clean(protocol); // 重置状态机状态 reset_parser_state(data); } From f0bb20766f2735d308ab2af1ac2812925e051c9b Mon Sep 17 00:00:00 2001 From: ttwards <12411711@mail.sustech.edu.cn> Date: Wed, 8 Jul 2026 01:30:09 +0800 Subject: [PATCH 3/6] refactor(motor): require explicit CAN send priority Remove the boolean motor_can_sched_send_prio() wrapper and route high-priority motor control frames through motor_can_sched_send_with_priority() with MOTOR_CAN_SCHED_PRIO_CRITICAL. Validation: git diff --cached --check; west twister -T tests/native_sim/motor_driver_sim --platform native_sim/native/64 --inline-logs. --- Documents/api-reference.md | 3 +-- Documents/motor/architecture.md | 1 - Documents/motor/can-scheduler.md | 2 +- drivers/motor/common/motor_can_sched.c | 11 -------- drivers/motor/common/motor_can_sched.h | 2 -- drivers/motor/dji/motor_dji.c | 13 ++++++--- drivers/motor/dm/motor_dm.c | 5 ++-- drivers/motor/lk/motor_lk.c | 37 ++++++++++++++++---------- drivers/motor/mi/motor_mi.c | 17 +++++++----- drivers/motor/robstride/motor_rs.c | 21 ++++++++++----- drivers/motor/vesc/motor_vesc.c | 3 ++- 11 files changed, 64 insertions(+), 51 deletions(-) diff --git a/Documents/api-reference.md b/Documents/api-reference.md index 128cc22..e4b1a23 100644 --- a/Documents/api-reference.md +++ b/Documents/api-reference.md @@ -11,7 +11,7 @@ Motor API 将不同厂商的旋转电机统一为 `set/get/control` 三类应用操作。 控制器类型、参数、选择与内置计算辅助位于 -`include/zephyr/drivers/motor/controller.h`；`motor.h` 会继续包含该头文件以保持兼容。 +`include/zephyr/drivers/motor/controller.h`。 #### 类型 @@ -190,7 +190,6 @@ PID 接口是旧版控制器工具。Motor 新 controller 不依赖该公共 API | `motor_can_sched_register_can(can_dev)` | 注册一个 CAN 设备。 | `0` 成功，负 errno 失败。 | | `motor_can_sched_send(can_dev, frame, param, handle_out)` | 通用发送入口，可配置周期、应答追踪与优先级。 | `0` 成功，负 errno 失败。 | | `motor_can_sched_send_with_priority(can_dev, frame, priority, tag)` | 按明确优先级发送单帧。 | `0` 成功，负 errno 失败。 | -| `motor_can_sched_send_prio(can_dev, frame, high_priority, tag)` | 发送单帧，高优先级布尔入口。 | `0` 成功，负 errno 失败。 | | `motor_can_sched_send_reply(can_dev, frame, reply_id, reply_mask, timeout_ms, tag)` | 发送请求回复帧。 | `0` 成功，负 errno 失败。 | | `motor_can_sched_update(handle, frame)` | 更新周期帧内容。 | `0` 成功，负 errno 失败。 | | `motor_can_sched_remove(handle)` | 删除周期帧。 | `0` 成功，负 errno 失败。 | diff --git a/Documents/motor/architecture.md b/Documents/motor/architecture.md index 8f28b8e..ee8ae1a 100644 --- a/Documents/motor/architecture.md +++ b/Documents/motor/architecture.md @@ -196,7 +196,6 @@ stateDiagram-v2 | 接口 | 用途 | | --- | --- | | `motor_can_sched_register_can()` | 注册 CAN 设备。 | -| `motor_can_sched_send_prio()` | 发送普通帧，可选高优先级。 | | `motor_can_sched_send_with_priority()` | 以明确优先级发送单帧。 | | `motor_can_sched_send_reply()` | 发送会触发回复的帧，并跟踪回复超时。 | | `motor_can_sched_send()` | 通用入口，支持周期帧和回复跟踪。 | diff --git a/Documents/motor/can-scheduler.md b/Documents/motor/can-scheduler.md index 27c4f52..8fda756 100644 --- a/Documents/motor/can-scheduler.md +++ b/Documents/motor/can-scheduler.md @@ -44,7 +44,7 @@ int motor_can_sched_register_can(const struct device *can_dev); - 周期控制帧默认使用 `NORMAL`。 - 不要把所有帧都提升到高优先级，否则调度器失去意义。 -`motor_can_sched_send_prio()` 是较窄的便捷接口，只暴露“普通”与“高优先级”的常用路径。需要更细粒度控制时，使用 `motor_can_sched_send_with_priority()` 或通用 `motor_can_sched_send()`。 +单帧发送应使用 `motor_can_sched_send_with_priority()` 明确声明优先级；需要周期帧或回复跟踪时，使用通用 `motor_can_sched_send()`。 ## 发送模型 diff --git a/drivers/motor/common/motor_can_sched.c b/drivers/motor/common/motor_can_sched.c index d675ca1..5d8e896 100644 --- a/drivers/motor/common/motor_can_sched.c +++ b/drivers/motor/common/motor_can_sched.c @@ -1010,17 +1010,6 @@ int motor_can_sched_send(const struct device *can_dev, const struct can_frame *f return 0; } -int motor_can_sched_send_prio(const struct device *can_dev, const struct can_frame *frame, - bool high_priority, const char *tag) -{ - const struct motor_can_sched_tx_param param = { - .high_priority = high_priority, - .tag = tag, - }; - - return motor_can_sched_send(can_dev, frame, ¶m, NULL); -} - int motor_can_sched_send_with_priority(const struct device *can_dev, const struct can_frame *frame, enum motor_can_sched_prio priority, const char *tag) { diff --git a/drivers/motor/common/motor_can_sched.h b/drivers/motor/common/motor_can_sched.h index 641b3b1..8d55f09 100644 --- a/drivers/motor/common/motor_can_sched.h +++ b/drivers/motor/common/motor_can_sched.h @@ -104,8 +104,6 @@ int motor_can_sched_send(const struct device *can_dev, const struct can_frame *f motor_can_sched_handle_t *handle_out); int motor_can_sched_send_with_priority(const struct device *can_dev, const struct can_frame *frame, enum motor_can_sched_prio priority, const char *tag); -int motor_can_sched_send_prio(const struct device *can_dev, const struct can_frame *frame, - bool high_priority, const char *tag); int motor_can_sched_send_reply(const struct device *can_dev, const struct can_frame *frame, uint32_t reply_id, uint32_t reply_mask, uint16_t timeout_ms, const char *tag); diff --git a/drivers/motor/dji/motor_dji.c b/drivers/motor/dji/motor_dji.c index 74ca5d7..d431317 100644 --- a/drivers/motor/dji/motor_dji.c +++ b/drivers/motor/dji/motor_dji.c @@ -342,7 +342,9 @@ void dji_control(const struct device *dev, enum motor_cmd cmd) frame.data[1] = (cfg->common.rx_id - 0x200) >> 8; frame.data[2] = 0x55; frame.data[3] = 0x3C; - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "dji-set-zero"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, + "dji-set-zero"); } break; case CLEAR_CONTROLLER: @@ -357,7 +359,9 @@ void dji_control(const struct device *dev, enum motor_cmd cmd) frame.data[1] = (cfg->common.rx_id - 0x200) >> 8; frame.data[2] = 0x55; frame.data[3] = 0x50; - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "dji-clear-error"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, + "dji-clear-error"); } break; } @@ -790,8 +794,9 @@ void dji_tx_handler(struct k_work *work) txframe.dlc = 8; txframe.flags = 0; const struct device *can_dev = ctrl_struct->can_dev; - motor_can_sched_send_prio(can_dev, &txframe, true, - "dji-feedback-control"); + motor_can_sched_send_with_priority( + can_dev, &txframe, MOTOR_CAN_SCHED_PRIO_CRITICAL, + "dji-feedback-control"); } } } diff --git a/drivers/motor/dm/motor_dm.c b/drivers/motor/dm/motor_dm.c index 9570a0c..273ba47 100644 --- a/drivers/motor/dm/motor_dm.c +++ b/drivers/motor/dm/motor_dm.c @@ -93,7 +93,8 @@ static int dm_send_cmd_frame(const struct device *dev, const uint8_t data[8], co }; memcpy(frame.data, data, 8); - return motor_can_sched_send_prio(cfg->common.phy, &frame, true, tag); + return motor_can_sched_send_with_priority(cfg->common.phy, &frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, tag); } static int dm_send_cmd_frame_reply(const struct device *dev, const uint8_t data[8], const char *tag) @@ -301,7 +302,7 @@ static void dm_edit_reg_value(const struct device *dev, uint16_t can_id, uint8_t frame.data[5] = reg_value >> 8; frame.data[6] = reg_value >> 16; frame.data[7] = reg_value >> 24; - motor_can_sched_send_prio(dev, &frame, true, "dm-reg"); + motor_can_sched_send_with_priority(dev, &frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, "dm-reg"); } static void dm_edit_reg_float(const struct device *dev, uint16_t can_id, uint8_t reg_addr, diff --git a/drivers/motor/lk/motor_lk.c b/drivers/motor/lk/motor_lk.c index 4a4158d..d868830 100644 --- a/drivers/motor/lk/motor_lk.c +++ b/drivers/motor/lk/motor_lk.c @@ -62,13 +62,15 @@ void lk_motor_control(const struct device *dev, enum motor_cmd cmd) switch (cmd) { case ENABLE_MOTOR: frame.data[0] = LK_CMD_MOTOR_RUN; // 0x88 - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "lk-enable"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, "lk-enable"); k_msleep(10); motor_link_request_enable(&data->common.link); break; case DISABLE_MOTOR: frame.data[0] = LK_CMD_MOTOR_OFF; // 0x80 - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "lk-disable"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, "lk-disable"); motor_link_request_disable(&data->common.link); break; case SET_ZERO: @@ -82,11 +84,13 @@ void lk_motor_control(const struct device *dev, enum motor_cmd cmd) frame.data[5] = (((uint32_t)(set_angle * LK_POS_FACTOR)) >> 8) & 0xFF; frame.data[6] = (((uint32_t)(set_angle * LK_POS_FACTOR)) >> 16) & 0xFF; frame.data[7] = (((uint32_t)(set_angle * LK_POS_FACTOR)) >> 24) & 0xFF; - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "lk-set-zero"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, "lk-set-zero"); break; case CLEAR_ERROR: frame.data[0] = LK_CMD_CLEAR_ERR; // 0x9B - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "lk-clear-error"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, "lk-clear-error"); break; case CLEAR_CONTROLLER: break; @@ -360,8 +364,9 @@ void lk_tx_data_handler(struct k_work *work) tx_frame.dlc = 8; tx_frame.flags = 0; tx_frame.data[0] = LK_CMD_MOTOR_RUN; - motor_can_sched_send_prio(cfg->common.phy, &tx_frame, true, - "lk-retry-enable"); + motor_can_sched_send_with_priority( + cfg->common.phy, &tx_frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, + "lk-retry-enable"); } continue; } @@ -371,8 +376,9 @@ void lk_tx_data_handler(struct k_work *work) tx_frame.dlc = 8; tx_frame.flags = 0; tx_frame.data[0] = LK_CMD_MOTOR_RUN; - motor_can_sched_send_prio(cfg->common.phy, &tx_frame, true, - "lk-reenable"); + motor_can_sched_send_with_priority( + cfg->common.phy, &tx_frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, + "lk-reenable"); data->params_update[0] = true; data->params_update[1] = true; data->params_update[2] = true; @@ -420,8 +426,9 @@ void lk_tx_params_data_handler(struct k_work *work) int16_t kd_val = (int16_t)(data->params[0].k_d); tx_frame.data[6] = kd_val & 0xFF; tx_frame.data[7] = (kd_val >> 8) & 0xFF; - motor_can_sched_send_prio(cfg->common.phy, &tx_frame, true, - "lk-param-angle"); + motor_can_sched_send_with_priority( + cfg->common.phy, &tx_frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, + "lk-param-angle"); } k_sleep(K_USEC(120)); if (data->params_update[1]) { @@ -441,8 +448,9 @@ void lk_tx_params_data_handler(struct k_work *work) int16_t kd_val = (int16_t)(data->params[1].k_d); tx_frame.data[6] = kd_val & 0xFF; tx_frame.data[7] = (kd_val >> 8) & 0xFF; - motor_can_sched_send_prio(cfg->common.phy, &tx_frame, true, - "lk-param-speed"); + motor_can_sched_send_with_priority( + cfg->common.phy, &tx_frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, + "lk-param-speed"); } k_sleep(K_USEC(120)); if (data->params_update[2]) { @@ -462,8 +470,9 @@ void lk_tx_params_data_handler(struct k_work *work) int16_t kd_val = (int16_t)(data->params[2].k_d); tx_frame.data[6] = kd_val & 0xFF; tx_frame.data[7] = (kd_val >> 8) & 0xFF; - motor_can_sched_send_prio(cfg->common.phy, &tx_frame, true, - "lk-param-torque"); + motor_can_sched_send_with_priority( + cfg->common.phy, &tx_frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, + "lk-param-torque"); } k_sleep(K_USEC(120)); } diff --git a/drivers/motor/mi/motor_mi.c b/drivers/motor/mi/motor_mi.c index 63b5a3b..94327f1 100644 --- a/drivers/motor/mi/motor_mi.c +++ b/drivers/motor/mi/motor_mi.c @@ -75,12 +75,14 @@ void mi_motor_control(const struct device *dev, enum motor_cmd cmd) switch (cmd) { case ENABLE_MOTOR: mi_can_id->mi_msg_mode = Communication_Type_MotorEnable; - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "mi-enable"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, "mi-enable"); motor_link_request_enable(&data->common.link); break; case DISABLE_MOTOR: mi_can_id->mi_msg_mode = Communication_Type_MotorStop; - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "mi-disable"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, "mi-disable"); motor_link_request_disable(&data->common.link); break; case SET_ZERO: @@ -88,7 +90,8 @@ void mi_motor_control(const struct device *dev, enum motor_cmd cmd) frame.data[0] = 0x01; data->delta_deg_sum = 0; data->common.angle = 0; - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "mi-set-zero"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, "mi-set-zero"); break; case CLEAR_CONTROLLER: @@ -221,7 +224,8 @@ static int mi_apply_controller_mode(const struct device *dev, enum motor_mode mo memcpy(&frame.data[0], &index, 2); frame.data[4] = (uint8_t)mode; - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "mi-set-mode"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, + "mi-set-mode"); for (int i = 0; i < motor_get_controller_count(dev); i++) { const struct motor_controller_config *ctrl_cfg = &cfg->common.controllers[i]; @@ -347,8 +351,9 @@ void mi_tx_data_handler(struct k_work *work) 0x1F00FF00, 5U, "mi-control"); if ((data->common.mode == PV) || (data->common.mode == VO)) { - motor_can_sched_send_prio(cfg->common.phy, &tx_frame[1], true, - "mi-follow"); + motor_can_sched_send_with_priority( + cfg->common.phy, &tx_frame[1], MOTOR_CAN_SCHED_PRIO_CRITICAL, + "mi-follow"); } } if (i % 2 == 1) { diff --git a/drivers/motor/robstride/motor_rs.c b/drivers/motor/robstride/motor_rs.c index 2ff15a4..dd5125e 100644 --- a/drivers/motor/robstride/motor_rs.c +++ b/drivers/motor/robstride/motor_rs.c @@ -158,12 +158,14 @@ static void rs_send_enable_frame(const struct device *dev, const char *tag_prefi frame.id = rs_pack_ext_id(Communication_Type_MotorStop, cfg->common.rx_id & 0xFF, cfg->common.tx_id & 0xFF, 0); frame.data[0] = 0x01; - motor_can_sched_send_prio(cfg->common.phy, &frame, true, tag_prefix); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, + tag_prefix); frame.id = rs_pack_ext_id(Communication_Type_MotorEnable, cfg->common.rx_id & 0xFF, cfg->common.tx_id & 0xFF, 0); frame.data[0] = 0x0; - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "rs-enable"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, + "rs-enable"); } static void rs_send_auto_report_frame(const struct device *dev, const char *tag) @@ -177,7 +179,8 @@ static void rs_send_auto_report_frame(const struct device *dev, const char *tag) frame.id = rs_pack_ext_id(Communication_Type_MotorReport, cfg->common.rx_id & 0xFF, cfg->common.tx_id & 0xFF, 0); - motor_can_sched_send_prio(cfg->common.phy, &frame, true, tag); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, + tag); } int rs_init(const struct device *dev) @@ -241,7 +244,8 @@ void rs_motor_control(const struct device *dev, enum motor_cmd cmd) case DISABLE_MOTOR: frame.id = rs_pack_ext_id(Communication_Type_MotorStop, cfg->common.rx_id & 0xFF, cfg->common.tx_id & 0xFF, 0); - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "rs-disable"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, "rs-disable"); motor_link_request_disable(&data->common.link); break; case SET_ZERO: @@ -249,13 +253,15 @@ void rs_motor_control(const struct device *dev, enum motor_cmd cmd) cfg->common.tx_id & 0xFF, 0); frame.data[0] = 0x01; data->common.angle = 0; - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "rs-set-zero"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, "rs-set-zero"); break; case CLEAR_ERROR: frame.id = rs_pack_ext_id(Communication_Type_MotorStop, cfg->common.rx_id & 0xFF, cfg->common.tx_id & 0xFF, 0); frame.data[0] = 0x01; - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "rs-clear-error"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, "rs-clear-error"); motor_link_request_disable(&data->common.link); break; case CLEAR_CONTROLLER: @@ -323,7 +329,8 @@ static int rs_send_control_frame(const struct device *dev) } rs_motor_pack(dev, &tx_frame); - ret = motor_can_sched_send_prio(cfg->common.phy, &tx_frame, true, "rs-control"); + ret = motor_can_sched_send_with_priority(cfg->common.phy, &tx_frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, "rs-control"); if (ret < 0) { motor_stats_inc(MOTOR_STAT_TX_ERROR); } diff --git a/drivers/motor/vesc/motor_vesc.c b/drivers/motor/vesc/motor_vesc.c index 5e23c36..e9dd118 100644 --- a/drivers/motor/vesc/motor_vesc.c +++ b/drivers/motor/vesc/motor_vesc.c @@ -109,7 +109,8 @@ static void vesc_send_ping(const struct device *dev) frame.dlc = 1; frame.data[0] = cfg->common.rx_id & 0xFF; - motor_can_sched_send_prio(cfg->common.phy, &frame, true, "vesc-ping"); + motor_can_sched_send_with_priority(cfg->common.phy, &frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, + "vesc-ping"); } static int vesc_send_control(const struct device *dev) From f93e5a149b3e4fb381d6a32f2c46d1e6f923dff9 Mon Sep 17 00:00:00 2001 From: ttwards <12411711@mail.sustech.edu.cn> Date: Wed, 8 Jul 2026 01:31:04 +0800 Subject: [PATCH 4/6] docs(communication): document ARES callback transport API Update the communication references for send_with_callback(), transport capability queries, and buffer ownership rules after the USB/UART interface callback update. --- Documents/api-reference.md | 33 ++++++++++-------- Documents/communication/interface.md | 51 +++++++++++++++++++--------- Documents/communication/protocol.md | 7 ++-- 3 files changed, 58 insertions(+), 33 deletions(-) diff --git a/Documents/api-reference.md b/Documents/api-reference.md index e4b1a23..e0af419 100644 --- a/Documents/api-reference.md +++ b/Documents/api-reference.md @@ -225,19 +225,22 @@ PID 接口是旧版控制器工具。Motor 新 controller 不依赖该公共 API 头文件：`include/ares/interface/ares_interface.h` -`struct AresInterfaceAPI` 是传输层驱动表： +`struct AresInterfaceAPI` 是传输层能力表。不是每个接口都会实现所有回调，协议层调用前应检查函数 +指针是否为空。 -| 回调 | 说明 | -| --- | --- | -| `send(interface, buf)` | 发送一个 `net_buf`。 | -| `send_with_lock(interface, buf, mutex)` | 在调用者给出的锁保护下发送。 | -| `send_raw(interface, data, len)` | 发送原始字节。 | -| `connect(interface)` | 建立连接。 | -| `disconnect(interface)` | 断开连接。 | -| `is_connected(interface)` | 查询连接状态。 | -| `alloc_buf(interface)` | 分配发送缓冲。 | -| `alloc_buf_with_data(interface, data, size)` | 分配并填充发送缓冲。 | -| `init(interface)` | 初始化接口对象。 | +| 回调 | 说明 | 当前实现 | +| --- | --- | --- | +| `send(interface, buf)` | 发送一个 `net_buf`。调用后缓冲所有权交给接口。 | UART、USB bulk | +| `send_with_callback(interface, buf, cb, user_data)` | 发送 `net_buf`，并在完成、中止或同步失败时通知调用者。 | UART、USB bulk | +| `send_raw(interface, data, len)` | 发送原始字节，不使用 `net_buf`。 | UART | +| `connect(interface)` | 建立连接。 | 预留，当前 UART/USB 未填 | +| `disconnect(interface)` | 断开连接。 | 预留，当前 UART/USB 未填 | +| `is_connected(interface)` | 查询连接状态。 | 预留，当前 UART/USB 未填 | +| `caps(interface)` | 返回 `AresInterfaceCaps` 能力位。 | UART、USB bulk | +| `mtu(interface)` | 返回接口建议帧长上限。 | UART、USB bulk | +| `alloc_buf(interface)` | 分配发送缓冲。 | UART、USB bulk | +| `alloc_buf_with_data(interface, data, size)` | 分配并携带现有数据块的发送缓冲。 | USB bulk | +| `init(interface)` | 初始化接口对象。 | UART、USB bulk | `struct AresInterface` 包含名称、API、绑定的协议和传输私有数据。 @@ -262,9 +265,11 @@ PID 接口是旧版控制器工具。Motor 新 controller 不依赖该公共 API | --- | --- | | `ares_usbd_init(interface)` | 初始化 USB bulk 接口。 | | `ares_usbd_write(interface, buf)` | 发送 `net_buf`。 | -| `ares_usbd_write_with_lock(interface, buf, mutex)` | 带外部锁发送。 | +| `ares_usbd_write_with_callback(interface, buf, cb, user_data)` | 发送并通知 TX 完成状态。 | +| `ares_usbd_caps(interface)` | 返回 USB bulk 能力位。 | +| `ares_usbd_mtu(interface)` | 返回当前 USB 速度下的 bulk endpoint MPS。 | | `ares_interface_alloc_buf(interface)` | 分配发送缓冲。 | -| `ares_interface_alloc_buf_with_data(interface, data, len)` | 分配并填充发送缓冲。 | +| `ares_interface_alloc_buf_with_data(interface, data, len)` | 分配并携带现有数据块的发送缓冲。 | | `ARES_BULK_INTERFACE_DEFINE(name)` | 定义 USB bulk 接口实例。 | ### 协议层 diff --git a/Documents/communication/interface.md b/Documents/communication/interface.md index b27a272..9cc2321 100644 --- a/Documents/communication/interface.md +++ b/Documents/communication/interface.md @@ -21,17 +21,29 @@ ARES 接口层统一承载底层链路，向协议层暴露稳定的发送、缓 ## `AresInterfaceAPI` -接口实现可以提供以下能力： +接口实现可以提供以下能力。这个结构体是能力表，不是强制每个接口完整实现的 vtable： - `send()`: 发送 `net_buf`。 -- `send_with_lock()`: 带互斥语义的发送。 -- `send_raw()`: 发送裸字节缓冲。 -- `connect()` / `disconnect()` / `is_connected()`: 连接状态接口。 +- `send_with_callback()`: 发送 `net_buf` 并在完成、abort 或同步失败时通知调用者。 +- `send_raw()`: 发送裸字节缓冲，当前由 UART 实现。 +- `connect()` / `disconnect()` / `is_connected()`: 连接状态接口，当前 UART/USB 宏未填，属于预留能力。 +- `caps()`: 返回 `AresInterfaceCaps` 能力位。 +- `mtu()`: 返回接口建议帧长上限。 - `alloc_buf()`: 分配发送缓冲。 -- `alloc_buf_with_data()`: 用现有数据块包装发送缓冲。 +- `alloc_buf_with_data()`: 用现有数据块创建发送缓冲，当前由 USB bulk 实现。 - `init()`: 初始化接口实例。 -并非所有接口都会实现全部入口。协议层在调用前应按空指针能力协商。 +当前宏填充情况： + +| 接口宏 | 已填回调 | +| --- | --- | +| `ARES_UART_INTERFACE_DEFINE()` | `init`、`send`、`send_with_callback`、`send_raw`、`caps`、`mtu`、`alloc_buf` | +| `ARES_BULK_INTERFACE_DEFINE()` | `init`、`send`、`send_with_callback`、`caps`、`mtu`、`alloc_buf`、`alloc_buf_with_data` | + +协议层在调用可选回调前必须按空指针能力协商。调用 `send()` 或 `send_with_callback()` 后， +`net_buf` 所有权交给接口；当前 UART/USB 实现会在发送完成、发送中止或同步入队失败时释放传入 +缓冲，调用者不应再次 `net_buf_unref()`。需要等待 TX 完成的协议应使用 `send_with_callback()`， +并在 `ares_interface_tx_done_cb_t` 里释放自己的在飞状态。 ## 绑定顺序 @@ -130,7 +142,7 @@ UART 异步回调收到 `UART_RX_RDY` 后： 这一设计保证了单通道串口发送有统一仲裁点，避免多个上下文直接打到底层驱动。 `ares_uart_send_raw()` 则是例外，它直接调用 `uart_tx()`，更适合 plotter/VOFA 这种自带帧组装、 -不依赖 `net_buf` 的快速路径。 +不依赖 `net_buf` 的快速路径。UART 接口没有实现 `alloc_buf_with_data()`。 ### 配置项 @@ -152,8 +164,8 @@ UART 接口常见失败点如下： - `uart_callback_set()` 失败：初始化直接返回底层错误码。 - `uart_tx()` 失败：发送线程记录错误并释放当前缓冲。 -调用者应将 `send()` 返回值视为“接口是否接管了缓冲”的边界；失败时不要再自行释放已经交给接口的 -缓冲，避免双重 `unref`。 +调用者应将 `send()` 调用视为缓冲所有权转移边界；失败时不要再自行释放已经交给接口的缓冲，避免双重 +`unref`。 ### 最小入口 @@ -183,7 +195,10 @@ USB bulk 接口适合： - `ares_usbd_init(struct AresInterface *interface)` - `ares_usbd_write(struct AresInterface *interface, struct net_buf *buf)` -- `ares_usbd_write_with_lock(struct AresInterface *interface, struct net_buf *buf, struct k_mutex *mutex)` +- `ares_usbd_write_with_callback(struct AresInterface *interface, struct net_buf *buf, + ares_interface_tx_done_cb_t cb, void *user_data)` +- `ares_usbd_caps(struct AresInterface *interface)` +- `ares_usbd_mtu(struct AresInterface *interface)` - `ares_interface_alloc_buf(struct AresInterface *interface)` - `ares_interface_alloc_buf_with_data(struct AresInterface *interface, void *data, size_t len)` @@ -231,20 +246,24 @@ OUT 端点启用后会持续预投递读请求。收到数据后： #### 发送 -发送调用 `ares_usbd_write()` 或 `ares_usbd_write_with_lock()`： +发送调用 `ares_usbd_write()` 或 `ares_usbd_write_with_callback()`： 1. 检查 USB class 和接口状态。 2. 检查 IN 端点是否已有在飞事务。 -3. 在 `net_buf` 用户区写入端点与互斥信息。 +3. 在 `net_buf` 用户区写入端点与完成回调信息。 4. 调用 `usbd_ep_enqueue()`。 -发送完成后，request handler 清除 IN engaged 状态，并通过 buffer 回调释放 `net_buf`； -若附带互斥锁，还会在缓冲释放回调中解锁。 +发送完成后，request handler 清除 IN engaged 状态，调用发送完成回调并释放 `net_buf`。同步入队失败时， +USB 层也会调用完成回调并释放缓冲。 + +USB bulk 接口没有实现 `send_raw()` 与 `connect()` / `disconnect()` / `is_connected()`。连接状态通过 +USBD 消息转换为协议事件，而不是通过 `AresInterfaceAPI` 查询。 ### `alloc_buf_with_data()` 的作用 -双向协议会优先使用 `alloc_buf_with_data()`。对 USB 来说，这允许直接把现成帧数据包装成 -发送缓冲，减少一次显式拷贝。维护者修改协议发送路径时，应保留这种能力协商。 +双向协议会优先使用 `alloc_buf_with_data()`。对 USB 来说，这允许用现成帧数据创建发送缓冲，减少协议层 +显式拷贝。维护者修改协议发送路径时，应保留这种能力协商，并为没有该能力的接口保留 +`alloc_buf()` + `net_buf_add_mem()` fallback。 ### 协议事件 diff --git a/Documents/communication/protocol.md b/Documents/communication/protocol.md index c6e97f6..213edcf 100644 --- a/Documents/communication/protocol.md +++ b/Documents/communication/protocol.md @@ -183,11 +183,12 @@ SYNC 帧长度不能仅靠帧头得知，必须在收到 ID 后查同步表长 双向协议的发送始终通过已绑定接口： -- 优先使用 `alloc_buf_with_data()` 包装现成帧 +- 优先使用 `alloc_buf_with_data()` 用现成帧数据创建发送缓冲 - 否则分配 `net_buf` 后复制数据 -- 若接口支持 `send_with_lock()`，则利用互斥保护在飞帧 +- 若需要知道 TX 完成，则使用 `send_with_callback()` 释放在飞状态 -这解释了为什么 USB 实现提供了 `send_with_lock()`，而 UART 没有。 +这解释了为什么 Dual 的 FUNC 回复和 SYNC 刷新会通过 `send_with_callback()` 等待 UART/USB +传输完成，而不是在 `send()` 返回时立即允许下一帧复用同一 backing buffer。 ### CRC From d194325449a377afabb9a0d51540476e34f94b7e Mon Sep 17 00:00:00 2001 From: ttwards <12411711@mail.sustech.edu.cn> Date: Wed, 8 Jul 2026 01:31:24 +0800 Subject: [PATCH 5/6] feat(communication): add MQTTLite protocol Add a lightweight MQTT-like publish/subscribe protocol for ARES interfaces with QoS0/1/2, topic-id registration, and a QoS0 zero-copy publish path. Also add a USB bulk sample, native_sim coverage, protocol Kconfig/CMake integration, and documentation for variable-length payloads and topic-id fast paths. Validation: git diff --cached --check; west twister -T tests/native_sim/mqttlite --platform native_sim/native/64 --inline-logs; west build -b dm_mc02 samples/communication/mqttlite_usb --pristine. --- Documents/api-reference.md | 23 + Documents/communication/protocol.md | 118 ++ Documents/modules.md | 2 +- Documents/samples/communication.md | 19 +- Documents/zephyr/kconfig.md | 2 + .../protocol/mqttlite/mqttlite_protocol.h | 163 +++ lib/ares/protocol/CMakeLists.txt | 1 + lib/ares/protocol/Kconfig | 1 + lib/ares/protocol/mqttlite/CMakeLists.txt | 2 + lib/ares/protocol/mqttlite/Kconfig | 55 + .../protocol/mqttlite/mqttlite_protocol.c | 1029 +++++++++++++++++ .../communication/mqttlite_usb/CMakeLists.txt | 6 + samples/communication/mqttlite_usb/README.md | 22 + .../mqttlite_usb/boards/dm_mc02.conf | 9 + .../mqttlite_usb/boards/dm_mc02.overlay | 5 + samples/communication/mqttlite_usb/prj.conf | 12 + .../communication/mqttlite_usb/sample.yaml | 14 + samples/communication/mqttlite_usb/src/main.c | 96 ++ tests/native_sim/mqttlite/CMakeLists.txt | 7 + tests/native_sim/mqttlite/prj.conf | 9 + tests/native_sim/mqttlite/src/main.c | 238 ++++ tests/native_sim/mqttlite/testcase.yaml | 14 + 22 files changed, 1844 insertions(+), 3 deletions(-) create mode 100644 include/ares/protocol/mqttlite/mqttlite_protocol.h create mode 100644 lib/ares/protocol/mqttlite/CMakeLists.txt create mode 100644 lib/ares/protocol/mqttlite/Kconfig create mode 100644 lib/ares/protocol/mqttlite/mqttlite_protocol.c create mode 100644 samples/communication/mqttlite_usb/CMakeLists.txt create mode 100644 samples/communication/mqttlite_usb/README.md create mode 100644 samples/communication/mqttlite_usb/boards/dm_mc02.conf create mode 100644 samples/communication/mqttlite_usb/boards/dm_mc02.overlay create mode 100644 samples/communication/mqttlite_usb/prj.conf create mode 100644 samples/communication/mqttlite_usb/sample.yaml create mode 100644 samples/communication/mqttlite_usb/src/main.c create mode 100644 tests/native_sim/mqttlite/CMakeLists.txt create mode 100644 tests/native_sim/mqttlite/prj.conf create mode 100644 tests/native_sim/mqttlite/src/main.c create mode 100644 tests/native_sim/mqttlite/testcase.yaml diff --git a/Documents/api-reference.md b/Documents/api-reference.md index e0af419..3b0aed7 100644 --- a/Documents/api-reference.md +++ b/Documents/api-reference.md @@ -301,6 +301,29 @@ PID 接口是旧版控制器工具。Motor 新 controller 不依赖该公共 API Dual Protocol 支持函数帧、同步帧、错误帧与回复帧。协议常量定义在同一头文件中。 +### MQTT-like 协议 + +头文件：`include/ares/protocol/mqttlite/mqttlite_protocol.h` + +| 接口 | 说明 | +| --- | --- | +| `ares_mqttlite_register_topic(protocol, topic_id, topic)` | 注册本地 topic id 映射。 | +| `ares_mqttlite_subscribe(protocol, topic_filter, cb, user_data)` | 注册本地 topic 订阅回调。 | +| `ares_mqttlite_subscribe_id(protocol, topic_id, cb, user_data)` | 按已约定 topic id 注册本地订阅回调。 | +| `ares_mqttlite_unsubscribe(protocol, topic_filter)` | 移除本地 topic 订阅。 | +| `ares_mqttlite_unsubscribe_id(protocol, topic_id)` | 移除本地 topic id 订阅。 | +| `ares_mqttlite_publish(protocol, topic, payload, payload_len, qos, cb, user_data)` | 发布消息，QoS1/QoS2 成功时返回 packet id。 | +| `ares_mqttlite_publish_id(protocol, topic_id, payload, payload_len, qos, cb, user_data)` | 使用 2 字节 topic id 发布消息。 | +| `ares_mqttlite_publish_prepare_id(protocol, topic_id, payload_len, pub)` | 准备 QoS0 topic-id 零拷贝发布缓冲。 | +| `ares_mqttlite_publish_commit(protocol, pub)` | 发送已填充的零拷贝发布缓冲。 | +| `ares_mqttlite_publish_abort(pub)` | 释放未提交的零拷贝发布缓冲。 | +| `ares_mqttlite_ping(protocol)` | 发送 PING 帧。 | +| `ARES_MQTTLITE_PROTOCOL_DEFINE(name)` | 定义 MQTT-like 协议实例。 | + +协议支持 `ARES_MQTTLITE_QOS0`、`ARES_MQTTLITE_QOS1` 与 `ARES_MQTTLITE_QOS2`。QoS1 使用 +`PUBLISH/PUBACK`，QoS2 使用 `PUBLISH/PUBREC/PUBREL/PUBCOMP`。payload 是变长字段，长度由每帧 +header 携带，并受 `CONFIG_ARES_MQTTLITE_MAX_PAYLOAD_SIZE` 限制。 + ### Plotter 协议 头文件：`include/ares/protocol/plotter/aresplot_protocol.h` diff --git a/Documents/communication/protocol.md b/Documents/communication/protocol.md index 213edcf..7c4f078 100644 --- a/Documents/communication/protocol.md +++ b/Documents/communication/protocol.md @@ -227,6 +227,124 @@ dual_func_add(&link_proto, 0x10, my_rpc); dual_sync_add(&link_proto, 0x20, sync_buf, sizeof(sync_buf), sync_cb); ``` +## MQTT-like 协议 + +### 角色 + +MQTT-like 协议面向 USB bulk 这类可靠包传输接口上的轻量发布/订阅。它复用 +`AresInterface`，不直接依赖 USB 类实现，因此接口层仍负责收发 `net_buf`，协议层只处理 topic、 +QoS 握手和消息分发。 + +实现位于： + +- 头文件：`include/ares/protocol/mqttlite/mqttlite_protocol.h` +- 实现：`lib/ares/protocol/mqttlite/mqttlite_protocol.c` + +### 帧类型 + +协议帧包含固定 12 字节头： + +- magic +- version +- frame type +- flags +- packet id +- topic length +- payload length + +当前定义的帧类型： + +- `PUBLISH` +- `PUBACK` +- `PUBREC` +- `PUBREL` +- `PUBCOMP` +- `PING` +- `PONG` + +topic 与 payload 紧跟在固定头之后。payload 是变长字段，长度由 frame header 携带，不是定长。 +最大 topic、payload、frame、订阅数量、topic id 注册数量和在飞 QoS 包数量由 +`CONFIG_ARES_MQTTLITE_*` 控制。 + +### Topic ID + +普通 `PUBLISH` 帧携带完整 topic 字符串。性能敏感路径可以先用 +`ares_mqttlite_register_topic()` 在本地注册 `uint16_t topic_id -> topic` 映射，再使用： + +- `ares_mqttlite_subscribe_id()` +- `ares_mqttlite_unsubscribe_id()` +- `ares_mqttlite_publish_id()` + +topic-id 帧会设置 frame flags 中的 topic-id 位，并把 topic 字段压缩为 2 字节 little-endian id。 +接收端若注册了对应 id，会把 id 反查为字符串传给原有消息回调；未注册时 id 订阅仍可匹配，但回调中的 +topic 字符串为空。 + +### QoS + +协议支持三档 QoS： + +- `ARES_MQTTLITE_QOS0`：最多一次，发送后不等待确认。 +- `ARES_MQTTLITE_QOS1`：至少一次，发送端等待 `PUBACK`，超时重发 `PUBLISH`。 +- `ARES_MQTTLITE_QOS2`：恰好一次语义，使用 `PUBLISH/PUBREC/PUBREL/PUBCOMP`，接收端在 + `PUBREL` 阶段交付。 + +QoS1/QoS2 的发送端会记录在飞包，按 `CONFIG_ARES_MQTTLITE_RETRY_INTERVAL_MS` 重试，超过 +`CONFIG_ARES_MQTTLITE_MAX_RETRIES` 后通过发布回调返回超时。 + +### Topic 匹配 + +订阅使用 `ares_mqttlite_subscribe()` 注册本地回调。topic filter 支持： + +- 精确匹配 +- 单层通配符 `+` +- 末尾多层通配符 `#` + +协议当前不向远端发送 SUBSCRIBE 控制帧；订阅表是本地分发表。两端需要按应用契约分别注册各自要接收的 +topic。 + +topic-id 订阅不做通配符匹配，只比较 `uint16_t topic_id`。这条路径适合高频 telemetry 和固定控制 +topic。 + +### 发送路径 + +发布消息使用： + +```c +ARES_MQTTLITE_PROTOCOL_DEFINE(link_proto); + +ares_bind_interface(&usb_bulk_interface, &link_proto); +ares_mqttlite_subscribe(&link_proto, "robot/+/state", state_cb, NULL); +ares_mqttlite_publish(&link_proto, "robot/chassis/state", payload, payload_len, + ARES_MQTTLITE_QOS1, publish_cb, NULL); +``` + +协议通过已绑定接口分配 `net_buf` 并调用 `send()`。因此它可以跑在 USB bulk 上，也可以跑在其他实现了 +`AresInterfaceAPI` 的块式接口上。 + +QoS0 的 topic-id 快速路径可以避免 payload 复制： + +```c +struct ares_mqttlite_publish_buffer pub; + +ares_mqttlite_register_topic(&link_proto, 1, "robot/chassis/state"); +ares_mqttlite_publish_prepare_id(&link_proto, 1, payload_len, &pub); +memcpy(pub.payload, payload, payload_len); +ares_mqttlite_publish_commit(&link_proto, &pub); +``` + +这条路径直接让调用者写入最终 `net_buf` 的 payload 区域。QoS1/QoS2 仍会保留 payload 副本用于重试。 + +### 错误边界 + +MQTT-like 协议的主要错误边界： + +- topic 或 payload 超过配置上限时，发布返回 `-EINVAL`。 +- frame 超过接收缓冲上限时，解析器丢弃该帧。 +- topic id 为 0 或未按应用契约双边约定时，远端无法按预期分发消息。 +- QoS 在飞表满时，QoS1/QoS2 发布返回 `-ENOMEM`。 +- QoS 重试耗尽时，发布回调收到 `ARES_MQTTLITE_PUBLISH_TIMEOUT`。 +- 断连事件会清空在飞 QoS 状态和已暂存的 QoS2 接收状态。 + ## 绘图协议 ### 角色 diff --git a/Documents/modules.md b/Documents/modules.md index 1a443ea..f3eb14c 100644 --- a/Documents/modules.md +++ b/Documents/modules.md @@ -156,7 +156,7 @@ Graph 当前承载 INSLink 相关代码。该目录并非通用图形库，维 - `include/ares/protocol/` - `lib/ares/protocol/` -协议层是帧解析和业务分发层，目前包含 Dual Protocol 与 Plotter Protocol。 +协议层是帧解析和业务分发层，目前包含 Dual Protocol、MQTT-like Protocol 与 Plotter Protocol。 ### ARES 通信（ARES Comm） diff --git a/Documents/samples/communication.md b/Documents/samples/communication.md index 2090de8..a6ec28a 100644 --- a/Documents/samples/communication.md +++ b/Documents/samples/communication.md @@ -1,11 +1,12 @@ # Communication 样例 -通信样例分为 `ARES 通信` 与 `Plotter 协议` 两条线，覆盖 USB Bulk、UART 接口和日志输出行为。 +通信样例分为 `ARES 通信`、`MQTT-like USB` 与 `Plotter 协议` 几条线，覆盖 USB Bulk、UART +接口和日志输出行为。 ## 共同边界 - `CONFIG_UART_INTERFACE` 与 `CONFIG_USB_BULK_INTERFACE` 在各样例中的组合不同，须按 `boards/*` 与 `prj.conf` 一起确认。 -- 示例流程以 `CONFIG_DUAL_PROPOSE_PROTOCOL` 或 `CONFIG_PLOTTER` 启用为入口，接口初始化失败时通常会跳过失败通道并继续运行。 +- 示例流程以 `CONFIG_DUAL_PROPOSE_PROTOCOL`、`CONFIG_ARES_MQTTLITE_PROTOCOL` 或 `CONFIG_PLOTTER` 启用为入口，接口初始化失败时通常会跳过失败通道并继续运行。 - 上位机联调前先确认 `usart6` 与 USB OTG 物理连接。 ## samples/communication/ares_communication @@ -24,6 +25,20 @@ - 若新增板适配，先确认 `boards/.conf` 是否覆盖了 `CONFIG_UART_INTERFACE`。 - 回调函数语义（`func_cb` / `sync_cb` / `func_ret_cb`）变化需同步示例文档与接收端联调脚本。 +## samples/communication/mqttlite_usb + +- 用途：MQTT-like 协议的 USB Bulk 发布/订阅演示，周期通过 topic-id 零拷贝路径发布 QoS0 heartbeat，并按 id 订阅 `host/command`。 +- 构建： + - `west build -b dm_mc02 samples/communication/mqttlite_usb --pristine` + - `west flash` +- 适用 board：`dm_mc02` +- 硬件依赖： + - USB 设备能力 + - 兼容 MQTT-like 帧格式的上位机 +- 维护规则： + - QoS 行为变化需同步 `include/ares/protocol/mqttlite/mqttlite_protocol.h` 和协议文档。 + - topic 名称变化需同步上位机接收端。 + ## samples/communication/plotter_auto - 用途：自动上报变量的 Plotter 示例。 diff --git a/Documents/zephyr/kconfig.md b/Documents/zephyr/kconfig.md index 940ceb4..0869202 100644 --- a/Documents/zephyr/kconfig.md +++ b/Documents/zephyr/kconfig.md @@ -61,6 +61,8 @@ Motor 子系统的调度和节拍选项： | `CONFIG_ARES` | 启用 ARES 库菜单。 | | `CONFIG_UART_INTERFACE` | 启用 UART 接口。 | | `CONFIG_USB_BULK_INTERFACE` | 启用 USB Bulk 接口。 | +| `CONFIG_ARES_MQTTLITE_PROTOCOL` | 启用 MQTT-like 发布/订阅协议。 | +| `CONFIG_ARES_MQTTLITE_MAX_TOPICS` | 设置 MQTT-like topic id 注册数量上限。 | | `CONFIG_ARES_BOARD_STATUS_LED` | 启用板级状态 LED 服务。 | | `CONFIG_IMU_PWM_TEMP_CTRL` | 启用 IMU PWM 温控。 | | `CONFIG_AUTO_PROBE_GYRO_BIAS` | 启用陀螺偏置自动探测。 | diff --git a/include/ares/protocol/mqttlite/mqttlite_protocol.h b/include/ares/protocol/mqttlite/mqttlite_protocol.h new file mode 100644 index 0000000..8d02e9c --- /dev/null +++ b/include/ares/protocol/mqttlite/mqttlite_protocol.h @@ -0,0 +1,163 @@ +#ifndef ARES_MQTTLITE_PROTOCOL_H +#define ARES_MQTTLITE_PROTOCOL_H + +#include +#include +#include +#include + +#include + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +enum ares_mqttlite_qos { + ARES_MQTTLITE_QOS0 = 0, + ARES_MQTTLITE_QOS1 = 1, + ARES_MQTTLITE_QOS2 = 2, +}; + +enum ares_mqttlite_publish_status { + ARES_MQTTLITE_PUBLISH_ACKED = 0, + ARES_MQTTLITE_PUBLISH_TIMEOUT = -ETIMEDOUT, +}; + +typedef void (*ares_mqttlite_msg_cb_t)(struct AresProtocol *protocol, const char *topic, + const uint8_t *payload, uint16_t payload_len, + enum ares_mqttlite_qos qos, void *user_data); + +typedef void (*ares_mqttlite_publish_cb_t)(struct AresProtocol *protocol, uint16_t packet_id, + int status, void *user_data); + +struct ares_mqttlite_publish_buffer { + struct net_buf *buf; + uint8_t *payload; + uint16_t payload_len; +}; + +int ares_mqttlite_register_topic(struct AresProtocol *protocol, uint16_t topic_id, + const char *topic); + +int ares_mqttlite_subscribe(struct AresProtocol *protocol, const char *topic_filter, + ares_mqttlite_msg_cb_t cb, void *user_data); + +int ares_mqttlite_subscribe_id(struct AresProtocol *protocol, uint16_t topic_id, + ares_mqttlite_msg_cb_t cb, void *user_data); + +int ares_mqttlite_unsubscribe(struct AresProtocol *protocol, const char *topic_filter); + +int ares_mqttlite_unsubscribe_id(struct AresProtocol *protocol, uint16_t topic_id); + +int ares_mqttlite_publish(struct AresProtocol *protocol, const char *topic, const uint8_t *payload, + uint16_t payload_len, enum ares_mqttlite_qos qos, + ares_mqttlite_publish_cb_t cb, void *user_data); + +int ares_mqttlite_publish_id(struct AresProtocol *protocol, uint16_t topic_id, + const uint8_t *payload, uint16_t payload_len, + enum ares_mqttlite_qos qos, ares_mqttlite_publish_cb_t cb, + void *user_data); + +int ares_mqttlite_publish_prepare_id(struct AresProtocol *protocol, uint16_t topic_id, + uint16_t payload_len, + struct ares_mqttlite_publish_buffer *pub); + +int ares_mqttlite_publish_commit(struct AresProtocol *protocol, + struct ares_mqttlite_publish_buffer *pub); + +void ares_mqttlite_publish_abort(struct ares_mqttlite_publish_buffer *pub); + +int ares_mqttlite_ping(struct AresProtocol *protocol); + +void ares_mqttlite_protocol_handle(struct AresProtocol *protocol, struct net_buf *buf); +void ares_mqttlite_protocol_handle_byte(struct AresProtocol *protocol, uint8_t byte); +void ares_mqttlite_protocol_event(struct AresProtocol *protocol, enum AresProtocolEvent event); +int ares_mqttlite_protocol_init(struct AresProtocol *protocol); + +struct ares_mqttlite_subscription { + bool use_topic_id; + uint16_t topic_id; + char topic_filter[CONFIG_ARES_MQTTLITE_MAX_TOPIC_LEN + 1]; + ares_mqttlite_msg_cb_t cb; + void *user_data; +}; + +struct ares_mqttlite_topic_entry { + bool used; + uint16_t topic_id; + char topic[CONFIG_ARES_MQTTLITE_MAX_TOPIC_LEN + 1]; +}; + +enum ares_mqttlite_tx_state { + ARES_MQTTLITE_TX_UNUSED, + ARES_MQTTLITE_TX_WAIT_PUBACK, + ARES_MQTTLITE_TX_WAIT_PUBREC, + ARES_MQTTLITE_TX_WAIT_PUBCOMP, +}; + +struct ares_mqttlite_tx_inflight { + enum ares_mqttlite_tx_state state; + uint16_t packet_id; + enum ares_mqttlite_qos qos; + uint8_t retries; + int64_t last_send_ms; + ares_mqttlite_publish_cb_t cb; + void *user_data; + bool use_topic_id; + uint16_t topic_id; + uint16_t topic_len; + uint16_t payload_len; + char topic[CONFIG_ARES_MQTTLITE_MAX_TOPIC_LEN + 1]; + uint8_t payload[CONFIG_ARES_MQTTLITE_MAX_PAYLOAD_SIZE]; +}; + +struct ares_mqttlite_rx_qos2 { + bool used; + uint16_t packet_id; + bool use_topic_id; + uint16_t topic_id; + uint16_t topic_len; + uint16_t payload_len; + char topic[CONFIG_ARES_MQTTLITE_MAX_TOPIC_LEN + 1]; + uint8_t payload[CONFIG_ARES_MQTTLITE_MAX_PAYLOAD_SIZE]; +}; + +struct ares_mqttlite_protocol_data { + const char *name; + bool online; + uint16_t next_packet_id; + struct k_mutex lock; + struct k_work_delayable retry_work; + struct AresProtocol *protocol; + struct ares_mqttlite_topic_entry topics[CONFIG_ARES_MQTTLITE_MAX_TOPICS]; + struct ares_mqttlite_subscription subscriptions[CONFIG_ARES_MQTTLITE_MAX_SUBSCRIPTIONS]; + struct ares_mqttlite_tx_inflight tx[CONFIG_ARES_MQTTLITE_MAX_INFLIGHT]; + struct ares_mqttlite_rx_qos2 rx_qos2[CONFIG_ARES_MQTTLITE_MAX_INFLIGHT]; + uint8_t rx_buf[CONFIG_ARES_MQTTLITE_MAX_FRAME_SIZE]; + uint16_t rx_pos; + uint16_t rx_expected; +}; + +#define ARES_MQTTLITE_PROTOCOL_DEFINE(Protocol_name) \ + struct AresProtocolAPI Protocol_name##_api = { \ + .handle = ares_mqttlite_protocol_handle, \ + .handle_byte = ares_mqttlite_protocol_handle_byte, \ + .event = ares_mqttlite_protocol_event, \ + .init = ares_mqttlite_protocol_init, \ + }; \ + struct ares_mqttlite_protocol_data Protocol_name##_data = { \ + .name = #Protocol_name, \ + }; \ + struct AresProtocol Protocol_name = { \ + .name = #Protocol_name, \ + .api = &Protocol_name##_api, \ + .priv_data = &Protocol_name##_data, \ + } + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/lib/ares/protocol/CMakeLists.txt b/lib/ares/protocol/CMakeLists.txt index e3005f8..5f44ded 100755 --- a/lib/ares/protocol/CMakeLists.txt +++ b/lib/ares/protocol/CMakeLists.txt @@ -1,2 +1,3 @@ add_subdirectory_ifdef(CONFIG_DUAL_PROPOSE_PROTOCOL dual) add_subdirectory_ifdef(CONFIG_ARES_PLOTTER_PROTOCOL plotter) +add_subdirectory_ifdef(CONFIG_ARES_MQTTLITE_PROTOCOL mqttlite) diff --git a/lib/ares/protocol/Kconfig b/lib/ares/protocol/Kconfig index 72a1673..f74c6f5 100755 --- a/lib/ares/protocol/Kconfig +++ b/lib/ares/protocol/Kconfig @@ -7,3 +7,4 @@ config DUAL_PROPOSE_PROTOCOL select ARES_COMM_LIB rsource "plotter/Kconfig" +rsource "mqttlite/Kconfig" diff --git a/lib/ares/protocol/mqttlite/CMakeLists.txt b/lib/ares/protocol/mqttlite/CMakeLists.txt new file mode 100644 index 0000000..bcd1997 --- /dev/null +++ b/lib/ares/protocol/mqttlite/CMakeLists.txt @@ -0,0 +1,2 @@ +zephyr_library() +zephyr_library_sources(mqttlite_protocol.c) diff --git a/lib/ares/protocol/mqttlite/Kconfig b/lib/ares/protocol/mqttlite/Kconfig new file mode 100644 index 0000000..6308b07 --- /dev/null +++ b/lib/ares/protocol/mqttlite/Kconfig @@ -0,0 +1,55 @@ +config ARES_MQTTLITE_PROTOCOL + bool "ARES MQTT-like protocol" + select ARES_COMM_LIB + select NET_BUF + help + Enable a small MQTT-like publish/subscribe protocol for ARES interfaces. + +if ARES_MQTTLITE_PROTOCOL + +config ARES_MQTTLITE_MAX_TOPIC_LEN + int "Maximum topic/filter length" + range 1 255 + default 64 + +config ARES_MQTTLITE_MAX_PAYLOAD_SIZE + int "Maximum payload size" + range 1 65535 + default 192 + +config ARES_MQTTLITE_MAX_FRAME_SIZE + int "Maximum received frame size" + range 16 65535 + default 272 + +config ARES_MQTTLITE_MAX_SUBSCRIPTIONS + int "Maximum local subscriptions" + range 1 32 + default 8 + +config ARES_MQTTLITE_MAX_TOPICS + int "Maximum registered topic ids" + range 1 64 + default 8 + +config ARES_MQTTLITE_MAX_INFLIGHT + int "Maximum QoS inflight packets" + range 1 32 + default 4 + +config ARES_MQTTLITE_RETRY_INTERVAL_MS + int "QoS retry interval in milliseconds" + range 10 60000 + default 200 + +config ARES_MQTTLITE_MAX_RETRIES + int "Maximum QoS retries before timeout" + range 1 255 + default 5 + +config ARES_MQTTLITE_LOG_LEVEL + int "ARES MQTT-like protocol log level" + range 0 4 + default 3 + +endif diff --git a/lib/ares/protocol/mqttlite/mqttlite_protocol.c b/lib/ares/protocol/mqttlite/mqttlite_protocol.c new file mode 100644 index 0000000..dc9e8e9 --- /dev/null +++ b/lib/ares/protocol/mqttlite/mqttlite_protocol.c @@ -0,0 +1,1029 @@ +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +LOG_MODULE_REGISTER(ares_mqttlite, CONFIG_ARES_MQTTLITE_LOG_LEVEL); + +#define MQTL_MAGIC 0x4d51 +#define MQTL_VERSION 1 + +#define MQTL_MAGIC_IDX 0 +#define MQTL_VERSION_IDX 2 +#define MQTL_TYPE_IDX 3 +#define MQTL_FLAGS_IDX 4 +#define MQTL_RESERVED_IDX 5 +#define MQTL_PACKET_ID_IDX 6 +#define MQTL_TOPIC_LEN_IDX 8 +#define MQTL_PAYLOAD_LEN_IDX 10 +#define MQTL_HEADER_LEN 12 + +#define MQTL_FLAG_QOS_MASK 0x03 +#define MQTL_FLAG_DUP BIT(2) +#define MQTL_FLAG_TOPIC_ID BIT(3) + +#define MQTL_TOPIC_ID_LEN 2 + +enum mqttlite_frame_type { + MQTL_FRAME_PUBLISH = 1, + MQTL_FRAME_PUBACK = 2, + MQTL_FRAME_PUBREC = 3, + MQTL_FRAME_PUBREL = 4, + MQTL_FRAME_PUBCOMP = 5, + MQTL_FRAME_PING = 6, + MQTL_FRAME_PONG = 7, +}; + +static uint16_t frame_topic_len(const uint8_t *frame) +{ + return sys_get_le16(&frame[MQTL_TOPIC_LEN_IDX]); +} + +static uint16_t frame_payload_len(const uint8_t *frame) +{ + return sys_get_le16(&frame[MQTL_PAYLOAD_LEN_IDX]); +} + +static uint16_t frame_packet_id(const uint8_t *frame) +{ + return sys_get_le16(&frame[MQTL_PACKET_ID_IDX]); +} + +static enum ares_mqttlite_qos frame_qos(const uint8_t *frame) +{ + return frame[MQTL_FLAGS_IDX] & MQTL_FLAG_QOS_MASK; +} + +static bool frame_uses_topic_id(const uint8_t *frame) +{ + return (frame[MQTL_FLAGS_IDX] & MQTL_FLAG_TOPIC_ID) != 0; +} + +static bool valid_qos(enum ares_mqttlite_qos qos) +{ + return qos == ARES_MQTTLITE_QOS0 || qos == ARES_MQTTLITE_QOS1 || qos == ARES_MQTTLITE_QOS2; +} + +static const char *find_topic_name_locked(struct ares_mqttlite_protocol_data *data, + uint16_t topic_id) +{ + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_TOPICS; i++) { + if (data->topics[i].used && data->topics[i].topic_id == topic_id) { + return data->topics[i].topic; + } + } + + return NULL; +} + +static bool topic_matches(const char *filter, const char *topic) +{ + const char *fp = filter; + const char *tp = topic; + + if (strcmp(filter, "#") == 0) { + return true; + } + + while (*fp != '\0' && *tp != '\0') { + if (*fp == '#') { + return fp[1] == '\0' && (fp == filter || fp[-1] == '/'); + } + if (*fp == '+') { + while (*tp != '\0' && *tp != '/') { + tp++; + } + fp++; + continue; + } + if (*fp != *tp) { + return false; + } + fp++; + tp++; + } + + return *fp == *tp || (strcmp(fp, "/#") == 0); +} + +static int alloc_packet_id_locked(struct ares_mqttlite_protocol_data *data) +{ + uint16_t packet_id = data->next_packet_id; + + if (packet_id == 0) { + packet_id = 1; + } + data->next_packet_id = packet_id + 1; + if (data->next_packet_id == 0) { + data->next_packet_id = 1; + } + + return packet_id; +} + +static struct ares_mqttlite_tx_inflight *find_tx_locked(struct ares_mqttlite_protocol_data *data, + uint16_t packet_id) +{ + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_INFLIGHT; i++) { + if (data->tx[i].state != ARES_MQTTLITE_TX_UNUSED && + data->tx[i].packet_id == packet_id) { + return &data->tx[i]; + } + } + + return NULL; +} + +static struct ares_mqttlite_tx_inflight *alloc_tx_locked(struct ares_mqttlite_protocol_data *data) +{ + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_INFLIGHT; i++) { + if (data->tx[i].state == ARES_MQTTLITE_TX_UNUSED) { + return &data->tx[i]; + } + } + + return NULL; +} + +static struct ares_mqttlite_rx_qos2 *find_rx_qos2_locked(struct ares_mqttlite_protocol_data *data, + uint16_t packet_id) +{ + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_INFLIGHT; i++) { + if (data->rx_qos2[i].used && data->rx_qos2[i].packet_id == packet_id) { + return &data->rx_qos2[i]; + } + } + + return NULL; +} + +static struct ares_mqttlite_rx_qos2 *alloc_rx_qos2_locked(struct ares_mqttlite_protocol_data *data) +{ + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_INFLIGHT; i++) { + if (!data->rx_qos2[i].used) { + return &data->rx_qos2[i]; + } + } + + return NULL; +} + +static int send_frame(struct AresProtocol *protocol, enum mqttlite_frame_type type, + enum ares_mqttlite_qos qos, uint8_t extra_flags, uint16_t packet_id, + bool use_topic_id, uint16_t topic_id, const char *topic, uint16_t topic_len, + const uint8_t *payload, uint16_t payload_len) +{ + struct AresInterface *interface = protocol->interface; + struct net_buf *buf; + uint16_t wire_topic_len = use_topic_id ? MQTL_TOPIC_ID_LEN : topic_len; + size_t frame_len = MQTL_HEADER_LEN + wire_topic_len + payload_len; + + if (interface == NULL || interface->api == NULL || interface->api->send == NULL || + interface->api->alloc_buf == NULL) { + return -ENODEV; + } + if (frame_len > CONFIG_ARES_MQTTLITE_MAX_FRAME_SIZE) { + return -EMSGSIZE; + } + + buf = interface->api->alloc_buf(interface); + if (buf == NULL) { + return -ENOMEM; + } + if (net_buf_tailroom(buf) < frame_len) { + net_buf_unref(buf); + return -EMSGSIZE; + } + + net_buf_add(buf, frame_len); + sys_put_le16(MQTL_MAGIC, &buf->data[MQTL_MAGIC_IDX]); + buf->data[MQTL_VERSION_IDX] = MQTL_VERSION; + buf->data[MQTL_TYPE_IDX] = type; + buf->data[MQTL_FLAGS_IDX] = + (qos & MQTL_FLAG_QOS_MASK) | extra_flags | (use_topic_id ? MQTL_FLAG_TOPIC_ID : 0); + buf->data[MQTL_RESERVED_IDX] = 0; + sys_put_le16(packet_id, &buf->data[MQTL_PACKET_ID_IDX]); + sys_put_le16(wire_topic_len, &buf->data[MQTL_TOPIC_LEN_IDX]); + sys_put_le16(payload_len, &buf->data[MQTL_PAYLOAD_LEN_IDX]); + + if (use_topic_id) { + sys_put_le16(topic_id, &buf->data[MQTL_HEADER_LEN]); + } else if (topic_len > 0) { + memcpy(&buf->data[MQTL_HEADER_LEN], topic, topic_len); + } + if (payload_len > 0) { + memcpy(&buf->data[MQTL_HEADER_LEN + wire_topic_len], payload, payload_len); + } + + return interface->api->send(interface, buf); +} + +static int send_ack(struct AresProtocol *protocol, enum mqttlite_frame_type type, + uint16_t packet_id) +{ + return send_frame(protocol, type, ARES_MQTTLITE_QOS0, 0, packet_id, false, 0, NULL, 0, NULL, + 0); +} + +static void schedule_retry(struct ares_mqttlite_protocol_data *data) +{ + k_work_schedule(&data->retry_work, K_MSEC(CONFIG_ARES_MQTTLITE_RETRY_INTERVAL_MS)); +} + +static void dispatch_publish(struct AresProtocol *protocol, const char *topic, bool use_topic_id, + uint16_t topic_id, const uint8_t *payload, uint16_t payload_len, + enum ares_mqttlite_qos qos) +{ + struct ares_mqttlite_protocol_data *data = protocol->priv_data; + ares_mqttlite_msg_cb_t callbacks[CONFIG_ARES_MQTTLITE_MAX_SUBSCRIPTIONS]; + void *user_data[CONFIG_ARES_MQTTLITE_MAX_SUBSCRIPTIONS]; + int count = 0; + + k_mutex_lock(&data->lock, K_FOREVER); + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_SUBSCRIPTIONS; i++) { + if (data->subscriptions[i].cb == NULL) { + continue; + } + if (data->subscriptions[i].use_topic_id) { + if (!use_topic_id || data->subscriptions[i].topic_id != topic_id) { + continue; + } + } else if (!topic_matches(data->subscriptions[i].topic_filter, topic)) { + continue; + } + { + callbacks[count] = data->subscriptions[i].cb; + user_data[count] = data->subscriptions[i].user_data; + count++; + } + } + k_mutex_unlock(&data->lock); + + for (int i = 0; i < count; i++) { + callbacks[i](protocol, topic, payload, payload_len, qos, user_data[i]); + } +} + +static void clear_tx(struct ares_mqttlite_tx_inflight *tx) +{ + memset(tx, 0, sizeof(*tx)); + tx->state = ARES_MQTTLITE_TX_UNUSED; +} + +static void complete_tx(struct AresProtocol *protocol, uint16_t packet_id, int status) +{ + struct ares_mqttlite_protocol_data *data = protocol->priv_data; + ares_mqttlite_publish_cb_t cb = NULL; + void *user_data = NULL; + + k_mutex_lock(&data->lock, K_FOREVER); + struct ares_mqttlite_tx_inflight *tx = find_tx_locked(data, packet_id); + if (tx != NULL) { + cb = tx->cb; + user_data = tx->user_data; + clear_tx(tx); + } + k_mutex_unlock(&data->lock); + + if (cb != NULL) { + cb(protocol, packet_id, status, user_data); + } +} + +static void handle_publish(struct AresProtocol *protocol, const uint8_t *frame) +{ + struct ares_mqttlite_protocol_data *data = protocol->priv_data; + uint16_t packet_id = frame_packet_id(frame); + uint16_t topic_len = frame_topic_len(frame); + uint16_t payload_len = frame_payload_len(frame); + enum ares_mqttlite_qos qos = frame_qos(frame); + bool use_topic_id = frame_uses_topic_id(frame); + uint16_t topic_id = 0; + char topic[CONFIG_ARES_MQTTLITE_MAX_TOPIC_LEN + 1]; + const uint8_t *payload = &frame[MQTL_HEADER_LEN + topic_len]; + + topic[0] = '\0'; + + if (!valid_qos(qos) || topic_len == 0 || + payload_len > CONFIG_ARES_MQTTLITE_MAX_PAYLOAD_SIZE) { + LOG_WRN("%s rejected malformed publish", data->name); + return; + } + if (use_topic_id && topic_len != MQTL_TOPIC_ID_LEN) { + LOG_WRN("%s rejected malformed topic-id publish", data->name); + return; + } + if (!use_topic_id && topic_len > CONFIG_ARES_MQTTLITE_MAX_TOPIC_LEN) { + LOG_WRN("%s rejected oversized topic publish", data->name); + return; + } + if (qos != ARES_MQTTLITE_QOS0 && packet_id == 0) { + LOG_WRN("%s rejected QoS publish without packet id", data->name); + return; + } + + if (use_topic_id) { + const char *registered_topic; + + topic_id = sys_get_le16(&frame[MQTL_HEADER_LEN]); + k_mutex_lock(&data->lock, K_FOREVER); + registered_topic = find_topic_name_locked(data, topic_id); + if (registered_topic != NULL) { + strncpy(topic, registered_topic, sizeof(topic) - 1); + topic[sizeof(topic) - 1] = '\0'; + } + k_mutex_unlock(&data->lock); + } else { + memcpy(topic, &frame[MQTL_HEADER_LEN], topic_len); + topic[topic_len] = '\0'; + } + + if (qos == ARES_MQTTLITE_QOS0) { + dispatch_publish(protocol, topic, use_topic_id, topic_id, payload, payload_len, + qos); + return; + } + + if (qos == ARES_MQTTLITE_QOS1) { + dispatch_publish(protocol, topic, use_topic_id, topic_id, payload, payload_len, + qos); + (void)send_ack(protocol, MQTL_FRAME_PUBACK, packet_id); + return; + } + + k_mutex_lock(&data->lock, K_FOREVER); + struct ares_mqttlite_rx_qos2 *rx = find_rx_qos2_locked(data, packet_id); + if (rx == NULL) { + rx = alloc_rx_qos2_locked(data); + if (rx != NULL) { + rx->used = true; + rx->packet_id = packet_id; + rx->use_topic_id = use_topic_id; + rx->topic_id = topic_id; + rx->topic_len = strlen(topic); + rx->payload_len = payload_len; + memcpy(rx->topic, topic, rx->topic_len + 1); + memcpy(rx->payload, payload, payload_len); + } + } + k_mutex_unlock(&data->lock); + + if (rx == NULL) { + LOG_WRN("%s has no room for QoS2 packet %u", data->name, packet_id); + return; + } + + (void)send_ack(protocol, MQTL_FRAME_PUBREC, packet_id); +} + +static void handle_pubrel(struct AresProtocol *protocol, uint16_t packet_id) +{ + struct ares_mqttlite_protocol_data *data = protocol->priv_data; + char topic[CONFIG_ARES_MQTTLITE_MAX_TOPIC_LEN + 1]; + uint8_t payload[CONFIG_ARES_MQTTLITE_MAX_PAYLOAD_SIZE]; + uint16_t payload_len = 0; + bool use_topic_id = false; + uint16_t topic_id = 0; + bool found = false; + + k_mutex_lock(&data->lock, K_FOREVER); + struct ares_mqttlite_rx_qos2 *rx = find_rx_qos2_locked(data, packet_id); + if (rx != NULL) { + memcpy(topic, rx->topic, rx->topic_len + 1); + memcpy(payload, rx->payload, rx->payload_len); + payload_len = rx->payload_len; + use_topic_id = rx->use_topic_id; + topic_id = rx->topic_id; + memset(rx, 0, sizeof(*rx)); + found = true; + } + k_mutex_unlock(&data->lock); + + if (found) { + dispatch_publish(protocol, topic, use_topic_id, topic_id, payload, payload_len, + ARES_MQTTLITE_QOS2); + } + (void)send_ack(protocol, MQTL_FRAME_PUBCOMP, packet_id); +} + +static void handle_ack_frame(struct AresProtocol *protocol, enum mqttlite_frame_type type, + uint16_t packet_id) +{ + struct ares_mqttlite_protocol_data *data = protocol->priv_data; + + if (packet_id == 0) { + return; + } + + if (type == MQTL_FRAME_PUBACK || type == MQTL_FRAME_PUBCOMP) { + complete_tx(protocol, packet_id, ARES_MQTTLITE_PUBLISH_ACKED); + return; + } + + if (type == MQTL_FRAME_PUBREC) { + bool send_pubrel = false; + + k_mutex_lock(&data->lock, K_FOREVER); + struct ares_mqttlite_tx_inflight *tx = find_tx_locked(data, packet_id); + if (tx != NULL && tx->state == ARES_MQTTLITE_TX_WAIT_PUBREC) { + tx->state = ARES_MQTTLITE_TX_WAIT_PUBCOMP; + tx->retries = 0; + tx->last_send_ms = k_uptime_get(); + send_pubrel = true; + schedule_retry(data); + } + k_mutex_unlock(&data->lock); + + if (send_pubrel) { + (void)send_ack(protocol, MQTL_FRAME_PUBREL, packet_id); + } + } +} + +static void process_frame(struct AresProtocol *protocol, const uint8_t *frame, uint16_t len) +{ + uint16_t topic_len; + uint16_t payload_len; + + if (len < MQTL_HEADER_LEN || sys_get_le16(&frame[MQTL_MAGIC_IDX]) != MQTL_MAGIC || + frame[MQTL_VERSION_IDX] != MQTL_VERSION) { + return; + } + + topic_len = frame_topic_len(frame); + payload_len = frame_payload_len(frame); + if ((uint32_t)MQTL_HEADER_LEN + topic_len + payload_len != len) { + return; + } + + switch (frame[MQTL_TYPE_IDX]) { + case MQTL_FRAME_PUBLISH: + handle_publish(protocol, frame); + break; + case MQTL_FRAME_PUBACK: + case MQTL_FRAME_PUBREC: + case MQTL_FRAME_PUBCOMP: + handle_ack_frame(protocol, frame[MQTL_TYPE_IDX], frame_packet_id(frame)); + break; + case MQTL_FRAME_PUBREL: + handle_pubrel(protocol, frame_packet_id(frame)); + break; + case MQTL_FRAME_PING: + (void)send_ack(protocol, MQTL_FRAME_PONG, 0); + break; + case MQTL_FRAME_PONG: + break; + default: + break; + } +} + +static void retry_work_handler(struct k_work *work) +{ + struct k_work_delayable *dwork = k_work_delayable_from_work(work); + struct ares_mqttlite_protocol_data *data = + CONTAINER_OF(dwork, struct ares_mqttlite_protocol_data, retry_work); + struct AresProtocol *protocol = data->protocol; + int64_t now = k_uptime_get(); + bool has_pending = false; + + k_mutex_lock(&data->lock, K_FOREVER); + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_INFLIGHT; i++) { + struct ares_mqttlite_tx_inflight *tx = &data->tx[i]; + enum ares_mqttlite_tx_state state; + enum ares_mqttlite_qos qos; + bool use_topic_id; + uint16_t packet_id; + uint16_t topic_id; + uint16_t topic_len; + uint16_t payload_len; + char topic[CONFIG_ARES_MQTTLITE_MAX_TOPIC_LEN + 1]; + uint8_t payload[CONFIG_ARES_MQTTLITE_MAX_PAYLOAD_SIZE]; + + if (tx->state == ARES_MQTTLITE_TX_UNUSED) { + continue; + } + has_pending = true; + if (now - tx->last_send_ms < CONFIG_ARES_MQTTLITE_RETRY_INTERVAL_MS) { + continue; + } + if (tx->retries >= CONFIG_ARES_MQTTLITE_MAX_RETRIES) { + uint16_t packet_id = tx->packet_id; + ares_mqttlite_publish_cb_t cb = tx->cb; + void *user_data = tx->user_data; + + clear_tx(tx); + k_mutex_unlock(&data->lock); + if (cb != NULL) { + cb(protocol, packet_id, ARES_MQTTLITE_PUBLISH_TIMEOUT, user_data); + } + k_mutex_lock(&data->lock, K_FOREVER); + has_pending = false; + for (int j = 0; j < CONFIG_ARES_MQTTLITE_MAX_INFLIGHT; j++) { + if (data->tx[j].state != ARES_MQTTLITE_TX_UNUSED) { + has_pending = true; + break; + } + } + continue; + } + + state = tx->state; + qos = tx->qos; + use_topic_id = tx->use_topic_id; + packet_id = tx->packet_id; + topic_id = tx->topic_id; + topic_len = tx->topic_len; + payload_len = tx->payload_len; + memcpy(topic, tx->topic, topic_len + 1); + memcpy(payload, tx->payload, payload_len); + tx->retries++; + tx->last_send_ms = now; + k_mutex_unlock(&data->lock); + if (state == ARES_MQTTLITE_TX_WAIT_PUBCOMP) { + (void)send_ack(protocol, MQTL_FRAME_PUBREL, packet_id); + } else { + (void)send_frame(protocol, MQTL_FRAME_PUBLISH, qos, MQTL_FLAG_DUP, + packet_id, use_topic_id, topic_id, topic, topic_len, + payload, payload_len); + } + k_mutex_lock(&data->lock, K_FOREVER); + } + k_mutex_unlock(&data->lock); + + if (has_pending) { + schedule_retry(data); + } +} + +int ares_mqttlite_register_topic(struct AresProtocol *protocol, uint16_t topic_id, + const char *topic) +{ + struct ares_mqttlite_protocol_data *data; + size_t topic_len; + int empty = -1; + + if (protocol == NULL || topic_id == 0 || topic == NULL) { + return -EINVAL; + } + + data = protocol->priv_data; + topic_len = strlen(topic); + if (topic_len == 0 || topic_len > CONFIG_ARES_MQTTLITE_MAX_TOPIC_LEN) { + return -EINVAL; + } + + k_mutex_lock(&data->lock, K_FOREVER); + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_TOPICS; i++) { + if (!data->topics[i].used && empty < 0) { + empty = i; + continue; + } + if (data->topics[i].used && data->topics[i].topic_id == topic_id) { + memcpy(data->topics[i].topic, topic, topic_len + 1); + k_mutex_unlock(&data->lock); + return 0; + } + } + if (empty < 0) { + k_mutex_unlock(&data->lock); + return -ENOMEM; + } + + data->topics[empty].used = true; + data->topics[empty].topic_id = topic_id; + memcpy(data->topics[empty].topic, topic, topic_len + 1); + k_mutex_unlock(&data->lock); + + return 0; +} + +int ares_mqttlite_subscribe(struct AresProtocol *protocol, const char *topic_filter, + ares_mqttlite_msg_cb_t cb, void *user_data) +{ + struct ares_mqttlite_protocol_data *data; + size_t topic_len; + int empty = -1; + + if (protocol == NULL || topic_filter == NULL || cb == NULL) { + return -EINVAL; + } + + data = protocol->priv_data; + topic_len = strlen(topic_filter); + if (topic_len == 0 || topic_len > CONFIG_ARES_MQTTLITE_MAX_TOPIC_LEN) { + return -EINVAL; + } + + k_mutex_lock(&data->lock, K_FOREVER); + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_SUBSCRIPTIONS; i++) { + if (data->subscriptions[i].cb == NULL && empty < 0) { + empty = i; + continue; + } + if (!data->subscriptions[i].use_topic_id && + strcmp(data->subscriptions[i].topic_filter, topic_filter) == 0) { + data->subscriptions[i].cb = cb; + data->subscriptions[i].user_data = user_data; + k_mutex_unlock(&data->lock); + return 0; + } + } + if (empty < 0) { + k_mutex_unlock(&data->lock); + return -ENOMEM; + } + + memcpy(data->subscriptions[empty].topic_filter, topic_filter, topic_len + 1); + data->subscriptions[empty].cb = cb; + data->subscriptions[empty].user_data = user_data; + k_mutex_unlock(&data->lock); + + return 0; +} + +int ares_mqttlite_subscribe_id(struct AresProtocol *protocol, uint16_t topic_id, + ares_mqttlite_msg_cb_t cb, void *user_data) +{ + struct ares_mqttlite_protocol_data *data; + int empty = -1; + + if (protocol == NULL || topic_id == 0 || cb == NULL) { + return -EINVAL; + } + + data = protocol->priv_data; + k_mutex_lock(&data->lock, K_FOREVER); + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_SUBSCRIPTIONS; i++) { + if (data->subscriptions[i].cb == NULL && empty < 0) { + empty = i; + continue; + } + if (data->subscriptions[i].use_topic_id && + data->subscriptions[i].topic_id == topic_id) { + data->subscriptions[i].cb = cb; + data->subscriptions[i].user_data = user_data; + k_mutex_unlock(&data->lock); + return 0; + } + } + if (empty < 0) { + k_mutex_unlock(&data->lock); + return -ENOMEM; + } + + data->subscriptions[empty].use_topic_id = true; + data->subscriptions[empty].topic_id = topic_id; + data->subscriptions[empty].topic_filter[0] = '\0'; + data->subscriptions[empty].cb = cb; + data->subscriptions[empty].user_data = user_data; + k_mutex_unlock(&data->lock); + + return 0; +} + +int ares_mqttlite_unsubscribe(struct AresProtocol *protocol, const char *topic_filter) +{ + struct ares_mqttlite_protocol_data *data; + + if (protocol == NULL || topic_filter == NULL) { + return -EINVAL; + } + + data = protocol->priv_data; + k_mutex_lock(&data->lock, K_FOREVER); + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_SUBSCRIPTIONS; i++) { + if (!data->subscriptions[i].use_topic_id && + strcmp(data->subscriptions[i].topic_filter, topic_filter) == 0) { + memset(&data->subscriptions[i], 0, sizeof(data->subscriptions[i])); + k_mutex_unlock(&data->lock); + return 0; + } + } + k_mutex_unlock(&data->lock); + + return -ENOENT; +} + +int ares_mqttlite_unsubscribe_id(struct AresProtocol *protocol, uint16_t topic_id) +{ + struct ares_mqttlite_protocol_data *data; + + if (protocol == NULL || topic_id == 0) { + return -EINVAL; + } + + data = protocol->priv_data; + k_mutex_lock(&data->lock, K_FOREVER); + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_SUBSCRIPTIONS; i++) { + if (data->subscriptions[i].use_topic_id && + data->subscriptions[i].topic_id == topic_id) { + memset(&data->subscriptions[i], 0, sizeof(data->subscriptions[i])); + k_mutex_unlock(&data->lock); + return 0; + } + } + k_mutex_unlock(&data->lock); + + return -ENOENT; +} + +static int publish_common(struct AresProtocol *protocol, bool use_topic_id, uint16_t topic_id, + const char *topic, uint16_t topic_len, const uint8_t *payload, + uint16_t payload_len, enum ares_mqttlite_qos qos, + ares_mqttlite_publish_cb_t cb, void *user_data) +{ + struct ares_mqttlite_protocol_data *data; + uint16_t packet_id = 0; + int ret; + + if (protocol == NULL || !valid_qos(qos)) { + return -EINVAL; + } + if (payload == NULL && payload_len > 0) { + return -EINVAL; + } + + data = protocol->priv_data; + if (use_topic_id) { + if (topic_id == 0) { + return -EINVAL; + } + topic_len = 0; + topic = ""; + } else if (topic == NULL || topic_len == 0 || + topic_len > CONFIG_ARES_MQTTLITE_MAX_TOPIC_LEN) { + return -EINVAL; + } + if (payload_len > CONFIG_ARES_MQTTLITE_MAX_PAYLOAD_SIZE) { + return -EINVAL; + } + + if (qos != ARES_MQTTLITE_QOS0) { + k_mutex_lock(&data->lock, K_FOREVER); + struct ares_mqttlite_tx_inflight *tx = alloc_tx_locked(data); + if (tx == NULL) { + k_mutex_unlock(&data->lock); + return -ENOMEM; + } + packet_id = alloc_packet_id_locked(data); + tx->state = qos == ARES_MQTTLITE_QOS1 ? ARES_MQTTLITE_TX_WAIT_PUBACK + : ARES_MQTTLITE_TX_WAIT_PUBREC; + tx->packet_id = packet_id; + tx->qos = qos; + tx->retries = 0; + tx->last_send_ms = k_uptime_get(); + tx->cb = cb; + tx->user_data = user_data; + tx->use_topic_id = use_topic_id; + tx->topic_id = topic_id; + tx->topic_len = topic_len; + tx->payload_len = payload_len; + memcpy(tx->topic, topic, topic_len + 1); + if (payload_len > 0) { + memcpy(tx->payload, payload, payload_len); + } + k_mutex_unlock(&data->lock); + } + + ret = send_frame(protocol, MQTL_FRAME_PUBLISH, qos, 0, packet_id, use_topic_id, topic_id, + topic, topic_len, payload, payload_len); + if (ret != 0 && qos != ARES_MQTTLITE_QOS0) { + complete_tx(protocol, packet_id, ret); + return ret; + } + + if (qos != ARES_MQTTLITE_QOS0) { + schedule_retry(data); + return packet_id; + } + + return ret; +} + +int ares_mqttlite_publish(struct AresProtocol *protocol, const char *topic, const uint8_t *payload, + uint16_t payload_len, enum ares_mqttlite_qos qos, + ares_mqttlite_publish_cb_t cb, void *user_data) +{ + if (topic == NULL) { + return -EINVAL; + } + + return publish_common(protocol, false, 0, topic, strlen(topic), payload, payload_len, qos, + cb, user_data); +} + +int ares_mqttlite_publish_id(struct AresProtocol *protocol, uint16_t topic_id, + const uint8_t *payload, uint16_t payload_len, + enum ares_mqttlite_qos qos, ares_mqttlite_publish_cb_t cb, + void *user_data) +{ + return publish_common(protocol, true, topic_id, NULL, 0, payload, payload_len, qos, cb, + user_data); +} + +int ares_mqttlite_publish_prepare_id(struct AresProtocol *protocol, uint16_t topic_id, + uint16_t payload_len, struct ares_mqttlite_publish_buffer *pub) +{ + struct AresInterface *interface; + size_t frame_len = MQTL_HEADER_LEN + MQTL_TOPIC_ID_LEN + payload_len; + + if (protocol == NULL || topic_id == 0 || pub == NULL || + payload_len > CONFIG_ARES_MQTTLITE_MAX_PAYLOAD_SIZE || + frame_len > CONFIG_ARES_MQTTLITE_MAX_FRAME_SIZE) { + return -EINVAL; + } + + interface = protocol->interface; + if (interface == NULL || interface->api == NULL || interface->api->send == NULL || + interface->api->alloc_buf == NULL) { + return -ENODEV; + } + + memset(pub, 0, sizeof(*pub)); + pub->buf = interface->api->alloc_buf(interface); + if (pub->buf == NULL) { + return -ENOMEM; + } + if (net_buf_tailroom(pub->buf) < frame_len) { + net_buf_unref(pub->buf); + memset(pub, 0, sizeof(*pub)); + return -EMSGSIZE; + } + + net_buf_add(pub->buf, frame_len); + sys_put_le16(MQTL_MAGIC, &pub->buf->data[MQTL_MAGIC_IDX]); + pub->buf->data[MQTL_VERSION_IDX] = MQTL_VERSION; + pub->buf->data[MQTL_TYPE_IDX] = MQTL_FRAME_PUBLISH; + pub->buf->data[MQTL_FLAGS_IDX] = ARES_MQTTLITE_QOS0 | MQTL_FLAG_TOPIC_ID; + pub->buf->data[MQTL_RESERVED_IDX] = 0; + sys_put_le16(0, &pub->buf->data[MQTL_PACKET_ID_IDX]); + sys_put_le16(MQTL_TOPIC_ID_LEN, &pub->buf->data[MQTL_TOPIC_LEN_IDX]); + sys_put_le16(payload_len, &pub->buf->data[MQTL_PAYLOAD_LEN_IDX]); + sys_put_le16(topic_id, &pub->buf->data[MQTL_HEADER_LEN]); + pub->payload = &pub->buf->data[MQTL_HEADER_LEN + MQTL_TOPIC_ID_LEN]; + pub->payload_len = payload_len; + + return 0; +} + +int ares_mqttlite_publish_commit(struct AresProtocol *protocol, + struct ares_mqttlite_publish_buffer *pub) +{ + struct AresInterface *interface; + struct net_buf *buf; + + if (protocol == NULL || pub == NULL || pub->buf == NULL) { + return -EINVAL; + } + + interface = protocol->interface; + if (interface == NULL || interface->api == NULL || interface->api->send == NULL) { + ares_mqttlite_publish_abort(pub); + return -ENODEV; + } + + buf = pub->buf; + memset(pub, 0, sizeof(*pub)); + return interface->api->send(interface, buf); +} + +void ares_mqttlite_publish_abort(struct ares_mqttlite_publish_buffer *pub) +{ + if (pub != NULL && pub->buf != NULL) { + net_buf_unref(pub->buf); + memset(pub, 0, sizeof(*pub)); + } +} + +int ares_mqttlite_ping(struct AresProtocol *protocol) +{ + return send_ack(protocol, MQTL_FRAME_PING, 0); +} + +void ares_mqttlite_protocol_handle_byte(struct AresProtocol *protocol, uint8_t byte) +{ + struct ares_mqttlite_protocol_data *data = protocol->priv_data; + + if (data->rx_pos >= sizeof(data->rx_buf)) { + data->rx_pos = 0; + data->rx_expected = 0; + } + + data->rx_buf[data->rx_pos++] = byte; + + if (data->rx_pos == MQTL_HEADER_LEN) { + if (sys_get_le16(&data->rx_buf[MQTL_MAGIC_IDX]) != MQTL_MAGIC || + data->rx_buf[MQTL_VERSION_IDX] != MQTL_VERSION) { + memmove(data->rx_buf, &data->rx_buf[1], data->rx_pos - 1); + data->rx_pos--; + return; + } + data->rx_expected = MQTL_HEADER_LEN + frame_topic_len(data->rx_buf) + + frame_payload_len(data->rx_buf); + if (data->rx_expected > sizeof(data->rx_buf)) { + data->rx_pos = 0; + data->rx_expected = 0; + return; + } + } + + if (data->rx_expected != 0 && data->rx_pos >= data->rx_expected) { + uint8_t frame[CONFIG_ARES_MQTTLITE_MAX_FRAME_SIZE]; + uint16_t frame_len = data->rx_expected; + + memcpy(frame, data->rx_buf, frame_len); + data->rx_pos = 0; + data->rx_expected = 0; + process_frame(protocol, frame, frame_len); + } +} + +void ares_mqttlite_protocol_handle(struct AresProtocol *protocol, struct net_buf *buf) +{ + struct ares_mqttlite_protocol_data *data = protocol->priv_data; + uint16_t pos = 0; + + if (data->rx_pos == 0) { + while (buf->len - pos >= MQTL_HEADER_LEN) { + uint16_t frame_len; + + if (sys_get_le16(&buf->data[pos + MQTL_MAGIC_IDX]) != MQTL_MAGIC || + buf->data[pos + MQTL_VERSION_IDX] != MQTL_VERSION) { + break; + } + + frame_len = MQTL_HEADER_LEN + + sys_get_le16(&buf->data[pos + MQTL_TOPIC_LEN_IDX]) + + sys_get_le16(&buf->data[pos + MQTL_PAYLOAD_LEN_IDX]); + if (frame_len > CONFIG_ARES_MQTTLITE_MAX_FRAME_SIZE) { + pos = buf->len; + break; + } + if (frame_len > buf->len - pos) { + break; + } + + process_frame(protocol, &buf->data[pos], frame_len); + pos += frame_len; + } + } + + for (uint16_t i = pos; i < buf->len; i++) { + ares_mqttlite_protocol_handle_byte(protocol, buf->data[i]); + } +} + +void ares_mqttlite_protocol_event(struct AresProtocol *protocol, enum AresProtocolEvent event) +{ + struct ares_mqttlite_protocol_data *data = protocol->priv_data; + + k_mutex_lock(&data->lock, K_FOREVER); + data->online = event == ARES_PROTOCOL_EVENT_CONNECTED; + if (!data->online) { + (void)k_work_cancel_delayable(&data->retry_work); + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_INFLIGHT; i++) { + clear_tx(&data->tx[i]); + memset(&data->rx_qos2[i], 0, sizeof(data->rx_qos2[i])); + } + data->rx_pos = 0; + data->rx_expected = 0; + } + k_mutex_unlock(&data->lock); +} + +int ares_mqttlite_protocol_init(struct AresProtocol *protocol) +{ + struct ares_mqttlite_protocol_data *data; + + if (protocol == NULL || protocol->priv_data == NULL) { + return -EINVAL; + } + + data = protocol->priv_data; + k_mutex_init(&data->lock); + k_work_init_delayable(&data->retry_work, retry_work_handler); + data->protocol = protocol; + data->next_packet_id = 1; + data->rx_pos = 0; + data->rx_expected = 0; + data->online = false; + memset(data->topics, 0, sizeof(data->topics)); + memset(data->subscriptions, 0, sizeof(data->subscriptions)); + memset(data->rx_qos2, 0, sizeof(data->rx_qos2)); + + for (int i = 0; i < CONFIG_ARES_MQTTLITE_MAX_INFLIGHT; i++) { + clear_tx(&data->tx[i]); + } + + LOG_INF("%s MQTT-like protocol init", data->name); + return 0; +} diff --git a/samples/communication/mqttlite_usb/CMakeLists.txt b/samples/communication/mqttlite_usb/CMakeLists.txt new file mode 100644 index 0000000..d31c8c9 --- /dev/null +++ b/samples/communication/mqttlite_usb/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 3.20.0) + +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(ares_mqttlite_usb) + +target_sources(app PRIVATE src/main.c) diff --git a/samples/communication/mqttlite_usb/README.md b/samples/communication/mqttlite_usb/README.md new file mode 100644 index 0000000..82b7665 --- /dev/null +++ b/samples/communication/mqttlite_usb/README.md @@ -0,0 +1,22 @@ +# ARES MQTT-like USB Bulk 示例 + +这个示例演示如何把 MQTT-like 协议绑定到 USB bulk 接口上。 + +## 功能 + +- 使用 `ARES_BULK_INTERFACE_DEFINE()` 定义 USB bulk 接口。 +- 使用 `ARES_MQTTLITE_PROTOCOL_DEFINE()` 定义协议实例。 +- 注册 `robot/mqttlite/heartbeat` 与 `host/command` 的 topic id。 +- 按 id 订阅 `host/command`。 +- 周期通过 QoS0 zero-copy 路径发布 `robot/mqttlite/heartbeat`。 + +## 构建 + +```sh +west build -b dm_mc02 samples/communication/mqttlite_usb --pristine +``` + +## 上位机约定 + +设备会发送带 topic-id 标志的 MQTT-like `PUBLISH` 帧，heartbeat 的 topic id 为 `1`， +`host/command` 的 topic id 为 `2`。当前示例 heartbeat 使用 QoS0，不等待上位机确认。 diff --git a/samples/communication/mqttlite_usb/boards/dm_mc02.conf b/samples/communication/mqttlite_usb/boards/dm_mc02.conf new file mode 100644 index 0000000..d9b3aa9 --- /dev/null +++ b/samples/communication/mqttlite_usb/boards/dm_mc02.conf @@ -0,0 +1,9 @@ +# dm_mc02 uses USART10 as the debug console, so keep this sample focused on USB bulk. +CONFIG_UART_INTERFACE=n + +CONFIG_CONSOLE=y +CONFIG_UART_CONSOLE=y +CONFIG_LOG_BACKEND_UART=y + +CONFIG_SENSOR=n +CONFIG_BMI08X=n diff --git a/samples/communication/mqttlite_usb/boards/dm_mc02.overlay b/samples/communication/mqttlite_usb/boards/dm_mc02.overlay new file mode 100644 index 0000000..c6cd80d --- /dev/null +++ b/samples/communication/mqttlite_usb/boards/dm_mc02.overlay @@ -0,0 +1,5 @@ +zephyr_udc0: &usbotg_hs { + pinctrl-0 = <&usb_otg_hs_dm_pa11 &usb_otg_hs_dp_pa12>; + pinctrl-names = "default"; + status = "okay"; +}; diff --git a/samples/communication/mqttlite_usb/prj.conf b/samples/communication/mqttlite_usb/prj.conf new file mode 100644 index 0000000..4d3387c --- /dev/null +++ b/samples/communication/mqttlite_usb/prj.conf @@ -0,0 +1,12 @@ +# ARES MQTT-like protocol over USB Bulk + +CONFIG_USB_BULK_INTERFACE=y +CONFIG_ARES_MQTTLITE_PROTOCOL=y +CONFIG_NET_BUF=y + +CONFIG_MAIN_THREAD_PRIORITY=10 + +CONFIG_LOG=y +CONFIG_LOG_DEFAULT_LEVEL=3 +CONFIG_UDC_DRIVER_LOG_LEVEL_INF=y +CONFIG_USBD_LOG_LEVEL_INF=y diff --git a/samples/communication/mqttlite_usb/sample.yaml b/samples/communication/mqttlite_usb/sample.yaml new file mode 100644 index 0000000..d597b3d --- /dev/null +++ b/samples/communication/mqttlite_usb/sample.yaml @@ -0,0 +1,14 @@ +# SPDX-License-Identifier: Apache-2.0 + +sample: + name: ARES MQTT-like USB Bulk +tests: + sample.communication.mqttlite_usb: + tags: + - ares + - communication + - usb + platform_allow: + - dm_mc02 + integration_platforms: + - dm_mc02 diff --git a/samples/communication/mqttlite_usb/src/main.c b/samples/communication/mqttlite_usb/src/main.c new file mode 100644 index 0000000..51a7b2d --- /dev/null +++ b/samples/communication/mqttlite_usb/src/main.c @@ -0,0 +1,96 @@ +/* + * Copyright (c) 2026 ttwards + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#include +#include +#include + +#include +#include +#include + +LOG_MODULE_REGISTER(mqttlite_usb_sample, LOG_LEVEL_INF); + +#define TOPIC_HEARTBEAT 1 +#define TOPIC_HOST_COMMAND 2 + +ARES_BULK_INTERFACE_DEFINE(usb_bulk_interface); +ARES_MQTTLITE_PROTOCOL_DEFINE(mqttlite_protocol); + +static void command_cb(struct AresProtocol *protocol, const char *topic, const uint8_t *payload, + uint16_t payload_len, enum ares_mqttlite_qos qos, void *user_data) +{ + char text[65]; + size_t len = MIN(payload_len, sizeof(text) - 1); + + ARG_UNUSED(protocol); + ARG_UNUSED(user_data); + + memcpy(text, payload, len); + text[len] = '\0'; + + LOG_INF("RX topic=%s qos=%u payload=%s", topic, qos, text); +} + +int main(void) +{ + uint32_t counter = 0; + int ret; + + LOG_INF("ARES MQTT-like USB Bulk sample starting"); + + ret = ares_bind_interface(&usb_bulk_interface, &mqttlite_protocol); + if (ret != 0) { + LOG_ERR("Failed to bind USB bulk interface (%d)", ret); + return ret; + } + + ret = ares_mqttlite_register_topic(&mqttlite_protocol, TOPIC_HEARTBEAT, + "robot/mqttlite/heartbeat"); + if (ret != 0) { + LOG_ERR("Failed to register heartbeat topic (%d)", ret); + return ret; + } + + ret = ares_mqttlite_register_topic(&mqttlite_protocol, TOPIC_HOST_COMMAND, "host/command"); + if (ret != 0) { + LOG_ERR("Failed to register command topic (%d)", ret); + return ret; + } + + ret = ares_mqttlite_subscribe_id(&mqttlite_protocol, TOPIC_HOST_COMMAND, command_cb, NULL); + if (ret != 0) { + LOG_ERR("Failed to subscribe host/command (%d)", ret); + return ret; + } + + while (1) { + char payload[32]; + struct ares_mqttlite_publish_buffer pub; + size_t payload_len; + + snprintk(payload, sizeof(payload), "heartbeat:%u", counter++); + payload_len = strlen(payload); + + ret = ares_mqttlite_publish_prepare_id(&mqttlite_protocol, TOPIC_HEARTBEAT, + payload_len, &pub); + if (ret == 0) { + memcpy(pub.payload, payload, payload_len); + ret = ares_mqttlite_publish_commit(&mqttlite_protocol, &pub); + } + if (ret < 0) { + LOG_WRN("Publish failed (%d)", ret); + } else { + LOG_INF("QoS0 heartbeat published"); + } + + k_sleep(K_SECONDS(1)); + } + + return 0; +} diff --git a/tests/native_sim/mqttlite/CMakeLists.txt b/tests/native_sim/mqttlite/CMakeLists.txt new file mode 100644 index 0000000..49e3892 --- /dev/null +++ b/tests/native_sim/mqttlite/CMakeLists.txt @@ -0,0 +1,7 @@ +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(ares_native_sim_mqttlite) + +target_sources(app PRIVATE src/main.c) diff --git a/tests/native_sim/mqttlite/prj.conf b/tests/native_sim/mqttlite/prj.conf new file mode 100644 index 0000000..439a995 --- /dev/null +++ b/tests/native_sim/mqttlite/prj.conf @@ -0,0 +1,9 @@ +CONFIG_ZTEST=y +CONFIG_ARES_MQTTLITE_PROTOCOL=y +CONFIG_ARES_MQTTLITE_MAX_TOPIC_LEN=32 +CONFIG_ARES_MQTTLITE_MAX_PAYLOAD_SIZE=64 +CONFIG_ARES_MQTTLITE_MAX_FRAME_SIZE=128 +CONFIG_ARES_MQTTLITE_MAX_INFLIGHT=4 +CONFIG_ARES_MQTTLITE_MAX_SUBSCRIPTIONS=8 +CONFIG_ARES_MQTTLITE_MAX_TOPICS=8 +CONFIG_ARES_MQTTLITE_RETRY_INTERVAL_MS=50 diff --git a/tests/native_sim/mqttlite/src/main.c b/tests/native_sim/mqttlite/src/main.c new file mode 100644 index 0000000..66082b2 --- /dev/null +++ b/tests/native_sim/mqttlite/src/main.c @@ -0,0 +1,238 @@ +/* + * Copyright (c) 2026 ttwards + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include + +#include +#include + +#include +#include +#include + +NET_BUF_POOL_DEFINE(fake_pool, 16, 512, 0, NULL); + +struct fake_link { + struct AresInterface *peer; +}; + +static int fake_init(struct AresInterface *interface) +{ + ARG_UNUSED(interface); + return 0; +} + +static struct net_buf *fake_alloc_buf(struct AresInterface *interface) +{ + ARG_UNUSED(interface); + return net_buf_alloc(&fake_pool, K_NO_WAIT); +} + +static int fake_send(struct AresInterface *interface, struct net_buf *buf) +{ + struct fake_link *link = interface->priv_data; + + zassert_not_null(link->peer, "fake link peer must be set"); + zassert_not_null(link->peer->protocol, "fake link peer protocol must be bound"); + + link->peer->protocol->api->handle(link->peer->protocol, buf); + net_buf_unref(buf); + + return 0; +} + +static const struct AresInterfaceAPI fake_api = { + .init = fake_init, + .send = fake_send, + .alloc_buf = fake_alloc_buf, +}; + +static struct fake_link client_link; +static struct fake_link server_link; + +static struct AresInterface client_if = { + .name = "client", + .api = &fake_api, + .priv_data = &client_link, +}; + +static struct AresInterface server_if = { + .name = "server", + .api = &fake_api, + .priv_data = &server_link, +}; + +ARES_MQTTLITE_PROTOCOL_DEFINE(client_protocol); +ARES_MQTTLITE_PROTOCOL_DEFINE(server_protocol); + +static int rx_count; +static enum ares_mqttlite_qos last_qos; +static char last_topic[CONFIG_ARES_MQTTLITE_MAX_TOPIC_LEN + 1]; +static char last_payload[CONFIG_ARES_MQTTLITE_MAX_PAYLOAD_SIZE + 1]; +static int publish_status; +static uint16_t publish_packet_id; + +static void reset_observed(void) +{ + rx_count = 0; + last_qos = ARES_MQTTLITE_QOS0; + memset(last_topic, 0, sizeof(last_topic)); + memset(last_payload, 0, sizeof(last_payload)); + publish_status = INT_MIN; + publish_packet_id = 0; +} + +static void message_cb(struct AresProtocol *protocol, const char *topic, const uint8_t *payload, + uint16_t payload_len, enum ares_mqttlite_qos qos, void *user_data) +{ + ARG_UNUSED(protocol); + ARG_UNUSED(user_data); + + rx_count++; + last_qos = qos; + strncpy(last_topic, topic, sizeof(last_topic) - 1); + last_topic[sizeof(last_topic) - 1] = '\0'; + memcpy(last_payload, payload, payload_len); + last_payload[payload_len] = '\0'; +} + +static void publish_cb(struct AresProtocol *protocol, uint16_t packet_id, int status, + void *user_data) +{ + ARG_UNUSED(protocol); + ARG_UNUSED(user_data); + + publish_packet_id = packet_id; + publish_status = status; +} + +static void bind_pair(void) +{ + client_link.peer = &server_if; + server_link.peer = &client_if; + + zassert_ok(ares_bind_interface(&client_if, &client_protocol)); + zassert_ok(ares_bind_interface(&server_if, &server_protocol)); + client_protocol.api->event(&client_protocol, ARES_PROTOCOL_EVENT_CONNECTED); + server_protocol.api->event(&server_protocol, ARES_PROTOCOL_EVENT_CONNECTED); +} + +static void *suite_setup(void) +{ + bind_pair(); + return NULL; +} + +ZTEST(ares_mqttlite, test_qos0_publish_delivers_to_subscriber) +{ + const uint8_t payload[] = "ready"; + + reset_observed(); + + zassert_ok(ares_mqttlite_subscribe(&server_protocol, "robot/state", message_cb, NULL)); + zassert_ok(ares_mqttlite_publish(&client_protocol, "robot/state", payload, + sizeof(payload) - 1, ARES_MQTTLITE_QOS0, NULL, NULL)); + + zassert_equal(rx_count, 1, "rx_count=%d", rx_count); + zassert_equal(last_qos, ARES_MQTTLITE_QOS0); + zassert_mem_equal(last_payload, "ready", sizeof("ready")); +} + +ZTEST(ares_mqttlite, test_qos1_publish_gets_puback) +{ + const uint8_t payload[] = "armed"; + int packet_id; + + reset_observed(); + + zassert_ok(ares_mqttlite_subscribe(&server_protocol, "robot/state", message_cb, NULL)); + packet_id = + ares_mqttlite_publish(&client_protocol, "robot/state", payload, sizeof(payload) - 1, + ARES_MQTTLITE_QOS1, publish_cb, NULL); + + zassert_true(packet_id > 0, "QoS1 publish should return a packet id"); + zassert_equal(rx_count, 1, "rx_count=%d", rx_count); + zassert_equal(last_qos, ARES_MQTTLITE_QOS1); + zassert_equal(publish_packet_id, packet_id); + zassert_equal(publish_status, ARES_MQTTLITE_PUBLISH_ACKED); +} + +ZTEST(ares_mqttlite, test_qos2_publish_completes_exactly_once) +{ + const uint8_t payload[] = "shoot"; + int packet_id; + + reset_observed(); + + zassert_ok(ares_mqttlite_subscribe(&server_protocol, "robot/command", message_cb, NULL)); + packet_id = + ares_mqttlite_publish(&client_protocol, "robot/command", payload, + sizeof(payload) - 1, ARES_MQTTLITE_QOS2, publish_cb, NULL); + + zassert_true(packet_id > 0, "QoS2 publish should return a packet id"); + zassert_equal(rx_count, 1, "rx_count=%d", rx_count); + zassert_equal(last_qos, ARES_MQTTLITE_QOS2); + zassert_mem_equal(last_payload, "shoot", sizeof("shoot")); + zassert_equal(publish_packet_id, packet_id); + zassert_equal(publish_status, ARES_MQTTLITE_PUBLISH_ACKED); +} + +ZTEST(ares_mqttlite, test_topic_wildcards_match) +{ + const uint8_t payload[] = "42"; + + reset_observed(); + + zassert_ok(ares_mqttlite_subscribe(&server_protocol, "robot/+/rpm", message_cb, NULL)); + zassert_ok(ares_mqttlite_publish(&client_protocol, "robot/front/rpm", payload, + sizeof(payload) - 1, ARES_MQTTLITE_QOS0, NULL, NULL)); + + zassert_equal(rx_count, 1); + zassert_mem_equal(last_payload, "42", sizeof("42")); +} + +ZTEST(ares_mqttlite, test_topic_id_qos1_publish_gets_puback) +{ + const uint8_t payload[] = "fast"; + int packet_id; + + reset_observed(); + + zassert_ok(ares_mqttlite_register_topic(&server_protocol, 7, "robot/fast")); + zassert_ok(ares_mqttlite_subscribe_id(&server_protocol, 7, message_cb, NULL)); + packet_id = ares_mqttlite_publish_id(&client_protocol, 7, payload, sizeof(payload) - 1, + ARES_MQTTLITE_QOS1, publish_cb, NULL); + + zassert_true(packet_id > 0, "QoS1 id publish should return a packet id"); + zassert_equal(rx_count, 1, "rx_count=%d", rx_count); + zassert_equal(last_qos, ARES_MQTTLITE_QOS1); + zassert_mem_equal(last_topic, "robot/fast", sizeof("robot/fast")); + zassert_mem_equal(last_payload, "fast", sizeof("fast")); + zassert_equal(publish_packet_id, packet_id); + zassert_equal(publish_status, ARES_MQTTLITE_PUBLISH_ACKED); +} + +ZTEST(ares_mqttlite, test_topic_id_qos0_prepare_commit_is_zero_copy) +{ + struct ares_mqttlite_publish_buffer pub; + + reset_observed(); + + zassert_ok(ares_mqttlite_register_topic(&server_protocol, 8, "robot/zero")); + zassert_ok(ares_mqttlite_subscribe_id(&server_protocol, 8, message_cb, NULL)); + zassert_ok(ares_mqttlite_publish_prepare_id(&client_protocol, 8, 4, &pub)); + zassert_not_null(pub.payload); + memcpy(pub.payload, "zero", 4); + zassert_ok(ares_mqttlite_publish_commit(&client_protocol, &pub)); + + zassert_equal(rx_count, 1, "rx_count=%d", rx_count); + zassert_equal(last_qos, ARES_MQTTLITE_QOS0); + zassert_mem_equal(last_topic, "robot/zero", sizeof("robot/zero")); + zassert_mem_equal(last_payload, "zero", sizeof("zero")); +} + +ZTEST_SUITE(ares_mqttlite, NULL, suite_setup, NULL, NULL, NULL); diff --git a/tests/native_sim/mqttlite/testcase.yaml b/tests/native_sim/mqttlite/testcase.yaml new file mode 100644 index 0000000..fcfa0c3 --- /dev/null +++ b/tests/native_sim/mqttlite/testcase.yaml @@ -0,0 +1,14 @@ +# SPDX-License-Identifier: Apache-2.0 + +tests: + ares.native_sim.mqttlite: + tags: + - ares + - native_sim + - protocol + platform_allow: + - native_sim + - native_sim/native/64 + integration_platforms: + - native_sim + - native_sim/native/64 From 7098b44641579924fc4c30de060d12f499a2f967 Mon Sep 17 00:00:00 2001 From: ttwards <12411711@mail.sustech.edu.cn> Date: Wed, 8 Jul 2026 01:46:51 +0800 Subject: [PATCH 6/6] style(motor): format CAN scheduler calls --- drivers/motor/dji/motor_dji.c | 6 +++--- drivers/motor/lk/motor_lk.c | 28 ++++++++++++++-------------- drivers/motor/mi/motor_mi.c | 6 +++--- 3 files changed, 20 insertions(+), 20 deletions(-) diff --git a/drivers/motor/dji/motor_dji.c b/drivers/motor/dji/motor_dji.c index d431317..a979ab8 100644 --- a/drivers/motor/dji/motor_dji.c +++ b/drivers/motor/dji/motor_dji.c @@ -794,9 +794,9 @@ void dji_tx_handler(struct k_work *work) txframe.dlc = 8; txframe.flags = 0; const struct device *can_dev = ctrl_struct->can_dev; - motor_can_sched_send_with_priority( - can_dev, &txframe, MOTOR_CAN_SCHED_PRIO_CRITICAL, - "dji-feedback-control"); + motor_can_sched_send_with_priority(can_dev, &txframe, + MOTOR_CAN_SCHED_PRIO_CRITICAL, + "dji-feedback-control"); } } } diff --git a/drivers/motor/lk/motor_lk.c b/drivers/motor/lk/motor_lk.c index d868830..54d29c9 100644 --- a/drivers/motor/lk/motor_lk.c +++ b/drivers/motor/lk/motor_lk.c @@ -365,8 +365,8 @@ void lk_tx_data_handler(struct k_work *work) tx_frame.flags = 0; tx_frame.data[0] = LK_CMD_MOTOR_RUN; motor_can_sched_send_with_priority( - cfg->common.phy, &tx_frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, - "lk-retry-enable"); + cfg->common.phy, &tx_frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, "lk-retry-enable"); } continue; } @@ -376,9 +376,9 @@ void lk_tx_data_handler(struct k_work *work) tx_frame.dlc = 8; tx_frame.flags = 0; tx_frame.data[0] = LK_CMD_MOTOR_RUN; - motor_can_sched_send_with_priority( - cfg->common.phy, &tx_frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, - "lk-reenable"); + motor_can_sched_send_with_priority(cfg->common.phy, &tx_frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, + "lk-reenable"); data->params_update[0] = true; data->params_update[1] = true; data->params_update[2] = true; @@ -426,9 +426,9 @@ void lk_tx_params_data_handler(struct k_work *work) int16_t kd_val = (int16_t)(data->params[0].k_d); tx_frame.data[6] = kd_val & 0xFF; tx_frame.data[7] = (kd_val >> 8) & 0xFF; - motor_can_sched_send_with_priority( - cfg->common.phy, &tx_frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, - "lk-param-angle"); + motor_can_sched_send_with_priority(cfg->common.phy, &tx_frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, + "lk-param-angle"); } k_sleep(K_USEC(120)); if (data->params_update[1]) { @@ -448,9 +448,9 @@ void lk_tx_params_data_handler(struct k_work *work) int16_t kd_val = (int16_t)(data->params[1].k_d); tx_frame.data[6] = kd_val & 0xFF; tx_frame.data[7] = (kd_val >> 8) & 0xFF; - motor_can_sched_send_with_priority( - cfg->common.phy, &tx_frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, - "lk-param-speed"); + motor_can_sched_send_with_priority(cfg->common.phy, &tx_frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, + "lk-param-speed"); } k_sleep(K_USEC(120)); if (data->params_update[2]) { @@ -470,9 +470,9 @@ void lk_tx_params_data_handler(struct k_work *work) int16_t kd_val = (int16_t)(data->params[2].k_d); tx_frame.data[6] = kd_val & 0xFF; tx_frame.data[7] = (kd_val >> 8) & 0xFF; - motor_can_sched_send_with_priority( - cfg->common.phy, &tx_frame, MOTOR_CAN_SCHED_PRIO_CRITICAL, - "lk-param-torque"); + motor_can_sched_send_with_priority(cfg->common.phy, &tx_frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, + "lk-param-torque"); } k_sleep(K_USEC(120)); } diff --git a/drivers/motor/mi/motor_mi.c b/drivers/motor/mi/motor_mi.c index 94327f1..c1fdaa8 100644 --- a/drivers/motor/mi/motor_mi.c +++ b/drivers/motor/mi/motor_mi.c @@ -351,9 +351,9 @@ void mi_tx_data_handler(struct k_work *work) 0x1F00FF00, 5U, "mi-control"); if ((data->common.mode == PV) || (data->common.mode == VO)) { - motor_can_sched_send_with_priority( - cfg->common.phy, &tx_frame[1], MOTOR_CAN_SCHED_PRIO_CRITICAL, - "mi-follow"); + motor_can_sched_send_with_priority(cfg->common.phy, &tx_frame[1], + MOTOR_CAN_SCHED_PRIO_CRITICAL, + "mi-follow"); } } if (i % 2 == 1) {