From 5058355c1b405a81cfeb9a83f0b132aed8479149 Mon Sep 17 00:00:00 2001 From: Zhang Lixu Date: Mon, 11 Dec 2023 16:56:10 +0800 Subject: [PATCH] sensing: add rtio implement for sensing subsys 1. add function sensing_iodev_submit and defined struct sensing_submit_config 2. fix the issue of phy_3d_sensor Signed-off-by: Zhang Lixu --- include/zephyr/sensing/sensing_sensor.h | 23 ++++- subsys/sensing/sensing_sensor.c | 17 ++++ .../sensor/phy_3d_sensor/phy_3d_sensor.c | 99 ++++++++----------- .../sensor/phy_3d_sensor/phy_3d_sensor.h | 6 +- subsys/sensing/sensor_mgmt.c | 35 +++++-- subsys/sensing/sensor_mgmt.h | 11 --- 6 files changed, 109 insertions(+), 82 deletions(-) diff --git a/include/zephyr/sensing/sensing_sensor.h b/include/zephyr/sensing/sensing_sensor.h index fb7eb1e2c9..65271d0870 100644 --- a/include/zephyr/sensing/sensing_sensor.h +++ b/include/zephyr/sensing/sensing_sensor.h @@ -153,13 +153,28 @@ extern struct sensing_sensor SENSING_SENSOR_SOURCE_NAME(idx, node); \ (,), node, cb_list_ptr) \ }; -#define SENSING_SENSOR_IODEV_NAME(node, idx) \ +struct sensing_submit_config { + enum sensor_channel chan; + const int info_index; + const bool is_streaming; +}; + +extern const struct rtio_iodev_api __sensing_iodev_api; + +#define SENSING_SUBMIT_CFG_NAME(node, idx) \ + _CONCAT(_CONCAT(__sensing_submit_cfg_, idx), DEVICE_DT_NAME_GET(node)) + +#define SENSING_SENSOR_IODEV_NAME(node, idx) \ _CONCAT(_CONCAT(__sensing_iodev_, idx), DEVICE_DT_NAME_GET(node)) #define SENSING_SENSOR_IODEV_DEFINE(node, idx) \ - COND_CODE_1(DT_PROP(node, stream_mode), \ - (SENSOR_DT_STREAM_IODEV(SENSING_SENSOR_IODEV_NAME(node, idx), node)), \ - (SENSOR_DT_READ_IODEV(SENSING_SENSOR_IODEV_NAME(node, idx), node))); + static struct sensing_submit_config SENSING_SUBMIT_CFG_NAME(node, idx) = { \ + .is_streaming = DT_PROP(node, stream_mode), \ + .info_index = idx, \ + }; \ + RTIO_IODEV_DEFINE(SENSING_SENSOR_IODEV_NAME(node, idx), \ + &__sensing_iodev_api, \ + &SENSING_SUBMIT_CFG_NAME(node, idx)); #define SENSING_SENSOR_NAME(node, idx) \ _CONCAT(_CONCAT(__sensing_sensor_, idx), DEVICE_DT_NAME_GET(node)) diff --git a/subsys/sensing/sensing_sensor.c b/subsys/sensing/sensing_sensor.c index da921c538a..a2ebd9ff7d 100644 --- a/subsys/sensing/sensing_sensor.c +++ b/subsys/sensing/sensing_sensor.c @@ -13,6 +13,23 @@ LOG_MODULE_DECLARE(sensing, CONFIG_SENSING_LOG_LEVEL); +static void sensing_iodev_submit(struct rtio_iodev_sqe *iodev_sqe) +{ + struct sensing_sensor *sensor = (struct sensing_sensor *)iodev_sqe->sqe.userdata; + const struct device *dev = sensor->dev; + const struct sensor_driver_api *api = dev->api; + + if (api->submit != NULL) { + api->submit(dev, iodev_sqe); + } else { + rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP); + } +} + +const struct rtio_iodev_api __sensing_iodev_api = { + .submit = sensing_iodev_submit, +}; + int sensing_sensor_get_reporters(const struct device *dev, int type, sensing_sensor_handle_t *reporter_handles, int max_handles) diff --git a/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.c b/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.c index 1a725a5bc0..3ad2e499ff 100644 --- a/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.c +++ b/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.c @@ -102,18 +102,21 @@ static int phy_3d_sensor_init(const struct device *dev) { const struct phy_3d_sensor_config *cfg = dev->config; struct phy_3d_sensor_data *data = dev->data; + int i; - switch (cfg->sensor_type) { - case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D: - data->custom = &custom_accel; + for (i = 0; i < cfg->sensor_num; i++) { + switch (cfg->sensor_types[i]) { + case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D: + data->customs[i] = &custom_accel; break; - case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D: - data->custom = &custom_gyro; + case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D: + data->customs[i] = &custom_gyro; break; - default: - LOG_ERR("phy_3d_sensor doesn't support sensor type %d", - cfg->sensor_type); - return -ENOTSUP; + default: + LOG_ERR("phy_3d_sensor doesn't support sensor type %d", + cfg->sensor_types[i]); + return -ENOTSUP; + } } LOG_INF("%s: Underlying device: %s", dev->name, cfg->hw_dev->name); @@ -126,38 +129,12 @@ static int phy_3d_sensor_attr_set_hyst(const struct device *dev, const struct sensor_value *val) { const struct phy_3d_sensor_config *cfg = dev->config; - struct phy_3d_sensor_data *data = dev->data; - int index = chan - SENSOR_CHAN_PRIV_START; - int i, min = 0; - if (index >= 0 && index < ARRAY_SIZE(data->sensitivities)) { - data->sensitivities[index] = *val; - } else if (index == SENSING_SENSITIVITY_INDEX_ALL) { - for (i = 0; i < ARRAY_SIZE(data->sensitivities); ++i) { - data->sensitivities[i] = *val; - } - min = ARRAY_SIZE(data->sensitivities) - 1; + if (chan == SENSOR_CHAN_ACCEL_XYZ || chan == SENSOR_CHAN_GYRO_XYZ) { + return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_SLOPE_TH, val); } else { - LOG_ERR("%s: set sensitivity: invalid index: %d", - dev->name, index); - return -EINVAL; + return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_HYSTERESIS, val); } - - for (i = min + 1; i < ARRAY_SIZE(data->sensitivities); ++i) { - if ((data->sensitivities[i].val1 < - data->sensitivities[min].val1) || - (data->sensitivities[i].val1 == - data->sensitivities[min].val1 && - data->sensitivities[i].val2 < - data->sensitivities[min].val2)) { - min = i; - } - } - - chan = data->custom->chan_all; - - return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_SLOPE_TH, - &data->sensitivities[min]); } static int phy_3d_sensor_attr_set(const struct device *dev, @@ -166,7 +143,6 @@ static int phy_3d_sensor_attr_set(const struct device *dev, const struct sensor_value *val) { const struct phy_3d_sensor_config *cfg = dev->config; - struct phy_3d_sensor_data *data = dev->data; int ret = 0; switch (attr) { @@ -175,7 +151,6 @@ static int phy_3d_sensor_attr_set(const struct device *dev, break; default: - chan = data->custom->chan_all; ret = sensor_attr_set(cfg->hw_dev, chan, attr, val); break; } @@ -187,8 +162,10 @@ static int phy_3d_sensor_attr_set(const struct device *dev, static int phy_3d_sensor_submit(const struct device *dev, struct rtio_iodev_sqe *sqe) { + struct sensing_submit_config *config = (struct sensing_submit_config *)sqe->sqe.iodev->data; const struct phy_3d_sensor_config *cfg = dev->config; struct phy_3d_sensor_data *data = dev->data; + const struct phy_3d_sensor_custom *custom = data->customs[config->info_index]; struct sensing_sensor_value_3d_q31 *sample; struct sensor_value value[PHY_3D_SENSOR_CHANNEL_NUM]; uint32_t buffer_len; @@ -201,14 +178,14 @@ static int phy_3d_sensor_submit(const struct device *dev, return ret; } - ret = sensor_sample_fetch_chan(cfg->hw_dev, data->custom->chan_all); + ret = sensor_sample_fetch_chan(cfg->hw_dev, custom->chan_all); if (ret) { LOG_ERR("%s: sample fetch failed: %d", dev->name, ret); rtio_iodev_sqe_err(sqe, ret); return ret; } - ret = sensor_channel_get(cfg->hw_dev, data->custom->chan_all, value); + ret = sensor_channel_get(cfg->hw_dev, custom->chan_all, value); if (ret) { LOG_ERR("%s: channel get failed: %d", dev->name, ret); rtio_iodev_sqe_err(sqe, ret); @@ -216,12 +193,11 @@ static int phy_3d_sensor_submit(const struct device *dev, } for (i = 0; i < ARRAY_SIZE(value); ++i) { - sample->readings[0].v[i] = - data->custom->sensor_value_to_q31(&value[i]); + sample->readings[0].v[i] = custom->sensor_value_to_q31(&value[i]); } sample->header.reading_count = 1; - sample->shift = data->custom->shift; + sample->shift = custom->shift; LOG_DBG("%s: Sample data:\t x: %d, y: %d, z: %d", dev->name, @@ -238,6 +214,7 @@ static const struct sensor_driver_api phy_3d_sensor_api = { .submit = phy_3d_sensor_submit, }; +/* To be removed */ static const struct sensing_sensor_register_info phy_3d_sensor_reg = { .flags = SENSING_SENSOR_FLAG_REPORT_ON_CHANGE, .sample_size = sizeof(struct sensing_sensor_value_3d_q31), @@ -245,18 +222,26 @@ static const struct sensing_sensor_register_info phy_3d_sensor_reg = { .version.value = SENSING_SENSOR_VERSION(0, 8, 0, 0), }; -#define SENSING_PHY_3D_SENSOR_DT_DEFINE(_inst) \ - static struct phy_3d_sensor_data _CONCAT(data, _inst); \ - static const struct phy_3d_sensor_config _CONCAT(cfg, _inst) = {\ - .hw_dev = DEVICE_DT_GET( \ - DT_PHANDLE(DT_DRV_INST(_inst), \ - underlying_device)), \ - .sensor_type = DT_PROP_BY_IDX(DT_DRV_INST(_inst), sensor_types, 0),\ - }; \ - SENSING_SENSORS_DT_INST_DEFINE(_inst, &phy_3d_sensor_reg, NULL, \ - &phy_3d_sensor_init, NULL, \ - &_CONCAT(data, _inst), &_CONCAT(cfg, _inst), \ - POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \ +#define SENSING_PHY_3D_SENSOR_DT_DEFINE(_inst) \ + static struct rtio_iodev_sqe _CONCAT(sqes, _inst)[DT_INST_PROP_LEN(_inst, \ + sensor_types)]; \ + static const struct phy_3d_sensor_custom *_CONCAT(customs, _inst) \ + [DT_INST_PROP_LEN(_inst, sensor_types)]; \ + static struct phy_3d_sensor_data _CONCAT(data, _inst) = { \ + .sqes = _CONCAT(sqes, _inst), \ + .customs = _CONCAT(customs, _inst), \ + }; \ + static const struct phy_3d_sensor_config _CONCAT(cfg, _inst) = { \ + .hw_dev = DEVICE_DT_GET( \ + DT_PHANDLE(DT_DRV_INST(_inst), \ + underlying_device)), \ + .sensor_num = DT_INST_PROP_LEN(_inst, sensor_types), \ + .sensor_types = DT_PROP(DT_DRV_INST(_inst), sensor_types), \ + }; \ + SENSING_SENSORS_DT_INST_DEFINE(_inst, &phy_3d_sensor_reg, NULL, \ + &phy_3d_sensor_init, NULL, \ + &_CONCAT(data, _inst), &_CONCAT(cfg, _inst), \ + POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \ &phy_3d_sensor_api); DT_INST_FOREACH_STATUS_OKAY(SENSING_PHY_3D_SENSOR_DT_DEFINE); diff --git a/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.h b/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.h index 071c40dfcf..8711c9a570 100644 --- a/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.h +++ b/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.h @@ -20,13 +20,15 @@ struct phy_3d_sensor_custom { }; struct phy_3d_sensor_data { - const struct phy_3d_sensor_custom *custom; struct sensor_value sensitivities[PHY_3D_SENSOR_CHANNEL_NUM]; + const struct phy_3d_sensor_custom **customs; + struct rtio_iodev_sqe *sqes; }; struct phy_3d_sensor_config { const struct device *hw_dev; - const int32_t sensor_type; + const int sensor_num; + const int32_t sensor_types[]; }; #endif diff --git a/subsys/sensing/sensor_mgmt.c b/subsys/sensing/sensor_mgmt.c index 08c5ea0462..9f8a1ae253 100644 --- a/subsys/sensing/sensor_mgmt.c +++ b/subsys/sensing/sensor_mgmt.c @@ -29,6 +29,20 @@ RTIO_DEFINE_WITH_MEMPOOL(sensing_rtio_ctx, CONFIG_SENSING_RTIO_SQE_NUM, CONFIG_SENSING_RTIO_BLOCK_COUNT, CONFIG_SENSING_RTIO_BLOCK_SIZE, 4); +static enum sensor_channel sensing_sensor_type_to_chan(const int32_t type) +{ + switch (type) { + case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D: + return SENSOR_CHAN_ACCEL_XYZ; + case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D: + return SENSOR_CHAN_GYRO_XYZ; + default: + break; + } + + return SENSOR_CHAN_PRIV_START; +} + /* sensor_later_config including arbitrate/set interval/sensitivity */ static uint32_t arbitrate_interval(struct sensing_sensor *sensor) @@ -61,7 +75,7 @@ static uint32_t arbitrate_interval(struct sensing_sensor *sensor) static int set_arbitrate_interval(struct sensing_sensor *sensor, uint32_t interval) { - struct sensor_read_config *config = sensor->iodev->data; + struct sensing_submit_config *config = sensor->iodev->data; struct sensor_value odr = {0}; int ret; @@ -75,8 +89,7 @@ static int set_arbitrate_interval(struct sensing_sensor *sensor, uint32_t interv odr.val2 = (uint64_t)USEC_PER_SEC * 1000000 / interval % 1000000; } - /* The SENSOR_CHAN_MAX should be overridden by sensing sensor driver */ - ret = sensor_attr_set(sensor->dev, SENSOR_CHAN_MAX, + ret = sensor_attr_set(sensor->dev, config->chan, SENSOR_ATTR_SAMPLING_FREQUENCY, &odr); if (ret) { LOG_ERR("%s set attr freq failed:%d", sensor->dev->name, ret); @@ -143,16 +156,18 @@ static uint32_t arbitrate_sensitivity(struct sensing_sensor *sensor, int index) static int set_arbitrate_sensitivity(struct sensing_sensor *sensor, int index, uint32_t sensitivity) { + struct sensing_submit_config *config = (struct sensing_submit_config *)sensor->iodev->data; struct sensor_value threshold = {.val1 = sensitivity}; + int i; /* update sensor sensitivity */ sensor->sensitivity[index] = sensitivity; - /* The SENSOR_CHAN_PRIV_START should be overridden by sensing sensor - * driver, SENSOR_ATTR_HYSTERESIS can be overridden for different - * sensora type. - */ - return sensor_attr_set(sensor->dev, SENSOR_CHAN_PRIV_START + index, + for (i = 0; i < sensor->sensitivity_count; i++) { + threshold.val1 = MIN(sensor->sensitivity[i], threshold.val1); + } + + return sensor_attr_set(sensor->dev, config->chan, SENSOR_ATTR_HYSTERESIS, &threshold); } @@ -275,6 +290,7 @@ static void sensing_sensor_polling_timer(struct k_timer *timer_id) static int init_sensor(struct sensing_sensor *sensor) { + struct sensing_submit_config *config; struct sensing_connection *conn; int i; @@ -293,6 +309,9 @@ static int init_sensor(struct sensing_sensor *sensor) conn->source->dev->name, sensor->dev->name, i, conn); } + config = sensor->iodev->data; + config->chan = sensing_sensor_type_to_chan(sensor->info->type); + return 0; } diff --git a/subsys/sensing/sensor_mgmt.h b/subsys/sensing/sensor_mgmt.h index b29bd184b2..4672833e00 100644 --- a/subsys/sensing/sensor_mgmt.h +++ b/subsys/sensing/sensor_mgmt.h @@ -71,17 +71,6 @@ static inline struct sensing_sensor *get_sensor_by_dev(const struct device *dev) return NULL; } -static inline uint16_t get_reporter_sample_size(const struct sensing_sensor *sensor, int i) -{ - const struct sensing_sensor *reporter; - - __ASSERT(i < sensor->reporter_num, "dt index should less than reporter num"); - - reporter = sensor->conns[i].source; - - return reporter->register_info->sample_size; -} - static inline struct sensing_sensor *get_reporter_sensor(struct sensing_sensor *sensor, int index) { if (!sensor || index >= sensor->reporter_num) {