sensing: implement sensor data dispatching

Create the dispatch thread to send sensor data to its client by
checking time.

Signed-off-by: Zhang Lixu <lixu.zhang@intel.com>

subsys/sensing/CMakeLists.txt

@@ -5,6 +5,7 @@ zephyr_library_include_directories(include)
 
 zephyr_library_sources(
 	sensor_mgmt.c
+	dispatch.c
 	sensing.c
 	sensing_sensor.c
 )

subsys/sensing/Kconfig

@@ -67,6 +67,24 @@ config SENSING_RUNTIME_THREAD_PRIORITY
 	  data, then ring buf will always be put into data until overflow.
 	  Typical values are 9
 
+config SENSING_DISPATCH_THREAD_STACK_SIZE
+	int "stack size for sensor dispatch thread"
+	depends on SENSING
+	default 1024
+	help
+	  This is the stack size for sensor dispatch thread
+	  Typical values are 1024
+
+config SENSING_DISPATCH_THREAD_PRIORITY
+	  int "priority for sensor dispatch thread"
+	  depends on SENSING
+	  default 8
+	  help
+	    This is the thread priority for sensing subsystem dispatch thread
+	    Ring buffer data should be fetched ASAP, so Dispatch
+	    thread priority should be higher than runtime thread
+	    Typical values are 8
+
 source "subsys/sensing/sensor/phy_3d_sensor/Kconfig"
 
 endif # SENSING

subsys/sensing/dispatch.c

@@ -0,0 +1,124 @@
+/*
+ * Copyright (c) 2022 Intel Corporation.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#include <zephyr/sys/__assert.h>
+#include <zephyr/logging/log.h>
+#include <zephyr/rtio/rtio.h>
+#include <zephyr/sensing/sensing_sensor.h>
+#include "sensor_mgmt.h"
+
+LOG_MODULE_DECLARE(sensing, CONFIG_SENSING_LOG_LEVEL);
+
+/* check whether it is right time for client to consume this sample */
+static inline bool sensor_test_consume_time(struct sensing_sensor *sensor,
+				     struct sensing_connection *conn,
+				     uint64_t cur_time)
+{
+	LOG_DBG("sensor:%s next_consume_time:%lld cur_time:%lld",
+			sensor->dev->name, conn->next_consume_time, cur_time);
+
+	return conn->next_consume_time <= cur_time;
+}
+
+static void update_client_consume_time(struct sensing_sensor *sensor,
+				       struct sensing_connection *conn)
+{
+	uint32_t interval = conn->interval;
+
+	if (conn->next_consume_time == 0) {
+		conn->next_consume_time = get_us();
+	}
+
+	conn->next_consume_time += interval;
+}
+
+/* send data to clients based on interval and sensitivity */
+static int send_data_to_clients(struct sensing_sensor *sensor,
+				void *data)
+{
+	struct sensing_sensor *client;
+	struct sensing_connection *conn;
+
+	for_each_client_conn(sensor, conn) {
+		client = conn->sink;
+		LOG_DBG("sensor:%s send data to client:%p", conn->source->dev->name, conn);
+
+		if (!is_client_request_data(conn)) {
+			continue;
+		}
+
+		/* sensor_test_consume_time(), check whether time is ready or not:
+		 * true: it's time for client consuming the data
+		 * false: client time not arrived yet, not consume the data
+		 */
+		if (!sensor_test_consume_time(sensor, conn, get_us())) {
+			continue;
+		}
+
+		update_client_consume_time(sensor, conn);
+
+		if (!conn->callback_list.on_data_event) {
+			LOG_WRN("sensor:%s event callback not registered",
+					conn->source->dev->name);
+			continue;
+		}
+		conn->callback_list.on_data_event(conn, data,
+				conn->callback_list.context);
+	}
+
+	return 0;
+}
+
+STRUCT_SECTION_START_EXTERN(sensing_sensor);
+STRUCT_SECTION_END_EXTERN(sensing_sensor);
+
+static void dispatch_task(void *a, void *b, void *c)
+{
+	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(dispatch_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;
+
+		/* Get the associated data */
+		get_data_rc =
+			rtio_cqe_get_mempool_buffer(&sensing_rtio_ctx, &cqe, &data, &data_len);
+		if (get_data_rc != 0 || data_len == 0) {
+			continue;
+		}
+
+		if ((uintptr_t)cqe.userdata >=
+			    (uintptr_t)STRUCT_SECTION_START(sensing_sensor) &&
+		    (uintptr_t)cqe.userdata < (uintptr_t)STRUCT_SECTION_END(sensing_sensor)) {
+			struct sensing_sensor *sensor = cqe.userdata;
+
+			send_data_to_clients(sensor, data);
+		}
+
+		rtio_release_buffer(&sensing_rtio_ctx, data, data_len);
+	}
+}
+
+K_THREAD_DEFINE(sensing_dispatch, CONFIG_SENSING_DISPATCH_THREAD_STACK_SIZE, dispatch_task,
+		NULL, NULL, NULL, CONFIG_SENSING_DISPATCH_THREAD_PRIORITY, 0, 0);