sensing: implement sensor data dispatching (7b6e1a32) · Commits · Wenxi XU / zephyr

subsys/sensing/CMakeLists.txt

+1 −0

Changes for subsys/sensing/CMakeLists.txt: 1 added line, 0 removed lines.
Original line number	Diff line number	Diff line
		@@ -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

+18 −0

Changes for subsys/sensing/Kconfig: 18 added lines, 0 removed lines.
Original line number	Diff line number	Diff line
		@@ -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 → 100644

+124 −0

Changes for subsys/sensing/dispatch.c: 124 added lines, 0 removed lines.
Original line number	Diff line number	Diff line
		/*
		* 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);

Admin message