1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * Copyright (C) 2022 ROHM Semiconductors
4  *
5  * ROHM/KIONIX KX022A accelerometer driver
6  */
7 
8 #include <linux/delay.h>
9 #include <linux/device.h>
10 #include <linux/interrupt.h>
11 #include <linux/module.h>
12 #include <linux/moduleparam.h>
13 #include <linux/mutex.h>
14 #include <linux/property.h>
15 #include <linux/regmap.h>
16 #include <linux/regulator/consumer.h>
17 #include <linux/slab.h>
18 #include <linux/string_helpers.h>
19 #include <linux/units.h>
20 
21 #include <linux/iio/iio.h>
22 #include <linux/iio/sysfs.h>
23 #include <linux/iio/trigger.h>
24 #include <linux/iio/trigger_consumer.h>
25 #include <linux/iio/triggered_buffer.h>
26 
27 #include "kionix-kx022a.h"
28 
29 /*
30  * The KX022A has FIFO which can store 43 samples of HiRes data from 2
31  * channels. This equals to 43 (samples) * 3 (channels) * 2 (bytes/sample) to
32  * 258 bytes of sample data. The quirk to know is that the amount of bytes in
33  * the FIFO is advertised via 8 bit register (max value 255). The thing to note
34  * is that full 258 bytes of data is indicated using the max value 255.
35  */
36 #define KX022A_FIFO_LENGTH			43
37 #define KX022A_FIFO_FULL_VALUE			255
38 #define KX022A_SOFT_RESET_WAIT_TIME_US		(5 * USEC_PER_MSEC)
39 #define KX022A_SOFT_RESET_TOTAL_WAIT_TIME_US	(500 * USEC_PER_MSEC)
40 
41 /* 3 axis, 2 bytes of data for each of the axis */
42 #define KX022A_FIFO_SAMPLES_SIZE_BYTES		6
43 #define KX022A_FIFO_MAX_BYTES					\
44 	(KX022A_FIFO_LENGTH * KX022A_FIFO_SAMPLES_SIZE_BYTES)
45 
46 enum {
47 	KX022A_STATE_SAMPLE,
48 	KX022A_STATE_FIFO,
49 };
50 
51 /* Regmap configs */
52 static const struct regmap_range kx022a_volatile_ranges[] = {
53 	{
54 		.range_min = KX022A_REG_XHP_L,
55 		.range_max = KX022A_REG_COTR,
56 	}, {
57 		.range_min = KX022A_REG_TSCP,
58 		.range_max = KX022A_REG_INT_REL,
59 	}, {
60 		/* The reset bit will be cleared by sensor */
61 		.range_min = KX022A_REG_CNTL2,
62 		.range_max = KX022A_REG_CNTL2,
63 	}, {
64 		.range_min = KX022A_REG_BUF_STATUS_1,
65 		.range_max = KX022A_REG_BUF_READ,
66 	},
67 };
68 
69 static const struct regmap_access_table kx022a_volatile_regs = {
70 	.yes_ranges = &kx022a_volatile_ranges[0],
71 	.n_yes_ranges = ARRAY_SIZE(kx022a_volatile_ranges),
72 };
73 
74 static const struct regmap_range kx022a_precious_ranges[] = {
75 	{
76 		.range_min = KX022A_REG_INT_REL,
77 		.range_max = KX022A_REG_INT_REL,
78 	},
79 };
80 
81 static const struct regmap_access_table kx022a_precious_regs = {
82 	.yes_ranges = &kx022a_precious_ranges[0],
83 	.n_yes_ranges = ARRAY_SIZE(kx022a_precious_ranges),
84 };
85 
86 /*
87  * The HW does not set WHO_AM_I reg as read-only but we don't want to write it
88  * so we still include it in the read-only ranges.
89  */
90 static const struct regmap_range kx022a_read_only_ranges[] = {
91 	{
92 		.range_min = KX022A_REG_XHP_L,
93 		.range_max = KX022A_REG_INT_REL,
94 	}, {
95 		.range_min = KX022A_REG_BUF_STATUS_1,
96 		.range_max = KX022A_REG_BUF_STATUS_2,
97 	}, {
98 		.range_min = KX022A_REG_BUF_READ,
99 		.range_max = KX022A_REG_BUF_READ,
100 	},
101 };
102 
103 static const struct regmap_access_table kx022a_ro_regs = {
104 	.no_ranges = &kx022a_read_only_ranges[0],
105 	.n_no_ranges = ARRAY_SIZE(kx022a_read_only_ranges),
106 };
107 
108 static const struct regmap_range kx022a_write_only_ranges[] = {
109 	{
110 		.range_min = KX022A_REG_BTS_WUF_TH,
111 		.range_max = KX022A_REG_BTS_WUF_TH,
112 	}, {
113 		.range_min = KX022A_REG_MAN_WAKE,
114 		.range_max = KX022A_REG_MAN_WAKE,
115 	}, {
116 		.range_min = KX022A_REG_SELF_TEST,
117 		.range_max = KX022A_REG_SELF_TEST,
118 	}, {
119 		.range_min = KX022A_REG_BUF_CLEAR,
120 		.range_max = KX022A_REG_BUF_CLEAR,
121 	},
122 };
123 
124 static const struct regmap_access_table kx022a_wo_regs = {
125 	.no_ranges = &kx022a_write_only_ranges[0],
126 	.n_no_ranges = ARRAY_SIZE(kx022a_write_only_ranges),
127 };
128 
129 static const struct regmap_range kx022a_noinc_read_ranges[] = {
130 	{
131 		.range_min = KX022A_REG_BUF_READ,
132 		.range_max = KX022A_REG_BUF_READ,
133 	},
134 };
135 
136 static const struct regmap_access_table kx022a_nir_regs = {
137 	.yes_ranges = &kx022a_noinc_read_ranges[0],
138 	.n_yes_ranges = ARRAY_SIZE(kx022a_noinc_read_ranges),
139 };
140 
141 const struct regmap_config kx022a_regmap = {
142 	.reg_bits = 8,
143 	.val_bits = 8,
144 	.volatile_table = &kx022a_volatile_regs,
145 	.rd_table = &kx022a_wo_regs,
146 	.wr_table = &kx022a_ro_regs,
147 	.rd_noinc_table = &kx022a_nir_regs,
148 	.precious_table = &kx022a_precious_regs,
149 	.max_register = KX022A_MAX_REGISTER,
150 	.cache_type = REGCACHE_RBTREE,
151 };
152 EXPORT_SYMBOL_NS_GPL(kx022a_regmap, IIO_KX022A);
153 
154 struct kx022a_data {
155 	struct regmap *regmap;
156 	struct iio_trigger *trig;
157 	struct device *dev;
158 	struct iio_mount_matrix orientation;
159 	int64_t timestamp, old_timestamp;
160 
161 	int irq;
162 	int inc_reg;
163 	int ien_reg;
164 
165 	unsigned int state;
166 	unsigned int odr_ns;
167 
168 	bool trigger_enabled;
169 	/*
170 	 * Prevent toggling the sensor stby/active state (PC1 bit) in the
171 	 * middle of a configuration, or when the fifo is enabled. Also,
172 	 * protect the data stored/retrieved from this structure from
173 	 * concurrent accesses.
174 	 */
175 	struct mutex mutex;
176 	u8 watermark;
177 
178 	/* 3 x 16bit accel data + timestamp */
179 	__le16 buffer[8] __aligned(IIO_DMA_MINALIGN);
180 	struct {
181 		__le16 channels[3];
182 		s64 ts __aligned(8);
183 	} scan;
184 };
185 
186 static const struct iio_mount_matrix *
187 kx022a_get_mount_matrix(const struct iio_dev *idev,
188 			const struct iio_chan_spec *chan)
189 {
190 	struct kx022a_data *data = iio_priv(idev);
191 
192 	return &data->orientation;
193 }
194 
195 enum {
196 	AXIS_X,
197 	AXIS_Y,
198 	AXIS_Z,
199 	AXIS_MAX
200 };
201 
202 static const unsigned long kx022a_scan_masks[] = {
203 	BIT(AXIS_X) | BIT(AXIS_Y) | BIT(AXIS_Z), 0
204 };
205 
206 static const struct iio_chan_spec_ext_info kx022a_ext_info[] = {
207 	IIO_MOUNT_MATRIX(IIO_SHARED_BY_TYPE, kx022a_get_mount_matrix),
208 	{ }
209 };
210 
211 #define KX022A_ACCEL_CHAN(axis, index)				\
212 {								\
213 	.type = IIO_ACCEL,					\
214 	.modified = 1,						\
215 	.channel2 = IIO_MOD_##axis,				\
216 	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
217 	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |	\
218 				BIT(IIO_CHAN_INFO_SAMP_FREQ),	\
219 	.info_mask_shared_by_type_available =			\
220 				BIT(IIO_CHAN_INFO_SCALE) |	\
221 				BIT(IIO_CHAN_INFO_SAMP_FREQ),	\
222 	.ext_info = kx022a_ext_info,				\
223 	.address = KX022A_REG_##axis##OUT_L,			\
224 	.scan_index = index,					\
225 	.scan_type = {                                          \
226 		.sign = 's',					\
227 		.realbits = 16,					\
228 		.storagebits = 16,				\
229 		.endianness = IIO_LE,				\
230 	},							\
231 }
232 
233 static const struct iio_chan_spec kx022a_channels[] = {
234 	KX022A_ACCEL_CHAN(X, 0),
235 	KX022A_ACCEL_CHAN(Y, 1),
236 	KX022A_ACCEL_CHAN(Z, 2),
237 	IIO_CHAN_SOFT_TIMESTAMP(3),
238 };
239 
240 /*
241  * The sensor HW can support ODR up to 1600 Hz, which is beyond what most of the
242  * Linux CPUs can handle without dropping samples. Also, the low power mode is
243  * not available for higher sample rates. Thus, the driver only supports 200 Hz
244  * and slower ODRs. The slowest is 0.78 Hz.
245  */
246 static const int kx022a_accel_samp_freq_table[][2] = {
247 	{ 0, 780000 },
248 	{ 1, 563000 },
249 	{ 3, 125000 },
250 	{ 6, 250000 },
251 	{ 12, 500000 },
252 	{ 25, 0 },
253 	{ 50, 0 },
254 	{ 100, 0 },
255 	{ 200, 0 },
256 };
257 
258 static const unsigned int kx022a_odrs[] = {
259 	1282051282,
260 	639795266,
261 	320 * MEGA,
262 	160 * MEGA,
263 	80 * MEGA,
264 	40 * MEGA,
265 	20 * MEGA,
266 	10 * MEGA,
267 	5 * MEGA,
268 };
269 
270 /*
271  * range is typically +-2G/4G/8G/16G, distributed over the amount of bits.
272  * The scale table can be calculated using
273  *	(range / 2^bits) * g = (range / 2^bits) * 9.80665 m/s^2
274  *	=> KX022A uses 16 bit (HiRes mode - assume the low 8 bits are zeroed
275  *	in low-power mode(?) )
276  *	=> +/-2G  => 4 / 2^16 * 9,80665 * 10^6 (to scale to micro)
277  *	=> +/-2G  - 598.550415
278  *	   +/-4G  - 1197.10083
279  *	   +/-8G  - 2394.20166
280  *	   +/-16G - 4788.40332
281  */
282 static const int kx022a_scale_table[][2] = {
283 	{ 598, 550415 },
284 	{ 1197, 100830 },
285 	{ 2394, 201660 },
286 	{ 4788, 403320 },
287 };
288 
289 static int kx022a_read_avail(struct iio_dev *indio_dev,
290 			     struct iio_chan_spec const *chan,
291 			     const int **vals, int *type, int *length,
292 			     long mask)
293 {
294 	switch (mask) {
295 	case IIO_CHAN_INFO_SAMP_FREQ:
296 		*vals = (const int *)kx022a_accel_samp_freq_table;
297 		*length = ARRAY_SIZE(kx022a_accel_samp_freq_table) *
298 			  ARRAY_SIZE(kx022a_accel_samp_freq_table[0]);
299 		*type = IIO_VAL_INT_PLUS_MICRO;
300 		return IIO_AVAIL_LIST;
301 	case IIO_CHAN_INFO_SCALE:
302 		*vals = (const int *)kx022a_scale_table;
303 		*length = ARRAY_SIZE(kx022a_scale_table) *
304 			  ARRAY_SIZE(kx022a_scale_table[0]);
305 		*type = IIO_VAL_INT_PLUS_MICRO;
306 		return IIO_AVAIL_LIST;
307 	default:
308 		return -EINVAL;
309 	}
310 }
311 
312 #define KX022A_DEFAULT_PERIOD_NS (20 * NSEC_PER_MSEC)
313 
314 static void kx022a_reg2freq(unsigned int val,  int *val1, int *val2)
315 {
316 	*val1 = kx022a_accel_samp_freq_table[val & KX022A_MASK_ODR][0];
317 	*val2 = kx022a_accel_samp_freq_table[val & KX022A_MASK_ODR][1];
318 }
319 
320 static void kx022a_reg2scale(unsigned int val, unsigned int *val1,
321 			     unsigned int *val2)
322 {
323 	val &= KX022A_MASK_GSEL;
324 	val >>= KX022A_GSEL_SHIFT;
325 
326 	*val1 = kx022a_scale_table[val][0];
327 	*val2 = kx022a_scale_table[val][1];
328 }
329 
330 static int kx022a_turn_on_off_unlocked(struct kx022a_data *data, bool on)
331 {
332 	int ret;
333 
334 	if (on)
335 		ret = regmap_set_bits(data->regmap, KX022A_REG_CNTL,
336 				      KX022A_MASK_PC1);
337 	else
338 		ret = regmap_clear_bits(data->regmap, KX022A_REG_CNTL,
339 					KX022A_MASK_PC1);
340 	if (ret)
341 		dev_err(data->dev, "Turn %s fail %d\n", str_on_off(on), ret);
342 
343 	return ret;
344 
345 }
346 
347 static int kx022a_turn_off_lock(struct kx022a_data *data)
348 {
349 	int ret;
350 
351 	mutex_lock(&data->mutex);
352 	ret = kx022a_turn_on_off_unlocked(data, false);
353 	if (ret)
354 		mutex_unlock(&data->mutex);
355 
356 	return ret;
357 }
358 
359 static int kx022a_turn_on_unlock(struct kx022a_data *data)
360 {
361 	int ret;
362 
363 	ret = kx022a_turn_on_off_unlocked(data, true);
364 	mutex_unlock(&data->mutex);
365 
366 	return ret;
367 }
368 
369 static int kx022a_write_raw(struct iio_dev *idev,
370 			    struct iio_chan_spec const *chan,
371 			    int val, int val2, long mask)
372 {
373 	struct kx022a_data *data = iio_priv(idev);
374 	int ret, n;
375 
376 	/*
377 	 * We should not allow changing scale or frequency when FIFO is running
378 	 * as it will mess the timestamp/scale for samples existing in the
379 	 * buffer. If this turns out to be an issue we can later change logic
380 	 * to internally flush the fifo before reconfiguring so the samples in
381 	 * fifo keep matching the freq/scale settings. (Such setup could cause
382 	 * issues if users trust the watermark to be reached within known
383 	 * time-limit).
384 	 */
385 	ret = iio_device_claim_direct_mode(idev);
386 	if (ret)
387 		return ret;
388 
389 	switch (mask) {
390 	case IIO_CHAN_INFO_SAMP_FREQ:
391 		n = ARRAY_SIZE(kx022a_accel_samp_freq_table);
392 
393 		while (n--)
394 			if (val == kx022a_accel_samp_freq_table[n][0] &&
395 			    val2 == kx022a_accel_samp_freq_table[n][1])
396 				break;
397 		if (n < 0) {
398 			ret = -EINVAL;
399 			goto unlock_out;
400 		}
401 		ret = kx022a_turn_off_lock(data);
402 		if (ret)
403 			break;
404 
405 		ret = regmap_update_bits(data->regmap,
406 					 KX022A_REG_ODCNTL,
407 					 KX022A_MASK_ODR, n);
408 		data->odr_ns = kx022a_odrs[n];
409 		kx022a_turn_on_unlock(data);
410 		break;
411 	case IIO_CHAN_INFO_SCALE:
412 		n = ARRAY_SIZE(kx022a_scale_table);
413 
414 		while (n-- > 0)
415 			if (val == kx022a_scale_table[n][0] &&
416 			    val2 == kx022a_scale_table[n][1])
417 				break;
418 		if (n < 0) {
419 			ret = -EINVAL;
420 			goto unlock_out;
421 		}
422 
423 		ret = kx022a_turn_off_lock(data);
424 		if (ret)
425 			break;
426 
427 		ret = regmap_update_bits(data->regmap, KX022A_REG_CNTL,
428 					 KX022A_MASK_GSEL,
429 					 n << KX022A_GSEL_SHIFT);
430 		kx022a_turn_on_unlock(data);
431 		break;
432 	default:
433 		ret = -EINVAL;
434 		break;
435 	}
436 
437 unlock_out:
438 	iio_device_release_direct_mode(idev);
439 
440 	return ret;
441 }
442 
443 static int kx022a_fifo_set_wmi(struct kx022a_data *data)
444 {
445 	u8 threshold;
446 
447 	threshold = data->watermark;
448 
449 	return regmap_update_bits(data->regmap, KX022A_REG_BUF_CNTL1,
450 				  KX022A_MASK_WM_TH, threshold);
451 }
452 
453 static int kx022a_get_axis(struct kx022a_data *data,
454 			   struct iio_chan_spec const *chan,
455 			   int *val)
456 {
457 	int ret;
458 
459 	ret = regmap_bulk_read(data->regmap, chan->address, &data->buffer[0],
460 			       sizeof(__le16));
461 	if (ret)
462 		return ret;
463 
464 	*val = le16_to_cpu(data->buffer[0]);
465 
466 	return IIO_VAL_INT;
467 }
468 
469 static int kx022a_read_raw(struct iio_dev *idev,
470 			   struct iio_chan_spec const *chan,
471 			   int *val, int *val2, long mask)
472 {
473 	struct kx022a_data *data = iio_priv(idev);
474 	unsigned int regval;
475 	int ret;
476 
477 	switch (mask) {
478 	case IIO_CHAN_INFO_RAW:
479 		ret = iio_device_claim_direct_mode(idev);
480 		if (ret)
481 			return ret;
482 
483 		mutex_lock(&data->mutex);
484 		ret = kx022a_get_axis(data, chan, val);
485 		mutex_unlock(&data->mutex);
486 
487 		iio_device_release_direct_mode(idev);
488 
489 		return ret;
490 
491 	case IIO_CHAN_INFO_SAMP_FREQ:
492 		ret = regmap_read(data->regmap, KX022A_REG_ODCNTL, &regval);
493 		if (ret)
494 			return ret;
495 
496 		if ((regval & KX022A_MASK_ODR) >
497 		    ARRAY_SIZE(kx022a_accel_samp_freq_table)) {
498 			dev_err(data->dev, "Invalid ODR\n");
499 			return -EINVAL;
500 		}
501 
502 		kx022a_reg2freq(regval, val, val2);
503 
504 		return IIO_VAL_INT_PLUS_MICRO;
505 
506 	case IIO_CHAN_INFO_SCALE:
507 		ret = regmap_read(data->regmap, KX022A_REG_CNTL, &regval);
508 		if (ret < 0)
509 			return ret;
510 
511 		kx022a_reg2scale(regval, val, val2);
512 
513 		return IIO_VAL_INT_PLUS_MICRO;
514 	}
515 
516 	return -EINVAL;
517 };
518 
519 static int kx022a_set_watermark(struct iio_dev *idev, unsigned int val)
520 {
521 	struct kx022a_data *data = iio_priv(idev);
522 
523 	if (val > KX022A_FIFO_LENGTH)
524 		val = KX022A_FIFO_LENGTH;
525 
526 	mutex_lock(&data->mutex);
527 	data->watermark = val;
528 	mutex_unlock(&data->mutex);
529 
530 	return 0;
531 }
532 
533 static ssize_t hwfifo_enabled_show(struct device *dev,
534 				   struct device_attribute *attr,
535 				   char *buf)
536 {
537 	struct iio_dev *idev = dev_to_iio_dev(dev);
538 	struct kx022a_data *data = iio_priv(idev);
539 	bool state;
540 
541 	mutex_lock(&data->mutex);
542 	state = data->state;
543 	mutex_unlock(&data->mutex);
544 
545 	return sysfs_emit(buf, "%d\n", state);
546 }
547 
548 static ssize_t hwfifo_watermark_show(struct device *dev,
549 				     struct device_attribute *attr,
550 				     char *buf)
551 {
552 	struct iio_dev *idev = dev_to_iio_dev(dev);
553 	struct kx022a_data *data = iio_priv(idev);
554 	int wm;
555 
556 	mutex_lock(&data->mutex);
557 	wm = data->watermark;
558 	mutex_unlock(&data->mutex);
559 
560 	return sysfs_emit(buf, "%d\n", wm);
561 }
562 
563 static IIO_DEVICE_ATTR_RO(hwfifo_enabled, 0);
564 static IIO_DEVICE_ATTR_RO(hwfifo_watermark, 0);
565 
566 static const struct iio_dev_attr *kx022a_fifo_attributes[] = {
567 	&iio_dev_attr_hwfifo_watermark,
568 	&iio_dev_attr_hwfifo_enabled,
569 	NULL
570 };
571 
572 static int kx022a_drop_fifo_contents(struct kx022a_data *data)
573 {
574 	/*
575 	 * We must clear the old time-stamp to avoid computing the timestamps
576 	 * based on samples acquired when buffer was last enabled.
577 	 *
578 	 * We don't need to protect the timestamp as long as we are only
579 	 * called from fifo-disable where we can guarantee the sensor is not
580 	 * triggering interrupts and where the mutex is locked to prevent the
581 	 * user-space access.
582 	 */
583 	data->timestamp = 0;
584 
585 	return regmap_write(data->regmap, KX022A_REG_BUF_CLEAR, 0x0);
586 }
587 
588 static int __kx022a_fifo_flush(struct iio_dev *idev, unsigned int samples,
589 			       bool irq)
590 {
591 	struct kx022a_data *data = iio_priv(idev);
592 	struct device *dev = regmap_get_device(data->regmap);
593 	__le16 buffer[KX022A_FIFO_LENGTH * 3];
594 	uint64_t sample_period;
595 	int count, fifo_bytes;
596 	bool renable = false;
597 	int64_t tstamp;
598 	int ret, i;
599 
600 	ret = regmap_read(data->regmap, KX022A_REG_BUF_STATUS_1, &fifo_bytes);
601 	if (ret) {
602 		dev_err(dev, "Error reading buffer status\n");
603 		return ret;
604 	}
605 
606 	/* Let's not overflow if we for some reason get bogus value from i2c */
607 	if (fifo_bytes == KX022A_FIFO_FULL_VALUE)
608 		fifo_bytes = KX022A_FIFO_MAX_BYTES;
609 
610 	if (fifo_bytes % KX022A_FIFO_SAMPLES_SIZE_BYTES)
611 		dev_warn(data->dev, "Bad FIFO alignment. Data may be corrupt\n");
612 
613 	count = fifo_bytes / KX022A_FIFO_SAMPLES_SIZE_BYTES;
614 	if (!count)
615 		return 0;
616 
617 	/*
618 	 * If we are being called from IRQ handler we know the stored timestamp
619 	 * is fairly accurate for the last stored sample. Otherwise, if we are
620 	 * called as a result of a read operation from userspace and hence
621 	 * before the watermark interrupt was triggered, take a timestamp
622 	 * now. We can fall anywhere in between two samples so the error in this
623 	 * case is at most one sample period.
624 	 */
625 	if (!irq) {
626 		/*
627 		 * We need to have the IRQ disabled or we risk of messing-up
628 		 * the timestamps. If we are ran from IRQ, then the
629 		 * IRQF_ONESHOT has us covered - but if we are ran by the
630 		 * user-space read we need to disable the IRQ to be on a safe
631 		 * side. We do this usng synchronous disable so that if the
632 		 * IRQ thread is being ran on other CPU we wait for it to be
633 		 * finished.
634 		 */
635 		disable_irq(data->irq);
636 		renable = true;
637 
638 		data->old_timestamp = data->timestamp;
639 		data->timestamp = iio_get_time_ns(idev);
640 	}
641 
642 	/*
643 	 * Approximate timestamps for each of the sample based on the sampling
644 	 * frequency, timestamp for last sample and number of samples.
645 	 *
646 	 * We'd better not use the current bandwidth settings to compute the
647 	 * sample period. The real sample rate varies with the device and
648 	 * small variation adds when we store a large number of samples.
649 	 *
650 	 * To avoid this issue we compute the actual sample period ourselves
651 	 * based on the timestamp delta between the last two flush operations.
652 	 */
653 	if (data->old_timestamp) {
654 		sample_period = data->timestamp - data->old_timestamp;
655 		do_div(sample_period, count);
656 	} else {
657 		sample_period = data->odr_ns;
658 	}
659 	tstamp = data->timestamp - (count - 1) * sample_period;
660 
661 	if (samples && count > samples) {
662 		/*
663 		 * Here we leave some old samples to the buffer. We need to
664 		 * adjust the timestamp to match the first sample in the buffer
665 		 * or we will miscalculate the sample_period at next round.
666 		 */
667 		data->timestamp -= (count - samples) * sample_period;
668 		count = samples;
669 	}
670 
671 	fifo_bytes = count * KX022A_FIFO_SAMPLES_SIZE_BYTES;
672 	ret = regmap_noinc_read(data->regmap, KX022A_REG_BUF_READ,
673 				&buffer[0], fifo_bytes);
674 	if (ret)
675 		goto renable_out;
676 
677 	for (i = 0; i < count; i++) {
678 		__le16 *sam = &buffer[i * 3];
679 		__le16 *chs;
680 		int bit;
681 
682 		chs = &data->scan.channels[0];
683 		for_each_set_bit(bit, idev->active_scan_mask, AXIS_MAX)
684 			chs[bit] = sam[bit];
685 
686 		iio_push_to_buffers_with_timestamp(idev, &data->scan, tstamp);
687 
688 		tstamp += sample_period;
689 	}
690 
691 	ret = count;
692 
693 renable_out:
694 	if (renable)
695 		enable_irq(data->irq);
696 
697 	return ret;
698 }
699 
700 static int kx022a_fifo_flush(struct iio_dev *idev, unsigned int samples)
701 {
702 	struct kx022a_data *data = iio_priv(idev);
703 	int ret;
704 
705 	mutex_lock(&data->mutex);
706 	ret = __kx022a_fifo_flush(idev, samples, false);
707 	mutex_unlock(&data->mutex);
708 
709 	return ret;
710 }
711 
712 static const struct iio_info kx022a_info = {
713 	.read_raw = &kx022a_read_raw,
714 	.write_raw = &kx022a_write_raw,
715 	.read_avail = &kx022a_read_avail,
716 
717 	.validate_trigger	= iio_validate_own_trigger,
718 	.hwfifo_set_watermark	= kx022a_set_watermark,
719 	.hwfifo_flush_to_buffer	= kx022a_fifo_flush,
720 };
721 
722 static int kx022a_set_drdy_irq(struct kx022a_data *data, bool en)
723 {
724 	if (en)
725 		return regmap_set_bits(data->regmap, KX022A_REG_CNTL,
726 				       KX022A_MASK_DRDY);
727 
728 	return regmap_clear_bits(data->regmap, KX022A_REG_CNTL,
729 				 KX022A_MASK_DRDY);
730 }
731 
732 static int kx022a_prepare_irq_pin(struct kx022a_data *data)
733 {
734 	/* Enable IRQ1 pin. Set polarity to active low */
735 	int mask = KX022A_MASK_IEN | KX022A_MASK_IPOL |
736 		   KX022A_MASK_ITYP;
737 	int val = KX022A_MASK_IEN | KX022A_IPOL_LOW |
738 		  KX022A_ITYP_LEVEL;
739 	int ret;
740 
741 	ret = regmap_update_bits(data->regmap, data->inc_reg, mask, val);
742 	if (ret)
743 		return ret;
744 
745 	/* We enable WMI to IRQ pin only at buffer_enable */
746 	mask = KX022A_MASK_INS2_DRDY;
747 
748 	return regmap_set_bits(data->regmap, data->ien_reg, mask);
749 }
750 
751 static int kx022a_fifo_disable(struct kx022a_data *data)
752 {
753 	int ret = 0;
754 
755 	ret = kx022a_turn_off_lock(data);
756 	if (ret)
757 		return ret;
758 
759 	ret = regmap_clear_bits(data->regmap, data->ien_reg, KX022A_MASK_WMI);
760 	if (ret)
761 		goto unlock_out;
762 
763 	ret = regmap_clear_bits(data->regmap, KX022A_REG_BUF_CNTL2,
764 				KX022A_MASK_BUF_EN);
765 	if (ret)
766 		goto unlock_out;
767 
768 	data->state &= ~KX022A_STATE_FIFO;
769 
770 	kx022a_drop_fifo_contents(data);
771 
772 	return kx022a_turn_on_unlock(data);
773 
774 unlock_out:
775 	mutex_unlock(&data->mutex);
776 
777 	return ret;
778 }
779 
780 static int kx022a_buffer_predisable(struct iio_dev *idev)
781 {
782 	struct kx022a_data *data = iio_priv(idev);
783 
784 	if (iio_device_get_current_mode(idev) == INDIO_BUFFER_TRIGGERED)
785 		return 0;
786 
787 	return kx022a_fifo_disable(data);
788 }
789 
790 static int kx022a_fifo_enable(struct kx022a_data *data)
791 {
792 	int ret;
793 
794 	ret = kx022a_turn_off_lock(data);
795 	if (ret)
796 		return ret;
797 
798 	/* Update watermark to HW */
799 	ret = kx022a_fifo_set_wmi(data);
800 	if (ret)
801 		goto unlock_out;
802 
803 	/* Enable buffer */
804 	ret = regmap_set_bits(data->regmap, KX022A_REG_BUF_CNTL2,
805 			      KX022A_MASK_BUF_EN);
806 	if (ret)
807 		goto unlock_out;
808 
809 	data->state |= KX022A_STATE_FIFO;
810 	ret = regmap_set_bits(data->regmap, data->ien_reg,
811 			      KX022A_MASK_WMI);
812 	if (ret)
813 		goto unlock_out;
814 
815 	return kx022a_turn_on_unlock(data);
816 
817 unlock_out:
818 	mutex_unlock(&data->mutex);
819 
820 	return ret;
821 }
822 
823 static int kx022a_buffer_postenable(struct iio_dev *idev)
824 {
825 	struct kx022a_data *data = iio_priv(idev);
826 
827 	/*
828 	 * If we use data-ready trigger, then the IRQ masks should be handled by
829 	 * trigger enable and the hardware buffer is not used but we just update
830 	 * results to the IIO fifo when data-ready triggers.
831 	 */
832 	if (iio_device_get_current_mode(idev) == INDIO_BUFFER_TRIGGERED)
833 		return 0;
834 
835 	return kx022a_fifo_enable(data);
836 }
837 
838 static const struct iio_buffer_setup_ops kx022a_buffer_ops = {
839 	.postenable = kx022a_buffer_postenable,
840 	.predisable = kx022a_buffer_predisable,
841 };
842 
843 static irqreturn_t kx022a_trigger_handler(int irq, void *p)
844 {
845 	struct iio_poll_func *pf = p;
846 	struct iio_dev *idev = pf->indio_dev;
847 	struct kx022a_data *data = iio_priv(idev);
848 	int ret;
849 
850 	ret = regmap_bulk_read(data->regmap, KX022A_REG_XOUT_L, data->buffer,
851 			       KX022A_FIFO_SAMPLES_SIZE_BYTES);
852 	if (ret < 0)
853 		goto err_read;
854 
855 	iio_push_to_buffers_with_timestamp(idev, data->buffer, data->timestamp);
856 err_read:
857 	iio_trigger_notify_done(idev->trig);
858 
859 	return IRQ_HANDLED;
860 }
861 
862 /* Get timestamps and wake the thread if we need to read data */
863 static irqreturn_t kx022a_irq_handler(int irq, void *private)
864 {
865 	struct iio_dev *idev = private;
866 	struct kx022a_data *data = iio_priv(idev);
867 
868 	data->old_timestamp = data->timestamp;
869 	data->timestamp = iio_get_time_ns(idev);
870 
871 	if (data->state & KX022A_STATE_FIFO || data->trigger_enabled)
872 		return IRQ_WAKE_THREAD;
873 
874 	return IRQ_NONE;
875 }
876 
877 /*
878  * WMI and data-ready IRQs are acked when results are read. If we add
879  * TILT/WAKE or other IRQs - then we may need to implement the acking
880  * (which is racy).
881  */
882 static irqreturn_t kx022a_irq_thread_handler(int irq, void *private)
883 {
884 	struct iio_dev *idev = private;
885 	struct kx022a_data *data = iio_priv(idev);
886 	irqreturn_t ret = IRQ_NONE;
887 
888 	mutex_lock(&data->mutex);
889 
890 	if (data->trigger_enabled) {
891 		iio_trigger_poll_nested(data->trig);
892 		ret = IRQ_HANDLED;
893 	}
894 
895 	if (data->state & KX022A_STATE_FIFO) {
896 		int ok;
897 
898 		ok = __kx022a_fifo_flush(idev, KX022A_FIFO_LENGTH, true);
899 		if (ok > 0)
900 			ret = IRQ_HANDLED;
901 	}
902 
903 	mutex_unlock(&data->mutex);
904 
905 	return ret;
906 }
907 
908 static int kx022a_trigger_set_state(struct iio_trigger *trig,
909 				    bool state)
910 {
911 	struct kx022a_data *data = iio_trigger_get_drvdata(trig);
912 	int ret = 0;
913 
914 	mutex_lock(&data->mutex);
915 
916 	if (data->trigger_enabled == state)
917 		goto unlock_out;
918 
919 	if (data->state & KX022A_STATE_FIFO) {
920 		dev_warn(data->dev, "Can't set trigger when FIFO enabled\n");
921 		ret = -EBUSY;
922 		goto unlock_out;
923 	}
924 
925 	ret = kx022a_turn_on_off_unlocked(data, false);
926 	if (ret)
927 		goto unlock_out;
928 
929 	data->trigger_enabled = state;
930 	ret = kx022a_set_drdy_irq(data, state);
931 	if (ret)
932 		goto unlock_out;
933 
934 	ret = kx022a_turn_on_off_unlocked(data, true);
935 
936 unlock_out:
937 	mutex_unlock(&data->mutex);
938 
939 	return ret;
940 }
941 
942 static const struct iio_trigger_ops kx022a_trigger_ops = {
943 	.set_trigger_state = kx022a_trigger_set_state,
944 };
945 
946 static int kx022a_chip_init(struct kx022a_data *data)
947 {
948 	int ret, val;
949 
950 	/* Reset the senor */
951 	ret = regmap_write(data->regmap, KX022A_REG_CNTL2, KX022A_MASK_SRST);
952 	if (ret)
953 		return ret;
954 
955 	/*
956 	 * I've seen I2C read failures if we poll too fast after the sensor
957 	 * reset. Slight delay gives I2C block the time to recover.
958 	 */
959 	msleep(1);
960 
961 	ret = regmap_read_poll_timeout(data->regmap, KX022A_REG_CNTL2, val,
962 				       !(val & KX022A_MASK_SRST),
963 				       KX022A_SOFT_RESET_WAIT_TIME_US,
964 				       KX022A_SOFT_RESET_TOTAL_WAIT_TIME_US);
965 	if (ret) {
966 		dev_err(data->dev, "Sensor reset %s\n",
967 			val & KX022A_MASK_SRST ? "timeout" : "fail#");
968 		return ret;
969 	}
970 
971 	ret = regmap_reinit_cache(data->regmap, &kx022a_regmap);
972 	if (ret) {
973 		dev_err(data->dev, "Failed to reinit reg cache\n");
974 		return ret;
975 	}
976 
977 	/* set data res 16bit */
978 	ret = regmap_set_bits(data->regmap, KX022A_REG_BUF_CNTL2,
979 			      KX022A_MASK_BRES16);
980 	if (ret) {
981 		dev_err(data->dev, "Failed to set data resolution\n");
982 		return ret;
983 	}
984 
985 	return kx022a_prepare_irq_pin(data);
986 }
987 
988 int kx022a_probe_internal(struct device *dev)
989 {
990 	static const char * const regulator_names[] = {"io-vdd", "vdd"};
991 	struct iio_trigger *indio_trig;
992 	struct fwnode_handle *fwnode;
993 	struct kx022a_data *data;
994 	struct regmap *regmap;
995 	unsigned int chip_id;
996 	struct iio_dev *idev;
997 	int ret, irq;
998 	char *name;
999 
1000 	regmap = dev_get_regmap(dev, NULL);
1001 	if (!regmap) {
1002 		dev_err(dev, "no regmap\n");
1003 		return -EINVAL;
1004 	}
1005 
1006 	fwnode = dev_fwnode(dev);
1007 	if (!fwnode)
1008 		return -ENODEV;
1009 
1010 	idev = devm_iio_device_alloc(dev, sizeof(*data));
1011 	if (!idev)
1012 		return -ENOMEM;
1013 
1014 	data = iio_priv(idev);
1015 
1016 	/*
1017 	 * VDD is the analog and digital domain voltage supply and
1018 	 * IO_VDD is the digital I/O voltage supply.
1019 	 */
1020 	ret = devm_regulator_bulk_get_enable(dev, ARRAY_SIZE(regulator_names),
1021 					     regulator_names);
1022 	if (ret && ret != -ENODEV)
1023 		return dev_err_probe(dev, ret, "failed to enable regulator\n");
1024 
1025 	ret = regmap_read(regmap, KX022A_REG_WHO, &chip_id);
1026 	if (ret)
1027 		return dev_err_probe(dev, ret, "Failed to access sensor\n");
1028 
1029 	if (chip_id != KX022A_ID) {
1030 		dev_err(dev, "unsupported device 0x%x\n", chip_id);
1031 		return -EINVAL;
1032 	}
1033 
1034 	irq = fwnode_irq_get_byname(fwnode, "INT1");
1035 	if (irq > 0) {
1036 		data->inc_reg = KX022A_REG_INC1;
1037 		data->ien_reg = KX022A_REG_INC4;
1038 	} else {
1039 		irq = fwnode_irq_get_byname(fwnode, "INT2");
1040 		if (irq < 0)
1041 			return dev_err_probe(dev, irq, "No suitable IRQ\n");
1042 
1043 		data->inc_reg = KX022A_REG_INC5;
1044 		data->ien_reg = KX022A_REG_INC6;
1045 	}
1046 
1047 	data->regmap = regmap;
1048 	data->dev = dev;
1049 	data->irq = irq;
1050 	data->odr_ns = KX022A_DEFAULT_PERIOD_NS;
1051 	mutex_init(&data->mutex);
1052 
1053 	idev->channels = kx022a_channels;
1054 	idev->num_channels = ARRAY_SIZE(kx022a_channels);
1055 	idev->name = "kx022-accel";
1056 	idev->info = &kx022a_info;
1057 	idev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
1058 	idev->available_scan_masks = kx022a_scan_masks;
1059 
1060 	/* Read the mounting matrix, if present */
1061 	ret = iio_read_mount_matrix(dev, &data->orientation);
1062 	if (ret)
1063 		return ret;
1064 
1065 	/* The sensor must be turned off for configuration */
1066 	ret = kx022a_turn_off_lock(data);
1067 	if (ret)
1068 		return ret;
1069 
1070 	ret = kx022a_chip_init(data);
1071 	if (ret) {
1072 		mutex_unlock(&data->mutex);
1073 		return ret;
1074 	}
1075 
1076 	ret = kx022a_turn_on_unlock(data);
1077 	if (ret)
1078 		return ret;
1079 
1080 	ret = devm_iio_triggered_buffer_setup_ext(dev, idev,
1081 						  &iio_pollfunc_store_time,
1082 						  kx022a_trigger_handler,
1083 						  IIO_BUFFER_DIRECTION_IN,
1084 						  &kx022a_buffer_ops,
1085 						  kx022a_fifo_attributes);
1086 
1087 	if (ret)
1088 		return dev_err_probe(data->dev, ret,
1089 				     "iio_triggered_buffer_setup_ext FAIL\n");
1090 	indio_trig = devm_iio_trigger_alloc(dev, "%sdata-rdy-dev%d", idev->name,
1091 					    iio_device_id(idev));
1092 	if (!indio_trig)
1093 		return -ENOMEM;
1094 
1095 	data->trig = indio_trig;
1096 
1097 	indio_trig->ops = &kx022a_trigger_ops;
1098 	iio_trigger_set_drvdata(indio_trig, data);
1099 
1100 	/*
1101 	 * No need to check for NULL. request_threaded_irq() defaults to
1102 	 * dev_name() should the alloc fail.
1103 	 */
1104 	name = devm_kasprintf(data->dev, GFP_KERNEL, "%s-kx022a",
1105 			      dev_name(data->dev));
1106 
1107 	ret = devm_request_threaded_irq(data->dev, irq, kx022a_irq_handler,
1108 					&kx022a_irq_thread_handler,
1109 					IRQF_ONESHOT, name, idev);
1110 	if (ret)
1111 		return dev_err_probe(data->dev, ret, "Could not request IRQ\n");
1112 
1113 
1114 	ret = devm_iio_trigger_register(dev, indio_trig);
1115 	if (ret)
1116 		return dev_err_probe(data->dev, ret,
1117 				     "Trigger registration failed\n");
1118 
1119 	ret = devm_iio_device_register(data->dev, idev);
1120 	if (ret < 0)
1121 		return dev_err_probe(dev, ret,
1122 				     "Unable to register iio device\n");
1123 
1124 	return ret;
1125 }
1126 EXPORT_SYMBOL_NS_GPL(kx022a_probe_internal, IIO_KX022A);
1127 
1128 MODULE_DESCRIPTION("ROHM/Kionix KX022A accelerometer driver");
1129 MODULE_AUTHOR("Matti Vaittinen <matti.vaittinen@fi.rohmeurope.com>");
1130 MODULE_LICENSE("GPL");
1131