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