1 /*
2 * Copyright (c) 2023 Google LLC
3 * Copyright (c) 2024 Croxel Inc.
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <zephyr/drivers/sensor.h>
9 #include <zephyr/rtio/work.h>
10 #include "icm42688.h"
11 #include "icm42688_decoder.h"
12 #include "icm42688_reg.h"
13 #include "icm42688_rtio.h"
14 #include "icm42688_spi.h"
15
16 #include <zephyr/logging/log.h>
17 LOG_MODULE_REGISTER(ICM42688_RTIO, CONFIG_SENSOR_LOG_LEVEL);
18
icm42688_rtio_sample_fetch(const struct device * dev,int16_t readings[7])19 static int icm42688_rtio_sample_fetch(const struct device *dev, int16_t readings[7])
20 {
21 uint8_t status;
22 const struct icm42688_dev_cfg *cfg = dev->config;
23 uint8_t *buffer = (uint8_t *)readings;
24
25 int res = icm42688_spi_read(&cfg->spi, REG_INT_STATUS, &status, 1);
26
27 if (res) {
28 return res;
29 }
30
31 if (!FIELD_GET(BIT_INT_STATUS_DATA_RDY, status)) {
32 return -EBUSY;
33 }
34
35 res = icm42688_read_all(dev, buffer);
36
37 if (res) {
38 return res;
39 }
40
41 for (int i = 0; i < 7; i++) {
42 readings[i] = sys_le16_to_cpu((buffer[i * 2] << 8) | buffer[i * 2 + 1]);
43 }
44
45 return 0;
46 }
47
icm42688_submit_one_shot(const struct device * dev,struct rtio_iodev_sqe * iodev_sqe)48 static void icm42688_submit_one_shot(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
49 {
50 const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
51 const struct sensor_chan_spec *const channels = cfg->channels;
52 const size_t num_channels = cfg->count;
53 uint32_t min_buf_len = sizeof(struct icm42688_encoded_data);
54 int rc;
55 uint8_t *buf;
56 uint32_t buf_len;
57 struct icm42688_encoded_data *edata;
58
59 /* Get the buffer for the frame, it may be allocated dynamically by the rtio context */
60 rc = rtio_sqe_rx_buf(iodev_sqe, min_buf_len, min_buf_len, &buf, &buf_len);
61 if (rc != 0) {
62 LOG_ERR("Failed to get a read buffer of size %u bytes", min_buf_len);
63 rtio_iodev_sqe_err(iodev_sqe, rc);
64 return;
65 }
66
67 edata = (struct icm42688_encoded_data *)buf;
68
69 rc = icm42688_encode(dev, channels, num_channels, buf);
70 if (rc != 0) {
71 LOG_ERR("Failed to encode sensor data");
72 rtio_iodev_sqe_err(iodev_sqe, rc);
73 return;
74 }
75
76 rc = icm42688_rtio_sample_fetch(dev, edata->readings);
77 /* Check that the fetch succeeded */
78 if (rc != 0) {
79 LOG_ERR("Failed to fetch samples");
80 rtio_iodev_sqe_err(iodev_sqe, rc);
81 return;
82 }
83
84 rtio_iodev_sqe_ok(iodev_sqe, 0);
85 }
86
icm42688_submit_sync(struct rtio_iodev_sqe * iodev_sqe)87 void icm42688_submit_sync(struct rtio_iodev_sqe *iodev_sqe)
88 {
89 const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
90 const struct device *dev = cfg->sensor;
91
92 if (!cfg->is_streaming) {
93 icm42688_submit_one_shot(dev, iodev_sqe);
94 } else if (IS_ENABLED(CONFIG_ICM42688_STREAM)) {
95 icm42688_submit_stream(dev, iodev_sqe);
96 } else {
97 rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP);
98 }
99 }
100
icm42688_submit(const struct device * dev,struct rtio_iodev_sqe * iodev_sqe)101 void icm42688_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
102 {
103 struct rtio_work_req *req = rtio_work_req_alloc();
104
105 if (req == NULL) {
106 LOG_ERR("RTIO work item allocation failed. Consider to increase "
107 "CONFIG_RTIO_WORKQ_POOL_ITEMS.");
108 rtio_iodev_sqe_err(iodev_sqe, -ENOMEM);
109 return;
110 }
111
112 rtio_work_req_submit(req, iodev_sqe, icm42688_submit_sync);
113 }
114
115 BUILD_ASSERT(sizeof(struct icm42688_decoder_header) == 9);
116
