1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * Copyright (C) 2020 Invensense, Inc. |
4 | */ |
5 | |
6 | #include <linux/kernel.h> |
7 | #include <linux/device.h> |
8 | #include <linux/mutex.h> |
9 | #include <linux/pm_runtime.h> |
10 | #include <linux/regmap.h> |
11 | #include <linux/delay.h> |
12 | |
13 | #include <linux/iio/buffer.h> |
14 | #include <linux/iio/common/inv_sensors_timestamp.h> |
15 | #include <linux/iio/iio.h> |
16 | |
17 | #include "inv_icm42600.h" |
18 | #include "inv_icm42600_buffer.h" |
19 | |
20 | /* FIFO header: 1 byte */ |
21 | #define BIT(7) |
22 | #define BIT(6) |
23 | #define BIT(5) |
24 | #define GENMASK(3, 2) |
25 | #define BIT(1) |
26 | #define BIT(0) |
27 | |
28 | struct inv_icm42600_fifo_1sensor_packet { |
29 | uint8_t ; |
30 | struct inv_icm42600_fifo_sensor_data data; |
31 | int8_t temp; |
32 | } __packed; |
33 | #define INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE 8 |
34 | |
35 | struct inv_icm42600_fifo_2sensors_packet { |
36 | uint8_t ; |
37 | struct inv_icm42600_fifo_sensor_data accel; |
38 | struct inv_icm42600_fifo_sensor_data gyro; |
39 | int8_t temp; |
40 | __be16 timestamp; |
41 | } __packed; |
42 | #define INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE 16 |
43 | |
44 | ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel, |
45 | const void **gyro, const int8_t **temp, |
46 | const void **timestamp, unsigned int *odr) |
47 | { |
48 | const struct inv_icm42600_fifo_1sensor_packet *pack1 = packet; |
49 | const struct inv_icm42600_fifo_2sensors_packet *pack2 = packet; |
50 | uint8_t = *((const uint8_t *)packet); |
51 | |
52 | /* FIFO empty */ |
53 | if (header & INV_ICM42600_FIFO_HEADER_MSG) { |
54 | *accel = NULL; |
55 | *gyro = NULL; |
56 | *temp = NULL; |
57 | *timestamp = NULL; |
58 | *odr = 0; |
59 | return 0; |
60 | } |
61 | |
62 | /* handle odr flags */ |
63 | *odr = 0; |
64 | if (header & INV_ICM42600_FIFO_HEADER_ODR_GYRO) |
65 | *odr |= INV_ICM42600_SENSOR_GYRO; |
66 | if (header & INV_ICM42600_FIFO_HEADER_ODR_ACCEL) |
67 | *odr |= INV_ICM42600_SENSOR_ACCEL; |
68 | |
69 | /* accel + gyro */ |
70 | if ((header & INV_ICM42600_FIFO_HEADER_ACCEL) && |
71 | (header & INV_ICM42600_FIFO_HEADER_GYRO)) { |
72 | *accel = &pack2->accel; |
73 | *gyro = &pack2->gyro; |
74 | *temp = &pack2->temp; |
75 | *timestamp = &pack2->timestamp; |
76 | return INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE; |
77 | } |
78 | |
79 | /* accel only */ |
80 | if (header & INV_ICM42600_FIFO_HEADER_ACCEL) { |
81 | *accel = &pack1->data; |
82 | *gyro = NULL; |
83 | *temp = &pack1->temp; |
84 | *timestamp = NULL; |
85 | return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE; |
86 | } |
87 | |
88 | /* gyro only */ |
89 | if (header & INV_ICM42600_FIFO_HEADER_GYRO) { |
90 | *accel = NULL; |
91 | *gyro = &pack1->data; |
92 | *temp = &pack1->temp; |
93 | *timestamp = NULL; |
94 | return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE; |
95 | } |
96 | |
97 | /* invalid packet if here */ |
98 | return -EINVAL; |
99 | } |
100 | |
101 | void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st) |
102 | { |
103 | uint32_t period_gyro, period_accel, period; |
104 | |
105 | if (st->fifo.en & INV_ICM42600_SENSOR_GYRO) |
106 | period_gyro = inv_icm42600_odr_to_period(odr: st->conf.gyro.odr); |
107 | else |
108 | period_gyro = U32_MAX; |
109 | |
110 | if (st->fifo.en & INV_ICM42600_SENSOR_ACCEL) |
111 | period_accel = inv_icm42600_odr_to_period(odr: st->conf.accel.odr); |
112 | else |
113 | period_accel = U32_MAX; |
114 | |
115 | if (period_gyro <= period_accel) |
116 | period = period_gyro; |
117 | else |
118 | period = period_accel; |
119 | |
120 | st->fifo.period = period; |
121 | } |
122 | |
123 | int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st, |
124 | unsigned int fifo_en) |
125 | { |
126 | unsigned int mask, val; |
127 | int ret; |
128 | |
129 | /* update only FIFO EN bits */ |
130 | mask = INV_ICM42600_FIFO_CONFIG1_TMST_FSYNC_EN | |
131 | INV_ICM42600_FIFO_CONFIG1_TEMP_EN | |
132 | INV_ICM42600_FIFO_CONFIG1_GYRO_EN | |
133 | INV_ICM42600_FIFO_CONFIG1_ACCEL_EN; |
134 | |
135 | val = 0; |
136 | if (fifo_en & INV_ICM42600_SENSOR_GYRO) |
137 | val |= INV_ICM42600_FIFO_CONFIG1_GYRO_EN; |
138 | if (fifo_en & INV_ICM42600_SENSOR_ACCEL) |
139 | val |= INV_ICM42600_FIFO_CONFIG1_ACCEL_EN; |
140 | if (fifo_en & INV_ICM42600_SENSOR_TEMP) |
141 | val |= INV_ICM42600_FIFO_CONFIG1_TEMP_EN; |
142 | |
143 | ret = regmap_update_bits(map: st->map, INV_ICM42600_REG_FIFO_CONFIG1, mask, val); |
144 | if (ret) |
145 | return ret; |
146 | |
147 | st->fifo.en = fifo_en; |
148 | inv_icm42600_buffer_update_fifo_period(st); |
149 | |
150 | return 0; |
151 | } |
152 | |
153 | static size_t inv_icm42600_get_packet_size(unsigned int fifo_en) |
154 | { |
155 | size_t packet_size; |
156 | |
157 | if ((fifo_en & INV_ICM42600_SENSOR_GYRO) && |
158 | (fifo_en & INV_ICM42600_SENSOR_ACCEL)) |
159 | packet_size = INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE; |
160 | else |
161 | packet_size = INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE; |
162 | |
163 | return packet_size; |
164 | } |
165 | |
166 | static unsigned int inv_icm42600_wm_truncate(unsigned int watermark, |
167 | size_t packet_size) |
168 | { |
169 | size_t wm_size; |
170 | unsigned int wm; |
171 | |
172 | wm_size = watermark * packet_size; |
173 | if (wm_size > INV_ICM42600_FIFO_WATERMARK_MAX) |
174 | wm_size = INV_ICM42600_FIFO_WATERMARK_MAX; |
175 | |
176 | wm = wm_size / packet_size; |
177 | |
178 | return wm; |
179 | } |
180 | |
181 | /** |
182 | * inv_icm42600_buffer_update_watermark - update watermark FIFO threshold |
183 | * @st: driver internal state |
184 | * |
185 | * Returns 0 on success, a negative error code otherwise. |
186 | * |
187 | * FIFO watermark threshold is computed based on the required watermark values |
188 | * set for gyro and accel sensors. Since watermark is all about acceptable data |
189 | * latency, use the smallest setting between the 2. It means choosing the |
190 | * smallest latency but this is not as simple as choosing the smallest watermark |
191 | * value. Latency depends on watermark and ODR. It requires several steps: |
192 | * 1) compute gyro and accel latencies and choose the smallest value. |
193 | * 2) adapt the choosen latency so that it is a multiple of both gyro and accel |
194 | * ones. Otherwise it is possible that you don't meet a requirement. (for |
195 | * example with gyro @100Hz wm 4 and accel @100Hz with wm 6, choosing the |
196 | * value of 4 will not meet accel latency requirement because 6 is not a |
197 | * multiple of 4. You need to use the value 2.) |
198 | * 3) Since all periods are multiple of each others, watermark is computed by |
199 | * dividing this computed latency by the smallest period, which corresponds |
200 | * to the FIFO frequency. Beware that this is only true because we are not |
201 | * using 500Hz frequency which is not a multiple of the others. |
202 | */ |
203 | int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st) |
204 | { |
205 | size_t packet_size, wm_size; |
206 | unsigned int wm_gyro, wm_accel, watermark; |
207 | uint32_t period_gyro, period_accel, period; |
208 | uint32_t latency_gyro, latency_accel, latency; |
209 | bool restore; |
210 | __le16 raw_wm; |
211 | int ret; |
212 | |
213 | packet_size = inv_icm42600_get_packet_size(fifo_en: st->fifo.en); |
214 | |
215 | /* compute sensors latency, depending on sensor watermark and odr */ |
216 | wm_gyro = inv_icm42600_wm_truncate(watermark: st->fifo.watermark.gyro, packet_size); |
217 | wm_accel = inv_icm42600_wm_truncate(watermark: st->fifo.watermark.accel, packet_size); |
218 | /* use us for odr to avoid overflow using 32 bits values */ |
219 | period_gyro = inv_icm42600_odr_to_period(odr: st->conf.gyro.odr) / 1000UL; |
220 | period_accel = inv_icm42600_odr_to_period(odr: st->conf.accel.odr) / 1000UL; |
221 | latency_gyro = period_gyro * wm_gyro; |
222 | latency_accel = period_accel * wm_accel; |
223 | |
224 | /* 0 value for watermark means that the sensor is turned off */ |
225 | if (latency_gyro == 0) { |
226 | watermark = wm_accel; |
227 | } else if (latency_accel == 0) { |
228 | watermark = wm_gyro; |
229 | } else { |
230 | /* compute the smallest latency that is a multiple of both */ |
231 | if (latency_gyro <= latency_accel) |
232 | latency = latency_gyro - (latency_accel % latency_gyro); |
233 | else |
234 | latency = latency_accel - (latency_gyro % latency_accel); |
235 | /* use the shortest period */ |
236 | if (period_gyro <= period_accel) |
237 | period = period_gyro; |
238 | else |
239 | period = period_accel; |
240 | /* all this works because periods are multiple of each others */ |
241 | watermark = latency / period; |
242 | if (watermark < 1) |
243 | watermark = 1; |
244 | } |
245 | |
246 | /* compute watermark value in bytes */ |
247 | wm_size = watermark * packet_size; |
248 | |
249 | /* changing FIFO watermark requires to turn off watermark interrupt */ |
250 | ret = regmap_update_bits_check(map: st->map, INV_ICM42600_REG_INT_SOURCE0, |
251 | INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, |
252 | val: 0, change: &restore); |
253 | if (ret) |
254 | return ret; |
255 | |
256 | raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size); |
257 | memcpy(st->buffer, &raw_wm, sizeof(raw_wm)); |
258 | ret = regmap_bulk_write(map: st->map, INV_ICM42600_REG_FIFO_WATERMARK, |
259 | val: st->buffer, val_count: sizeof(raw_wm)); |
260 | if (ret) |
261 | return ret; |
262 | |
263 | /* restore watermark interrupt */ |
264 | if (restore) { |
265 | ret = regmap_update_bits(map: st->map, INV_ICM42600_REG_INT_SOURCE0, |
266 | INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, |
267 | INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN); |
268 | if (ret) |
269 | return ret; |
270 | } |
271 | |
272 | return 0; |
273 | } |
274 | |
275 | static int inv_icm42600_buffer_preenable(struct iio_dev *indio_dev) |
276 | { |
277 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
278 | struct device *dev = regmap_get_device(map: st->map); |
279 | struct inv_sensors_timestamp *ts = iio_priv(indio_dev); |
280 | |
281 | pm_runtime_get_sync(dev); |
282 | |
283 | mutex_lock(&st->lock); |
284 | inv_sensors_timestamp_reset(ts); |
285 | mutex_unlock(lock: &st->lock); |
286 | |
287 | return 0; |
288 | } |
289 | |
290 | /* |
291 | * update_scan_mode callback is turning sensors on and setting data FIFO enable |
292 | * bits. |
293 | */ |
294 | static int inv_icm42600_buffer_postenable(struct iio_dev *indio_dev) |
295 | { |
296 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
297 | int ret; |
298 | |
299 | mutex_lock(&st->lock); |
300 | |
301 | /* exit if FIFO is already on */ |
302 | if (st->fifo.on) { |
303 | ret = 0; |
304 | goto out_on; |
305 | } |
306 | |
307 | /* set FIFO threshold interrupt */ |
308 | ret = regmap_update_bits(map: st->map, INV_ICM42600_REG_INT_SOURCE0, |
309 | INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, |
310 | INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN); |
311 | if (ret) |
312 | goto out_unlock; |
313 | |
314 | /* flush FIFO data */ |
315 | ret = regmap_write(map: st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET, |
316 | INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH); |
317 | if (ret) |
318 | goto out_unlock; |
319 | |
320 | /* set FIFO in streaming mode */ |
321 | ret = regmap_write(map: st->map, INV_ICM42600_REG_FIFO_CONFIG, |
322 | INV_ICM42600_FIFO_CONFIG_STREAM); |
323 | if (ret) |
324 | goto out_unlock; |
325 | |
326 | /* workaround: first read of FIFO count after reset is always 0 */ |
327 | ret = regmap_bulk_read(map: st->map, INV_ICM42600_REG_FIFO_COUNT, val: st->buffer, val_count: 2); |
328 | if (ret) |
329 | goto out_unlock; |
330 | |
331 | out_on: |
332 | /* increase FIFO on counter */ |
333 | st->fifo.on++; |
334 | out_unlock: |
335 | mutex_unlock(lock: &st->lock); |
336 | return ret; |
337 | } |
338 | |
339 | static int inv_icm42600_buffer_predisable(struct iio_dev *indio_dev) |
340 | { |
341 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
342 | int ret; |
343 | |
344 | mutex_lock(&st->lock); |
345 | |
346 | /* exit if there are several sensors using the FIFO */ |
347 | if (st->fifo.on > 1) { |
348 | ret = 0; |
349 | goto out_off; |
350 | } |
351 | |
352 | /* set FIFO in bypass mode */ |
353 | ret = regmap_write(map: st->map, INV_ICM42600_REG_FIFO_CONFIG, |
354 | INV_ICM42600_FIFO_CONFIG_BYPASS); |
355 | if (ret) |
356 | goto out_unlock; |
357 | |
358 | /* flush FIFO data */ |
359 | ret = regmap_write(map: st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET, |
360 | INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH); |
361 | if (ret) |
362 | goto out_unlock; |
363 | |
364 | /* disable FIFO threshold interrupt */ |
365 | ret = regmap_update_bits(map: st->map, INV_ICM42600_REG_INT_SOURCE0, |
366 | INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, val: 0); |
367 | if (ret) |
368 | goto out_unlock; |
369 | |
370 | out_off: |
371 | /* decrease FIFO on counter */ |
372 | st->fifo.on--; |
373 | out_unlock: |
374 | mutex_unlock(lock: &st->lock); |
375 | return ret; |
376 | } |
377 | |
378 | static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev) |
379 | { |
380 | struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); |
381 | struct device *dev = regmap_get_device(map: st->map); |
382 | unsigned int sensor; |
383 | unsigned int *watermark; |
384 | struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; |
385 | unsigned int sleep_temp = 0; |
386 | unsigned int sleep_sensor = 0; |
387 | unsigned int sleep; |
388 | int ret; |
389 | |
390 | if (indio_dev == st->indio_gyro) { |
391 | sensor = INV_ICM42600_SENSOR_GYRO; |
392 | watermark = &st->fifo.watermark.gyro; |
393 | } else if (indio_dev == st->indio_accel) { |
394 | sensor = INV_ICM42600_SENSOR_ACCEL; |
395 | watermark = &st->fifo.watermark.accel; |
396 | } else { |
397 | return -EINVAL; |
398 | } |
399 | |
400 | mutex_lock(&st->lock); |
401 | |
402 | ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en: st->fifo.en & ~sensor); |
403 | if (ret) |
404 | goto out_unlock; |
405 | |
406 | *watermark = 0; |
407 | ret = inv_icm42600_buffer_update_watermark(st); |
408 | if (ret) |
409 | goto out_unlock; |
410 | |
411 | conf.mode = INV_ICM42600_SENSOR_MODE_OFF; |
412 | if (sensor == INV_ICM42600_SENSOR_GYRO) |
413 | ret = inv_icm42600_set_gyro_conf(st, conf: &conf, sleep_ms: &sleep_sensor); |
414 | else |
415 | ret = inv_icm42600_set_accel_conf(st, conf: &conf, sleep_ms: &sleep_sensor); |
416 | if (ret) |
417 | goto out_unlock; |
418 | |
419 | /* if FIFO is off, turn temperature off */ |
420 | if (!st->fifo.on) |
421 | ret = inv_icm42600_set_temp_conf(st, enable: false, sleep_ms: &sleep_temp); |
422 | |
423 | out_unlock: |
424 | mutex_unlock(lock: &st->lock); |
425 | |
426 | /* sleep maximum required time */ |
427 | sleep = max(sleep_sensor, sleep_temp); |
428 | if (sleep) |
429 | msleep(msecs: sleep); |
430 | |
431 | pm_runtime_mark_last_busy(dev); |
432 | pm_runtime_put_autosuspend(dev); |
433 | |
434 | return ret; |
435 | } |
436 | |
437 | const struct iio_buffer_setup_ops inv_icm42600_buffer_ops = { |
438 | .preenable = inv_icm42600_buffer_preenable, |
439 | .postenable = inv_icm42600_buffer_postenable, |
440 | .predisable = inv_icm42600_buffer_predisable, |
441 | .postdisable = inv_icm42600_buffer_postdisable, |
442 | }; |
443 | |
444 | int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st, |
445 | unsigned int max) |
446 | { |
447 | size_t max_count; |
448 | __be16 *raw_fifo_count; |
449 | ssize_t i, size; |
450 | const void *accel, *gyro, *timestamp; |
451 | const int8_t *temp; |
452 | unsigned int odr; |
453 | int ret; |
454 | |
455 | /* reset all samples counters */ |
456 | st->fifo.count = 0; |
457 | st->fifo.nb.gyro = 0; |
458 | st->fifo.nb.accel = 0; |
459 | st->fifo.nb.total = 0; |
460 | |
461 | /* compute maximum FIFO read size */ |
462 | if (max == 0) |
463 | max_count = sizeof(st->fifo.data); |
464 | else |
465 | max_count = max * inv_icm42600_get_packet_size(fifo_en: st->fifo.en); |
466 | |
467 | /* read FIFO count value */ |
468 | raw_fifo_count = (__be16 *)st->buffer; |
469 | ret = regmap_bulk_read(map: st->map, INV_ICM42600_REG_FIFO_COUNT, |
470 | val: raw_fifo_count, val_count: sizeof(*raw_fifo_count)); |
471 | if (ret) |
472 | return ret; |
473 | st->fifo.count = be16_to_cpup(p: raw_fifo_count); |
474 | |
475 | /* check and clamp FIFO count value */ |
476 | if (st->fifo.count == 0) |
477 | return 0; |
478 | if (st->fifo.count > max_count) |
479 | st->fifo.count = max_count; |
480 | |
481 | /* read all FIFO data in internal buffer */ |
482 | ret = regmap_noinc_read(map: st->map, INV_ICM42600_REG_FIFO_DATA, |
483 | val: st->fifo.data, val_len: st->fifo.count); |
484 | if (ret) |
485 | return ret; |
486 | |
487 | /* compute number of samples for each sensor */ |
488 | for (i = 0; i < st->fifo.count; i += size) { |
489 | size = inv_icm42600_fifo_decode_packet(packet: &st->fifo.data[i], |
490 | accel: &accel, gyro: &gyro, temp: &temp, timestamp: ×tamp, odr: &odr); |
491 | if (size <= 0) |
492 | break; |
493 | if (gyro != NULL && inv_icm42600_fifo_is_data_valid(s: gyro)) |
494 | st->fifo.nb.gyro++; |
495 | if (accel != NULL && inv_icm42600_fifo_is_data_valid(s: accel)) |
496 | st->fifo.nb.accel++; |
497 | st->fifo.nb.total++; |
498 | } |
499 | |
500 | return 0; |
501 | } |
502 | |
503 | int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st) |
504 | { |
505 | struct inv_sensors_timestamp *ts; |
506 | int ret; |
507 | |
508 | if (st->fifo.nb.total == 0) |
509 | return 0; |
510 | |
511 | /* handle gyroscope timestamp and FIFO data parsing */ |
512 | ts = iio_priv(indio_dev: st->indio_gyro); |
513 | inv_sensors_timestamp_interrupt(ts, fifo_period: st->fifo.period, fifo_nb: st->fifo.nb.total, |
514 | sensor_nb: st->fifo.nb.gyro, timestamp: st->timestamp.gyro); |
515 | if (st->fifo.nb.gyro > 0) { |
516 | ret = inv_icm42600_gyro_parse_fifo(indio_dev: st->indio_gyro); |
517 | if (ret) |
518 | return ret; |
519 | } |
520 | |
521 | /* handle accelerometer timestamp and FIFO data parsing */ |
522 | ts = iio_priv(indio_dev: st->indio_accel); |
523 | inv_sensors_timestamp_interrupt(ts, fifo_period: st->fifo.period, fifo_nb: st->fifo.nb.total, |
524 | sensor_nb: st->fifo.nb.accel, timestamp: st->timestamp.accel); |
525 | if (st->fifo.nb.accel > 0) { |
526 | ret = inv_icm42600_accel_parse_fifo(indio_dev: st->indio_accel); |
527 | if (ret) |
528 | return ret; |
529 | } |
530 | |
531 | return 0; |
532 | } |
533 | |
534 | int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st, |
535 | unsigned int count) |
536 | { |
537 | struct inv_sensors_timestamp *ts; |
538 | int64_t gyro_ts, accel_ts; |
539 | int ret; |
540 | |
541 | gyro_ts = iio_get_time_ns(indio_dev: st->indio_gyro); |
542 | accel_ts = iio_get_time_ns(indio_dev: st->indio_accel); |
543 | |
544 | ret = inv_icm42600_buffer_fifo_read(st, max: count); |
545 | if (ret) |
546 | return ret; |
547 | |
548 | if (st->fifo.nb.total == 0) |
549 | return 0; |
550 | |
551 | if (st->fifo.nb.gyro > 0) { |
552 | ts = iio_priv(indio_dev: st->indio_gyro); |
553 | inv_sensors_timestamp_interrupt(ts, fifo_period: st->fifo.period, |
554 | fifo_nb: st->fifo.nb.total, sensor_nb: st->fifo.nb.gyro, |
555 | timestamp: gyro_ts); |
556 | ret = inv_icm42600_gyro_parse_fifo(indio_dev: st->indio_gyro); |
557 | if (ret) |
558 | return ret; |
559 | } |
560 | |
561 | if (st->fifo.nb.accel > 0) { |
562 | ts = iio_priv(indio_dev: st->indio_accel); |
563 | inv_sensors_timestamp_interrupt(ts, fifo_period: st->fifo.period, |
564 | fifo_nb: st->fifo.nb.total, sensor_nb: st->fifo.nb.accel, |
565 | timestamp: accel_ts); |
566 | ret = inv_icm42600_accel_parse_fifo(indio_dev: st->indio_accel); |
567 | if (ret) |
568 | return ret; |
569 | } |
570 | |
571 | return 0; |
572 | } |
573 | |
574 | int inv_icm42600_buffer_init(struct inv_icm42600_state *st) |
575 | { |
576 | unsigned int val; |
577 | int ret; |
578 | |
579 | /* |
580 | * Default FIFO configuration (bits 7 to 5) |
581 | * - use invalid value |
582 | * - FIFO count in bytes |
583 | * - FIFO count in big endian |
584 | */ |
585 | val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN; |
586 | ret = regmap_update_bits(map: st->map, INV_ICM42600_REG_INTF_CONFIG0, |
587 | GENMASK(7, 5), val); |
588 | if (ret) |
589 | return ret; |
590 | |
591 | /* |
592 | * Enable FIFO partial read and continuous watermark interrupt. |
593 | * Disable all FIFO EN bits. |
594 | */ |
595 | val = INV_ICM42600_FIFO_CONFIG1_RESUME_PARTIAL_RD | |
596 | INV_ICM42600_FIFO_CONFIG1_WM_GT_TH; |
597 | return regmap_update_bits(map: st->map, INV_ICM42600_REG_FIFO_CONFIG1, |
598 | GENMASK(6, 5) | GENMASK(3, 0), val); |
599 | } |
600 | |