inv_icm42600_buffer.c

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