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 #include <linux/math64.h>
13 #include <linux/iio/iio.h>
14 #include <linux/iio/buffer.h>
15 #include <linux/iio/kfifo_buf.h>
16 
17 #include "inv_icm42600.h"
18 #include "inv_icm42600_temp.h"
19 #include "inv_icm42600_buffer.h"
20 #include "inv_icm42600_timestamp.h"
21 
22 #define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info)		\
23 	{								\
24 		.type = IIO_ANGL_VEL,					\
25 		.modified = 1,						\
26 		.channel2 = _modifier,					\
27 		.info_mask_separate =					\
28 			BIT(IIO_CHAN_INFO_RAW) |			\
29 			BIT(IIO_CHAN_INFO_CALIBBIAS),			\
30 		.info_mask_shared_by_type =				\
31 			BIT(IIO_CHAN_INFO_SCALE),			\
32 		.info_mask_shared_by_type_available =			\
33 			BIT(IIO_CHAN_INFO_SCALE) |			\
34 			BIT(IIO_CHAN_INFO_CALIBBIAS),			\
35 		.info_mask_shared_by_all =				\
36 			BIT(IIO_CHAN_INFO_SAMP_FREQ),			\
37 		.info_mask_shared_by_all_available =			\
38 			BIT(IIO_CHAN_INFO_SAMP_FREQ),			\
39 		.scan_index = _index,					\
40 		.scan_type = {						\
41 			.sign = 's',					\
42 			.realbits = 16,					\
43 			.storagebits = 16,				\
44 			.endianness = IIO_BE,				\
45 		},							\
46 		.ext_info = _ext_info,					\
47 	}
48 
49 enum inv_icm42600_gyro_scan {
50 	INV_ICM42600_GYRO_SCAN_X,
51 	INV_ICM42600_GYRO_SCAN_Y,
52 	INV_ICM42600_GYRO_SCAN_Z,
53 	INV_ICM42600_GYRO_SCAN_TEMP,
54 	INV_ICM42600_GYRO_SCAN_TIMESTAMP,
55 };
56 
57 static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = {
58 	IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix),
59 	{},
60 };
61 
62 static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
63 	INV_ICM42600_GYRO_CHAN(IIO_MOD_X, INV_ICM42600_GYRO_SCAN_X,
64 			       inv_icm42600_gyro_ext_infos),
65 	INV_ICM42600_GYRO_CHAN(IIO_MOD_Y, INV_ICM42600_GYRO_SCAN_Y,
66 			       inv_icm42600_gyro_ext_infos),
67 	INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z,
68 			       inv_icm42600_gyro_ext_infos),
69 	INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
70 	IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
71 };
72 
73 /*
74  * IIO buffer data: size must be a power of 2 and timestamp aligned
75  * 16 bytes: 6 bytes angular velocity, 2 bytes temperature, 8 bytes timestamp
76  */
77 struct inv_icm42600_gyro_buffer {
78 	struct inv_icm42600_fifo_sensor_data gyro;
79 	int16_t temp;
80 	int64_t timestamp __aligned(8);
81 };
82 
83 #define INV_ICM42600_SCAN_MASK_GYRO_3AXIS				\
84 	(BIT(INV_ICM42600_GYRO_SCAN_X) |				\
85 	BIT(INV_ICM42600_GYRO_SCAN_Y) |					\
86 	BIT(INV_ICM42600_GYRO_SCAN_Z))
87 
88 #define INV_ICM42600_SCAN_MASK_TEMP	BIT(INV_ICM42600_GYRO_SCAN_TEMP)
89 
90 static const unsigned long inv_icm42600_gyro_scan_masks[] = {
91 	/* 3-axis gyro + temperature */
92 	INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
93 	0,
94 };
95 
96 /* enable gyroscope sensor and FIFO write */
97 static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
98 					      const unsigned long *scan_mask)
99 {
100 	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
101 	struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
102 	struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
103 	unsigned int fifo_en = 0;
104 	unsigned int sleep_gyro = 0;
105 	unsigned int sleep_temp = 0;
106 	unsigned int sleep;
107 	int ret;
108 
109 	mutex_lock(&st->lock);
110 
111 	if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
112 		/* enable temp sensor */
113 		ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
114 		if (ret)
115 			goto out_unlock;
116 		fifo_en |= INV_ICM42600_SENSOR_TEMP;
117 	}
118 
119 	if (*scan_mask & INV_ICM42600_SCAN_MASK_GYRO_3AXIS) {
120 		/* enable gyro sensor */
121 		conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
122 		ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_gyro);
123 		if (ret)
124 			goto out_unlock;
125 		fifo_en |= INV_ICM42600_SENSOR_GYRO;
126 	}
127 
128 	/* update data FIFO write */
129 	inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0);
130 	ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
131 	if (ret)
132 		goto out_unlock;
133 
134 	ret = inv_icm42600_buffer_update_watermark(st);
135 
136 out_unlock:
137 	mutex_unlock(&st->lock);
138 	/* sleep maximum required time */
139 	if (sleep_gyro > sleep_temp)
140 		sleep = sleep_gyro;
141 	else
142 		sleep = sleep_temp;
143 	if (sleep)
144 		msleep(sleep);
145 	return ret;
146 }
147 
148 static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
149 					 struct iio_chan_spec const *chan,
150 					 int16_t *val)
151 {
152 	struct device *dev = regmap_get_device(st->map);
153 	struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
154 	unsigned int reg;
155 	__be16 *data;
156 	int ret;
157 
158 	if (chan->type != IIO_ANGL_VEL)
159 		return -EINVAL;
160 
161 	switch (chan->channel2) {
162 	case IIO_MOD_X:
163 		reg = INV_ICM42600_REG_GYRO_DATA_X;
164 		break;
165 	case IIO_MOD_Y:
166 		reg = INV_ICM42600_REG_GYRO_DATA_Y;
167 		break;
168 	case IIO_MOD_Z:
169 		reg = INV_ICM42600_REG_GYRO_DATA_Z;
170 		break;
171 	default:
172 		return -EINVAL;
173 	}
174 
175 	pm_runtime_get_sync(dev);
176 	mutex_lock(&st->lock);
177 
178 	/* enable gyro sensor */
179 	conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
180 	ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
181 	if (ret)
182 		goto exit;
183 
184 	/* read gyro register data */
185 	data = (__be16 *)&st->buffer[0];
186 	ret = regmap_bulk_read(st->map, reg, data, sizeof(*data));
187 	if (ret)
188 		goto exit;
189 
190 	*val = (int16_t)be16_to_cpup(data);
191 	if (*val == INV_ICM42600_DATA_INVALID)
192 		ret = -EINVAL;
193 exit:
194 	mutex_unlock(&st->lock);
195 	pm_runtime_mark_last_busy(dev);
196 	pm_runtime_put_autosuspend(dev);
197 	return ret;
198 }
199 
200 /* IIO format int + nano */
201 static const int inv_icm42600_gyro_scale[] = {
202 	/* +/- 2000dps => 0.001065264 rad/s */
203 	[2 * INV_ICM42600_GYRO_FS_2000DPS] = 0,
204 	[2 * INV_ICM42600_GYRO_FS_2000DPS + 1] = 1065264,
205 	/* +/- 1000dps => 0.000532632 rad/s */
206 	[2 * INV_ICM42600_GYRO_FS_1000DPS] = 0,
207 	[2 * INV_ICM42600_GYRO_FS_1000DPS + 1] = 532632,
208 	/* +/- 500dps => 0.000266316 rad/s */
209 	[2 * INV_ICM42600_GYRO_FS_500DPS] = 0,
210 	[2 * INV_ICM42600_GYRO_FS_500DPS + 1] = 266316,
211 	/* +/- 250dps => 0.000133158 rad/s */
212 	[2 * INV_ICM42600_GYRO_FS_250DPS] = 0,
213 	[2 * INV_ICM42600_GYRO_FS_250DPS + 1] = 133158,
214 	/* +/- 125dps => 0.000066579 rad/s */
215 	[2 * INV_ICM42600_GYRO_FS_125DPS] = 0,
216 	[2 * INV_ICM42600_GYRO_FS_125DPS + 1] = 66579,
217 	/* +/- 62.5dps => 0.000033290 rad/s */
218 	[2 * INV_ICM42600_GYRO_FS_62_5DPS] = 0,
219 	[2 * INV_ICM42600_GYRO_FS_62_5DPS + 1] = 33290,
220 	/* +/- 31.25dps => 0.000016645 rad/s */
221 	[2 * INV_ICM42600_GYRO_FS_31_25DPS] = 0,
222 	[2 * INV_ICM42600_GYRO_FS_31_25DPS + 1] = 16645,
223 	/* +/- 15.625dps => 0.000008322 rad/s */
224 	[2 * INV_ICM42600_GYRO_FS_15_625DPS] = 0,
225 	[2 * INV_ICM42600_GYRO_FS_15_625DPS + 1] = 8322,
226 };
227 
228 static int inv_icm42600_gyro_read_scale(struct inv_icm42600_state *st,
229 					int *val, int *val2)
230 {
231 	unsigned int idx;
232 
233 	idx = st->conf.gyro.fs;
234 
235 	*val = inv_icm42600_gyro_scale[2 * idx];
236 	*val2 = inv_icm42600_gyro_scale[2 * idx + 1];
237 	return IIO_VAL_INT_PLUS_NANO;
238 }
239 
240 static int inv_icm42600_gyro_write_scale(struct inv_icm42600_state *st,
241 					 int val, int val2)
242 {
243 	struct device *dev = regmap_get_device(st->map);
244 	unsigned int idx;
245 	struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
246 	int ret;
247 
248 	for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_scale); idx += 2) {
249 		if (val == inv_icm42600_gyro_scale[idx] &&
250 		    val2 == inv_icm42600_gyro_scale[idx + 1])
251 			break;
252 	}
253 	if (idx >= ARRAY_SIZE(inv_icm42600_gyro_scale))
254 		return -EINVAL;
255 
256 	conf.fs = idx / 2;
257 
258 	pm_runtime_get_sync(dev);
259 	mutex_lock(&st->lock);
260 
261 	ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
262 
263 	mutex_unlock(&st->lock);
264 	pm_runtime_mark_last_busy(dev);
265 	pm_runtime_put_autosuspend(dev);
266 
267 	return ret;
268 }
269 
270 /* IIO format int + micro */
271 static const int inv_icm42600_gyro_odr[] = {
272 	/* 12.5Hz */
273 	12, 500000,
274 	/* 25Hz */
275 	25, 0,
276 	/* 50Hz */
277 	50, 0,
278 	/* 100Hz */
279 	100, 0,
280 	/* 200Hz */
281 	200, 0,
282 	/* 1kHz */
283 	1000, 0,
284 	/* 2kHz */
285 	2000, 0,
286 	/* 4kHz */
287 	4000, 0,
288 };
289 
290 static const int inv_icm42600_gyro_odr_conv[] = {
291 	INV_ICM42600_ODR_12_5HZ,
292 	INV_ICM42600_ODR_25HZ,
293 	INV_ICM42600_ODR_50HZ,
294 	INV_ICM42600_ODR_100HZ,
295 	INV_ICM42600_ODR_200HZ,
296 	INV_ICM42600_ODR_1KHZ_LN,
297 	INV_ICM42600_ODR_2KHZ_LN,
298 	INV_ICM42600_ODR_4KHZ_LN,
299 };
300 
301 static int inv_icm42600_gyro_read_odr(struct inv_icm42600_state *st,
302 				      int *val, int *val2)
303 {
304 	unsigned int odr;
305 	unsigned int i;
306 
307 	odr = st->conf.gyro.odr;
308 
309 	for (i = 0; i < ARRAY_SIZE(inv_icm42600_gyro_odr_conv); ++i) {
310 		if (inv_icm42600_gyro_odr_conv[i] == odr)
311 			break;
312 	}
313 	if (i >= ARRAY_SIZE(inv_icm42600_gyro_odr_conv))
314 		return -EINVAL;
315 
316 	*val = inv_icm42600_gyro_odr[2 * i];
317 	*val2 = inv_icm42600_gyro_odr[2 * i + 1];
318 
319 	return IIO_VAL_INT_PLUS_MICRO;
320 }
321 
322 static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev,
323 				       int val, int val2)
324 {
325 	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
326 	struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
327 	struct device *dev = regmap_get_device(st->map);
328 	unsigned int idx;
329 	struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
330 	int ret;
331 
332 	for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_odr); idx += 2) {
333 		if (val == inv_icm42600_gyro_odr[idx] &&
334 		    val2 == inv_icm42600_gyro_odr[idx + 1])
335 			break;
336 	}
337 	if (idx >= ARRAY_SIZE(inv_icm42600_gyro_odr))
338 		return -EINVAL;
339 
340 	conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
341 
342 	pm_runtime_get_sync(dev);
343 	mutex_lock(&st->lock);
344 
345 	ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
346 						iio_buffer_enabled(indio_dev));
347 	if (ret)
348 		goto out_unlock;
349 
350 	ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
351 	if (ret)
352 		goto out_unlock;
353 	inv_icm42600_buffer_update_fifo_period(st);
354 	inv_icm42600_buffer_update_watermark(st);
355 
356 out_unlock:
357 	mutex_unlock(&st->lock);
358 	pm_runtime_mark_last_busy(dev);
359 	pm_runtime_put_autosuspend(dev);
360 
361 	return ret;
362 }
363 
364 /*
365  * Calibration bias values, IIO range format int + nano.
366  * Value is limited to +/-64dps coded on 12 bits signed. Step is 1/32 dps.
367  */
368 static int inv_icm42600_gyro_calibbias[] = {
369 	-1, 117010721,		/* min: -1.117010721 rad/s */
370 	0, 545415,		/* step: 0.000545415 rad/s */
371 	1, 116465306,		/* max: 1.116465306 rad/s */
372 };
373 
374 static int inv_icm42600_gyro_read_offset(struct inv_icm42600_state *st,
375 					 struct iio_chan_spec const *chan,
376 					 int *val, int *val2)
377 {
378 	struct device *dev = regmap_get_device(st->map);
379 	int64_t val64;
380 	int32_t bias;
381 	unsigned int reg;
382 	int16_t offset;
383 	uint8_t data[2];
384 	int ret;
385 
386 	if (chan->type != IIO_ANGL_VEL)
387 		return -EINVAL;
388 
389 	switch (chan->channel2) {
390 	case IIO_MOD_X:
391 		reg = INV_ICM42600_REG_OFFSET_USER0;
392 		break;
393 	case IIO_MOD_Y:
394 		reg = INV_ICM42600_REG_OFFSET_USER1;
395 		break;
396 	case IIO_MOD_Z:
397 		reg = INV_ICM42600_REG_OFFSET_USER3;
398 		break;
399 	default:
400 		return -EINVAL;
401 	}
402 
403 	pm_runtime_get_sync(dev);
404 	mutex_lock(&st->lock);
405 
406 	ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data));
407 	memcpy(data, st->buffer, sizeof(data));
408 
409 	mutex_unlock(&st->lock);
410 	pm_runtime_mark_last_busy(dev);
411 	pm_runtime_put_autosuspend(dev);
412 	if (ret)
413 		return ret;
414 
415 	/* 12 bits signed value */
416 	switch (chan->channel2) {
417 	case IIO_MOD_X:
418 		offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
419 		break;
420 	case IIO_MOD_Y:
421 		offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
422 		break;
423 	case IIO_MOD_Z:
424 		offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
425 		break;
426 	default:
427 		return -EINVAL;
428 	}
429 
430 	/*
431 	 * convert raw offset to dps then to rad/s
432 	 * 12 bits signed raw max 64 to dps: 64 / 2048
433 	 * dps to rad: Pi / 180
434 	 * result in nano (1000000000)
435 	 * (offset * 64 * Pi * 1000000000) / (2048 * 180)
436 	 */
437 	val64 = (int64_t)offset * 64LL * 3141592653LL;
438 	/* for rounding, add + or - divisor (2048 * 180) divided by 2 */
439 	if (val64 >= 0)
440 		val64 += 2048 * 180 / 2;
441 	else
442 		val64 -= 2048 * 180 / 2;
443 	bias = div_s64(val64, 2048 * 180);
444 	*val = bias / 1000000000L;
445 	*val2 = bias % 1000000000L;
446 
447 	return IIO_VAL_INT_PLUS_NANO;
448 }
449 
450 static int inv_icm42600_gyro_write_offset(struct inv_icm42600_state *st,
451 					  struct iio_chan_spec const *chan,
452 					  int val, int val2)
453 {
454 	struct device *dev = regmap_get_device(st->map);
455 	int64_t val64, min, max;
456 	unsigned int reg, regval;
457 	int16_t offset;
458 	int ret;
459 
460 	if (chan->type != IIO_ANGL_VEL)
461 		return -EINVAL;
462 
463 	switch (chan->channel2) {
464 	case IIO_MOD_X:
465 		reg = INV_ICM42600_REG_OFFSET_USER0;
466 		break;
467 	case IIO_MOD_Y:
468 		reg = INV_ICM42600_REG_OFFSET_USER1;
469 		break;
470 	case IIO_MOD_Z:
471 		reg = INV_ICM42600_REG_OFFSET_USER3;
472 		break;
473 	default:
474 		return -EINVAL;
475 	}
476 
477 	/* inv_icm42600_gyro_calibbias: min - step - max in nano */
478 	min = (int64_t)inv_icm42600_gyro_calibbias[0] * 1000000000LL +
479 	      (int64_t)inv_icm42600_gyro_calibbias[1];
480 	max = (int64_t)inv_icm42600_gyro_calibbias[4] * 1000000000LL +
481 	      (int64_t)inv_icm42600_gyro_calibbias[5];
482 	val64 = (int64_t)val * 1000000000LL + (int64_t)val2;
483 	if (val64 < min || val64 > max)
484 		return -EINVAL;
485 
486 	/*
487 	 * convert rad/s to dps then to raw value
488 	 * rad to dps: 180 / Pi
489 	 * dps to raw 12 bits signed, max 64: 2048 / 64
490 	 * val in nano (1000000000)
491 	 * val * 180 * 2048 / (Pi * 1000000000 * 64)
492 	 */
493 	val64 = val64 * 180LL * 2048LL;
494 	/* for rounding, add + or - divisor (3141592653 * 64) divided by 2 */
495 	if (val64 >= 0)
496 		val64 += 3141592653LL * 64LL / 2LL;
497 	else
498 		val64 -= 3141592653LL * 64LL / 2LL;
499 	offset = div64_s64(val64, 3141592653LL * 64LL);
500 
501 	/* clamp value limited to 12 bits signed */
502 	if (offset < -2048)
503 		offset = -2048;
504 	else if (offset > 2047)
505 		offset = 2047;
506 
507 	pm_runtime_get_sync(dev);
508 	mutex_lock(&st->lock);
509 
510 	switch (chan->channel2) {
511 	case IIO_MOD_X:
512 		/* OFFSET_USER1 register is shared */
513 		ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
514 				  &regval);
515 		if (ret)
516 			goto out_unlock;
517 		st->buffer[0] = offset & 0xFF;
518 		st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8);
519 		break;
520 	case IIO_MOD_Y:
521 		/* OFFSET_USER1 register is shared */
522 		ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
523 				  &regval);
524 		if (ret)
525 			goto out_unlock;
526 		st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
527 		st->buffer[1] = offset & 0xFF;
528 		break;
529 	case IIO_MOD_Z:
530 		/* OFFSET_USER4 register is shared */
531 		ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
532 				  &regval);
533 		if (ret)
534 			goto out_unlock;
535 		st->buffer[0] = offset & 0xFF;
536 		st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8);
537 		break;
538 	default:
539 		ret = -EINVAL;
540 		goto out_unlock;
541 	}
542 
543 	ret = regmap_bulk_write(st->map, reg, st->buffer, 2);
544 
545 out_unlock:
546 	mutex_unlock(&st->lock);
547 	pm_runtime_mark_last_busy(dev);
548 	pm_runtime_put_autosuspend(dev);
549 	return ret;
550 }
551 
552 static int inv_icm42600_gyro_read_raw(struct iio_dev *indio_dev,
553 				      struct iio_chan_spec const *chan,
554 				      int *val, int *val2, long mask)
555 {
556 	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
557 	int16_t data;
558 	int ret;
559 
560 	switch (chan->type) {
561 	case IIO_ANGL_VEL:
562 		break;
563 	case IIO_TEMP:
564 		return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
565 	default:
566 		return -EINVAL;
567 	}
568 
569 	switch (mask) {
570 	case IIO_CHAN_INFO_RAW:
571 		ret = iio_device_claim_direct_mode(indio_dev);
572 		if (ret)
573 			return ret;
574 		ret = inv_icm42600_gyro_read_sensor(st, chan, &data);
575 		iio_device_release_direct_mode(indio_dev);
576 		if (ret)
577 			return ret;
578 		*val = data;
579 		return IIO_VAL_INT;
580 	case IIO_CHAN_INFO_SCALE:
581 		return inv_icm42600_gyro_read_scale(st, val, val2);
582 	case IIO_CHAN_INFO_SAMP_FREQ:
583 		return inv_icm42600_gyro_read_odr(st, val, val2);
584 	case IIO_CHAN_INFO_CALIBBIAS:
585 		return inv_icm42600_gyro_read_offset(st, chan, val, val2);
586 	default:
587 		return -EINVAL;
588 	}
589 }
590 
591 static int inv_icm42600_gyro_read_avail(struct iio_dev *indio_dev,
592 					struct iio_chan_spec const *chan,
593 					const int **vals,
594 					int *type, int *length, long mask)
595 {
596 	if (chan->type != IIO_ANGL_VEL)
597 		return -EINVAL;
598 
599 	switch (mask) {
600 	case IIO_CHAN_INFO_SCALE:
601 		*vals = inv_icm42600_gyro_scale;
602 		*type = IIO_VAL_INT_PLUS_NANO;
603 		*length = ARRAY_SIZE(inv_icm42600_gyro_scale);
604 		return IIO_AVAIL_LIST;
605 	case IIO_CHAN_INFO_SAMP_FREQ:
606 		*vals = inv_icm42600_gyro_odr;
607 		*type = IIO_VAL_INT_PLUS_MICRO;
608 		*length = ARRAY_SIZE(inv_icm42600_gyro_odr);
609 		return IIO_AVAIL_LIST;
610 	case IIO_CHAN_INFO_CALIBBIAS:
611 		*vals = inv_icm42600_gyro_calibbias;
612 		*type = IIO_VAL_INT_PLUS_NANO;
613 		return IIO_AVAIL_RANGE;
614 	default:
615 		return -EINVAL;
616 	}
617 }
618 
619 static int inv_icm42600_gyro_write_raw(struct iio_dev *indio_dev,
620 				       struct iio_chan_spec const *chan,
621 				       int val, int val2, long mask)
622 {
623 	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
624 	int ret;
625 
626 	if (chan->type != IIO_ANGL_VEL)
627 		return -EINVAL;
628 
629 	switch (mask) {
630 	case IIO_CHAN_INFO_SCALE:
631 		ret = iio_device_claim_direct_mode(indio_dev);
632 		if (ret)
633 			return ret;
634 		ret = inv_icm42600_gyro_write_scale(st, val, val2);
635 		iio_device_release_direct_mode(indio_dev);
636 		return ret;
637 	case IIO_CHAN_INFO_SAMP_FREQ:
638 		return inv_icm42600_gyro_write_odr(indio_dev, val, val2);
639 	case IIO_CHAN_INFO_CALIBBIAS:
640 		ret = iio_device_claim_direct_mode(indio_dev);
641 		if (ret)
642 			return ret;
643 		ret = inv_icm42600_gyro_write_offset(st, chan, val, val2);
644 		iio_device_release_direct_mode(indio_dev);
645 		return ret;
646 	default:
647 		return -EINVAL;
648 	}
649 }
650 
651 static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev,
652 					       struct iio_chan_spec const *chan,
653 					       long mask)
654 {
655 	if (chan->type != IIO_ANGL_VEL)
656 		return -EINVAL;
657 
658 	switch (mask) {
659 	case IIO_CHAN_INFO_SCALE:
660 		return IIO_VAL_INT_PLUS_NANO;
661 	case IIO_CHAN_INFO_SAMP_FREQ:
662 		return IIO_VAL_INT_PLUS_MICRO;
663 	case IIO_CHAN_INFO_CALIBBIAS:
664 		return IIO_VAL_INT_PLUS_NANO;
665 	default:
666 		return -EINVAL;
667 	}
668 }
669 
670 static int inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev,
671 						  unsigned int val)
672 {
673 	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
674 	int ret;
675 
676 	mutex_lock(&st->lock);
677 
678 	st->fifo.watermark.gyro = val;
679 	ret = inv_icm42600_buffer_update_watermark(st);
680 
681 	mutex_unlock(&st->lock);
682 
683 	return ret;
684 }
685 
686 static int inv_icm42600_gyro_hwfifo_flush(struct iio_dev *indio_dev,
687 					  unsigned int count)
688 {
689 	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
690 	int ret;
691 
692 	if (count == 0)
693 		return 0;
694 
695 	mutex_lock(&st->lock);
696 
697 	ret = inv_icm42600_buffer_hwfifo_flush(st, count);
698 	if (!ret)
699 		ret = st->fifo.nb.gyro;
700 
701 	mutex_unlock(&st->lock);
702 
703 	return ret;
704 }
705 
706 static const struct iio_info inv_icm42600_gyro_info = {
707 	.read_raw = inv_icm42600_gyro_read_raw,
708 	.read_avail = inv_icm42600_gyro_read_avail,
709 	.write_raw = inv_icm42600_gyro_write_raw,
710 	.write_raw_get_fmt = inv_icm42600_gyro_write_raw_get_fmt,
711 	.debugfs_reg_access = inv_icm42600_debugfs_reg,
712 	.update_scan_mode = inv_icm42600_gyro_update_scan_mode,
713 	.hwfifo_set_watermark = inv_icm42600_gyro_hwfifo_set_watermark,
714 	.hwfifo_flush_to_buffer = inv_icm42600_gyro_hwfifo_flush,
715 };
716 
717 struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
718 {
719 	struct device *dev = regmap_get_device(st->map);
720 	const char *name;
721 	struct inv_icm42600_timestamp *ts;
722 	struct iio_dev *indio_dev;
723 	struct iio_buffer *buffer;
724 	int ret;
725 
726 	name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
727 	if (!name)
728 		return ERR_PTR(-ENOMEM);
729 
730 	indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
731 	if (!indio_dev)
732 		return ERR_PTR(-ENOMEM);
733 
734 	buffer = devm_iio_kfifo_allocate(dev);
735 	if (!buffer)
736 		return ERR_PTR(-ENOMEM);
737 
738 	ts = iio_priv(indio_dev);
739 	inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.gyro.odr));
740 
741 	iio_device_set_drvdata(indio_dev, st);
742 	indio_dev->name = name;
743 	indio_dev->info = &inv_icm42600_gyro_info;
744 	indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
745 	indio_dev->channels = inv_icm42600_gyro_channels;
746 	indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels);
747 	indio_dev->available_scan_masks = inv_icm42600_gyro_scan_masks;
748 	indio_dev->setup_ops = &inv_icm42600_buffer_ops;
749 
750 	iio_device_attach_buffer(indio_dev, buffer);
751 
752 	ret = devm_iio_device_register(dev, indio_dev);
753 	if (ret)
754 		return ERR_PTR(ret);
755 
756 	return indio_dev;
757 }
758 
759 int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
760 {
761 	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
762 	struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
763 	ssize_t i, size;
764 	unsigned int no;
765 	const void *accel, *gyro, *timestamp;
766 	const int8_t *temp;
767 	unsigned int odr;
768 	int64_t ts_val;
769 	struct inv_icm42600_gyro_buffer buffer;
770 
771 	/* parse all fifo packets */
772 	for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
773 		size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
774 				&accel, &gyro, &temp, &timestamp, &odr);
775 		/* quit if error or FIFO is empty */
776 		if (size <= 0)
777 			return size;
778 
779 		/* skip packet if no gyro data or data is invalid */
780 		if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro))
781 			continue;
782 
783 		/* update odr */
784 		if (odr & INV_ICM42600_SENSOR_GYRO)
785 			inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
786 							 st->fifo.nb.total, no);
787 
788 		/* buffer is copied to userspace, zeroing it to avoid any data leak */
789 		memset(&buffer, 0, sizeof(buffer));
790 		memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
791 		/* convert 8 bits FIFO temperature in high resolution format */
792 		buffer.temp = temp ? (*temp * 64) : 0;
793 		ts_val = inv_icm42600_timestamp_pop(ts);
794 		iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
795 	}
796 
797 	return 0;
798 }
799