~hc/RK356X_SDK_RELEASE.git

..	..	@@ -1,10 +1,11 @@
	1	+// SPDX-License-Identifier: GPL-2.0-only
1	2	/*
2	3	* STMicroelectronics st_lsm6dsx FIFO buffer library driver
3	4	*
4		- * LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM/ISM330DLC: The FIFO buffer can be
5		- * configured to store data from gyroscope and accelerometer. Samples are
6		- * queued without any tag according to a specific pattern based on
7		- * 'FIFO data sets' (6 bytes each):
	5	+ * LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM/ISM330DLC/LSM6DS3TR-C:
	6	+ * The FIFO buffer can be configured to store data from gyroscope and
	7	+ * accelerometer. Samples are queued without any tag according to a
	8	+ * specific pattern based on 'FIFO data sets' (6 bytes each):
8	9	* - 1st data set is reserved for gyroscope data
9	10	* - 2nd data set is reserved for accelerometer data
10	11	* The FIFO pattern changes depending on the ODRs and decimation factors
..	..	@@ -12,6 +13,12 @@
12	13	* buffer contains the data of all the enabled FIFO data sets
13	14	* (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
14	15	* value of the decimation factor and ODR set for each FIFO data set.
	16	+ *
	17	+ * LSM6DSO/LSM6DSOX/ASM330LHH/LSM6DSR/LSM6DSRX/ISM330DHCX:
	18	+ * The FIFO buffer can be configured to store data from gyroscope and
	19	+ * accelerometer. Each sample is queued with a tag (1B) indicating data
	20	+ * source (gyroscope, accelerometer, hw timer).
	21	+ *
15	22	* FIFO supported modes:
16	23	* - BYPASS: FIFO disabled
17	24	* - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
..	..	@@ -21,12 +28,8 @@
21	28	*
22	29	* Lorenzo Bianconi <lorenzo.bianconi@st.com>
23	30	* Denis Ciocca <denis.ciocca@st.com>
24		- *
25		- * Licensed under the GPL-2.
26	31	*/
27	32	#include <linux/module.h>
28		-#include <linux/interrupt.h>
29		-#include <linux/irq.h>
30	33	#include <linux/iio/kfifo_buf.h>
31	34	#include <linux/iio/iio.h>
32	35	#include <linux/iio/buffer.h>
..	..	@@ -37,25 +40,30 @@
37	40
38	41	#include "st_lsm6dsx.h"
39	42
40		-#define ST_LSM6DSX_REG_HLACTIVE_ADDR 0x12
41		-#define ST_LSM6DSX_REG_HLACTIVE_MASK BIT(5)
42		-#define ST_LSM6DSX_REG_PP_OD_ADDR 0x12
43		-#define ST_LSM6DSX_REG_PP_OD_MASK BIT(4)
44	43	#define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a
45	44	#define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0)
46	45	#define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3)
47	46	#define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12)
48	47	#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e
	48	+#define ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR 0x78
49	49	#define ST_LSM6DSX_REG_TS_RESET_ADDR 0x42
50	50
51	51	#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
52	52
53		-#define ST_LSM6DSX_TS_SENSITIVITY 25000UL /* 25us */
54	53	#define ST_LSM6DSX_TS_RESET_VAL 0xaa
55	54
56	55	struct st_lsm6dsx_decimator_entry {
57	56	u8 decimator;
58	57	u8 val;
	58	+};
	59	+
	60	+enum st_lsm6dsx_fifo_tag {
	61	+ ST_LSM6DSX_GYRO_TAG = 0x01,
	62	+ ST_LSM6DSX_ACC_TAG = 0x02,
	63	+ ST_LSM6DSX_TS_TAG = 0x04,
	64	+ ST_LSM6DSX_EXT0_TAG = 0x0f,
	65	+ ST_LSM6DSX_EXT1_TAG = 0x10,
	66	+ ST_LSM6DSX_EXT2_TAG = 0x11,
59	67	};
60	68
61	69	static const
..	..	@@ -70,41 +78,59 @@
70	78	{ 32, 0x7 },
71	79	};
72	80
73		-static int st_lsm6dsx_get_decimator_val(u8 val)
	81	+static int
	82	+st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr)
74	83	{
75	84	const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
	85	+ u32 decimator = max_odr / sensor->odr;
76	86	int i;
77	87
78		- for (i = 0; i < max_size; i++)
79		- if (st_lsm6dsx_decimator_table[i].decimator == val)
80		- break;
	88	+ if (decimator > 1)
	89	+ decimator = round_down(decimator, 2);
81	90
	91	+ for (i = 0; i < max_size; i++) {
	92	+ if (st_lsm6dsx_decimator_table[i].decimator == decimator)
	93	+ break;
	94	+ }
	95	+
	96	+ sensor->decimator = decimator;
82	97	return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
83	98	}
84	99
85	100	static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
86		- u16 max_odr, u16 min_odr)
	101	+ u32 max_odr, u32 min_odr)
87	102	{
88	103	struct st_lsm6dsx_sensor *sensor;
89	104	int i;
90	105
91	106	max_odr = 0, min_odr = ~0;
92	107	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
	108	+ if (!hw->iio_devs[i])
	109	+ continue;
	110	+
93	111	sensor = iio_priv(hw->iio_devs[i]);
94	112
95	113	if (!(hw->enable_mask & BIT(sensor->id)))
96	114	continue;
97	115
98		- max_odr = max_t(u16, max_odr, sensor->odr);
99		- min_odr = min_t(u16, min_odr, sensor->odr);
	116	+ max_odr = max_t(u32, max_odr, sensor->odr);
	117	+ min_odr = min_t(u32, min_odr, sensor->odr);
100	118	}
	119	+}
	120	+
	121	+static u8 st_lsm6dsx_get_sip(struct st_lsm6dsx_sensor *sensor, u32 min_odr)
	122	+{
	123	+ u8 sip = sensor->odr / min_odr;
	124	+
	125	+ return sip > 1 ? round_down(sip, 2) : sip;
101	126	}
102	127
103	128	static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
104	129	{
105		- u16 max_odr, min_odr, sip = 0, ts_sip = 0;
106	130	const struct st_lsm6dsx_reg *ts_dec_reg;
107	131	struct st_lsm6dsx_sensor *sensor;
	132	+ u16 sip = 0, ts_sip = 0;
	133	+ u32 max_odr, min_odr;
108	134	int err = 0, i;
109	135	u8 data;
110	136
..	..	@@ -113,15 +139,16 @@
113	139	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
114	140	const struct st_lsm6dsx_reg *dec_reg;
115	141
	142	+ if (!hw->iio_devs[i])
	143	+ continue;
	144	+
116	145	sensor = iio_priv(hw->iio_devs[i]);
117	146	/* update fifo decimators and sample in pattern */
118	147	if (hw->enable_mask & BIT(sensor->id)) {
119		- sensor->sip = sensor->odr / min_odr;
120		- sensor->decimator = max_odr / sensor->odr;
121		- data = st_lsm6dsx_get_decimator_val(sensor->decimator);
	148	+ sensor->sip = st_lsm6dsx_get_sip(sensor, min_odr);
	149	+ data = st_lsm6dsx_get_decimator_val(sensor, max_odr);
122	150	} else {
123	151	sensor->sip = 0;
124		- sensor->decimator = 0;
125	152	data = 0;
126	153	}
127	154	ts_sip = max_t(u16, ts_sip, sensor->sip);
..	..	@@ -130,8 +157,9 @@
130	157	if (dec_reg->addr) {
131	158	int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);
132	159
133		- err = regmap_update_bits(hw->regmap, dec_reg->addr,
134		- dec_reg->mask, val);
	160	+ err = st_lsm6dsx_update_bits_locked(hw, dec_reg->addr,
	161	+ dec_reg->mask,
	162	+ val);
135	163	if (err < 0)
136	164	return err;
137	165	}
..	..	@@ -150,39 +178,54 @@
150	178	int val, ts_dec = !!hw->ts_sip;
151	179
152	180	val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask);
153		- err = regmap_update_bits(hw->regmap, ts_dec_reg->addr,
154		- ts_dec_reg->mask, val);
	181	+ err = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr,
	182	+ ts_dec_reg->mask, val);
155	183	}
156	184	return err;
157	185	}
158	186
159		-int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
160		- enum st_lsm6dsx_fifo_mode fifo_mode)
	187	+static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
	188	+ enum st_lsm6dsx_fifo_mode fifo_mode)
161	189	{
162		- int err;
	190	+ unsigned int data;
163	191
164		- err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
165		- ST_LSM6DSX_FIFO_MODE_MASK,
166		- FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK,
167		- fifo_mode));
168		- if (err < 0)
169		- return err;
170		-
171		- hw->fifo_mode = fifo_mode;
172		-
173		- return 0;
	192	+ data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
	193	+ return st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
	194	+ ST_LSM6DSX_FIFO_MODE_MASK, data);
174	195	}
175	196
176	197	static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
177	198	bool enable)
178	199	{
179	200	struct st_lsm6dsx_hw *hw = sensor->hw;
	201	+ const struct st_lsm6dsx_reg *batch_reg;
180	202	u8 data;
181	203
182		- data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
183		- return regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
184		- ST_LSM6DSX_FIFO_ODR_MASK,
185		- FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK, data));
	204	+ batch_reg = &hw->settings->batch[sensor->id];
	205	+ if (batch_reg->addr) {
	206	+ int val;
	207	+
	208	+ if (enable) {
	209	+ int err;
	210	+
	211	+ err = st_lsm6dsx_check_odr(sensor, sensor->odr,
	212	+ &data);
	213	+ if (err < 0)
	214	+ return err;
	215	+ } else {
	216	+ data = 0;
	217	+ }
	218	+ val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask);
	219	+ return st_lsm6dsx_update_bits_locked(hw, batch_reg->addr,
	220	+ batch_reg->mask, val);
	221	+ } else {
	222	+ data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
	223	+ return st_lsm6dsx_update_bits_locked(hw,
	224	+ ST_LSM6DSX_REG_FIFO_MODE_ADDR,
	225	+ ST_LSM6DSX_FIFO_ODR_MASK,
	226	+ FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK,
	227	+ data));
	228	+ }
186	229	}
187	230
188	231	int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
..	..	@@ -197,6 +240,9 @@
197	240	return 0;
198	241
199	242	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
	243	+ if (!hw->iio_devs[i])
	244	+ continue;
	245	+
200	246	cur_sensor = iio_priv(hw->iio_devs[i]);
201	247
202	248	if (!(hw->enable_mask & BIT(cur_sensor->id)))
..	..	@@ -212,19 +258,23 @@
212	258	fifo_watermark = (fifo_watermark / hw->sip) * hw->sip;
213	259	fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl;
214	260
	261	+ mutex_lock(&hw->page_lock);
215	262	err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,
216	263	&data);
217	264	if (err < 0)
218		- return err;
	265	+ goto out;
219	266
220	267	fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask;
221	268	fifo_watermark = ((data << 8) & ~fifo_th_mask) \|
222	269	(fifo_watermark & fifo_th_mask);
223	270
224	271	wdata = cpu_to_le16(fifo_watermark);
225		- return regmap_bulk_write(hw->regmap,
226		- hw->settings->fifo_ops.fifo_th.addr,
227		- &wdata, sizeof(wdata));
	272	+ err = regmap_bulk_write(hw->regmap,
	273	+ hw->settings->fifo_ops.fifo_th.addr,
	274	+ &wdata, sizeof(wdata));
	275	+out:
	276	+ mutex_unlock(&hw->page_lock);
	277	+ return err;
228	278	}
229	279
230	280	static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
..	..	@@ -233,12 +283,15 @@
233	283	int i, err;
234	284
235	285	/* reset hw ts counter */
236		- err = regmap_write(hw->regmap, ST_LSM6DSX_REG_TS_RESET_ADDR,
237		- ST_LSM6DSX_TS_RESET_VAL);
	286	+ err = st_lsm6dsx_write_locked(hw, ST_LSM6DSX_REG_TS_RESET_ADDR,
	287	+ ST_LSM6DSX_TS_RESET_VAL);
238	288	if (err < 0)
239	289	return err;
240	290
241	291	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
	292	+ if (!hw->iio_devs[i])
	293	+ continue;
	294	+
242	295	sensor = iio_priv(hw->iio_devs[i]);
243	296	/*
244	297	* store enable buffer timestamp as reference for
..	..	@@ -249,22 +302,34 @@
249	302	return 0;
250	303	}
251	304
	305	+int st_lsm6dsx_resume_fifo(struct st_lsm6dsx_hw *hw)
	306	+{
	307	+ int err;
	308	+
	309	+ /* reset hw ts counter */
	310	+ err = st_lsm6dsx_reset_hw_ts(hw);
	311	+ if (err < 0)
	312	+ return err;
	313	+
	314	+ return st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
	315	+}
	316	+
252	317	/*
253		- * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN in order to avoid
254		- * a kmalloc for each bus access
	318	+ * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN/ST_LSM6DSX_MAX_TAGGED_WORD_LEN
	319	+ * in order to avoid a kmalloc for each bus access
255	320	*/
256		-static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw hw, u8 data,
257		- unsigned int data_len)
	321	+static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
	322	+ u8 *data, unsigned int data_len,
	323	+ unsigned int max_word_len)
258	324	{
259	325	unsigned int word_len, read_len = 0;
260	326	int err;
261	327
262	328	while (read_len < data_len) {
263	329	word_len = min_t(unsigned int, data_len - read_len,
264		- ST_LSM6DSX_MAX_WORD_LEN);
265		- err = regmap_bulk_read(hw->regmap,
266		- ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
267		- data + read_len, word_len);
	330	+ max_word_len);
	331	+ err = st_lsm6dsx_read_locked(hw, addr, data + read_len,
	332	+ word_len);
268	333	if (err < 0)
269	334	return err;
270	335	read_len += word_len;
..	..	@@ -282,21 +347,19 @@
282	347	*
283	348	* Return: Number of bytes read from the FIFO
284	349	*/
285		-static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
	350	+int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
286	351	{
	352	+ struct st_lsm6dsx_sensor acc_sensor, gyro_sensor, *ext_sensor = NULL;
	353	+ int err, sip, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
287	354	u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
288	355	u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
289		- int err, acc_sip, gyro_sip, ts_sip, read_len, offset;
290		- struct st_lsm6dsx_sensor acc_sensor, gyro_sensor;
291		- u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
292		- u8 acc_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
293	356	bool reset_ts = false;
294	357	__le16 fifo_status;
295	358	s64 ts = 0;
296	359
297		- err = regmap_bulk_read(hw->regmap,
298		- hw->settings->fifo_ops.fifo_diff.addr,
299		- &fifo_status, sizeof(fifo_status));
	360	+ err = st_lsm6dsx_read_locked(hw,
	361	+ hw->settings->fifo_ops.fifo_diff.addr,
	362	+ &fifo_status, sizeof(fifo_status));
300	363	if (err < 0) {
301	364	dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
302	365	err);
..	..	@@ -312,9 +375,13 @@
312	375
313	376	acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
314	377	gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
	378	+ if (hw->iio_devs[ST_LSM6DSX_ID_EXT0])
	379	+ ext_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_EXT0]);
315	380
316	381	for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
317		- err = st_lsm6dsx_read_block(hw, hw->buff, pattern_len);
	382	+ err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
	383	+ hw->buff, pattern_len,
	384	+ ST_LSM6DSX_MAX_WORD_LEN);
318	385	if (err < 0) {
319	386	dev_err(hw->dev,
320	387	"failed to read pattern from fifo (err=%d)\n",
..	..	@@ -337,21 +404,31 @@
337	404	* following pattern is repeated every 9 samples:
338	405	* - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
339	406	*/
	407	+ ext_sip = ext_sensor ? ext_sensor->sip : 0;
340	408	gyro_sip = gyro_sensor->sip;
341	409	acc_sip = acc_sensor->sip;
342	410	ts_sip = hw->ts_sip;
343	411	offset = 0;
	412	+ sip = 0;
344	413
345		- while (acc_sip > 0 \|\| gyro_sip > 0) {
346		- if (gyro_sip > 0) {
347		- memcpy(gyro_buff, &hw->buff[offset],
348		- ST_LSM6DSX_SAMPLE_SIZE);
349		- offset += ST_LSM6DSX_SAMPLE_SIZE;
	414	+ while (acc_sip > 0 \|\| gyro_sip > 0 \|\| ext_sip > 0) {
	415	+ if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
	416	+ memcpy(hw->scan[ST_LSM6DSX_ID_GYRO].channels,
	417	+ &hw->buff[offset],
	418	+ sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels));
	419	+ offset += sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels);
350	420	}
351		- if (acc_sip > 0) {
352		- memcpy(acc_buff, &hw->buff[offset],
353		- ST_LSM6DSX_SAMPLE_SIZE);
354		- offset += ST_LSM6DSX_SAMPLE_SIZE;
	421	+ if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
	422	+ memcpy(hw->scan[ST_LSM6DSX_ID_ACC].channels,
	423	+ &hw->buff[offset],
	424	+ sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels));
	425	+ offset += sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels);
	426	+ }
	427	+ if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
	428	+ memcpy(hw->scan[ST_LSM6DSX_ID_EXT0].channels,
	429	+ &hw->buff[offset],
	430	+ sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels));
	431	+ offset += sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels);
355	432	}
356	433
357	434	if (ts_sip-- > 0) {
..	..	@@ -373,19 +450,33 @@
373	450	*/
374	451	if (!reset_ts && ts >= 0xff0000)
375	452	reset_ts = true;
376		- ts *= ST_LSM6DSX_TS_SENSITIVITY;
	453	+ ts *= hw->ts_gain;
377	454
378	455	offset += ST_LSM6DSX_SAMPLE_SIZE;
379	456	}
380	457
381		- if (gyro_sip-- > 0)
	458	+ if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
382	459	iio_push_to_buffers_with_timestamp(
383	460	hw->iio_devs[ST_LSM6DSX_ID_GYRO],
384		- gyro_buff, gyro_sensor->ts_ref + ts);
385		- if (acc_sip-- > 0)
	461	+ &hw->scan[ST_LSM6DSX_ID_GYRO],
	462	+ gyro_sensor->ts_ref + ts);
	463	+ gyro_sip--;
	464	+ }
	465	+ if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
386	466	iio_push_to_buffers_with_timestamp(
387	467	hw->iio_devs[ST_LSM6DSX_ID_ACC],
388		- acc_buff, acc_sensor->ts_ref + ts);
	468	+ &hw->scan[ST_LSM6DSX_ID_ACC],
	469	+ acc_sensor->ts_ref + ts);
	470	+ acc_sip--;
	471	+ }
	472	+ if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
	473	+ iio_push_to_buffers_with_timestamp(
	474	+ hw->iio_devs[ST_LSM6DSX_ID_EXT0],
	475	+ &hw->scan[ST_LSM6DSX_ID_EXT0],
	476	+ ext_sensor->ts_ref + ts);
	477	+ ext_sip--;
	478	+ }
	479	+ sip++;
389	480	}
390	481	}
391	482
..	..	@@ -400,13 +491,161 @@
400	491	return read_len;
401	492	}
402	493
	494	+#define ST_LSM6DSX_INVALID_SAMPLE 0x7ffd
	495	+static int
	496	+st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
	497	+ u8 *data, s64 ts)
	498	+{
	499	+ s16 val = le16_to_cpu((__le16 )data);
	500	+ struct st_lsm6dsx_sensor *sensor;
	501	+ struct iio_dev *iio_dev;
	502	+
	503	+ /* invalid sample during bootstrap phase */
	504	+ if (val >= ST_LSM6DSX_INVALID_SAMPLE)
	505	+ return -EINVAL;
	506	+
	507	+ /*
	508	+ * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG
	509	+ * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG
	510	+ * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled
	511	+ * channel
	512	+ */
	513	+ switch (tag) {
	514	+ case ST_LSM6DSX_GYRO_TAG:
	515	+ iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO];
	516	+ break;
	517	+ case ST_LSM6DSX_ACC_TAG:
	518	+ iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC];
	519	+ break;
	520	+ case ST_LSM6DSX_EXT0_TAG:
	521	+ if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0))
	522	+ iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0];
	523	+ else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))
	524	+ iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
	525	+ else
	526	+ iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
	527	+ break;
	528	+ case ST_LSM6DSX_EXT1_TAG:
	529	+ if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) &&
	530	+ (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)))
	531	+ iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
	532	+ else
	533	+ iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
	534	+ break;
	535	+ case ST_LSM6DSX_EXT2_TAG:
	536	+ iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
	537	+ break;
	538	+ default:
	539	+ return -EINVAL;
	540	+ }
	541	+
	542	+ sensor = iio_priv(iio_dev);
	543	+ iio_push_to_buffers_with_timestamp(iio_dev, data,
	544	+ ts + sensor->ts_ref);
	545	+
	546	+ return 0;
	547	+}
	548	+
	549	+/**
	550	+ * st_lsm6dsx_read_tagged_fifo() - tagged hw FIFO read routine
	551	+ * @hw: Pointer to instance of struct st_lsm6dsx_hw.
	552	+ *
	553	+ * Read samples from the hw FIFO and push them to IIO buffers.
	554	+ *
	555	+ * Return: Number of bytes read from the FIFO
	556	+ */
	557	+int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
	558	+{
	559	+ u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
	560	+ u16 fifo_len, fifo_diff_mask;
	561	+ /*
	562	+ * Alignment needed as this can ultimately be passed to a
	563	+ * call to iio_push_to_buffers_with_timestamp() which
	564	+ * must be passed a buffer that is aligned to 8 bytes so
	565	+ * as to allow insertion of a naturally aligned timestamp.
	566	+ */
	567	+ u8 iio_buff[ST_LSM6DSX_IIO_BUFF_SIZE] __aligned(8);
	568	+ u8 tag;
	569	+ bool reset_ts = false;
	570	+ int i, err, read_len;
	571	+ __le16 fifo_status;
	572	+ s64 ts = 0;
	573	+
	574	+ err = st_lsm6dsx_read_locked(hw,
	575	+ hw->settings->fifo_ops.fifo_diff.addr,
	576	+ &fifo_status, sizeof(fifo_status));
	577	+ if (err < 0) {
	578	+ dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
	579	+ err);
	580	+ return err;
	581	+ }
	582	+
	583	+ fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
	584	+ fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
	585	+ ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
	586	+ if (!fifo_len)
	587	+ return 0;
	588	+
	589	+ for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
	590	+ err = st_lsm6dsx_read_block(hw,
	591	+ ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR,
	592	+ hw->buff, pattern_len,
	593	+ ST_LSM6DSX_MAX_TAGGED_WORD_LEN);
	594	+ if (err < 0) {
	595	+ dev_err(hw->dev,
	596	+ "failed to read pattern from fifo (err=%d)\n",
	597	+ err);
	598	+ return err;
	599	+ }
	600	+
	601	+ for (i = 0; i < pattern_len;
	602	+ i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) {
	603	+ memcpy(iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE],
	604	+ ST_LSM6DSX_SAMPLE_SIZE);
	605	+
	606	+ tag = hw->buff[i] >> 3;
	607	+ if (tag == ST_LSM6DSX_TS_TAG) {
	608	+ /*
	609	+ * hw timestamp is 4B long and it is stored
	610	+ * in FIFO according to this schema:
	611	+ * B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16],
	612	+ * B3 = ts[31:24]
	613	+ */
	614	+ ts = le32_to_cpu(((__le32 )iio_buff));
	615	+ /*
	616	+ * check if hw timestamp engine is going to
	617	+ * reset (the sensor generates an interrupt
	618	+ * to signal the hw timestamp will reset in
	619	+ * 1.638s)
	620	+ */
	621	+ if (!reset_ts && ts >= 0xffff0000)
	622	+ reset_ts = true;
	623	+ ts *= hw->ts_gain;
	624	+ } else {
	625	+ st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,
	626	+ ts);
	627	+ }
	628	+ }
	629	+ }
	630	+
	631	+ if (unlikely(reset_ts)) {
	632	+ err = st_lsm6dsx_reset_hw_ts(hw);
	633	+ if (err < 0)
	634	+ return err;
	635	+ }
	636	+ return read_len;
	637	+}
	638	+
403	639	int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
404	640	{
405	641	int err;
406	642
	643	+ if (!hw->settings->fifo_ops.read_fifo)
	644	+ return -ENOTSUPP;
	645	+
407	646	mutex_lock(&hw->fifo_lock);
408	647
409		- st_lsm6dsx_read_fifo(hw);
	648	+ hw->settings->fifo_ops.read_fifo(hw);
410	649	err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
411	650
412	651	mutex_unlock(&hw->fifo_lock);
..	..	@@ -414,26 +653,33 @@
414	653	return err;
415	654	}
416	655
417		-static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
	656	+int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
418	657	{
419		- struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
420	658	struct st_lsm6dsx_hw *hw = sensor->hw;
	659	+ u8 fifo_mask;
421	660	int err;
422	661
423	662	mutex_lock(&hw->conf_lock);
424	663
425		- if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
	664	+ if (enable)
	665	+ fifo_mask = hw->fifo_mask \| BIT(sensor->id);
	666	+ else
	667	+ fifo_mask = hw->fifo_mask & ~BIT(sensor->id);
	668	+
	669	+ if (hw->fifo_mask) {
426	670	err = st_lsm6dsx_flush_fifo(hw);
427	671	if (err < 0)
428	672	goto out;
429	673	}
430	674
431		- if (enable) {
432		- err = st_lsm6dsx_sensor_enable(sensor);
	675	+ if (sensor->id == ST_LSM6DSX_ID_EXT0 \|\|
	676	+ sensor->id == ST_LSM6DSX_ID_EXT1 \|\|
	677	+ sensor->id == ST_LSM6DSX_ID_EXT2) {
	678	+ err = st_lsm6dsx_shub_set_enable(sensor, enable);
433	679	if (err < 0)
434	680	goto out;
435	681	} else {
436		- err = st_lsm6dsx_sensor_disable(sensor);
	682	+ err = st_lsm6dsx_sensor_set_enable(sensor, enable);
437	683	if (err < 0)
438	684	goto out;
439	685	}
..	..	@@ -450,14 +696,13 @@
450	696	if (err < 0)
451	697	goto out;
452	698
453		- if (hw->enable_mask) {
454		- /* reset hw ts counter */
455		- err = st_lsm6dsx_reset_hw_ts(hw);
	699	+ if (fifo_mask) {
	700	+ err = st_lsm6dsx_resume_fifo(hw);
456	701	if (err < 0)
457	702	goto out;
458		-
459		- err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
460	703	}
	704	+
	705	+ hw->fifo_mask = fifo_mask;
461	706
462	707	out:
463	708	mutex_unlock(&hw->conf_lock);
..	..	@@ -465,49 +710,26 @@
465	710	return err;
466	711	}
467	712
468		-static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
469		-{
470		- struct st_lsm6dsx_hw *hw = private;
471		-
472		- return hw->sip > 0 ? IRQ_WAKE_THREAD : IRQ_NONE;
473		-}
474		-
475		-static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
476		-{
477		- struct st_lsm6dsx_hw *hw = private;
478		- int fifo_len = 0, len;
479		-
480		- /*
481		- * If we are using edge IRQs, new samples can arrive while
482		- * processing current interrupt since there are no hw
483		- * guarantees the irq line stays "low" long enough to properly
484		- * detect the new interrupt. In this case the new sample will
485		- * be missed.
486		- * Polling FIFO status register allow us to read new
487		- * samples even if the interrupt arrives while processing
488		- * previous data and the timeslot where the line is "low" is
489		- * too short to be properly detected.
490		- */
491		- do {
492		- mutex_lock(&hw->fifo_lock);
493		- len = st_lsm6dsx_read_fifo(hw);
494		- mutex_unlock(&hw->fifo_lock);
495		-
496		- if (len > 0)
497		- fifo_len += len;
498		- } while (len > 0);
499		-
500		- return fifo_len ? IRQ_HANDLED : IRQ_NONE;
501		-}
502		-
503	713	static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
504	714	{
505		- return st_lsm6dsx_update_fifo(iio_dev, true);
	715	+ struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
	716	+ struct st_lsm6dsx_hw *hw = sensor->hw;
	717	+
	718	+ if (!hw->settings->fifo_ops.update_fifo)
	719	+ return -ENOTSUPP;
	720	+
	721	+ return hw->settings->fifo_ops.update_fifo(sensor, true);
506	722	}
507	723
508	724	static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
509	725	{
510		- return st_lsm6dsx_update_fifo(iio_dev, false);
	726	+ struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
	727	+ struct st_lsm6dsx_hw *hw = sensor->hw;
	728	+
	729	+ if (!hw->settings->fifo_ops.update_fifo)
	730	+ return -ENOTSUPP;
	731	+
	732	+ return hw->settings->fifo_ops.update_fifo(sensor, false);
511	733	}
512	734
513	735	static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
..	..	@@ -517,61 +739,13 @@
517	739
518	740	int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
519	741	{
520		- struct device_node *np = hw->dev->of_node;
521		- struct st_sensors_platform_data *pdata;
522	742	struct iio_buffer *buffer;
523		- unsigned long irq_type;
524		- bool irq_active_low;
525		- int i, err;
526		-
527		- irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
528		-
529		- switch (irq_type) {
530		- case IRQF_TRIGGER_HIGH:
531		- case IRQF_TRIGGER_RISING:
532		- irq_active_low = false;
533		- break;
534		- case IRQF_TRIGGER_LOW:
535		- case IRQF_TRIGGER_FALLING:
536		- irq_active_low = true;
537		- break;
538		- default:
539		- dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
540		- return -EINVAL;
541		- }
542		-
543		- err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR,
544		- ST_LSM6DSX_REG_HLACTIVE_MASK,
545		- FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK,
546		- irq_active_low));
547		- if (err < 0)
548		- return err;
549		-
550		- pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
551		- if ((np && of_property_read_bool(np, "drive-open-drain")) \|\|
552		- (pdata && pdata->open_drain)) {
553		- err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR,
554		- ST_LSM6DSX_REG_PP_OD_MASK,
555		- FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK,
556		- 1));
557		- if (err < 0)
558		- return err;
559		-
560		- irq_type \|= IRQF_SHARED;
561		- }
562		-
563		- err = devm_request_threaded_irq(hw->dev, hw->irq,
564		- st_lsm6dsx_handler_irq,
565		- st_lsm6dsx_handler_thread,
566		- irq_type \| IRQF_ONESHOT,
567		- "lsm6dsx", hw);
568		- if (err) {
569		- dev_err(hw->dev, "failed to request trigger irq %d\n",
570		- hw->irq);
571		- return err;
572		- }
	743	+ int i;
573	744
574	745	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
	746	+ if (!hw->iio_devs[i])
	747	+ continue;
	748	+
575	749	buffer = devm_iio_kfifo_allocate(hw->dev);
576	750	if (!buffer)
577	751	return -ENOMEM;