1 /* ST Microelectronics LSM6DSV16X 6-axis IMU sensor driver
2 *
3 * Copyright (c) 2023 Google LLC
4 * Copyright (c) 2024 STMicroelectronics
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #define DT_DRV_COMPAT st_lsm6dsv16x
10
11 #include <zephyr/dt-bindings/sensor/lsm6dsv16x.h>
12 #include <zephyr/drivers/sensor.h>
13 #include "lsm6dsv16x.h"
14 #include "lsm6dsv16x_decoder.h"
15 #include <zephyr/rtio/work.h>
16
17 #include <zephyr/logging/log.h>
18 LOG_MODULE_DECLARE(LSM6DSV16X_RTIO);
19
20 #define FIFO_TH 1
21 #define FIFO_FULL 2
22
lsm6dsv16x_config_fifo(const struct device * dev,uint8_t fifo_irq)23 static void lsm6dsv16x_config_fifo(const struct device *dev, uint8_t fifo_irq)
24 {
25 struct lsm6dsv16x_data *lsm6dsv16x = dev->data;
26 const struct lsm6dsv16x_config *config = dev->config;
27 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&config->ctx;
28 uint8_t fifo_wtm = 0;
29 lsm6dsv16x_pin_int_route_t pin_int = { 0 };
30 lsm6dsv16x_fifo_xl_batch_t xl_batch = LSM6DSV16X_DT_XL_NOT_BATCHED;
31 lsm6dsv16x_fifo_gy_batch_t gy_batch = LSM6DSV16X_DT_GY_NOT_BATCHED;
32 lsm6dsv16x_fifo_temp_batch_t temp_batch = LSM6DSV16X_DT_TEMP_NOT_BATCHED;
33 lsm6dsv16x_fifo_mode_t fifo_mode = LSM6DSV16X_BYPASS_MODE;
34 lsm6dsv16x_sflp_data_rate_t sflp_odr = LSM6DSV16X_SFLP_120Hz;
35 lsm6dsv16x_fifo_sflp_raw_t sflp_fifo = { 0 };
36
37 /* disable FIFO as first thing */
38 lsm6dsv16x_fifo_mode_set(ctx, LSM6DSV16X_BYPASS_MODE);
39
40 pin_int.fifo_th = PROPERTY_DISABLE;
41 pin_int.fifo_full = PROPERTY_DISABLE;
42
43 if (fifo_irq != 0) {
44 pin_int.fifo_th = (fifo_irq & FIFO_TH) ? PROPERTY_ENABLE : PROPERTY_DISABLE;
45 pin_int.fifo_full = (fifo_irq & FIFO_FULL) ? PROPERTY_ENABLE : PROPERTY_DISABLE;
46
47 xl_batch = config->accel_batch;
48 gy_batch = config->gyro_batch;
49 temp_batch = config->temp_batch;
50
51 fifo_mode = LSM6DSV16X_STREAM_MODE;
52 fifo_wtm = config->fifo_wtm;
53
54 if (config->sflp_fifo_en & LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION) {
55 sflp_fifo.game_rotation = 1;
56 }
57
58 if (config->sflp_fifo_en & LSM6DSV16X_DT_SFLP_FIFO_GRAVITY) {
59 sflp_fifo.gravity = 1;
60 }
61
62 if (config->sflp_fifo_en & LSM6DSV16X_DT_SFLP_FIFO_GBIAS) {
63 sflp_fifo.gbias = 1;
64 }
65
66 sflp_odr = config->sflp_odr;
67 }
68
69 /*
70 * Set FIFO watermark (number of unread sensor data TAG + 6 bytes
71 * stored in FIFO) to FIFO_WATERMARK samples
72 */
73 lsm6dsv16x_fifo_watermark_set(ctx, config->fifo_wtm);
74
75 /* Turn on/off FIFO */
76 lsm6dsv16x_fifo_mode_set(ctx, fifo_mode);
77
78 /* Set FIFO batch rates */
79 lsm6dsv16x_fifo_xl_batch_set(ctx, xl_batch);
80 lsm6dsv16x->accel_batch_odr = xl_batch;
81 lsm6dsv16x_fifo_gy_batch_set(ctx, gy_batch);
82 lsm6dsv16x->gyro_batch_odr = gy_batch;
83 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
84 lsm6dsv16x_fifo_temp_batch_set(ctx, temp_batch);
85 lsm6dsv16x->temp_batch_odr = temp_batch;
86 #endif
87
88 lsm6dsv16x_sflp_data_rate_set(ctx, sflp_odr);
89 lsm6dsv16x->sflp_batch_odr = sflp_odr;
90 lsm6dsv16x_fifo_sflp_batch_set(ctx, sflp_fifo);
91 lsm6dsv16x_sflp_game_rotation_set(ctx, PROPERTY_ENABLE);
92
93 /* Set pin interrupt (fifo_th could be on or off) */
94 if ((config->drdy_pin == 1) || (ON_I3C_BUS(config) && (!I3C_INT_PIN(config)))) {
95 lsm6dsv16x_pin_int1_route_set(ctx, &pin_int);
96 } else {
97 lsm6dsv16x_pin_int2_route_set(ctx, &pin_int);
98 }
99 }
100
lsm6dsv16x_submit_stream(const struct device * dev,struct rtio_iodev_sqe * iodev_sqe)101 void lsm6dsv16x_submit_stream(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
102 {
103 struct lsm6dsv16x_data *lsm6dsv16x = dev->data;
104 #if DT_ANY_INST_ON_BUS_STATUS_OKAY(i3c)
105 const struct lsm6dsv16x_config *config = dev->config;
106 #endif
107 const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
108 uint8_t fifo_irq = 0;
109
110 if (!ON_I3C_BUS(config) || (I3C_INT_PIN(config))) {
111 gpio_pin_interrupt_configure_dt(lsm6dsv16x->drdy_gpio, GPIO_INT_DISABLE);
112 }
113
114 for (size_t i = 0; i < cfg->count; i++) {
115 if (cfg->triggers[i].trigger == SENSOR_TRIG_FIFO_WATERMARK) {
116 fifo_irq = FIFO_TH;
117 } else if (cfg->triggers[i].trigger == SENSOR_TRIG_FIFO_FULL) {
118 fifo_irq = FIFO_FULL;
119 }
120 }
121
122 /* if any change in fifo irq */
123 if (fifo_irq != lsm6dsv16x->fifo_irq) {
124 lsm6dsv16x->fifo_irq = fifo_irq;
125
126 /* enable/disable the FIFO */
127 lsm6dsv16x_config_fifo(dev, fifo_irq);
128 }
129
130 lsm6dsv16x->streaming_sqe = iodev_sqe;
131
132 if (!ON_I3C_BUS(config) || (I3C_INT_PIN(config))) {
133 gpio_pin_interrupt_configure_dt(lsm6dsv16x->drdy_gpio, GPIO_INT_EDGE_TO_ACTIVE);
134 }
135 }
136
lsm6dsv16x_complete_op_cb(struct rtio * r,const struct rtio_sqe * sqe,void * arg)137 static void lsm6dsv16x_complete_op_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg)
138 {
139 const struct device *dev = arg;
140 #if DT_ANY_INST_ON_BUS_STATUS_OKAY(i3c)
141 const struct lsm6dsv16x_config *config = dev->config;
142 #endif
143 struct lsm6dsv16x_data *lsm6dsv16x = dev->data;
144
145 /*
146 * Mark operation completed
147 */
148 rtio_iodev_sqe_ok(sqe->userdata, 0);
149 lsm6dsv16x->streaming_sqe = NULL;
150 if (!ON_I3C_BUS(config) || (I3C_INT_PIN(config))) {
151 gpio_pin_interrupt_configure_dt(lsm6dsv16x->drdy_gpio, GPIO_INT_EDGE_TO_ACTIVE);
152 }
153 }
154
lsm6dsv16x_read_fifo_cb(struct rtio * r,const struct rtio_sqe * sqe,void * arg)155 static void lsm6dsv16x_read_fifo_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg)
156 {
157 const struct device *dev = arg;
158 #if DT_ANY_INST_ON_BUS_STATUS_OKAY(i3c)
159 const struct lsm6dsv16x_config *config = dev->config;
160 #endif
161 struct lsm6dsv16x_data *lsm6dsv16x = dev->data;
162 struct gpio_dt_spec *irq_gpio = lsm6dsv16x->drdy_gpio;
163 struct rtio_iodev *iodev = lsm6dsv16x->iodev;
164 struct sensor_read_config *read_config;
165 uint8_t fifo_th = 0, fifo_full = 0;
166 uint16_t fifo_count;
167
168 /* At this point, no sqe request is queued should be considered as a bug */
169 __ASSERT_NO_MSG(lsm6dsv16x->streaming_sqe != NULL);
170
171 read_config = (struct sensor_read_config *)lsm6dsv16x->streaming_sqe->sqe.iodev->data;
172 __ASSERT_NO_MSG(read_config != NULL);
173 __ASSERT_NO_MSG(read_config->is_streaming == true);
174
175 /* parse the configuration in search for any configured trigger */
176 struct sensor_stream_trigger *fifo_ths_cfg = NULL;
177 struct sensor_stream_trigger *fifo_full_cfg = NULL;
178
179 for (int i = 0; i < read_config->count; ++i) {
180 if (read_config->triggers[i].trigger == SENSOR_TRIG_FIFO_WATERMARK) {
181 fifo_ths_cfg = &read_config->triggers[i];
182 continue;
183 }
184
185 if (read_config->triggers[i].trigger == SENSOR_TRIG_FIFO_FULL) {
186 fifo_full_cfg = &read_config->triggers[i];
187 continue;
188 }
189 }
190
191 /* fill fifo h/w status */
192 fifo_th = (lsm6dsv16x->fifo_status[1] & 0x80) ? 1 : 0;
193 fifo_full = (lsm6dsv16x->fifo_status[1] & 0x20) ? 1 : 0;
194 fifo_count = (uint16_t)lsm6dsv16x->fifo_status[1] & 0x1U;
195 fifo_count = (fifo_count << 8) | lsm6dsv16x->fifo_status[0];
196 lsm6dsv16x->fifo_count = fifo_count;
197
198 bool has_fifo_ths_trig = fifo_ths_cfg != NULL && fifo_th == 1;
199 bool has_fifo_full_trig = fifo_full_cfg != NULL && fifo_full == 1;
200
201 /* check if no theshold/full fifo interrupt or spurious interrupts */
202 if (!has_fifo_ths_trig && !has_fifo_full_trig) {
203 /* complete operation with no error */
204 rtio_iodev_sqe_ok(sqe->userdata, 0);
205
206 lsm6dsv16x->streaming_sqe = NULL;
207 if (!ON_I3C_BUS(config) || (I3C_INT_PIN(config))) {
208 gpio_pin_interrupt_configure_dt(irq_gpio, GPIO_INT_EDGE_TO_ACTIVE);
209 }
210 return;
211 }
212
213 struct rtio_cqe *cqe;
214 int res = 0;
215
216 do {
217 cqe = rtio_cqe_consume(lsm6dsv16x->rtio_ctx);
218 if (cqe != NULL) {
219 if ((cqe->result < 0) && (res == 0)) {
220 LOG_ERR("Bus error: %d", cqe->result);
221 res = cqe->result;
222 }
223 rtio_cqe_release(lsm6dsv16x->rtio_ctx, cqe);
224 }
225 } while (cqe != NULL);
226
227 /* Bail/cancel attempt to read sensor on any error */
228 if (res != 0) {
229 rtio_iodev_sqe_err(lsm6dsv16x->streaming_sqe, res);
230 lsm6dsv16x->streaming_sqe = NULL;
231 return;
232 }
233
234 enum sensor_stream_data_opt data_opt;
235
236 if (has_fifo_ths_trig && !has_fifo_full_trig) {
237 /* Only care about fifo threshold */
238 data_opt = fifo_ths_cfg->opt;
239 } else if (!has_fifo_ths_trig && has_fifo_full_trig) {
240 /* Only care about fifo full */
241 data_opt = fifo_full_cfg->opt;
242 } else {
243 /* Both fifo threshold and full */
244 data_opt = MIN(fifo_ths_cfg->opt, fifo_full_cfg->opt);
245 }
246
247 if (data_opt == SENSOR_STREAM_DATA_NOP || data_opt == SENSOR_STREAM_DATA_DROP) {
248 uint8_t *buf;
249 uint32_t buf_len;
250
251 /* Clear streaming_sqe since we're done with the call */
252 if (rtio_sqe_rx_buf(lsm6dsv16x->streaming_sqe, sizeof(struct lsm6dsv16x_fifo_data),
253 sizeof(struct lsm6dsv16x_fifo_data), &buf, &buf_len) != 0) {
254 rtio_iodev_sqe_err(lsm6dsv16x->streaming_sqe, -ENOMEM);
255 lsm6dsv16x->streaming_sqe = NULL;
256 if (!ON_I3C_BUS(config) || (I3C_INT_PIN(config))) {
257 gpio_pin_interrupt_configure_dt(irq_gpio, GPIO_INT_EDGE_TO_ACTIVE);
258 }
259 return;
260 }
261
262 struct lsm6dsv16x_fifo_data *rx_data = (struct lsm6dsv16x_fifo_data *)buf;
263
264 memset(buf, 0, buf_len);
265 rx_data->header.is_fifo = 1;
266 rx_data->header.timestamp = lsm6dsv16x->fifo_timestamp;
267 rx_data->int_status = lsm6dsv16x->fifo_status[1];
268 rx_data->fifo_count = 0;
269
270 /* complete request with ok */
271 rtio_iodev_sqe_ok(lsm6dsv16x->streaming_sqe, 0);
272 lsm6dsv16x->streaming_sqe = NULL;
273 if (!ON_I3C_BUS(config) || (I3C_INT_PIN(config))) {
274 gpio_pin_interrupt_configure_dt(irq_gpio, GPIO_INT_EDGE_TO_ACTIVE);
275 }
276
277 if (data_opt == SENSOR_STREAM_DATA_DROP) {
278
279 /*
280 * Flush the FIFO by setting the mode to LSM6DSV16X_BYPASS_MODE
281 *
282 * STMEMSC API equivalent code:
283 *
284 * lsm6dsv16x_fifo_mode_set(ctx, LSM6DSV16X_BYPASS_MODE);
285 */
286 struct rtio_sqe *write_fifo_mode = rtio_sqe_acquire(lsm6dsv16x->rtio_ctx);
287 uint8_t lsm6dsv16x_fifo_mode_set[] = {
288 LSM6DSV16X_FIFO_CTRL4,
289 LSM6DSV16X_BYPASS_MODE,
290 };
291
292 write_fifo_mode->flags |= RTIO_SQE_NO_RESPONSE;
293 rtio_sqe_prep_tiny_write(write_fifo_mode, iodev,
294 RTIO_PRIO_NORM, lsm6dsv16x_fifo_mode_set,
295 ARRAY_SIZE(lsm6dsv16x_fifo_mode_set), NULL);
296
297 rtio_submit(lsm6dsv16x->rtio_ctx, 0);
298 }
299
300 return;
301 }
302
303 uint8_t *buf, *read_buf;
304 uint32_t buf_len, buf_avail;
305 uint32_t req_len = LSM6DSV16X_FIFO_SIZE(fifo_count) + sizeof(struct lsm6dsv16x_fifo_data);
306
307 if (rtio_sqe_rx_buf(lsm6dsv16x->streaming_sqe, req_len, req_len, &buf, &buf_len) != 0) {
308 LOG_ERR("Failed to get buffer");
309 rtio_iodev_sqe_err(lsm6dsv16x->streaming_sqe, -ENOMEM);
310 lsm6dsv16x->streaming_sqe = NULL;
311 if (!ON_I3C_BUS(config) || (I3C_INT_PIN(config))) {
312 gpio_pin_interrupt_configure_dt(irq_gpio, GPIO_INT_EDGE_TO_ACTIVE);
313 }
314 return;
315 }
316
317 struct lsm6dsv16x_fifo_data hdr = {
318 .header = {
319 .is_fifo = true,
320 .accel_fs = lsm6dsv16x->accel_fs,
321 .gyro_fs = lsm6dsv16x->gyro_fs,
322 .timestamp = lsm6dsv16x->fifo_timestamp,
323 },
324 .fifo_count = fifo_count,
325 .accel_batch_odr = lsm6dsv16x->accel_batch_odr,
326 .gyro_batch_odr = lsm6dsv16x->gyro_batch_odr,
327 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
328 .temp_batch_odr = lsm6dsv16x->temp_batch_odr,
329 #endif
330 .sflp_batch_odr = lsm6dsv16x->sflp_batch_odr,
331 };
332
333 memcpy(buf, &hdr, sizeof(hdr));
334 read_buf = buf + sizeof(hdr);
335 buf_avail = buf_len - sizeof(hdr);
336
337 /*
338 * Prepare rtio enabled bus to read all fifo_count entries from
339 * LSM6DSV16X_FIFO_DATA_OUT_TAG. Then lsm6dsv16x_complete_op_cb
340 * callback will be invoked.
341 *
342 * STMEMSC API equivalent code:
343 *
344 * num = fifo_status.fifo_level;
345 *
346 * while (num--) {
347 * lsm6dsv16x_fifo_out_raw_t f_data;
348 *
349 * lsm6dsv16x_fifo_out_raw_get(&dev_ctx, &f_data);
350 * }
351 */
352 struct rtio_sqe *write_fifo_dout_addr = rtio_sqe_acquire(lsm6dsv16x->rtio_ctx);
353 struct rtio_sqe *read_fifo_dout_reg = rtio_sqe_acquire(lsm6dsv16x->rtio_ctx);
354 struct rtio_sqe *complete_op = rtio_sqe_acquire(lsm6dsv16x->rtio_ctx);
355 uint8_t reg = lsm6dsv16x_bus_reg(lsm6dsv16x, LSM6DSV16X_FIFO_DATA_OUT_TAG);
356
357 rtio_sqe_prep_tiny_write(write_fifo_dout_addr, iodev, RTIO_PRIO_NORM, ®, 1, NULL);
358 write_fifo_dout_addr->flags = RTIO_SQE_TRANSACTION;
359 rtio_sqe_prep_read(read_fifo_dout_reg, iodev, RTIO_PRIO_NORM,
360 read_buf, buf_avail, lsm6dsv16x->streaming_sqe);
361 read_fifo_dout_reg->flags = RTIO_SQE_CHAINED;
362 if (lsm6dsv16x->bus_type == BUS_I2C) {
363 read_fifo_dout_reg->iodev_flags |= RTIO_IODEV_I2C_STOP | RTIO_IODEV_I2C_RESTART;
364 } else if (lsm6dsv16x->bus_type == BUS_I3C) {
365 read_fifo_dout_reg->iodev_flags |= RTIO_IODEV_I3C_STOP | RTIO_IODEV_I3C_RESTART;
366 }
367 rtio_sqe_prep_callback_no_cqe(complete_op, lsm6dsv16x_complete_op_cb, (void *)dev,
368 lsm6dsv16x->streaming_sqe);
369
370 rtio_submit(lsm6dsv16x->rtio_ctx, 0);
371 }
372
lsm6dsv16x_stream_irq_handler(const struct device * dev)373 void lsm6dsv16x_stream_irq_handler(const struct device *dev)
374 {
375 struct lsm6dsv16x_data *lsm6dsv16x = dev->data;
376 struct rtio_iodev *iodev = lsm6dsv16x->iodev;
377 #if DT_ANY_INST_ON_BUS_STATUS_OKAY(i3c)
378 const struct lsm6dsv16x_config *config = dev->config;
379 #endif
380
381 if (lsm6dsv16x->streaming_sqe == NULL) {
382 return;
383 }
384
385 /* get timestamp as soon as the irq is served */
386 lsm6dsv16x->fifo_timestamp = k_ticks_to_ns_floor64(k_uptime_ticks());
387
388 #if DT_ANY_INST_ON_BUS_STATUS_OKAY(i3c)
389 if (ON_I3C_BUS(config) && (!I3C_INT_PIN(config))) {
390 /*
391 * If we are on an I3C bus, then it should be expected that the fifo status was
392 * already received in the IBI payload and we don't need to read it again.
393 */
394 lsm6dsv16x->fifo_status[0] = lsm6dsv16x->ibi_payload.fifo_status1;
395 lsm6dsv16x->fifo_status[1] = lsm6dsv16x->ibi_payload.fifo_status2;
396 } else
397 #endif
398 {
399 lsm6dsv16x->fifo_status[0] = lsm6dsv16x->fifo_status[1] = 0;
400
401 /*
402 * Prepare rtio enabled bus to read LSM6DSV16X_FIFO_STATUS1 and
403 * LSM6DSV16X_FIFO_STATUS2 registers where FIFO threshold condition and count are
404 * reported. Then lsm6dsv16x_read_fifo_cb callback will be invoked.
405 *
406 * STMEMSC API equivalent code:
407 *
408 * lsm6dsv16x_fifo_status_t fifo_status;
409 *
410 * lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);
411 */
412 struct rtio_sqe *write_fifo_status_addr = rtio_sqe_acquire(lsm6dsv16x->rtio_ctx);
413 struct rtio_sqe *read_fifo_status_reg = rtio_sqe_acquire(lsm6dsv16x->rtio_ctx);
414 uint8_t reg = lsm6dsv16x_bus_reg(lsm6dsv16x, LSM6DSV16X_FIFO_STATUS1);
415
416 rtio_sqe_prep_tiny_write(write_fifo_status_addr, iodev, RTIO_PRIO_NORM, ®, 1,
417 NULL);
418 write_fifo_status_addr->flags = RTIO_SQE_TRANSACTION;
419 rtio_sqe_prep_read(read_fifo_status_reg, iodev, RTIO_PRIO_NORM,
420 lsm6dsv16x->fifo_status, 2, NULL);
421 read_fifo_status_reg->flags = RTIO_SQE_CHAINED;
422 if (lsm6dsv16x->bus_type == BUS_I2C) {
423 read_fifo_status_reg->iodev_flags |=
424 RTIO_IODEV_I2C_STOP | RTIO_IODEV_I2C_RESTART;
425 } else if (lsm6dsv16x->bus_type == BUS_I3C) {
426 read_fifo_status_reg->iodev_flags |=
427 RTIO_IODEV_I3C_STOP | RTIO_IODEV_I3C_RESTART;
428 }
429 }
430 struct rtio_sqe *check_fifo_status_reg = rtio_sqe_acquire(lsm6dsv16x->rtio_ctx);
431
432 rtio_sqe_prep_callback_no_cqe(check_fifo_status_reg,
433 lsm6dsv16x_read_fifo_cb, (void *)dev, NULL);
434 rtio_submit(lsm6dsv16x->rtio_ctx, 0);
435 }
436
