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