1 /*
2 * Copyright (c) 2025 Croxel Inc.
3 * Copyright (c) 2025 CogniPilot Foundation
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <zephyr/drivers/gpio.h>
9 #include <zephyr/drivers/sensor.h>
10 #include <zephyr/drivers/sensor_clock.h>
11 #include <zephyr/sys/check.h>
12 #include <zephyr/rtio/rtio.h>
13 #include <zephyr/rtio/work.h>
14
15 #include "paa3905.h"
16 #include "paa3905_reg.h"
17 #include "paa3905_stream.h"
18 #include "paa3905_decoder.h"
19 #include "paa3905_bus.h"
20
21 #include <zephyr/logging/log.h>
22 LOG_MODULE_REGISTER(PAA3905_STREAM, CONFIG_SENSOR_LOG_LEVEL);
23
paa3905_chip_recovery_handler(struct rtio_iodev_sqe * iodev_sqe)24 static void paa3905_chip_recovery_handler(struct rtio_iodev_sqe *iodev_sqe)
25 {
26 const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
27 const struct device *dev = cfg->sensor;
28 int err;
29
30 err = paa3905_recover(dev);
31
32 if (err) {
33 rtio_iodev_sqe_err(iodev_sqe, err);
34 } else {
35 rtio_iodev_sqe_ok(iodev_sqe, 0);
36 }
37 }
38
start_drdy_backup_timer(const struct device * dev)39 static void start_drdy_backup_timer(const struct device *dev)
40 {
41 struct paa3905_data *data = dev->data;
42 const struct paa3905_config *cfg = dev->config;
43
44 k_timer_start(&data->stream.timer,
45 K_MSEC(cfg->backup_timer_period),
46 K_NO_WAIT);
47 }
48
paa3905_complete_result(struct rtio * ctx,const struct rtio_sqe * sqe,void * arg)49 static void paa3905_complete_result(struct rtio *ctx,
50 const struct rtio_sqe *sqe,
51 void *arg)
52 {
53 const struct device *dev = (const struct device *)arg;
54 struct paa3905_data *data = dev->data;
55 struct rtio_iodev_sqe *iodev_sqe = data->stream.iodev_sqe;
56 struct paa3905_encoded_data *edata = sqe->userdata;
57 struct rtio_cqe *cqe;
58 int err;
59
60 edata->header.events.drdy = true &&
61 data->stream.settings.enabled.drdy;
62 edata->header.events.motion = REG_MOTION_DETECTED(edata->motion) &&
63 data->stream.settings.enabled.motion;
64 edata->header.channels = 0;
65
66 if ((data->stream.settings.enabled.drdy &&
67 data->stream.settings.opt.drdy == SENSOR_STREAM_DATA_INCLUDE) ||
68 (data->stream.settings.enabled.motion &&
69 data->stream.settings.opt.motion == SENSOR_STREAM_DATA_INCLUDE)) {
70 edata->header.channels |= paa3905_encode_channel(SENSOR_CHAN_POS_DXYZ);
71 }
72
73 if (data->stream.settings.enabled.drdy) {
74 start_drdy_backup_timer(dev);
75 }
76
77 /** Attempt chip recovery if erratic behavior is detected */
78 if (!REG_OBSERVATION_CHIP_OK(edata->observation)) {
79
80 LOG_WRN("CHIP OK register indicates issues. Attempting chip recovery");
81 struct rtio_work_req *req = rtio_work_req_alloc();
82
83 CHECKIF(!req) {
84 LOG_ERR("Failed to allocate RTIO work request");
85 rtio_iodev_sqe_err(iodev_sqe, -ENOMEM);
86 return;
87 }
88
89 rtio_work_req_submit(req, iodev_sqe, paa3905_chip_recovery_handler);
90 } else {
91 rtio_iodev_sqe_ok(iodev_sqe, 0);
92 }
93
94 /* Flush RTIO bus CQEs */
95 do {
96 cqe = rtio_cqe_consume(ctx);
97 if (cqe != NULL) {
98 err = cqe->result;
99 rtio_cqe_release(ctx, cqe);
100 }
101 } while (cqe != NULL);
102 }
103
paa3905_stream_get_data(const struct device * dev)104 static void paa3905_stream_get_data(const struct device *dev)
105 {
106 struct paa3905_data *data = dev->data;
107 uint64_t cycles;
108 int err;
109
110 CHECKIF(!data->stream.iodev_sqe) {
111 LOG_WRN("No RTIO submission with an INT GPIO event");
112 return;
113 }
114
115 struct paa3905_encoded_data *buf;
116 uint32_t buf_len;
117 uint32_t buf_len_required = sizeof(struct paa3905_encoded_data);
118
119 err = rtio_sqe_rx_buf(data->stream.iodev_sqe,
120 buf_len_required,
121 buf_len_required,
122 (uint8_t **)&buf,
123 &buf_len);
124 __ASSERT(err == 0, "Failed to acquire buffer (len: %d) for encoded data: %d. "
125 "Please revisit RTIO queue sizing and look for "
126 "bottlenecks during sensor data processing",
127 buf_len_required, err);
128
129 /** Still throw an error even if asserts are off */
130 if (err) {
131 struct rtio_iodev_sqe *iodev_sqe = data->stream.iodev_sqe;
132
133 LOG_ERR("Failed to acquire buffer for encoded data: %d", err);
134
135 data->stream.iodev_sqe = NULL;
136 rtio_iodev_sqe_err(iodev_sqe, err);
137 return;
138 }
139
140 struct rtio_sqe *write_sqe = rtio_sqe_acquire(data->rtio.ctx);
141 struct rtio_sqe *read_sqe = rtio_sqe_acquire(data->rtio.ctx);
142 struct rtio_sqe *cb_sqe = rtio_sqe_acquire(data->rtio.ctx);
143 uint8_t val;
144
145 err = sensor_clock_get_cycles(&cycles);
146 CHECKIF(err) {
147 struct rtio_iodev_sqe *iodev_sqe = data->stream.iodev_sqe;
148
149 LOG_ERR("Failed to get timestamp: %d", err);
150
151 data->stream.iodev_sqe = NULL;
152 rtio_iodev_sqe_err(iodev_sqe, err);
153 return;
154 }
155 buf->header.timestamp = sensor_clock_cycles_to_ns(cycles);
156
157 CHECKIF(!write_sqe || !read_sqe || !cb_sqe) {
158 LOG_ERR("Failed to acquire RTIO SQE's");
159 rtio_iodev_sqe_err(data->stream.iodev_sqe, -ENOMEM);
160 return;
161 }
162
163 val = REG_BURST_READ | REG_SPI_READ_BIT;
164
165 rtio_sqe_prep_tiny_write(write_sqe,
166 data->rtio.iodev,
167 RTIO_PRIO_HIGH,
168 &val,
169 1,
170 NULL);
171 write_sqe->flags |= RTIO_SQE_TRANSACTION;
172
173 rtio_sqe_prep_read(read_sqe,
174 data->rtio.iodev,
175 RTIO_PRIO_HIGH,
176 buf->buf,
177 sizeof(buf->buf),
178 NULL);
179 read_sqe->flags |= RTIO_SQE_CHAINED;
180
181 rtio_sqe_prep_callback_no_cqe(cb_sqe,
182 paa3905_complete_result,
183 (void *)dev,
184 buf);
185
186 rtio_submit(data->rtio.ctx, 0);
187 }
188
paa3905_gpio_callback(const struct device * gpio_dev,struct gpio_callback * cb,uint32_t pins)189 static void paa3905_gpio_callback(const struct device *gpio_dev,
190 struct gpio_callback *cb,
191 uint32_t pins)
192 {
193 struct paa3905_stream *stream = CONTAINER_OF(cb,
194 struct paa3905_stream,
195 cb);
196 const struct device *dev = stream->dev;
197 const struct paa3905_config *cfg = dev->config;
198 int err;
199
200 /* Disable interrupts */
201 err = gpio_pin_interrupt_configure_dt(&cfg->int_gpio,
202 GPIO_INT_MODE_DISABLED);
203 if (err) {
204 LOG_ERR("Failed to disable interrupt");
205 return;
206 }
207
208 paa3905_stream_get_data(dev);
209 }
210
paa3905_stream_drdy_timeout(struct k_timer * timer)211 static void paa3905_stream_drdy_timeout(struct k_timer *timer)
212 {
213 struct paa3905_stream *stream = CONTAINER_OF(timer,
214 struct paa3905_stream,
215 timer);
216 const struct device *dev = stream->dev;
217 const struct paa3905_config *cfg = dev->config;
218 int err;
219
220 /* Disable interrupts */
221 err = gpio_pin_interrupt_configure_dt(&cfg->int_gpio,
222 GPIO_INT_MODE_DISABLED);
223 if (err) {
224 LOG_ERR("Failed to disable interrupt");
225 return;
226 }
227
228 paa3905_stream_get_data(dev);
229 }
230
settings_changed(const struct paa3905_stream * a,const struct paa3905_stream * b)231 static inline bool settings_changed(const struct paa3905_stream *a,
232 const struct paa3905_stream *b)
233 {
234 return (a->settings.enabled.drdy != b->settings.enabled.drdy) ||
235 (a->settings.opt.drdy != b->settings.opt.drdy) ||
236 (a->settings.enabled.motion != b->settings.enabled.motion) ||
237 (a->settings.opt.motion != b->settings.opt.motion);
238 }
239
paa3905_stream_submit(const struct device * dev,struct rtio_iodev_sqe * iodev_sqe)240 void paa3905_stream_submit(const struct device *dev,
241 struct rtio_iodev_sqe *iodev_sqe)
242 {
243 const struct sensor_read_config *read_config = iodev_sqe->sqe.iodev->data;
244 struct paa3905_data *data = dev->data;
245 const struct paa3905_config *cfg = dev->config;
246 int err;
247
248 /** This separate struct is required due to the streaming API using a
249 * multi-shot RTIO submission: meaning, re-submitting itself after
250 * completion; hence, we don't have context to determine if this was
251 * the first submission that kicked things off. We're then, inferring
252 * this by comparing if the read-config has changed, and only restart
253 * in such case.
254 */
255 struct paa3905_stream stream = {0};
256
257 for (size_t i = 0 ; i < read_config->count ; i++) {
258 switch (read_config->channels[i].chan_type) {
259 case SENSOR_TRIG_DATA_READY:
260 stream.settings.enabled.drdy = true;
261 stream.settings.opt.drdy = read_config->triggers[i].opt;
262 break;
263 case SENSOR_TRIG_MOTION:
264 stream.settings.enabled.motion = true;
265 stream.settings.opt.motion = read_config->triggers[i].opt;
266 break;
267 default:
268 LOG_ERR("Unsupported trigger (%d)", read_config->triggers[i].trigger);
269 rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP);
270 return;
271 }
272 }
273
274 /* Store context for next submission (handled within callbacks) */
275 data->stream.iodev_sqe = iodev_sqe;
276
277 if (settings_changed(&data->stream, &stream)) {
278 uint8_t motion_data[6];
279
280 data->stream.settings = stream.settings;
281
282 /* Disable interrupts */
283 err = gpio_pin_interrupt_configure_dt(&cfg->int_gpio,
284 GPIO_INT_MODE_DISABLED);
285 if (err) {
286 LOG_ERR("Failed to disable interrupt");
287 rtio_iodev_sqe_err(iodev_sqe, err);
288 return;
289 }
290
291 /* Read reg's 0x02-0x06 to clear motion data. */
292 err = paa3905_bus_read(dev, REG_MOTION, motion_data, sizeof(motion_data));
293 if (err) {
294 LOG_ERR("Failed to read motion data");
295 rtio_iodev_sqe_err(iodev_sqe, err);
296 return;
297 }
298 }
299
300 /* Re-enable interrupts */
301 err = gpio_pin_interrupt_configure_dt(&cfg->int_gpio,
302 GPIO_INT_LEVEL_ACTIVE);
303 if (err) {
304 LOG_ERR("Failed to enable interrupt");
305 rtio_iodev_sqe_err(iodev_sqe, err);
306 return;
307 }
308
309 /** Back-up timer allows us to keep checking in with the sensor in
310 * spite of not having any motion. This results in allowing sensor
311 * recovery if falling in erratic state.
312 */
313 if (data->stream.settings.enabled.drdy) {
314 start_drdy_backup_timer(dev);
315 }
316 }
317
paa3905_stream_init(const struct device * dev)318 int paa3905_stream_init(const struct device *dev)
319 {
320 const struct paa3905_config *cfg = dev->config;
321 struct paa3905_data *data = dev->data;
322 int err;
323
324 /** Needed to get back the device handle from the callback context */
325 data->stream.dev = dev;
326
327 if (!cfg->int_gpio.port) {
328 LOG_ERR("Interrupt GPIO not supplied");
329 return -ENODEV;
330 }
331
332 if (!gpio_is_ready_dt(&cfg->int_gpio)) {
333 LOG_ERR("Interrupt GPIO not ready");
334 return -ENODEV;
335 }
336
337 err = gpio_pin_configure_dt(&cfg->int_gpio, GPIO_INPUT);
338 if (err) {
339 LOG_ERR("Failed to configure interrupt GPIO");
340 return -EIO;
341 }
342
343 gpio_init_callback(&data->stream.cb,
344 paa3905_gpio_callback,
345 BIT(cfg->int_gpio.pin));
346
347 err = gpio_add_callback(cfg->int_gpio.port, &data->stream.cb);
348 if (err) {
349 LOG_ERR("Failed to add interrupt callback");
350 return -EIO;
351 }
352
353 k_timer_init(&data->stream.timer, paa3905_stream_drdy_timeout, NULL);
354
355 return err;
356 }
357
