| /* |
| * 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); |