convert common processing loop body to function provided by sensor.h

drivers/sensor/CMakeLists.txt

@@ -130,7 +130,7 @@ add_subdirectory_ifdef(CONFIG_TMD2620       tmd2620)
 add_subdirectory_ifdef(CONFIG_ZEPHYR_NTC_THERMISTOR	zephyr_thermistor)
 
 zephyr_library()
-zephyr_library_sources(default_decoder.c)
+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)

drivers/sensor/default_rtio_sensor.c

@@ -0,0 +1,212 @@
+//
+// Created by peress on 14/04/23.
+//
+
+#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);
+
+int __sensor_read_fallback(const struct device *dev, struct rtio *ctx,
+			   enum sensor_channel *channels, size_t num_channels)
+{
+
+	uint64_t timestamp_ns = k_uptime_get();
+	rc = sensor_sample_fetch(dev);
+	if (rc != 0) {
+		return rc;
+	}
+
+	LOG_DBG("reading %d channels", (int)num_channels);
+	for (size_t i = 0; i < num_channels; ++i) {
+		struct sensor_value value[3];
+		struct rtio_sqe *sqe = rtio_sqe_acquire(ctx);
+		struct rtio_iodev_sqe iodev_sqe = {
+			.sqe = sqe,
+			.r = ctx,
+		};
+		int num_samples = SENSOR_CHANNEL_3_AXIS(channels[i]) ? 3 : 1;
+		uint32_t min_buf_len =
+			sizeof(struct sensor_data_generic_header) + num_channels * sizeof(q31_t);
+		uint8_t *buf;
+		uint32_t buf_len;
+
+		if (sqe == NULL) {
+			LOG_DBG("Failed to get sqe");
+			rtio_sqe_drop_all(ctx);
+			return -ENOMEM;
+		}
+		sqe->op = RTIO_OP_RX;
+		sqe->flags = RTIO_SQE_MEMPOOL_BUFFER;
+		sqe->buf = NULL;
+		sqe->buf_len = 0;
+		LOG_DBG("  [%d] num_samples=%d", (int)i, num_samples);
+		LOG_DBG("  [%d] requesting buffer of length %u bytes", (int)i, min_buf_len);
+		rc = rtio_sqe_rx_buf(&iodev_sqe, min_buf_len, min_buf_len, &buf, &buf_len);
+		if (rc != 0) {
+			/* Failed to allocate read buffer */
+			LOG_DBG("  [%d] failed to allocate memory", (int)i);
+			rtio_cqe_submit(ctx, rc, (void *)dev, 0);
+			rtio_sqe_drop_all(ctx);
+			return rc;
+		}
+		LOG_DBG("  [%d] writing to %p", (int)i, buf);
+		rc = sensor_channel_get(dev, channels[i], value);
+		if (rc != 0) {
+			LOG_DBG("  [%d] failed to get channel %d", (int)i, channels[i]);
+			rtio_cqe_submit(ctx, rc, (void *)dev, 0);
+			rtio_sqe_drop_all(ctx);
+			return rc;
+		}
+
+		/* Package the data */
+		struct sensor_data_generic_header *header =
+			(struct sensor_data_generic_header *)buf;
+		header->timestamp_ns = timestamp_ns;
+		header->channel = channels[i];
+		for (int sample = 0; sample < num_samples; ++sample) {
+			uint32_t scale = (uint32_t)llabs((int64_t)value[sample].val1 + 1);
+
+			if (sample == 0 || header->scale < scale) {
+				header->scale = scale;
+			}
+		}
+		LOG_DBG("  [%d] scale=%u", (int)i, header->scale);
+		for (int sample = 0; sample < num_samples; ++sample) {
+			q31_t *q = (q31_t *)(buf + sizeof(struct sensor_data_generic_header) +
+					     sizeof(q31_t) * sample);
+
+			int64_t value_u =
+				value[sample].val1 * INT64_C(1000000) + value[sample].val2;
+			*q = (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);
+		}
+
+		uint32_t flags = rtio_cqe_compute_flags(&iodev_sqe);
+		LOG_DBG("  [%d] flags=0x%08x", (int)i, flags);
+		rtio_cqe_submit(ctx, rc, (void *)dev, flags);
+		rtio_sqe_drop_all(ctx);
+	}
+	return 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(&rtio_ctx);
+
+	if (cqe == NULL) {
+		k_msleep(1);
+		return;
+	}
+
+	rc = cqe->result;
+	read_dev = (struct device *)cqe->userdata;
+	rtio_cqe_get_mempool_buffer(&rtio_ctx, cqe, &buf, &buf_len)
+	rtio_cqe_release(&rtio_ctx);
+
+	cb(read_dev, rc, buf, buf_len);
+
+	rtio_release_buffer(&rtio_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 int get_scale(const uint8_t *buffer, uint32_t channel_type, uint32_t *scale)
+{
+	struct sensor_data_generic_header *header = (struct sensor_data_generic_header *)buffer;
+
+	switch (header->channel) {
+	case SENSOR_CHAN_ACCEL_XYZ:
+		if (channel_type != SENSOR_CHAN_ACCEL_X && channel_type != SENSOR_CHAN_ACCEL_Y &&
+		    channel_type != SENSOR_CHAN_ACCEL_Z) {
+			return -EINVAL;
+		}
+		break;
+	case SENSOR_CHAN_GYRO_XYZ:
+		if (channel_type != SENSOR_CHAN_GYRO_X && channel_type != SENSOR_CHAN_GYRO_Y &&
+		    channel_type != SENSOR_CHAN_GYRO_Z) {
+			return -EINVAL;
+		}
+		break;
+	case SENSOR_CHAN_MAGN_XYZ:
+		if (channel_type != SENSOR_CHAN_MAGN_X && channel_type != SENSOR_CHAN_MAGN_Y &&
+		    channel_type != SENSOR_CHAN_MAGN_Z) {
+			return -EINVAL;
+		}
+		break;
+	default:
+		if (header->channel != channel_type) {
+			return -EINVAL;
+		}
+	}
+	*scale = header->scale;
+	return 0;
+}
+
+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));
+
+	if (*fit != 0) {
+		LOG_ERR("Frame iterator must be 0");
+		return -EINVAL;
+	}
+
+	if (!SENSOR_CHANNEL_3_AXIS(header->channel)) {
+		/* Frame contains only 1 data field */
+		if (*cit != 0) {
+			LOG_ERR("Channel iterator must be 0");
+			return -EINVAL;
+		}
+		channels[0] = header->channel;
+		values[0] = q[0];
+		*fit = 1;
+		return 1;
+	}
+
+	/* 3 Axis data */
+	int i = 0;
+	sensor_channel_iterator_t end = MIN(3, *cit + max_count);
+	LOG_DBG("Reading from cit %" PRIu32 " to %" PRIu32, *cit, end);
+	for (; i + *cit < end; ++i) {
+		channels[i] = header->channel - 3 + i + *cit;
+		values[i] = q[i + *cit];
+	}
+	if (i + *cit == 3) {
+		/* Wrap to next frame */
+		*fit = 1;
+		*cit = 0;
+	} else {
+		*cit += i;
+	}
+	return i;
+}
+
+struct sensor_decoder_api __sensor_default_decoder = {
+	.get_frame_count = get_frame_count,
+	.get_timestamp = get_timestamp,
+	.get_scale = get_scale,
+	.decode = decode,
+};