125 lines
3.2 KiB
C
125 lines
3.2 KiB
C
/*
|
|
* 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);
|