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