1 /*
2 * Copyright (c) 2024 Analog Devices Inc.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 #include <zephyr/drivers/sensor.h>
9
10 #include "adxl372.h"
11
12 LOG_MODULE_DECLARE(ADXL372, CONFIG_SENSOR_LOG_LEVEL);
13
adxl372_irq_en_cb(struct rtio * r,const struct rtio_sqe * sqr,void * arg)14 static void adxl372_irq_en_cb(struct rtio *r, const struct rtio_sqe *sqr, void *arg)
15 {
16 const struct device *dev = (const struct device *)arg;
17 const struct adxl372_dev_config *cfg = dev->config;
18
19 gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
20 }
21
adxl372_fifo_flush_rtio(const struct device * dev)22 static void adxl372_fifo_flush_rtio(const struct device *dev)
23 {
24 struct adxl372_data *data = dev->data;
25 uint8_t pow_reg = data->pwr_reg;
26 const struct adxl372_dev_config *cfg = dev->config;
27 uint8_t fifo_config;
28
29 pow_reg &= ~ADXL372_POWER_CTL_MODE_MSK;
30 pow_reg |= ADXL372_POWER_CTL_MODE(ADXL372_STANDBY);
31
32 struct rtio_sqe *sqe = rtio_sqe_acquire(data->rtio_ctx);
33 const uint8_t reg_addr_w[2] = {ADXL372_REG_WRITE(ADXL372_POWER_CTL), pow_reg};
34
35 rtio_sqe_prep_tiny_write(sqe, data->iodev, RTIO_PRIO_NORM, reg_addr_w, 2, NULL);
36
37 fifo_config = (ADXL372_FIFO_CTL_FORMAT_MODE(data->fifo_config.fifo_format) |
38 ADXL372_FIFO_CTL_MODE_MODE(ADXL372_FIFO_BYPASSED) |
39 ADXL372_FIFO_CTL_SAMPLES_MODE(data->fifo_config.fifo_samples));
40
41 sqe = rtio_sqe_acquire(data->rtio_ctx);
42 const uint8_t reg_addr_w2[2] = {ADXL372_REG_WRITE(ADXL372_FIFO_CTL), fifo_config};
43
44 rtio_sqe_prep_tiny_write(sqe, data->iodev, RTIO_PRIO_NORM, reg_addr_w2, 2, NULL);
45
46 fifo_config = (ADXL372_FIFO_CTL_FORMAT_MODE(data->fifo_config.fifo_format) |
47 ADXL372_FIFO_CTL_MODE_MODE(data->fifo_config.fifo_mode) |
48 ADXL372_FIFO_CTL_SAMPLES_MODE(data->fifo_config.fifo_samples));
49
50 sqe = rtio_sqe_acquire(data->rtio_ctx);
51 const uint8_t reg_addr_w3[2] = {ADXL372_REG_WRITE(ADXL372_FIFO_CTL), fifo_config};
52
53 rtio_sqe_prep_tiny_write(sqe, data->iodev, RTIO_PRIO_NORM, reg_addr_w3, 2, NULL);
54
55 pow_reg = data->pwr_reg;
56
57 pow_reg &= ~ADXL372_POWER_CTL_MODE_MSK;
58 pow_reg |= ADXL372_POWER_CTL_MODE(cfg->op_mode);
59
60 sqe = rtio_sqe_acquire(data->rtio_ctx);
61 struct rtio_sqe *complete_op = rtio_sqe_acquire(data->rtio_ctx);
62 const uint8_t reg_addr_w4[2] = {ADXL372_REG_WRITE(ADXL372_POWER_CTL), pow_reg};
63
64 rtio_sqe_prep_tiny_write(sqe, data->iodev, RTIO_PRIO_NORM, reg_addr_w4, 2, NULL);
65 sqe->flags |= RTIO_SQE_CHAINED;
66 rtio_sqe_prep_callback(complete_op, adxl372_irq_en_cb, (void *)dev, NULL);
67 rtio_submit(data->rtio_ctx, 0);
68 }
69
adxl372_submit_stream(const struct device * dev,struct rtio_iodev_sqe * iodev_sqe)70 void adxl372_submit_stream(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
71 {
72 const struct sensor_read_config *cfg =
73 (const struct sensor_read_config *)iodev_sqe->sqe.iodev->data;
74 struct adxl372_data *data = (struct adxl372_data *)dev->data;
75 const struct adxl372_dev_config *cfg_372 = dev->config;
76 uint8_t int_value = (uint8_t)~ADXL372_INT1_MAP_FIFO_FULL_MSK;
77 uint8_t fifo_full_irq = 0;
78
79 int rc = gpio_pin_interrupt_configure_dt(&cfg_372->interrupt, GPIO_INT_DISABLE);
80
81 if (rc < 0) {
82 return;
83 }
84
85 for (size_t i = 0; i < cfg->count; i++) {
86 if ((cfg->triggers[i].trigger == SENSOR_TRIG_FIFO_WATERMARK) ||
87 (cfg->triggers[i].trigger == SENSOR_TRIG_FIFO_FULL)) {
88 int_value = ADXL372_INT1_MAP_FIFO_FULL_MSK;
89 fifo_full_irq = 1;
90 }
91 }
92
93 if (fifo_full_irq != data->fifo_full_irq) {
94 data->fifo_full_irq = fifo_full_irq;
95
96 rc = data->hw_tf->write_reg_mask(dev, ADXL372_INT1_MAP,
97 ADXL372_INT1_MAP_FIFO_FULL_MSK, int_value);
98 if (rc < 0) {
99 return;
100 }
101
102 /* Flush the FIFO by disabling it. Save current mode for after the reset. */
103 enum adxl372_fifo_mode current_fifo_mode = data->fifo_config.fifo_mode;
104
105 adxl372_configure_fifo(dev, ADXL372_FIFO_BYPASSED, data->fifo_config.fifo_format,
106 data->fifo_config.fifo_samples);
107
108 if (current_fifo_mode == ADXL372_FIFO_BYPASSED) {
109 current_fifo_mode = ADXL372_FIFO_STREAMED;
110 }
111
112 adxl372_configure_fifo(dev, current_fifo_mode, data->fifo_config.fifo_format,
113 data->fifo_config.fifo_samples);
114
115 adxl372_set_op_mode(dev, cfg_372->op_mode);
116 }
117
118 rc = gpio_pin_interrupt_configure_dt(&cfg_372->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
119 if (rc < 0) {
120 return;
121 }
122
123 data->sqe = iodev_sqe;
124 }
125
adxl372_fifo_read_cb(struct rtio * rtio_ctx,const struct rtio_sqe * sqe,void * arg)126 static void adxl372_fifo_read_cb(struct rtio *rtio_ctx, const struct rtio_sqe *sqe, void *arg)
127 {
128 const struct device *dev = (const struct device *)arg;
129 const struct adxl372_dev_config *cfg = (const struct adxl372_dev_config *)dev->config;
130 struct rtio_iodev_sqe *iodev_sqe = sqe->userdata;
131
132 rtio_iodev_sqe_ok(iodev_sqe, 0);
133
134 gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
135 }
136
adxl372_get_packet_size(const struct adxl372_dev_config * cfg)137 size_t adxl372_get_packet_size(const struct adxl372_dev_config *cfg)
138 {
139 /* If one sample contains XYZ values. */
140 size_t packet_size;
141
142 switch (cfg->fifo_config.fifo_format) {
143 case ADXL372_X_FIFO:
144 case ADXL372_Y_FIFO:
145 case ADXL372_Z_FIFO:
146 packet_size = 2;
147 break;
148
149 case ADXL372_XY_FIFO:
150 case ADXL372_XZ_FIFO:
151 case ADXL372_YZ_FIFO:
152 packet_size = 4;
153 break;
154
155 default:
156 packet_size = 6;
157 break;
158 }
159
160 return packet_size;
161 }
162
adxl372_process_fifo_samples_cb(struct rtio * r,const struct rtio_sqe * sqr,void * arg)163 static void adxl372_process_fifo_samples_cb(struct rtio *r, const struct rtio_sqe *sqr, void *arg)
164 {
165 const struct device *dev = (const struct device *)arg;
166 struct adxl372_data *data = (struct adxl372_data *)dev->data;
167 const struct adxl372_dev_config *cfg = (const struct adxl372_dev_config *)dev->config;
168 struct rtio_iodev_sqe *current_sqe = data->sqe;
169 uint16_t fifo_samples = (((data->fifo_ent[0] & 0x3) << 8) | data->fifo_ent[1]);
170 size_t sample_set_size = adxl372_get_packet_size(cfg);
171
172 /* At least one sample set must remain in FIFO to encure that data
173 * is not overwritten and stored out of order.
174 */
175 if (fifo_samples > sample_set_size / 2) {
176 fifo_samples -= sample_set_size / 2;
177 } else {
178 LOG_ERR("fifo sample count error %d\n", fifo_samples);
179 gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
180 return;
181 }
182
183 uint16_t fifo_bytes = fifo_samples * 2 /*sample size*/;
184
185 data->sqe = NULL;
186
187 /* Not inherently an underrun/overrun as we may have a buffer to fill next time */
188 if (current_sqe == NULL) {
189 LOG_ERR("No pending SQE");
190 gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
191 return;
192 }
193
194 const size_t min_read_size = sizeof(struct adxl372_fifo_data) + sample_set_size;
195 const size_t ideal_read_size = sizeof(struct adxl372_fifo_data) + fifo_bytes;
196
197 uint8_t *buf;
198 uint32_t buf_len;
199
200 if (rtio_sqe_rx_buf(current_sqe, min_read_size, ideal_read_size, &buf, &buf_len) != 0) {
201 LOG_ERR("Failed to get buffer");
202 rtio_iodev_sqe_err(current_sqe, -ENOMEM);
203 gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
204 return;
205 }
206
207 LOG_DBG("Requesting buffer [%u, %u] got %u", (unsigned int)min_read_size,
208 (unsigned int)ideal_read_size, buf_len);
209
210 /* Read FIFO and call back to rtio with rtio_sqe completion */
211 struct adxl372_fifo_data *hdr = (struct adxl372_fifo_data *)buf;
212
213 hdr->is_fifo = 1;
214 hdr->timestamp = data->timestamp;
215 hdr->int_status = data->status1;
216 hdr->accel_odr = cfg->odr;
217 hdr->sample_set_size = sample_set_size;
218
219 if ((cfg->fifo_config.fifo_format == ADXL372_X_FIFO) ||
220 (cfg->fifo_config.fifo_format == ADXL372_XY_FIFO) ||
221 (cfg->fifo_config.fifo_format == ADXL372_XZ_FIFO) ||
222 (cfg->fifo_config.fifo_format == ADXL372_XYZ_FIFO)) {
223 hdr->has_x = 1;
224 }
225
226 if ((cfg->fifo_config.fifo_format == ADXL372_Y_FIFO) ||
227 (cfg->fifo_config.fifo_format == ADXL372_XY_FIFO) ||
228 (cfg->fifo_config.fifo_format == ADXL372_YZ_FIFO) ||
229 (cfg->fifo_config.fifo_format == ADXL372_XYZ_FIFO)) {
230 hdr->has_y = 1;
231 }
232
233 if ((cfg->fifo_config.fifo_format == ADXL372_Z_FIFO) ||
234 (cfg->fifo_config.fifo_format == ADXL372_XZ_FIFO) ||
235 (cfg->fifo_config.fifo_format == ADXL372_YZ_FIFO) ||
236 (cfg->fifo_config.fifo_format == ADXL372_XYZ_FIFO)) {
237 hdr->has_z = 1;
238 }
239
240 uint32_t buf_avail = buf_len;
241
242 buf_avail -= sizeof(*hdr);
243
244 uint32_t read_len = MIN(fifo_bytes, buf_avail);
245 uint32_t pkts = read_len / sample_set_size;
246
247 read_len = pkts * sample_set_size;
248
249 ((struct adxl372_fifo_data *)buf)->fifo_byte_count = read_len;
250
251 __ASSERT_NO_MSG(read_len % pkt_size == 0);
252
253 uint8_t *read_buf = buf + sizeof(*hdr);
254
255 /* Flush completions */
256 struct rtio_cqe *cqe;
257 int res = 0;
258
259 do {
260 cqe = rtio_cqe_consume(data->rtio_ctx);
261 if (cqe != NULL) {
262 if ((cqe->result < 0 && res == 0)) {
263 LOG_ERR("Bus error: %d", cqe->result);
264 res = cqe->result;
265 }
266 rtio_cqe_release(data->rtio_ctx, cqe);
267 }
268 } while (cqe != NULL);
269
270 /* Bail/cancel attempt to read sensor on any error */
271 if (res != 0) {
272 rtio_iodev_sqe_err(current_sqe, res);
273 return;
274 }
275
276 /* Setup new rtio chain to read the fifo data and report then check the
277 * result
278 */
279 struct rtio_sqe *write_fifo_addr = rtio_sqe_acquire(data->rtio_ctx);
280 struct rtio_sqe *read_fifo_data = rtio_sqe_acquire(data->rtio_ctx);
281 struct rtio_sqe *complete_op = rtio_sqe_acquire(data->rtio_ctx);
282 const uint8_t reg_addr = ADXL372_REG_READ(ADXL372_FIFO_DATA);
283
284 rtio_sqe_prep_tiny_write(write_fifo_addr, data->iodev, RTIO_PRIO_NORM, ®_addr, 1, NULL);
285 write_fifo_addr->flags = RTIO_SQE_TRANSACTION;
286 rtio_sqe_prep_read(read_fifo_data, data->iodev, RTIO_PRIO_NORM, read_buf, read_len,
287 current_sqe);
288 read_fifo_data->flags = RTIO_SQE_CHAINED;
289 rtio_sqe_prep_callback(complete_op, adxl372_fifo_read_cb, (void *)dev, current_sqe);
290
291 rtio_submit(data->rtio_ctx, 0);
292 }
293
adxl372_process_status1_cb(struct rtio * r,const struct rtio_sqe * sqr,void * arg)294 static void adxl372_process_status1_cb(struct rtio *r, const struct rtio_sqe *sqr, void *arg)
295 {
296 const struct device *dev = (const struct device *)arg;
297 struct adxl372_data *data = (struct adxl372_data *)dev->data;
298 const struct adxl372_dev_config *cfg = (const struct adxl372_dev_config *)dev->config;
299 struct rtio_iodev_sqe *current_sqe = data->sqe;
300 struct sensor_read_config *read_config;
301 uint8_t status1 = data->status1;
302
303 if (data->sqe == NULL) {
304 return;
305 }
306
307 read_config = (struct sensor_read_config *)data->sqe->sqe.iodev->data;
308
309 if (read_config == NULL) {
310 return;
311 }
312
313 if (read_config->is_streaming == false) {
314 return;
315 }
316
317 gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_DISABLE);
318
319 struct sensor_stream_trigger *fifo_wmark_cfg = NULL;
320 struct sensor_stream_trigger *fifo_full_cfg = NULL;
321
322 for (int i = 0; i < read_config->count; ++i) {
323 if (read_config->triggers[i].trigger == SENSOR_TRIG_FIFO_WATERMARK) {
324 fifo_wmark_cfg = &read_config->triggers[i];
325 continue;
326 }
327
328 if (read_config->triggers[i].trigger == SENSOR_TRIG_FIFO_FULL) {
329 fifo_full_cfg = &read_config->triggers[i];
330 continue;
331 }
332 }
333
334 bool fifo_full_irq = false;
335
336 if ((fifo_wmark_cfg != NULL) && (fifo_full_cfg != NULL) &&
337 FIELD_GET(ADXL372_INT1_MAP_FIFO_FULL_MSK, status1)) {
338 fifo_full_irq = true;
339 }
340
341 if (!fifo_full_irq) {
342 gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
343 return;
344 }
345
346 struct rtio_cqe *cqe;
347 int res = 0;
348
349 do {
350 cqe = rtio_cqe_consume(data->rtio_ctx);
351 if (cqe != NULL) {
352 if ((cqe->result < 0) && (res == 0)) {
353 LOG_ERR("Bus error: %d", cqe->result);
354 res = cqe->result;
355 }
356 rtio_cqe_release(data->rtio_ctx, cqe);
357 }
358 } while (cqe != NULL);
359
360 /* Bail/cancel attempt to read sensor on any error */
361 if (res != 0) {
362 rtio_iodev_sqe_err(current_sqe, res);
363 return;
364 }
365
366 enum sensor_stream_data_opt data_opt;
367
368 if ((fifo_wmark_cfg != NULL) && (fifo_full_cfg == NULL)) {
369 data_opt = fifo_wmark_cfg->opt;
370 } else if ((fifo_wmark_cfg == NULL) && (fifo_full_cfg != NULL)) {
371 data_opt = fifo_full_cfg->opt;
372 } else {
373 data_opt = MIN(fifo_wmark_cfg->opt, fifo_full_cfg->opt);
374 }
375
376 if (data_opt == SENSOR_STREAM_DATA_NOP || data_opt == SENSOR_STREAM_DATA_DROP) {
377 uint8_t *buf;
378 uint32_t buf_len;
379
380 /* Clear streaming_sqe since we're done with the call */
381 data->sqe = NULL;
382 if (rtio_sqe_rx_buf(current_sqe, sizeof(struct adxl372_fifo_data),
383 sizeof(struct adxl372_fifo_data), &buf, &buf_len) != 0) {
384 rtio_iodev_sqe_err(current_sqe, -ENOMEM);
385 gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
386 return;
387 }
388
389 struct adxl372_fifo_data *rx_data = (struct adxl372_fifo_data *)buf;
390
391 memset(buf, 0, buf_len);
392 rx_data->is_fifo = 1;
393 rx_data->timestamp = data->timestamp;
394 rx_data->int_status = status1;
395 rx_data->fifo_byte_count = 0;
396 rtio_iodev_sqe_ok(current_sqe, 0);
397
398 if (data_opt == SENSOR_STREAM_DATA_DROP) {
399 /* Flush the FIFO by disabling it. Save current mode for after the reset. */
400 adxl372_fifo_flush_rtio(dev);
401 return;
402 }
403
404 gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
405 return;
406 }
407
408 struct rtio_sqe *write_fifo_addr = rtio_sqe_acquire(data->rtio_ctx);
409 struct rtio_sqe *read_fifo_data = rtio_sqe_acquire(data->rtio_ctx);
410 struct rtio_sqe *complete_op = rtio_sqe_acquire(data->rtio_ctx);
411 const uint8_t reg_addr = ADXL372_REG_READ(ADXL372_FIFO_ENTRIES_2);
412
413 rtio_sqe_prep_tiny_write(write_fifo_addr, data->iodev, RTIO_PRIO_NORM, ®_addr, 1, NULL);
414 write_fifo_addr->flags = RTIO_SQE_TRANSACTION;
415 rtio_sqe_prep_read(read_fifo_data, data->iodev, RTIO_PRIO_NORM, data->fifo_ent, 2,
416 current_sqe);
417 read_fifo_data->flags = RTIO_SQE_CHAINED;
418 rtio_sqe_prep_callback(complete_op, adxl372_process_fifo_samples_cb, (void *)dev,
419 current_sqe);
420
421 rtio_submit(data->rtio_ctx, 0);
422 }
423
adxl372_stream_irq_handler(const struct device * dev)424 void adxl372_stream_irq_handler(const struct device *dev)
425 {
426 struct adxl372_data *data = (struct adxl372_data *)dev->data;
427
428 if (data->sqe == NULL) {
429 return;
430 }
431
432 data->timestamp = k_ticks_to_ns_floor64(k_uptime_ticks());
433
434 struct rtio_sqe *write_status_addr = rtio_sqe_acquire(data->rtio_ctx);
435 struct rtio_sqe *read_status_reg = rtio_sqe_acquire(data->rtio_ctx);
436 struct rtio_sqe *check_status_reg = rtio_sqe_acquire(data->rtio_ctx);
437 uint8_t reg = ADXL372_REG_READ(ADXL372_STATUS_1);
438
439 rtio_sqe_prep_tiny_write(write_status_addr, data->iodev, RTIO_PRIO_NORM, ®, 1, NULL);
440 write_status_addr->flags = RTIO_SQE_TRANSACTION;
441 rtio_sqe_prep_read(read_status_reg, data->iodev, RTIO_PRIO_NORM, &data->status1, 1, NULL);
442 read_status_reg->flags = RTIO_SQE_CHAINED;
443 rtio_sqe_prep_callback(check_status_reg, adxl372_process_status1_cb, (void *)dev, NULL);
444 rtio_submit(data->rtio_ctx, 0);
445 }
446
