first attempt at sensors + rtio

drivers/sensor/CMakeLists.txt

@@ -129,13 +129,8 @@ add_subdirectory_ifdef(CONFIG_XMC4XXX_TEMP	xmc4xxx_temp)
 add_subdirectory_ifdef(CONFIG_TMD2620       tmd2620)
 add_subdirectory_ifdef(CONFIG_ZEPHYR_NTC_THERMISTOR	zephyr_thermistor)
 
-if(CONFIG_USERSPACE OR CONFIG_SENSOR_SHELL OR CONFIG_SENSOR_SHELL_BATTERY)
-# The above if() is needed or else CMake would complain about
-# empty library.
-
 zephyr_library()
+zephyr_library_sources(default_rtio_sensor.c)
 zephyr_library_sources_ifdef(CONFIG_USERSPACE sensor_handlers.c)
 zephyr_library_sources_ifdef(CONFIG_SENSOR_SHELL sensor_shell.c)
 zephyr_library_sources_ifdef(CONFIG_SENSOR_SHELL_BATTERY shell_battery.c)
-
-endif()

drivers/sensor/default_rtio_sensor.c

@@ -0,0 +1,221 @@
+/*
+ * Copyright (c) 2023 Google LLC.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <errno.h>
+
+#include <zephyr/drivers/sensor.h>
+#include <zephyr/dsp/dsp.h>
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(rtio_sensor, CONFIG_SENSOR_LOG_LEVEL);
+
+static void sensor_submit_fallback(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe);
+
+static void sensor_iodev_submit(struct rtio_iodev_sqe *iodev_sqe)
+{
+	const struct sensor_iodev_data *data = iodev_sqe->sqe->iodev->data;
+	const struct device *dev = data->sensor;
+	const struct sensor_driver_api *api = dev->api;
+
+	if (api->submit != NULL) {
+		api->submit(dev, iodev_sqe);
+	} else {
+		sensor_submit_fallback(dev, iodev_sqe);
+	}
+}
+
+struct rtio_iodev_api sensor_iodev_api = {
+	.submit = sensor_iodev_submit,
+};
+
+static inline int compute_num_samples(const enum sensor_channel *channels, size_t num_channels)
+{
+	int num_samples = 0;
+
+	for (size_t i = 0; i < num_channels; ++i) {
+		num_samples += SENSOR_CHANNEL_3_AXIS(channels[i]) ? 3 : 1;
+	}
+
+	return num_samples;
+}
+
+static void sensor_submit_fallback(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
+{
+	const struct sensor_iodev_data *data = iodev_sqe->sqe->iodev->data;
+	const enum sensor_channel *const channels = data->channels;
+	const size_t num_channels = data->num_channels;
+	int num_output_samples = compute_num_samples(channels, num_channels);
+	uint32_t min_buf_len =
+		sizeof(struct sensor_data_generic_header) +
+		num_output_samples * (sizeof(q31_t) + sizeof(struct sensor_data_generic_channel));
+	uint64_t timestamp_ns = k_uptime_get();
+	int rc = sensor_sample_fetch(dev);
+	uint8_t *buf;
+	uint32_t buf_len;
+
+	if (rc != 0) {
+		rtio_iodev_sqe_err(iodev_sqe, rc);
+		return;
+	}
+	if (num_output_samples != 6) {
+		LOG_ERR("Not 6");
+		rtio_iodev_sqe_err(iodev_sqe, -1);
+		return;
+	}
+	LOG_DBG("Trying to allocate %u bytes for %d samples", min_buf_len, num_output_samples);
+	rc = rtio_sqe_rx_buf(iodev_sqe, min_buf_len, min_buf_len, &buf, &buf_len);
+	if (rc != 0) {
+		LOG_INF("Failed to allocate memory");
+		rtio_iodev_sqe_err(iodev_sqe, rc);
+		return;
+	}
+	LOG_INF("Buffer allocated at %p", (void*)buf);
+
+	/* Set the timestamp and reset num_channels */
+	struct sensor_data_generic_header *header = (struct sensor_data_generic_header *)buf;
+	header->timestamp_ns = timestamp_ns;
+	header->num_channels = num_output_samples;
+	LOG_DBG("Reading %d channels @%" PRIu64, (int)num_channels, timestamp_ns);
+	uint8_t current_channel_index = 0;
+	q31_t *q = (q31_t *)(buf + sizeof(struct sensor_data_generic_header) +
+			     num_output_samples * sizeof(struct sensor_data_generic_channel));
+	for (size_t i = 0; i < num_channels; ++i) {
+		struct sensor_value value[3];
+		int num_samples = SENSOR_CHANNEL_3_AXIS(channels[i]) ? 3 : 1;
+
+		header->num_channels += num_samples;
+		rc = sensor_channel_get(dev, channels[i], value);
+		if (rc != 0) {
+			LOG_ERR("Failed to get channel %d", channels[i]);
+			rtio_iodev_sqe_err(iodev_sqe, rc);
+		}
+
+		/* Package the data */
+		uint32_t header_scale = 0;
+		for (int sample = 0; sample < num_samples; ++sample) {
+			uint32_t scale = (uint32_t)llabs((int64_t)value[sample].val1 + 1);
+
+			header_scale = MAX(header_scale, scale);
+		}
+		LOG_DBG("  [%d] scale=%u", (int)i, header_scale);
+		for (int sample = 0; sample < num_samples; ++sample) {
+			header->channel_info[current_channel_index + sample].scale = header_scale;
+
+			int64_t value_u =
+				value[sample].val1 * INT64_C(1000000) + value[sample].val2;
+			q[current_channel_index + sample] =
+				(value_u / header_scale) * ((INT64_C(1) << 31) - 1) / 1000000;
+			LOG_DBG("value[%d]=%s%d.%06d, q=%d", sample, value_u < 0 ? "-" : "",
+				abs((int)(value_u / 1000000)), abs((int)(value_u % 1000000)), *q);
+		}
+		current_channel_index += num_samples;
+	}
+	rtio_iodev_sqe_ok(iodev_sqe, 0);
+}
+
+void z_sensor_processing_loop(struct rtio *ctx, sensor_processing_callback_t cb)
+{
+	struct device *read_dev;
+	uint8_t *buf = NULL;
+	uint32_t buf_len = 0;
+	int rc;
+
+	struct rtio_cqe *cqe = rtio_cqe_consume(ctx);
+
+	if (cqe == NULL) {
+		k_msleep(1);
+		return;
+	}
+
+	rc = cqe->result;
+	read_dev = (struct device *)cqe->userdata;
+	rtio_cqe_get_mempool_buffer(ctx, cqe, &buf, &buf_len);
+	rtio_cqe_release(ctx);
+
+	cb(read_dev, rc, buf, buf_len);
+
+	rtio_release_buffer(ctx, buf);
+}
+
+static int get_frame_count(const uint8_t *buffer, uint16_t *frame_count)
+{
+	*frame_count = 1;
+	return 0;
+}
+
+static int get_timestamp(const uint8_t *buffer, uint64_t *timestamp_ns)
+{
+	*timestamp_ns = ((struct sensor_data_generic_header *)buffer)->timestamp_ns;
+	return 0;
+}
+
+static inline bool is_subchannel(enum sensor_channel needle, enum sensor_channel haystack)
+{
+	if (needle == haystack) {
+		return true;
+	}
+	if (!SENSOR_CHANNEL_3_AXIS(haystack)) {
+		return false;
+	}
+	return (haystack == SENSOR_CHAN_ACCEL_XYZ &&
+		(needle == SENSOR_CHAN_ACCEL_X || needle == SENSOR_CHAN_ACCEL_Y ||
+		 needle == SENSOR_CHAN_ACCEL_Z)) ||
+	       (haystack == SENSOR_CHAN_GYRO_XYZ &&
+		(needle == SENSOR_CHAN_GYRO_X || needle == SENSOR_CHAN_GYRO_Y ||
+		 needle == SENSOR_CHAN_GYRO_Z)) ||
+	       (haystack == SENSOR_CHAN_MAGN_XYZ &&
+		(needle == SENSOR_CHAN_MAGN_X || needle == SENSOR_CHAN_MAGN_Y ||
+		 needle == SENSOR_CHAN_MAGN_Z));
+}
+
+static int get_scale(const uint8_t *buffer, enum sensor_channel channel_type, uint32_t *scale)
+{
+	struct sensor_data_generic_header *header = (struct sensor_data_generic_header *)buffer;
+
+	for (uint8_t i = 0; i < header->num_channels; ++i) {
+		if (is_subchannel(header->channel_info[i].channel, channel_type)) {
+			*scale = header->channel_info[i].scale;
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+static int decode(const uint8_t *buffer, sensor_frame_iterator_t *fit,
+		  sensor_channel_iterator_t *cit, enum sensor_channel *channels, q31_t *values,
+		  uint8_t max_count)
+{
+	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(struct sensor_data_generic_channel));
+
+	if (*fit != 0) {
+		return -EINVAL;
+	}
+
+	int count = 0;
+	for (; *cit < header->num_channels && count < max_count; ++count) {
+		channels[count] = header->channel_info[count].channel;
+		values[count] = q[count];
+	}
+
+	*cit += count;
+	printk("%p cit=%u/%u\n", buffer, *cit, header->num_channels);
+	if (*cit == header->num_channels) {
+		*fit = 1;
+		*cit = 0;
+	}
+	return count;
+}
+
+struct sensor_decoder_api __sensor_default_decoder = {
+	.get_frame_count = get_frame_count,
+	.get_timestamp = get_timestamp,
+	.get_scale = get_scale,
+	.decode = decode,
+};