diff --git a/drivers/sensor/default_rtio_sensor.c b/drivers/sensor/default_rtio_sensor.c index a86bd81ec1d48fe..bc95c1d777aa6a3 100644 --- a/drivers/sensor/default_rtio_sensor.c +++ b/drivers/sensor/default_rtio_sensor.c @@ -316,9 +316,8 @@ static int decode(const uint8_t *buffer, sensor_frame_iterator_t *fit, { const struct sensor_data_generic_header *header = (const struct sensor_data_generic_header *)buffer; - const q31_t *q = - (const q31_t *)(buffer + sizeof(struct sensor_data_generic_header) + - header->num_channels * sizeof(enum sensor_channel)); + const q31_t *q = (const q31_t *)(buffer + sizeof(struct sensor_data_generic_header) + + header->num_channels * sizeof(enum sensor_channel)); int count = 0; if (*fit != 0 || *cit >= header->num_channels) { diff --git a/include/zephyr/sensing/sensing.h b/include/zephyr/sensing/sensing.h index 3fc6df3c25ac7a5..7dd2b78b83660c2 100644 --- a/include/zephyr/sensing/sensing.h +++ b/include/zephyr/sensing/sensing.h @@ -168,6 +168,7 @@ typedef void *sensing_sensor_handle_t; */ typedef void (*sensing_data_event_t)(sensing_sensor_handle_t handle, const void *buf); +#include /** * @struct sensing_sensor_info * @brief Sensor basic constant information @@ -180,6 +181,9 @@ struct sensing_sensor_info { /** Sensor type */ int32_t type; + + /****** TODO hide these (private members) ******/ + struct rtio_iodev *iodev; }; /** @@ -241,7 +245,7 @@ struct sensing_sensor_attribute { int8_t shift; }; -int sensing_set_attributes(sensing_sensor_handle_t handle, +int sensing_set_attributes(sensing_sensor_handle_t handle, enum sensing_sensor_mode mode, struct sensing_sensor_attribute *attributes, size_t count); /** diff --git a/include/zephyr/sensing/sensor.h b/include/zephyr/sensing/sensor.h index cd4614a8187d7d0..e9d6a6b4e4232cb 100644 --- a/include/zephyr/sensing/sensor.h +++ b/include/zephyr/sensing/sensor.h @@ -14,17 +14,23 @@ #define SENSING_SENSOR_INFO_DT_NAME(node_id, type) \ _CONCAT(_CONCAT(__sensing_sensor_info_, DEVICE_DT_NAME_GET(node_id)), type) -#define SENSING_SENSOR_INFO_INST_DEFINE(node_id, prop, idx) \ - const STRUCT_SECTION_ITERABLE( \ - sensing_sensor_info, \ - SENSING_SENSOR_INFO_DT_NAME(node_id, DT_PROP_BY_IDX(node_id, prop, idx))) = { \ +#define SENSING_SENSOR_INFO_INST_DEFINE_NAMED(node_id, name, prop, idx, _iodev) \ + const STRUCT_SECTION_ITERABLE(sensing_sensor_info, name) = { \ .info = &SENSOR_INFO_DT_NAME(DT_PHANDLE(node_id, dev)), \ .dev = DEVICE_DT_GET(node_id), \ .type = DT_PROP_BY_IDX(node_id, prop, idx), \ + .iodev = &(_iodev), \ }; +#define SENSING_SENSOR_INFO_INST_DEFINE(node_id, prop, idx, _iodev) \ + SENSING_SENSOR_INFO_INST_DEFINE_NAMED( \ + node_id, SENSING_SENSOR_INFO_DT_NAME(node_id, DT_PROP_BY_IDX(node_id, prop, idx)), \ + prop, idx, _iodev) + #define SENSING_SENSOR_INFO_DT_DEFINE(node_id) \ - DT_FOREACH_PROP_ELEM(node_id, sensor_types, SENSING_SENSOR_INFO_INST_DEFINE) + SENSOR_DT_READ_IODEV(node_id##_read_iodev, node_id, SENSOR_CHAN_ALL); \ + DT_FOREACH_PROP_ELEM_VARGS(node_id, sensor_types, SENSING_SENSOR_INFO_INST_DEFINE, \ + node_id##_read_iodev) #define SENSING_SENSOR_DT_DEFINE(node_id, init_fn, pm_device, data_ptr, cfg_ptr, level, prio, \ api_ptr, ...) \ @@ -38,6 +44,7 @@ #define SENSING_SENSOR_INFO_GET(node_id, type) &SENSING_SENSOR_INFO_DT_NAME(node_id, type) STRUCT_SECTION_START_EXTERN(sensing_sensor_info); +STRUCT_SECTION_END_EXTERN(sensing_sensor_info); #if defined(CONFIG_HAS_DTS) || defined(__DOXYGEN__) #define Z_MAYBE_SENSING_SENSOR_INFO_DECLARE_INTERNAL_DEFINE(node_id, prop, idx) \ diff --git a/samples/sensor/sensor_shell/prj.conf b/samples/sensor/sensor_shell/prj.conf index 6605bdf9f1e0da4..0e6ac047463ce63 100644 --- a/samples/sensor/sensor_shell/prj.conf +++ b/samples/sensor/sensor_shell/prj.conf @@ -10,5 +10,6 @@ CONFIG_RTIO_CONSUME_SEM=y CONFIG_USERSPACE=y CONFIG_SENSING=y CONFIG_SENSING_SHELL=y +CONFIG_RTIO_LOG_LEVEL_DBG=y CONFIG_SENSOR_LOG_LEVEL_DBG=y CONFIG_SENSING_LOG_LEVEL_DBG=y diff --git a/subsys/sensing/CMakeLists.txt b/subsys/sensing/CMakeLists.txt index 0ca14419e76a1c8..1d19a3c49e4185a 100644 --- a/subsys/sensing/CMakeLists.txt +++ b/subsys/sensing/CMakeLists.txt @@ -6,6 +6,7 @@ zephyr_library_sources( src/sensor_info.c src/sensor_pipe.c src/sensor_connections.c + src/processing.c ) zephyr_library_sources_ifdef(CONFIG_SENSING_SHELL src/shell.c) zephyr_library_sources_ifdef(CONFIG_USERSPACE src/userspace.c) diff --git a/subsys/sensing/Kconfig b/subsys/sensing/Kconfig index c7d10fb9436276c..c73ac89183e0142 100644 --- a/subsys/sensing/Kconfig +++ b/subsys/sensing/Kconfig @@ -6,6 +6,7 @@ config SENSING select DSP select SENSOR_INFO select SYS_MEM_BLOCKS + select SENSOR_ASYNC_API help Enable Sensing Subsystem. @@ -25,6 +26,28 @@ config SENSING_MAX_CONNECTIONS same sensor by different clients or the same client to different sensors). +config SENSING_RTIO_BLOCK_SIZE + int "Block size of the RTIO context" + default 64 + +config SENSING_RTIO_BLOCK_COUNT + int "Number of memory blocks of the RTIO context" + default 32 + +config SENSING_PROCESSING_THREAD_STACK_SIZE + int "Stack size for the subsystem's data processing thread" + default 1024 + help + Thread stack size to use for the sensor data processing of the sensing + subsystem. The more data is expected to come through prior to being + dispatched to clients, the larger the stack will need to be. + +config SENSING_PROCESSING_THREAD_PRIORITY + int "Thread priority of the subsystem's data processing thread" + default 10 + help + The sensor subsystem's data processing thread priority + config SENSING_SHELL bool "Shell commands for sensing subsystem" diff --git a/subsys/sensing/include/sensing/internal/sensing.h b/subsys/sensing/include/sensing/internal/sensing.h index bfa1cfc652f02a1..15a5ed9177d8e25 100644 --- a/subsys/sensing/include/sensing/internal/sensing.h +++ b/subsys/sensing/include/sensing/internal/sensing.h @@ -16,6 +16,7 @@ struct sensing_connection { const struct sensing_sensor_info *info; const struct sensing_callback_list *cb_list; + enum sensing_sensor_mode mode; q31_t attributes[SENSOR_ATTR_COMMON_COUNT]; uint32_t attribute_mask; } __packed __aligned(4); @@ -28,6 +29,8 @@ extern struct sensing_connection_pool { BUILD_ASSERT(SENSOR_ATTR_COMMON_COUNT <= 32, "Too many sensor attributes"); +extern struct rtio sensing_rtio_ctx; + static inline bool __sensing_is_connected(const struct sensing_sensor_info *info, const struct sensing_connection *connection) { diff --git a/subsys/sensing/src/processing.c b/subsys/sensing/src/processing.c new file mode 100644 index 000000000000000..ab001ae501219c9 --- /dev/null +++ b/subsys/sensing/src/processing.c @@ -0,0 +1,78 @@ +// +// Created by peress on 05/07/23. +// + +#include +#include +#include +#include +#include + +#include "sensing/internal/sensing.h" + +LOG_MODULE_REGISTER(sensing_processing, CONFIG_SENSING_LOG_LEVEL); + +static void processing_task(void *a, void *b, void *c) +{ + struct sensing_sensor_info *info; + uint8_t *data = NULL; + uint32_t data_len = 0; + int rc; + int get_data_rc; + + ARG_UNUSED(a); + ARG_UNUSED(b); + ARG_UNUSED(c); + + if (IS_ENABLED(CONFIG_USERSPACE) && !k_is_user_context()) { + rtio_access_grant(&sensing_rtio_ctx, k_current_get()); + k_thread_user_mode_enter(processing_task, a, b, c); + } + + while (true) { + struct rtio_cqe cqe; + + rc = rtio_cqe_copy_out(&sensing_rtio_ctx, &cqe, 1, K_FOREVER); + + if (rc < 1) { + continue; + } + + /* Cache the data from the CQE */ + rc = cqe.result; + info = cqe.userdata; + get_data_rc = + rtio_cqe_get_mempool_buffer(&sensing_rtio_ctx, &cqe, &data, &data_len); + + /* Do something with the data */ + LOG_DBG("Processing data for [%d], rc=%d, has_data=%d, data=%p, len=%" PRIu32, + (int)(info - STRUCT_SECTION_START(sensing_sensor_info)), rc, + get_data_rc == 0, (void *)data, data_len); + + if (get_data_rc != 0 || data_len == 0) { + continue; + } + + sys_mutex_lock(__sensing_connection_pool.lock, K_FOREVER); + for (int i = 0; i < ARRAY_SIZE(__sensing_connection_pool.pool); ++i) { + struct sensing_connection *connection = &__sensing_connection_pool.pool[i]; + if (!__sensing_is_connected(info, connection)) { + continue; + } + sensing_data_event_t cb = connection->cb_list->on_data_event; + + if (cb == NULL) { + continue; + } + /* TODO we actually need to decode the data first before this for loop */ + cb(connection, data); + } + sys_mutex_unlock(__sensing_connection_pool.lock); + + /* Release the memory */ + rtio_release_buffer(&sensing_rtio_ctx, data, data_len); + } +} + +K_THREAD_DEFINE(sensing_processor, CONFIG_SENSING_PROCESSING_THREAD_STACK_SIZE, processing_task, + NULL, NULL, NULL, CONFIG_SENSING_PROCESSING_THREAD_PRIORITY, 0, 0); diff --git a/subsys/sensing/src/sensor_connections.c b/subsys/sensing/src/sensor_connections.c index 7079d49c71f6698..71134e8581f6993 100644 --- a/subsys/sensing/src/sensor_connections.c +++ b/subsys/sensing/src/sensor_connections.c @@ -1,6 +1,7 @@ #include #include +#include #include #include #include @@ -13,7 +14,7 @@ LOG_MODULE_REGISTER(sensing_connect, CONFIG_SENSING_LOG_LEVEL); struct sensing_connection *name = handle; \ __ASSERT_NO_MSG((uintptr_t)name >= (uintptr_t)__sensing_connection_pool.pool); \ __ASSERT_NO_MSG((uintptr_t)name < (uintptr_t)__sensing_connection_pool.pool + \ - sizeof(__sensing_connection_pool.pool)); + sizeof(__sensing_connection_pool.pool)) SENSING_DMEM SYS_MUTEX_DEFINE(connection_lock); @@ -32,6 +33,10 @@ SENSING_DMEM struct sensing_connection_pool __sensing_connection_pool = { #define __lock sys_mutex_lock(__sensing_connection_pool.lock, K_FOREVER) #define __unlock sys_mutex_unlock(__sensing_connection_pool.lock) +RTIO_DEFINE_WITH_MEMPOOL(sensing_rtio_ctx, CONFIG_SENSING_MAX_CONNECTIONS, + CONFIG_SENSING_MAX_CONNECTIONS, CONFIG_SENSING_RTIO_BLOCK_COUNT, + CONFIG_SENSING_RTIO_BLOCK_SIZE, 4); + int sensing_open_sensor(const struct sensing_sensor_info *info, const struct sensing_callback_list *cb_list, sensing_sensor_handle_t *handle) @@ -84,10 +89,11 @@ int sensing_close_sensor(sensing_sensor_handle_t handle) return rc; } -int sensing_set_attributes(sensing_sensor_handle_t handle, +int sensing_set_attributes(sensing_sensor_handle_t handle, enum sensing_sensor_mode mode, struct sensing_sensor_attribute *attributes, size_t count) { __HANDLE_TO_CONNECTION(connection, handle); + int rc; __lock; for (size_t i = 0; i < count; ++i) { @@ -101,9 +107,21 @@ int sensing_set_attributes(sensing_sensor_handle_t handle, LOG_DBG("Updated attribute (%d) to 0x%08x->0x%08x", attributes[i].attribute, attributes[i].value, value); } + connection->mode = mode; __sensing_arbitrate(); + + switch (mode) { + case SENSING_SENSOR_MODE_ONE_SHOT: + LOG_DBG("Starting one-shot read"); + rc = sensor_read(connection->info->iodev, &sensing_rtio_ctx, + (void *)connection->info); + break; + default: + rc = -EINVAL; + break; + } __unlock; - return 0; + return rc; } const struct sensing_sensor_info *sensing_get_sensor_info(sensing_sensor_handle_t handle) diff --git a/subsys/sensing/src/sensor_pipe.c b/subsys/sensing/src/sensor_pipe.c index 09def4b61368942..c3b264bfbb0922c 100644 --- a/subsys/sensing/src/sensor_pipe.c +++ b/subsys/sensing/src/sensor_pipe.c @@ -9,6 +9,8 @@ #include #include +#include "sensing/internal/sensing.h" + LOG_MODULE_REGISTER(sensing_pipe, CONFIG_SENSING_LOG_LEVEL); #define DT_DRV_COMPAT zephyr_sensing_pipe @@ -17,6 +19,10 @@ struct sensor_pipe_config { const struct sensor_info *parent_info; }; +struct sensor_pipe_data { + struct rtio_iodev *oneshot_iodev; +}; + static int attribute_set(const struct device *dev, enum sensor_channel chan, enum sensor_attribute attr, const struct sensor_value *val) { @@ -26,11 +32,30 @@ static int attribute_set(const struct device *dev, enum sensor_channel chan, return sensor_attr_set(cfg->parent_info->dev, chan, attr, val); } -static const struct sensor_driver_api sensor_pipe_api = { +static int submit(const struct device *sensor, struct rtio_iodev_sqe *sqe) +{ + struct sensor_pipe_data *data = sensor->data; + + const struct sensing_sensor_info *info = sqe->sqe.userdata; + + LOG_DBG("Trying to read %s [%d] type=%d", info->info->dev->name, + (int)(info - STRUCT_SECTION_START(sensing_sensor_info)), info->type); + + int rc = sensor_read(data->oneshot_iodev, &sensing_rtio_ctx, sqe->sqe.userdata); + + if (rc == 0) { + rtio_iodev_sqe_ok(sqe, 0); + } else { + rtio_iodev_sqe_err(sqe, rc); + } + return rc; +} + +SENSING_DMEM static const struct sensor_driver_api sensor_pipe_api = { .attr_set = attribute_set, .attr_get = NULL, .get_decoder = NULL, - .submit = NULL, + .submit = submit, }; static int sensing_sensor_pipe_init(const struct device *dev) @@ -45,7 +70,12 @@ static int sensing_sensor_pipe_init(const struct device *dev) static const struct sensor_pipe_config cfg_##inst = { \ .parent_info = &SENSOR_INFO_DT_NAME(DT_INST_PHANDLE(inst, dev)), \ }; \ - SENSING_SENSOR_DT_INST_DEFINE(inst, sensing_sensor_pipe_init, NULL, NULL, &cfg_##inst, \ - APPLICATION, 10, &sensor_pipe_api); + SENSOR_DT_READ_IODEV(underlying_reader_##inst, DT_INST_PHANDLE(inst, dev), \ + SENSOR_CHAN_ALL); \ + static struct sensor_pipe_data data_##inst = { \ + .oneshot_iodev = &underlying_reader_##inst, \ + }; \ + SENSING_SENSOR_DT_INST_DEFINE(inst, sensing_sensor_pipe_init, NULL, &data_##inst, \ + &cfg_##inst, APPLICATION, 10, &sensor_pipe_api); DT_INST_FOREACH_STATUS_OKAY(SENSING_PIPE_INIT) diff --git a/subsys/sensing/src/shell.c b/subsys/sensing/src/shell.c index eea8b2773f912e9..5dcf6823f9e8549 100644 --- a/subsys/sensing/src/shell.c +++ b/subsys/sensing/src/shell.c @@ -75,8 +75,15 @@ struct shell_cmd_connection { }; static struct shell_cmd_connection open_connections[CONFIG_SENSING_MAX_CONNECTIONS]; +static void sensing_shell_on_data_event(sensing_sensor_handle_t handle, const void *data) +{ + const struct sensing_sensor_info *info = sensing_get_sensor_info(handle); + + printk("Got data for '%s' at %p\n", get_sensor_type_string(info->type), data); +} + static const struct sensing_callback_list callback_list = { - .on_data_event = NULL, + .on_data_event = sensing_shell_on_data_event, }; static int cmd_open_connection(const struct shell *sh, size_t argc, char **argv) @@ -137,7 +144,8 @@ static int cmd_open_connection(const struct shell *sh, size_t argc, char **argv) } open_connections[connection_idx].is_used = true; - shell_print(sh, "New connection [%d] to sensor [%ld] created", connection_idx, sensor_index); + shell_print(sh, "New connection [%d] to sensor [%ld] created", connection_idx, + sensor_index); return 0; } @@ -239,7 +247,8 @@ static int cmd_config(const struct shell *sh, size_t argc, char **argv) return -EINVAL; } - rc = sensing_set_attributes(open_connections[connection_index].handle, &config, 1); + rc = sensing_set_attributes(open_connections[connection_index].handle, + SENSING_SENSOR_MODE_ONE_SHOT, &config, 1); if (rc != 0) { shell_error(sh, "Failed to set attribute '%s' to '%s'", argv[2], argv[3]); return rc; diff --git a/subsys/sensing/src/userspace.c b/subsys/sensing/src/userspace.c index f24550bd915467b..486814808093b1f 100644 --- a/subsys/sensing/src/userspace.c +++ b/subsys/sensing/src/userspace.c @@ -2,9 +2,12 @@ // Created by peress on 29/06/23. // +#include +#include #include #include -#include + +LOG_MODULE_REGISTER(sensing_userspace, CONFIG_SENSING_LOG_LEVEL); K_APPMEM_PARTITION_DEFINE(sensing_mem_partition); @@ -14,9 +17,15 @@ static int sensing_mem_init(void) rc = k_mem_domain_add_partition(&k_mem_domain_default, &sensing_mem_partition); if (rc != 0) { + LOG_ERR("Failed to add sensing memory partition to the default domain"); + return rc; + } + rc = k_mem_domain_add_partition(&k_mem_domain_default, &rtio_partition); + if (rc != 0) { + LOG_ERR("Failed to add rtio partition to the default domain"); return rc; } - return k_mem_domain_add_partition(&k_mem_domain_default, &rtio_partition); + return 0; } SYS_INIT(sensing_mem_init, POST_KERNEL, 99);