Implement pipe

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) {

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 <zephyr/rtio/rtio.h>
 /**
  * @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);
 
 /**

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)                    \

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

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)

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"
 

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)
 {

subsys/sensing/src/processing.c

@@ -0,0 +1,78 @@
+//
+// Created by peress on 05/07/23.
+//
+
+#include <zephyr/kernel.h>
+#include <zephyr/sensing/sensor.h>
+#include <zephyr/sensing/sensing.h>
+#include <zephyr/logging/log.h>
+#include <zephyr/rtio/rtio.h>
+
+#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);

subsys/sensing/src/sensor_connections.c

@@ -1,6 +1,7 @@
 
 #include <zephyr/drivers/sensor.h>
 #include <zephyr/sensing/sensing.h>
+#include <zephyr/sensing/sensor.h>
 #include <zephyr/sys/mem_blocks.h>
 #include <zephyr/dsp/types.h>
 #include <zephyr/logging/log.h>
@@ -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)

subsys/sensing/src/sensor_pipe.c

@@ -9,6 +9,8 @@
 #include <zephyr/sensing/sensor.h>
 #include <zephyr/logging/log.h>
 
+#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)

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;