1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /* Copyright (C) 2025 Invensense, Inc. */
3
4 #include <linux/bitfield.h>
5 #include <linux/delay.h>
6 #include <linux/device.h>
7 #include <linux/err.h>
8 #include <linux/minmax.h>
9 #include <linux/mutex.h>
10 #include <linux/pm_runtime.h>
11 #include <linux/regmap.h>
12 #include <linux/time.h>
13 #include <linux/types.h>
14
15 #include <asm/byteorder.h>
16
17 #include <linux/iio/buffer.h>
18 #include <linux/iio/common/inv_sensors_timestamp.h>
19 #include <linux/iio/iio.h>
20
21 #include "inv_icm45600_buffer.h"
22 #include "inv_icm45600.h"
23
24 /* FIFO header: 1 byte */
25 #define INV_ICM45600_FIFO_EXT_HEADER BIT(7)
26 #define INV_ICM45600_FIFO_HEADER_ACCEL BIT(6)
27 #define INV_ICM45600_FIFO_HEADER_GYRO BIT(5)
28 #define INV_ICM45600_FIFO_HEADER_HIGH_RES BIT(4)
29 #define INV_ICM45600_FIFO_HEADER_TMST_FSYNC GENMASK(3, 2)
30 #define INV_ICM45600_FIFO_HEADER_ODR_ACCEL BIT(1)
31 #define INV_ICM45600_FIFO_HEADER_ODR_GYRO BIT(0)
32
33 struct inv_icm45600_fifo_1sensor_packet {
34 u8 header;
35 struct inv_icm45600_fifo_sensor_data data;
36 s8 temp;
37 } __packed;
38
39 struct inv_icm45600_fifo_2sensors_packet {
40 u8 header;
41 struct inv_icm45600_fifo_sensor_data accel;
42 struct inv_icm45600_fifo_sensor_data gyro;
43 s8 temp;
44 __le16 timestamp;
45 } __packed;
46
inv_icm45600_fifo_decode_packet(const void * packet,const struct inv_icm45600_fifo_sensor_data ** accel,const struct inv_icm45600_fifo_sensor_data ** gyro,const s8 ** temp,const __le16 ** timestamp,unsigned int * odr)47 ssize_t inv_icm45600_fifo_decode_packet(const void *packet,
48 const struct inv_icm45600_fifo_sensor_data **accel,
49 const struct inv_icm45600_fifo_sensor_data **gyro,
50 const s8 **temp,
51 const __le16 **timestamp, unsigned int *odr)
52 {
53 const struct inv_icm45600_fifo_1sensor_packet *pack1 = packet;
54 const struct inv_icm45600_fifo_2sensors_packet *pack2 = packet;
55 u8 header = *((const u8 *)packet);
56
57 /* FIFO extended header */
58 if (header & INV_ICM45600_FIFO_EXT_HEADER) {
59 /* Not yet supported */
60 return 0;
61 }
62
63 /* handle odr flags. */
64 *odr = 0;
65 if (header & INV_ICM45600_FIFO_HEADER_ODR_GYRO)
66 *odr |= INV_ICM45600_SENSOR_GYRO;
67 if (header & INV_ICM45600_FIFO_HEADER_ODR_ACCEL)
68 *odr |= INV_ICM45600_SENSOR_ACCEL;
69
70 /* Accel + Gyro data are present. */
71 if ((header & INV_ICM45600_FIFO_HEADER_ACCEL) &&
72 (header & INV_ICM45600_FIFO_HEADER_GYRO)) {
73 *accel = &pack2->accel;
74 *gyro = &pack2->gyro;
75 *temp = &pack2->temp;
76 *timestamp = &pack2->timestamp;
77 return sizeof(*pack2);
78 }
79
80 /* Accel data only. */
81 if (header & INV_ICM45600_FIFO_HEADER_ACCEL) {
82 *accel = &pack1->data;
83 *gyro = NULL;
84 *temp = &pack1->temp;
85 *timestamp = NULL;
86 return sizeof(*pack1);
87 }
88
89 /* Gyro data only. */
90 if (header & INV_ICM45600_FIFO_HEADER_GYRO) {
91 *accel = NULL;
92 *gyro = &pack1->data;
93 *temp = &pack1->temp;
94 *timestamp = NULL;
95 return sizeof(*pack1);
96 }
97
98 /* Invalid packet if here. */
99 return -EINVAL;
100 }
101
inv_icm45600_buffer_update_fifo_period(struct inv_icm45600_state * st)102 void inv_icm45600_buffer_update_fifo_period(struct inv_icm45600_state *st)
103 {
104 u32 period_gyro, period_accel;
105
106 if (st->fifo.en & INV_ICM45600_SENSOR_GYRO)
107 period_gyro = inv_icm45600_odr_to_period(st->conf.gyro.odr);
108 else
109 period_gyro = U32_MAX;
110
111 if (st->fifo.en & INV_ICM45600_SENSOR_ACCEL)
112 period_accel = inv_icm45600_odr_to_period(st->conf.accel.odr);
113 else
114 period_accel = U32_MAX;
115
116 st->fifo.period = min(period_gyro, period_accel);
117 }
118
inv_icm45600_buffer_set_fifo_en(struct inv_icm45600_state * st,unsigned int fifo_en)119 int inv_icm45600_buffer_set_fifo_en(struct inv_icm45600_state *st,
120 unsigned int fifo_en)
121 {
122 unsigned int mask;
123 int ret;
124
125 mask = INV_ICM45600_FIFO_CONFIG3_GYRO_EN |
126 INV_ICM45600_FIFO_CONFIG3_ACCEL_EN;
127
128 ret = regmap_assign_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3, mask,
129 (fifo_en & INV_ICM45600_SENSOR_GYRO) ||
130 (fifo_en & INV_ICM45600_SENSOR_ACCEL));
131 if (ret)
132 return ret;
133
134 st->fifo.en = fifo_en;
135 inv_icm45600_buffer_update_fifo_period(st);
136
137 return 0;
138 }
139
inv_icm45600_wm_truncate(unsigned int watermark,size_t packet_size,unsigned int fifo_period)140 static unsigned int inv_icm45600_wm_truncate(unsigned int watermark, size_t packet_size,
141 unsigned int fifo_period)
142 {
143 size_t watermark_max, grace_samples;
144
145 /* Keep 20ms for processing FIFO.*/
146 grace_samples = (20U * NSEC_PER_MSEC) / fifo_period;
147 if (grace_samples < 1)
148 grace_samples = 1;
149
150 watermark_max = INV_ICM45600_FIFO_SIZE_MAX / packet_size;
151 watermark_max -= grace_samples;
152
153 return min(watermark, watermark_max);
154 }
155
156 /**
157 * inv_icm45600_buffer_update_watermark - update watermark FIFO threshold
158 * @st: driver internal state
159 *
160 * FIFO watermark threshold is computed based on the required watermark values
161 * set for gyro and accel sensors. Since watermark is all about acceptable data
162 * latency, use the smallest setting between the 2. It means choosing the
163 * smallest latency but this is not as simple as choosing the smallest watermark
164 * value. Latency depends on watermark and ODR. It requires several steps:
165 * 1) compute gyro and accel latencies and choose the smallest value.
166 * 2) adapt the chosen latency so that it is a multiple of both gyro and accel
167 * ones. Otherwise it is possible that you don't meet a requirement. (for
168 * example with gyro @100Hz wm 4 and accel @100Hz with wm 6, choosing the
169 * value of 4 will not meet accel latency requirement because 6 is not a
170 * multiple of 4. You need to use the value 2.)
171 * 3) Since all periods are multiple of each others, watermark is computed by
172 * dividing this computed latency by the smallest period, which corresponds
173 * to the FIFO frequency.
174 *
175 * Returns: 0 on success, a negative error code otherwise.
176 */
inv_icm45600_buffer_update_watermark(struct inv_icm45600_state * st)177 int inv_icm45600_buffer_update_watermark(struct inv_icm45600_state *st)
178 {
179 const size_t packet_size = sizeof(struct inv_icm45600_fifo_2sensors_packet);
180 unsigned int wm_gyro, wm_accel, watermark;
181 u32 period_gyro, period_accel, period;
182 u32 latency_gyro, latency_accel, latency;
183
184 /* Compute sensors latency, depending on sensor watermark and odr. */
185 wm_gyro = inv_icm45600_wm_truncate(st->fifo.watermark.gyro, packet_size,
186 st->fifo.period);
187 wm_accel = inv_icm45600_wm_truncate(st->fifo.watermark.accel, packet_size,
188 st->fifo.period);
189 /* Use us for odr to avoid overflow using 32 bits values. */
190 period_gyro = inv_icm45600_odr_to_period(st->conf.gyro.odr) / NSEC_PER_USEC;
191 period_accel = inv_icm45600_odr_to_period(st->conf.accel.odr) / NSEC_PER_USEC;
192 latency_gyro = period_gyro * wm_gyro;
193 latency_accel = period_accel * wm_accel;
194
195 /* 0 value for watermark means that the sensor is turned off. */
196 if (wm_gyro == 0 && wm_accel == 0)
197 return 0;
198
199 if (latency_gyro == 0) {
200 watermark = wm_accel;
201 st->fifo.watermark.eff_accel = wm_accel;
202 } else if (latency_accel == 0) {
203 watermark = wm_gyro;
204 st->fifo.watermark.eff_gyro = wm_gyro;
205 } else {
206 /* Compute the smallest latency that is a multiple of both. */
207 if (latency_gyro <= latency_accel)
208 latency = latency_gyro - (latency_accel % latency_gyro);
209 else
210 latency = latency_accel - (latency_gyro % latency_accel);
211 /* Use the shortest period. */
212 period = min(period_gyro, period_accel);
213 /* All this works because periods are multiple of each others. */
214 watermark = max(latency / period, 1);
215 /* Update effective watermark. */
216 st->fifo.watermark.eff_gyro = max(latency / period_gyro, 1);
217 st->fifo.watermark.eff_accel = max(latency / period_accel, 1);
218 }
219
220 st->buffer.u16 = cpu_to_le16(watermark);
221 return regmap_bulk_write(st->map, INV_ICM45600_REG_FIFO_WATERMARK,
222 &st->buffer.u16, sizeof(st->buffer.u16));
223 }
224
inv_icm45600_buffer_preenable(struct iio_dev * indio_dev)225 static int inv_icm45600_buffer_preenable(struct iio_dev *indio_dev)
226 {
227 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
228 struct device *dev = regmap_get_device(st->map);
229 struct inv_icm45600_sensor_state *sensor_st = iio_priv(indio_dev);
230 struct inv_sensors_timestamp *ts = &sensor_st->ts;
231 int ret;
232
233 ret = pm_runtime_resume_and_get(dev);
234 if (ret)
235 return ret;
236
237 guard(mutex)(&st->lock);
238 inv_sensors_timestamp_reset(ts);
239
240 return 0;
241 }
242
243 /*
244 * Update_scan_mode callback is turning sensors on and setting data FIFO enable
245 * bits.
246 */
inv_icm45600_buffer_postenable(struct iio_dev * indio_dev)247 static int inv_icm45600_buffer_postenable(struct iio_dev *indio_dev)
248 {
249 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
250 unsigned int val;
251 int ret;
252
253 guard(mutex)(&st->lock);
254
255 /* Exit if FIFO is already on. */
256 if (st->fifo.on) {
257 st->fifo.on++;
258 return 0;
259 }
260
261 ret = regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG2,
262 INV_ICM45600_REG_FIFO_CONFIG2_FIFO_FLUSH);
263 if (ret)
264 return ret;
265
266 ret = regmap_set_bits(st->map, INV_ICM45600_REG_INT1_CONFIG0,
267 INV_ICM45600_INT1_CONFIG0_FIFO_THS_EN |
268 INV_ICM45600_INT1_CONFIG0_FIFO_FULL_EN);
269 if (ret)
270 return ret;
271
272 val = FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK,
273 INV_ICM45600_FIFO_CONFIG0_MODE_STREAM);
274 ret = regmap_update_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG0,
275 INV_ICM45600_FIFO_CONFIG0_MODE_MASK, val);
276 if (ret)
277 return ret;
278
279 /* Enable writing sensor data to FIFO. */
280 ret = regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3,
281 INV_ICM45600_FIFO_CONFIG3_IF_EN);
282 if (ret)
283 return ret;
284
285 st->fifo.on++;
286 return 0;
287 }
288
inv_icm45600_buffer_predisable(struct iio_dev * indio_dev)289 static int inv_icm45600_buffer_predisable(struct iio_dev *indio_dev)
290 {
291 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
292 unsigned int val;
293 int ret;
294
295 guard(mutex)(&st->lock);
296
297 /* Exit if there are several sensors using the FIFO. */
298 if (st->fifo.on > 1) {
299 st->fifo.on--;
300 return 0;
301 }
302
303 /* Disable writing sensor data to FIFO. */
304 ret = regmap_clear_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3,
305 INV_ICM45600_FIFO_CONFIG3_IF_EN);
306 if (ret)
307 return ret;
308
309 val = FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK,
310 INV_ICM45600_FIFO_CONFIG0_MODE_BYPASS);
311 ret = regmap_update_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG0,
312 INV_ICM45600_FIFO_CONFIG0_MODE_MASK, val);
313 if (ret)
314 return ret;
315
316 ret = regmap_clear_bits(st->map, INV_ICM45600_REG_INT1_CONFIG0,
317 INV_ICM45600_INT1_CONFIG0_FIFO_THS_EN |
318 INV_ICM45600_INT1_CONFIG0_FIFO_FULL_EN);
319 if (ret)
320 return ret;
321
322 ret = regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG2,
323 INV_ICM45600_REG_FIFO_CONFIG2_FIFO_FLUSH);
324 if (ret)
325 return ret;
326
327 st->fifo.on--;
328 return 0;
329 }
330
_inv_icm45600_buffer_postdisable(struct inv_icm45600_state * st,unsigned int sensor,unsigned int * watermark,unsigned int * sleep)331 static int _inv_icm45600_buffer_postdisable(struct inv_icm45600_state *st,
332 unsigned int sensor, unsigned int *watermark,
333 unsigned int *sleep)
334 {
335 struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES;
336 int ret;
337
338 ret = inv_icm45600_buffer_set_fifo_en(st, st->fifo.en & ~sensor);
339 if (ret)
340 return ret;
341
342 *watermark = 0;
343 ret = inv_icm45600_buffer_update_watermark(st);
344 if (ret)
345 return ret;
346
347 conf.mode = INV_ICM45600_SENSOR_MODE_OFF;
348 if (sensor == INV_ICM45600_SENSOR_GYRO)
349 return inv_icm45600_set_gyro_conf(st, &conf, sleep);
350 else
351 return inv_icm45600_set_accel_conf(st, &conf, sleep);
352 }
353
inv_icm45600_buffer_postdisable(struct iio_dev * indio_dev)354 static int inv_icm45600_buffer_postdisable(struct iio_dev *indio_dev)
355 {
356 struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
357 struct device *dev = regmap_get_device(st->map);
358 unsigned int sensor;
359 unsigned int *watermark;
360 unsigned int sleep;
361 int ret;
362
363 if (indio_dev == st->indio_gyro) {
364 sensor = INV_ICM45600_SENSOR_GYRO;
365 watermark = &st->fifo.watermark.gyro;
366 } else if (indio_dev == st->indio_accel) {
367 sensor = INV_ICM45600_SENSOR_ACCEL;
368 watermark = &st->fifo.watermark.accel;
369 } else {
370 return -EINVAL;
371 }
372
373 sleep = 0;
374 scoped_guard(mutex, &st->lock)
375 ret = _inv_icm45600_buffer_postdisable(st, sensor, watermark, &sleep);
376
377 /* Sleep required time. */
378 if (sleep)
379 msleep(sleep);
380
381 pm_runtime_put_autosuspend(dev);
382
383 return ret;
384 }
385
386 const struct iio_buffer_setup_ops inv_icm45600_buffer_ops = {
387 .preenable = inv_icm45600_buffer_preenable,
388 .postenable = inv_icm45600_buffer_postenable,
389 .predisable = inv_icm45600_buffer_predisable,
390 .postdisable = inv_icm45600_buffer_postdisable,
391 };
392
inv_icm45600_buffer_fifo_read(struct inv_icm45600_state * st,unsigned int max)393 int inv_icm45600_buffer_fifo_read(struct inv_icm45600_state *st,
394 unsigned int max)
395 {
396 const ssize_t packet_size = sizeof(struct inv_icm45600_fifo_2sensors_packet);
397 __le16 *raw_fifo_count;
398 size_t fifo_nb, i;
399 ssize_t size;
400 const struct inv_icm45600_fifo_sensor_data *accel, *gyro;
401 const __le16 *timestamp;
402 const s8 *temp;
403 unsigned int odr;
404 int ret;
405
406 /* Reset all samples counters. */
407 st->fifo.count = 0;
408 st->fifo.nb.gyro = 0;
409 st->fifo.nb.accel = 0;
410 st->fifo.nb.total = 0;
411
412 raw_fifo_count = &st->buffer.u16;
413 ret = regmap_bulk_read(st->map, INV_ICM45600_REG_FIFO_COUNT,
414 raw_fifo_count, sizeof(*raw_fifo_count));
415 if (ret)
416 return ret;
417
418 /* Check and limit number of samples if requested. */
419 fifo_nb = le16_to_cpup(raw_fifo_count);
420 if (fifo_nb == 0)
421 return 0;
422 if (max > 0 && fifo_nb > max)
423 fifo_nb = max;
424
425 /* Try to read all FIFO data in internal buffer. */
426 st->fifo.count = fifo_nb * packet_size;
427 ret = regmap_noinc_read(st->map, INV_ICM45600_REG_FIFO_DATA,
428 st->fifo.data, st->fifo.count);
429 if (ret == -ENOTSUPP || ret == -EFBIG) {
430 /* Read full fifo is not supported, read samples one by one. */
431 ret = 0;
432 for (i = 0; i < st->fifo.count && ret == 0; i += packet_size)
433 ret = regmap_noinc_read(st->map, INV_ICM45600_REG_FIFO_DATA,
434 &st->fifo.data[i], packet_size);
435 }
436 if (ret)
437 return ret;
438
439 for (i = 0; i < st->fifo.count; i += size) {
440 size = inv_icm45600_fifo_decode_packet(&st->fifo.data[i], &accel, &gyro,
441 &temp, ×tamp, &odr);
442 if (size <= 0)
443 /* No more sample in buffer */
444 break;
445 if (gyro && inv_icm45600_fifo_is_data_valid(gyro))
446 st->fifo.nb.gyro++;
447 if (accel && inv_icm45600_fifo_is_data_valid(accel))
448 st->fifo.nb.accel++;
449 st->fifo.nb.total++;
450 }
451
452 return 0;
453 }
454
inv_icm45600_buffer_fifo_parse(struct inv_icm45600_state * st)455 int inv_icm45600_buffer_fifo_parse(struct inv_icm45600_state *st)
456 {
457 struct inv_icm45600_sensor_state *gyro_st = iio_priv(st->indio_gyro);
458 struct inv_icm45600_sensor_state *accel_st = iio_priv(st->indio_accel);
459 struct inv_sensors_timestamp *ts;
460 int ret;
461
462 if (st->fifo.nb.total == 0)
463 return 0;
464
465 /* Handle gyroscope timestamp and FIFO data parsing. */
466 if (st->fifo.nb.gyro > 0) {
467 ts = &gyro_st->ts;
468 inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_gyro,
469 st->timestamp.gyro);
470 ret = inv_icm45600_gyro_parse_fifo(st->indio_gyro);
471 if (ret)
472 return ret;
473 }
474
475 /* Handle accelerometer timestamp and FIFO data parsing. */
476 if (st->fifo.nb.accel > 0) {
477 ts = &accel_st->ts;
478 inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_accel,
479 st->timestamp.accel);
480 ret = inv_icm45600_accel_parse_fifo(st->indio_accel);
481 if (ret)
482 return ret;
483 }
484
485 return 0;
486 }
487
inv_icm45600_buffer_hwfifo_flush(struct inv_icm45600_state * st,unsigned int count)488 int inv_icm45600_buffer_hwfifo_flush(struct inv_icm45600_state *st,
489 unsigned int count)
490 {
491 struct inv_icm45600_sensor_state *gyro_st = iio_priv(st->indio_gyro);
492 struct inv_icm45600_sensor_state *accel_st = iio_priv(st->indio_accel);
493 struct inv_sensors_timestamp *ts;
494 s64 gyro_ts, accel_ts;
495 int ret;
496
497 gyro_ts = iio_get_time_ns(st->indio_gyro);
498 accel_ts = iio_get_time_ns(st->indio_accel);
499
500 ret = inv_icm45600_buffer_fifo_read(st, count);
501 if (ret)
502 return ret;
503
504 if (st->fifo.nb.total == 0)
505 return 0;
506
507 if (st->fifo.nb.gyro > 0) {
508 ts = &gyro_st->ts;
509 inv_sensors_timestamp_interrupt(ts, st->fifo.nb.gyro, gyro_ts);
510 ret = inv_icm45600_gyro_parse_fifo(st->indio_gyro);
511 if (ret)
512 return ret;
513 }
514
515 if (st->fifo.nb.accel > 0) {
516 ts = &accel_st->ts;
517 inv_sensors_timestamp_interrupt(ts, st->fifo.nb.accel, accel_ts);
518 ret = inv_icm45600_accel_parse_fifo(st->indio_accel);
519 if (ret)
520 return ret;
521 }
522
523 return 0;
524 }
525
inv_icm45600_buffer_init(struct inv_icm45600_state * st)526 int inv_icm45600_buffer_init(struct inv_icm45600_state *st)
527 {
528 int ret;
529 unsigned int val;
530
531 st->fifo.watermark.eff_gyro = 1;
532 st->fifo.watermark.eff_accel = 1;
533
534 /* Disable all FIFO EN bits. */
535 ret = regmap_write(st->map, INV_ICM45600_REG_FIFO_CONFIG3, 0);
536 if (ret)
537 return ret;
538
539 /* Disable FIFO and set depth. */
540 val = FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK,
541 INV_ICM45600_FIFO_CONFIG0_MODE_BYPASS) |
542 FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_FIFO_DEPTH_MASK,
543 INV_ICM45600_FIFO_CONFIG0_FIFO_DEPTH_MAX);
544
545 ret = regmap_write(st->map, INV_ICM45600_REG_FIFO_CONFIG0, val);
546 if (ret)
547 return ret;
548
549 /* Enable only timestamp in fifo, disable compression. */
550 ret = regmap_write(st->map, INV_ICM45600_REG_FIFO_CONFIG4,
551 INV_ICM45600_FIFO_CONFIG4_TMST_FSYNC_EN);
552 if (ret)
553 return ret;
554
555 /* Enable FIFO continuous watermark interrupt. */
556 return regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG2,
557 INV_ICM45600_REG_FIFO_CONFIG2_WM_GT_TH);
558 }
559