Commit | Line | Data |
---|---|---|
fda8d26e | 1 | // SPDX-License-Identifier: GPL-2.0-only |
290a6ce1 LB |
2 | /* |
3 | * STMicroelectronics st_lsm6dsx FIFO buffer library driver | |
4 | * | |
0e2bf22a | 5 | * Pattern FIFO: |
dbcd2088 LB |
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): | |
290a6ce1 LB |
9 | * - 1st data set is reserved for gyroscope data |
10 | * - 2nd data set is reserved for accelerometer data | |
11 | * The FIFO pattern changes depending on the ODRs and decimation factors | |
12 | * assigned to the FIFO data sets. The first sequence of data stored in FIFO | |
13 | * buffer contains the data of all the enabled FIFO data sets | |
14 | * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the | |
15 | * value of the decimation factor and ODR set for each FIFO data set. | |
801a6e0a | 16 | * |
0e2bf22a LB |
17 | * Supported devices: |
18 | * - ISM330DLC | |
19 | * - LSM6DS3 | |
20 | * - LSM6DS3H | |
21 | * - LSM6DS3TR-C | |
22 | * - LSM6DSL | |
23 | * - LSM6DSM | |
24 | * | |
25 | * Tagged FIFO: | |
cf9c71b3 LB |
26 | * The FIFO buffer can be configured to store data from gyroscope and |
27 | * accelerometer. Each sample is queued with a tag (1B) indicating data | |
28 | * source (gyroscope, accelerometer, hw timer). | |
801a6e0a | 29 | * |
0e2bf22a LB |
30 | * Supported devices: |
31 | * - ASM330LHB | |
32 | * - ASM330LHH | |
33 | * - ASM330LHHX | |
16ac43a1 | 34 | * - ASM330LHHXG1 |
0e2bf22a LB |
35 | * - ISM330DHCX |
36 | * - LSM6DSO | |
37 | * - LSM6DSOP | |
38 | * - LSM6DSOX | |
39 | * - LSM6DSR | |
40 | * - LSM6DSRX | |
41 | * - LSM6DST | |
42 | * - LSM6DSTX | |
43 | * - LSM6DSV | |
44 | * | |
290a6ce1 LB |
45 | * FIFO supported modes: |
46 | * - BYPASS: FIFO disabled | |
47 | * - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index | |
48 | * restarts from the beginning and the oldest sample is overwritten | |
49 | * | |
50 | * Copyright 2016 STMicroelectronics Inc. | |
51 | * | |
52 | * Lorenzo Bianconi <lorenzo.bianconi@st.com> | |
53 | * Denis Ciocca <denis.ciocca@st.com> | |
290a6ce1 LB |
54 | */ |
55 | #include <linux/module.h> | |
290a6ce1 LB |
56 | #include <linux/iio/kfifo_buf.h> |
57 | #include <linux/iio/iio.h> | |
58 | #include <linux/iio/buffer.h> | |
51a8b707 LB |
59 | #include <linux/regmap.h> |
60 | #include <linux/bitfield.h> | |
290a6ce1 | 61 | |
ff5fff4a LB |
62 | #include <linux/platform_data/st_sensors_pdata.h> |
63 | ||
290a6ce1 LB |
64 | #include "st_lsm6dsx.h" |
65 | ||
290a6ce1 LB |
66 | #define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a |
67 | #define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0) | |
68 | #define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3) | |
290a6ce1 LB |
69 | #define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12) |
70 | #define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e | |
801a6e0a | 71 | #define ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR 0x78 |
21345107 | 72 | #define ST_LSM6DSX_REG_TS_RESET_ADDR 0x42 |
290a6ce1 LB |
73 | |
74 | #define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08 | |
75 | ||
21345107 LB |
76 | #define ST_LSM6DSX_TS_RESET_VAL 0xaa |
77 | ||
290a6ce1 LB |
78 | struct st_lsm6dsx_decimator_entry { |
79 | u8 decimator; | |
80 | u8 val; | |
81 | }; | |
82 | ||
801a6e0a LB |
83 | enum st_lsm6dsx_fifo_tag { |
84 | ST_LSM6DSX_GYRO_TAG = 0x01, | |
85 | ST_LSM6DSX_ACC_TAG = 0x02, | |
86 | ST_LSM6DSX_TS_TAG = 0x04, | |
6d0205fd LB |
87 | ST_LSM6DSX_EXT0_TAG = 0x0f, |
88 | ST_LSM6DSX_EXT1_TAG = 0x10, | |
89 | ST_LSM6DSX_EXT2_TAG = 0x11, | |
801a6e0a LB |
90 | }; |
91 | ||
290a6ce1 LB |
92 | static const |
93 | struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = { | |
94 | { 0, 0x0 }, | |
95 | { 1, 0x1 }, | |
96 | { 2, 0x2 }, | |
97 | { 3, 0x3 }, | |
98 | { 4, 0x4 }, | |
99 | { 8, 0x5 }, | |
100 | { 16, 0x6 }, | |
101 | { 32, 0x7 }, | |
102 | }; | |
103 | ||
5685b145 LB |
104 | static int |
105 | st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr) | |
290a6ce1 LB |
106 | { |
107 | const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table); | |
5685b145 | 108 | u32 decimator = max_odr / sensor->odr; |
290a6ce1 LB |
109 | int i; |
110 | ||
5685b145 LB |
111 | if (decimator > 1) |
112 | decimator = round_down(decimator, 2); | |
113 | ||
114 | for (i = 0; i < max_size; i++) { | |
115 | if (st_lsm6dsx_decimator_table[i].decimator == decimator) | |
290a6ce1 | 116 | break; |
5685b145 | 117 | } |
290a6ce1 | 118 | |
7762902c | 119 | sensor->decimator = decimator; |
290a6ce1 LB |
120 | return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val; |
121 | } | |
122 | ||
123 | static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw, | |
f8710f03 | 124 | u32 *max_odr, u32 *min_odr) |
290a6ce1 LB |
125 | { |
126 | struct st_lsm6dsx_sensor *sensor; | |
127 | int i; | |
128 | ||
129 | *max_odr = 0, *min_odr = ~0; | |
130 | for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { | |
6ffb55e5 LB |
131 | if (!hw->iio_devs[i]) |
132 | continue; | |
133 | ||
290a6ce1 LB |
134 | sensor = iio_priv(hw->iio_devs[i]); |
135 | ||
136 | if (!(hw->enable_mask & BIT(sensor->id))) | |
137 | continue; | |
138 | ||
f8710f03 LB |
139 | *max_odr = max_t(u32, *max_odr, sensor->odr); |
140 | *min_odr = min_t(u32, *min_odr, sensor->odr); | |
290a6ce1 LB |
141 | } |
142 | } | |
143 | ||
5685b145 LB |
144 | static u8 st_lsm6dsx_get_sip(struct st_lsm6dsx_sensor *sensor, u32 min_odr) |
145 | { | |
146 | u8 sip = sensor->odr / min_odr; | |
147 | ||
148 | return sip > 1 ? round_down(sip, 2) : sip; | |
149 | } | |
150 | ||
290a6ce1 LB |
151 | static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw) |
152 | { | |
21345107 | 153 | const struct st_lsm6dsx_reg *ts_dec_reg; |
290a6ce1 | 154 | struct st_lsm6dsx_sensor *sensor; |
f8710f03 LB |
155 | u16 sip = 0, ts_sip = 0; |
156 | u32 max_odr, min_odr; | |
21345107 | 157 | int err = 0, i; |
290a6ce1 LB |
158 | u8 data; |
159 | ||
160 | st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr); | |
161 | ||
162 | for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { | |
7ca3ac9e | 163 | const struct st_lsm6dsx_reg *dec_reg; |
290a6ce1 | 164 | |
6ffb55e5 LB |
165 | if (!hw->iio_devs[i]) |
166 | continue; | |
167 | ||
7ca3ac9e | 168 | sensor = iio_priv(hw->iio_devs[i]); |
290a6ce1 LB |
169 | /* update fifo decimators and sample in pattern */ |
170 | if (hw->enable_mask & BIT(sensor->id)) { | |
5685b145 LB |
171 | sensor->sip = st_lsm6dsx_get_sip(sensor, min_odr); |
172 | data = st_lsm6dsx_get_decimator_val(sensor, max_odr); | |
290a6ce1 LB |
173 | } else { |
174 | sensor->sip = 0; | |
290a6ce1 LB |
175 | data = 0; |
176 | } | |
21345107 | 177 | ts_sip = max_t(u16, ts_sip, sensor->sip); |
290a6ce1 | 178 | |
7ca3ac9e LB |
179 | dec_reg = &hw->settings->decimator[sensor->id]; |
180 | if (dec_reg->addr) { | |
51a8b707 LB |
181 | int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask); |
182 | ||
739aff87 LB |
183 | err = st_lsm6dsx_update_bits_locked(hw, dec_reg->addr, |
184 | dec_reg->mask, | |
185 | val); | |
7ca3ac9e LB |
186 | if (err < 0) |
187 | return err; | |
188 | } | |
290a6ce1 LB |
189 | sip += sensor->sip; |
190 | } | |
21345107 LB |
191 | hw->sip = sip + ts_sip; |
192 | hw->ts_sip = ts_sip; | |
290a6ce1 | 193 | |
21345107 LB |
194 | /* |
195 | * update hw ts decimator if necessary. Decimator for hw timestamp | |
196 | * is always 1 or 0 in order to have a ts sample for each data | |
197 | * sample in FIFO | |
198 | */ | |
199 | ts_dec_reg = &hw->settings->ts_settings.decimator; | |
200 | if (ts_dec_reg->addr) { | |
201 | int val, ts_dec = !!hw->ts_sip; | |
202 | ||
203 | val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask); | |
739aff87 LB |
204 | err = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr, |
205 | ts_dec_reg->mask, val); | |
21345107 LB |
206 | } |
207 | return err; | |
290a6ce1 LB |
208 | } |
209 | ||
a1bab939 LB |
210 | static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw, |
211 | enum st_lsm6dsx_fifo_mode fifo_mode) | |
290a6ce1 | 212 | { |
739aff87 | 213 | unsigned int data; |
290a6ce1 | 214 | |
739aff87 | 215 | data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode); |
c2686eb2 LB |
216 | return st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR, |
217 | ST_LSM6DSX_FIFO_MODE_MASK, data); | |
290a6ce1 LB |
218 | } |
219 | ||
ff81a933 LB |
220 | static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor, |
221 | bool enable) | |
222 | { | |
223 | struct st_lsm6dsx_hw *hw = sensor->hw; | |
801a6e0a | 224 | const struct st_lsm6dsx_reg *batch_reg; |
ff81a933 LB |
225 | u8 data; |
226 | ||
801a6e0a LB |
227 | batch_reg = &hw->settings->batch[sensor->id]; |
228 | if (batch_reg->addr) { | |
229 | int val; | |
230 | ||
231 | if (enable) { | |
232 | int err; | |
233 | ||
234 | err = st_lsm6dsx_check_odr(sensor, sensor->odr, | |
235 | &data); | |
236 | if (err < 0) | |
237 | return err; | |
238 | } else { | |
239 | data = 0; | |
240 | } | |
241 | val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask); | |
739aff87 LB |
242 | return st_lsm6dsx_update_bits_locked(hw, batch_reg->addr, |
243 | batch_reg->mask, val); | |
801a6e0a LB |
244 | } else { |
245 | data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0; | |
739aff87 LB |
246 | return st_lsm6dsx_update_bits_locked(hw, |
247 | ST_LSM6DSX_REG_FIFO_MODE_ADDR, | |
248 | ST_LSM6DSX_FIFO_ODR_MASK, | |
249 | FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK, | |
250 | data)); | |
801a6e0a | 251 | } |
ff81a933 LB |
252 | } |
253 | ||
290a6ce1 LB |
254 | int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark) |
255 | { | |
a13bf65f | 256 | u16 fifo_watermark = ~0, cur_watermark, fifo_th_mask; |
290a6ce1 LB |
257 | struct st_lsm6dsx_hw *hw = sensor->hw; |
258 | struct st_lsm6dsx_sensor *cur_sensor; | |
51a8b707 | 259 | int i, err, data; |
290a6ce1 | 260 | __le16 wdata; |
290a6ce1 | 261 | |
a13bf65f LB |
262 | if (!hw->sip) |
263 | return 0; | |
264 | ||
290a6ce1 | 265 | for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { |
6ffb55e5 LB |
266 | if (!hw->iio_devs[i]) |
267 | continue; | |
268 | ||
290a6ce1 LB |
269 | cur_sensor = iio_priv(hw->iio_devs[i]); |
270 | ||
271 | if (!(hw->enable_mask & BIT(cur_sensor->id))) | |
272 | continue; | |
273 | ||
274 | cur_watermark = (cur_sensor == sensor) ? watermark | |
275 | : cur_sensor->watermark; | |
276 | ||
277 | fifo_watermark = min_t(u16, fifo_watermark, cur_watermark); | |
290a6ce1 LB |
278 | } |
279 | ||
a13bf65f LB |
280 | fifo_watermark = max_t(u16, fifo_watermark, hw->sip); |
281 | fifo_watermark = (fifo_watermark / hw->sip) * hw->sip; | |
92617c15 | 282 | fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl; |
290a6ce1 | 283 | |
739aff87 | 284 | mutex_lock(&hw->page_lock); |
51a8b707 LB |
285 | err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1, |
286 | &data); | |
290a6ce1 | 287 | if (err < 0) |
739aff87 | 288 | goto out; |
290a6ce1 | 289 | |
92617c15 LB |
290 | fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask; |
291 | fifo_watermark = ((data << 8) & ~fifo_th_mask) | | |
292 | (fifo_watermark & fifo_th_mask); | |
290a6ce1 LB |
293 | |
294 | wdata = cpu_to_le16(fifo_watermark); | |
739aff87 LB |
295 | err = regmap_bulk_write(hw->regmap, |
296 | hw->settings->fifo_ops.fifo_th.addr, | |
297 | &wdata, sizeof(wdata)); | |
298 | out: | |
299 | mutex_unlock(&hw->page_lock); | |
300 | return err; | |
51a8b707 | 301 | } |
290a6ce1 | 302 | |
21345107 LB |
303 | static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw) |
304 | { | |
305 | struct st_lsm6dsx_sensor *sensor; | |
306 | int i, err; | |
307 | ||
308 | /* reset hw ts counter */ | |
739aff87 LB |
309 | err = st_lsm6dsx_write_locked(hw, ST_LSM6DSX_REG_TS_RESET_ADDR, |
310 | ST_LSM6DSX_TS_RESET_VAL); | |
21345107 LB |
311 | if (err < 0) |
312 | return err; | |
313 | ||
314 | for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { | |
6ffb55e5 LB |
315 | if (!hw->iio_devs[i]) |
316 | continue; | |
317 | ||
21345107 LB |
318 | sensor = iio_priv(hw->iio_devs[i]); |
319 | /* | |
320 | * store enable buffer timestamp as reference for | |
321 | * hw timestamp | |
322 | */ | |
323 | sensor->ts_ref = iio_get_time_ns(hw->iio_devs[i]); | |
324 | } | |
325 | return 0; | |
326 | } | |
327 | ||
a1bab939 LB |
328 | int st_lsm6dsx_resume_fifo(struct st_lsm6dsx_hw *hw) |
329 | { | |
330 | int err; | |
331 | ||
332 | /* reset hw ts counter */ | |
333 | err = st_lsm6dsx_reset_hw_ts(hw); | |
334 | if (err < 0) | |
335 | return err; | |
336 | ||
337 | return st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT); | |
338 | } | |
339 | ||
51a8b707 | 340 | /* |
801a6e0a LB |
341 | * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN/ST_LSM6DSX_MAX_TAGGED_WORD_LEN |
342 | * in order to avoid a kmalloc for each bus access | |
51a8b707 | 343 | */ |
5b3c87fd LB |
344 | static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr, |
345 | u8 *data, unsigned int data_len, | |
346 | unsigned int max_word_len) | |
51a8b707 LB |
347 | { |
348 | unsigned int word_len, read_len = 0; | |
349 | int err; | |
350 | ||
351 | while (read_len < data_len) { | |
352 | word_len = min_t(unsigned int, data_len - read_len, | |
5b3c87fd | 353 | max_word_len); |
739aff87 LB |
354 | err = st_lsm6dsx_read_locked(hw, addr, data + read_len, |
355 | word_len); | |
51a8b707 LB |
356 | if (err < 0) |
357 | return err; | |
358 | read_len += word_len; | |
359 | } | |
360 | return 0; | |
290a6ce1 LB |
361 | } |
362 | ||
21345107 LB |
363 | #define ST_LSM6DSX_IIO_BUFF_SIZE (ALIGN(ST_LSM6DSX_SAMPLE_SIZE, \ |
364 | sizeof(s64)) + sizeof(s64)) | |
290a6ce1 | 365 | /** |
179c8d60 | 366 | * st_lsm6dsx_read_fifo() - hw FIFO read routine |
290a6ce1 LB |
367 | * @hw: Pointer to instance of struct st_lsm6dsx_hw. |
368 | * | |
369 | * Read samples from the hw FIFO and push them to IIO buffers. | |
370 | * | |
371 | * Return: Number of bytes read from the FIFO | |
372 | */ | |
50ff457d | 373 | int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) |
290a6ce1 | 374 | { |
e485e2a2 | 375 | struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL; |
7762902c | 376 | int err, sip, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset; |
290a6ce1 | 377 | u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE; |
92617c15 | 378 | u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask; |
21345107 | 379 | bool reset_ts = false; |
290a6ce1 | 380 | __le16 fifo_status; |
21345107 | 381 | s64 ts = 0; |
290a6ce1 | 382 | |
739aff87 LB |
383 | err = st_lsm6dsx_read_locked(hw, |
384 | hw->settings->fifo_ops.fifo_diff.addr, | |
385 | &fifo_status, sizeof(fifo_status)); | |
a4217498 LB |
386 | if (err < 0) { |
387 | dev_err(hw->dev, "failed to read fifo status (err=%d)\n", | |
388 | err); | |
290a6ce1 | 389 | return err; |
a4217498 | 390 | } |
290a6ce1 LB |
391 | |
392 | if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK)) | |
393 | return 0; | |
394 | ||
92617c15 | 395 | fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) * |
290a6ce1 | 396 | ST_LSM6DSX_CHAN_SIZE; |
290a6ce1 LB |
397 | fifo_len = (fifo_len / pattern_len) * pattern_len; |
398 | ||
290a6ce1 | 399 | acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]); |
290a6ce1 | 400 | gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]); |
e485e2a2 LB |
401 | if (hw->iio_devs[ST_LSM6DSX_ID_EXT0]) |
402 | ext_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_EXT0]); | |
290a6ce1 LB |
403 | |
404 | for (read_len = 0; read_len < fifo_len; read_len += pattern_len) { | |
5b3c87fd LB |
405 | err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR, |
406 | hw->buff, pattern_len, | |
407 | ST_LSM6DSX_MAX_WORD_LEN); | |
a4217498 LB |
408 | if (err < 0) { |
409 | dev_err(hw->dev, | |
410 | "failed to read pattern from fifo (err=%d)\n", | |
411 | err); | |
290a6ce1 | 412 | return err; |
a4217498 | 413 | } |
290a6ce1 LB |
414 | |
415 | /* | |
416 | * Data are written to the FIFO with a specific pattern | |
417 | * depending on the configured ODRs. The first sequence of data | |
418 | * stored in FIFO contains the data of all enabled sensors | |
21345107 | 419 | * (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated |
290a6ce1 LB |
420 | * depending on the value of the decimation factor set for each |
421 | * sensor. | |
422 | * | |
423 | * Supposing the FIFO is storing data from gyroscope and | |
424 | * accelerometer at different ODRs: | |
425 | * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz | |
426 | * Since the gyroscope ODR is twice the accelerometer one, the | |
427 | * following pattern is repeated every 9 samples: | |
21345107 | 428 | * - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, .. |
290a6ce1 | 429 | */ |
e485e2a2 | 430 | ext_sip = ext_sensor ? ext_sensor->sip : 0; |
290a6ce1 LB |
431 | gyro_sip = gyro_sensor->sip; |
432 | acc_sip = acc_sensor->sip; | |
21345107 | 433 | ts_sip = hw->ts_sip; |
290a6ce1 | 434 | offset = 0; |
7762902c | 435 | sip = 0; |
290a6ce1 | 436 | |
e485e2a2 | 437 | while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) { |
7762902c | 438 | if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) { |
c14edb4d JC |
439 | memcpy(hw->scan[ST_LSM6DSX_ID_GYRO].channels, |
440 | &hw->buff[offset], | |
441 | sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels)); | |
442 | offset += sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels); | |
290a6ce1 | 443 | } |
7762902c | 444 | if (acc_sip > 0 && !(sip % acc_sensor->decimator)) { |
c14edb4d JC |
445 | memcpy(hw->scan[ST_LSM6DSX_ID_ACC].channels, |
446 | &hw->buff[offset], | |
447 | sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels)); | |
448 | offset += sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels); | |
290a6ce1 | 449 | } |
7762902c | 450 | if (ext_sip > 0 && !(sip % ext_sensor->decimator)) { |
c14edb4d JC |
451 | memcpy(hw->scan[ST_LSM6DSX_ID_EXT0].channels, |
452 | &hw->buff[offset], | |
453 | sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels)); | |
454 | offset += sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels); | |
e485e2a2 | 455 | } |
21345107 LB |
456 | |
457 | if (ts_sip-- > 0) { | |
458 | u8 data[ST_LSM6DSX_SAMPLE_SIZE]; | |
459 | ||
460 | memcpy(data, &hw->buff[offset], sizeof(data)); | |
461 | /* | |
462 | * hw timestamp is 3B long and it is stored | |
463 | * in FIFO using 6B as 4th FIFO data set | |
464 | * according to this schema: | |
465 | * B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0] | |
466 | */ | |
467 | ts = data[1] << 16 | data[0] << 8 | data[3]; | |
468 | /* | |
469 | * check if hw timestamp engine is going to | |
470 | * reset (the sensor generates an interrupt | |
471 | * to signal the hw timestamp will reset in | |
472 | * 1.638s) | |
473 | */ | |
474 | if (!reset_ts && ts >= 0xff0000) | |
475 | reset_ts = true; | |
cb3b6b8e | 476 | ts *= hw->ts_gain; |
21345107 LB |
477 | |
478 | offset += ST_LSM6DSX_SAMPLE_SIZE; | |
479 | } | |
480 | ||
7762902c | 481 | if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) { |
db3c4905 LB |
482 | /* |
483 | * We need to discards gyro samples during | |
484 | * filters settling time | |
485 | */ | |
486 | if (gyro_sensor->samples_to_discard > 0) | |
487 | gyro_sensor->samples_to_discard--; | |
488 | else | |
489 | iio_push_to_buffers_with_timestamp( | |
490 | hw->iio_devs[ST_LSM6DSX_ID_GYRO], | |
491 | &hw->scan[ST_LSM6DSX_ID_GYRO], | |
492 | gyro_sensor->ts_ref + ts); | |
7762902c LB |
493 | gyro_sip--; |
494 | } | |
495 | if (acc_sip > 0 && !(sip % acc_sensor->decimator)) { | |
db3c4905 LB |
496 | /* |
497 | * We need to discards accel samples during | |
498 | * filters settling time | |
499 | */ | |
500 | if (acc_sensor->samples_to_discard > 0) | |
501 | acc_sensor->samples_to_discard--; | |
502 | else | |
503 | iio_push_to_buffers_with_timestamp( | |
504 | hw->iio_devs[ST_LSM6DSX_ID_ACC], | |
505 | &hw->scan[ST_LSM6DSX_ID_ACC], | |
506 | acc_sensor->ts_ref + ts); | |
7762902c LB |
507 | acc_sip--; |
508 | } | |
509 | if (ext_sip > 0 && !(sip % ext_sensor->decimator)) { | |
e485e2a2 LB |
510 | iio_push_to_buffers_with_timestamp( |
511 | hw->iio_devs[ST_LSM6DSX_ID_EXT0], | |
c14edb4d JC |
512 | &hw->scan[ST_LSM6DSX_ID_EXT0], |
513 | ext_sensor->ts_ref + ts); | |
7762902c LB |
514 | ext_sip--; |
515 | } | |
516 | sip++; | |
290a6ce1 LB |
517 | } |
518 | } | |
519 | ||
21345107 LB |
520 | if (unlikely(reset_ts)) { |
521 | err = st_lsm6dsx_reset_hw_ts(hw); | |
a4217498 LB |
522 | if (err < 0) { |
523 | dev_err(hw->dev, "failed to reset hw ts (err=%d)\n", | |
524 | err); | |
21345107 | 525 | return err; |
a4217498 | 526 | } |
21345107 | 527 | } |
290a6ce1 LB |
528 | return read_len; |
529 | } | |
530 | ||
960506ed | 531 | #define ST_LSM6DSX_INVALID_SAMPLE 0x7ffd |
14c7c6e1 LB |
532 | static int |
533 | st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag, | |
534 | u8 *data, s64 ts) | |
535 | { | |
960506ed | 536 | s16 val = le16_to_cpu(*(__le16 *)data); |
14c7c6e1 LB |
537 | struct st_lsm6dsx_sensor *sensor; |
538 | struct iio_dev *iio_dev; | |
539 | ||
960506ed LB |
540 | /* invalid sample during bootstrap phase */ |
541 | if (val >= ST_LSM6DSX_INVALID_SAMPLE) | |
542 | return -EINVAL; | |
543 | ||
6d0205fd LB |
544 | /* |
545 | * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG | |
546 | * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG | |
547 | * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled | |
548 | * channel | |
549 | */ | |
14c7c6e1 LB |
550 | switch (tag) { |
551 | case ST_LSM6DSX_GYRO_TAG: | |
552 | iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO]; | |
553 | break; | |
554 | case ST_LSM6DSX_ACC_TAG: | |
555 | iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC]; | |
556 | break; | |
6d0205fd LB |
557 | case ST_LSM6DSX_EXT0_TAG: |
558 | if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) | |
559 | iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0]; | |
560 | else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)) | |
561 | iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1]; | |
562 | else | |
563 | iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2]; | |
564 | break; | |
565 | case ST_LSM6DSX_EXT1_TAG: | |
566 | if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) && | |
567 | (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))) | |
568 | iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1]; | |
569 | else | |
570 | iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2]; | |
571 | break; | |
572 | case ST_LSM6DSX_EXT2_TAG: | |
573 | iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2]; | |
574 | break; | |
14c7c6e1 LB |
575 | default: |
576 | return -EINVAL; | |
577 | } | |
578 | ||
579 | sensor = iio_priv(iio_dev); | |
580 | iio_push_to_buffers_with_timestamp(iio_dev, data, | |
581 | ts + sensor->ts_ref); | |
582 | ||
583 | return 0; | |
584 | } | |
585 | ||
801a6e0a | 586 | /** |
43901008 | 587 | * st_lsm6dsx_read_tagged_fifo() - tagged hw FIFO read routine |
801a6e0a LB |
588 | * @hw: Pointer to instance of struct st_lsm6dsx_hw. |
589 | * | |
590 | * Read samples from the hw FIFO and push them to IIO buffers. | |
591 | * | |
592 | * Return: Number of bytes read from the FIFO | |
593 | */ | |
594 | int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw) | |
595 | { | |
596 | u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE; | |
597 | u16 fifo_len, fifo_diff_mask; | |
c14edb4d JC |
598 | /* |
599 | * Alignment needed as this can ultimately be passed to a | |
600 | * call to iio_push_to_buffers_with_timestamp() which | |
601 | * must be passed a buffer that is aligned to 8 bytes so | |
602 | * as to allow insertion of a naturally aligned timestamp. | |
603 | */ | |
604 | u8 iio_buff[ST_LSM6DSX_IIO_BUFF_SIZE] __aligned(8); | |
605 | u8 tag; | |
801a6e0a LB |
606 | bool reset_ts = false; |
607 | int i, err, read_len; | |
608 | __le16 fifo_status; | |
609 | s64 ts = 0; | |
610 | ||
739aff87 LB |
611 | err = st_lsm6dsx_read_locked(hw, |
612 | hw->settings->fifo_ops.fifo_diff.addr, | |
613 | &fifo_status, sizeof(fifo_status)); | |
801a6e0a LB |
614 | if (err < 0) { |
615 | dev_err(hw->dev, "failed to read fifo status (err=%d)\n", | |
616 | err); | |
617 | return err; | |
618 | } | |
619 | ||
620 | fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask; | |
621 | fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) * | |
622 | ST_LSM6DSX_TAGGED_SAMPLE_SIZE; | |
623 | if (!fifo_len) | |
624 | return 0; | |
625 | ||
801a6e0a LB |
626 | for (read_len = 0; read_len < fifo_len; read_len += pattern_len) { |
627 | err = st_lsm6dsx_read_block(hw, | |
628 | ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR, | |
629 | hw->buff, pattern_len, | |
630 | ST_LSM6DSX_MAX_TAGGED_WORD_LEN); | |
631 | if (err < 0) { | |
632 | dev_err(hw->dev, | |
633 | "failed to read pattern from fifo (err=%d)\n", | |
634 | err); | |
635 | return err; | |
636 | } | |
637 | ||
638 | for (i = 0; i < pattern_len; | |
639 | i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) { | |
640 | memcpy(iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE], | |
641 | ST_LSM6DSX_SAMPLE_SIZE); | |
642 | ||
643 | tag = hw->buff[i] >> 3; | |
14c7c6e1 | 644 | if (tag == ST_LSM6DSX_TS_TAG) { |
801a6e0a LB |
645 | /* |
646 | * hw timestamp is 4B long and it is stored | |
647 | * in FIFO according to this schema: | |
648 | * B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16], | |
649 | * B3 = ts[31:24] | |
650 | */ | |
651 | ts = le32_to_cpu(*((__le32 *)iio_buff)); | |
652 | /* | |
653 | * check if hw timestamp engine is going to | |
654 | * reset (the sensor generates an interrupt | |
655 | * to signal the hw timestamp will reset in | |
656 | * 1.638s) | |
657 | */ | |
658 | if (!reset_ts && ts >= 0xffff0000) | |
659 | reset_ts = true; | |
cb3b6b8e | 660 | ts *= hw->ts_gain; |
14c7c6e1 LB |
661 | } else { |
662 | st_lsm6dsx_push_tagged_data(hw, tag, iio_buff, | |
663 | ts); | |
801a6e0a LB |
664 | } |
665 | } | |
666 | } | |
667 | ||
668 | if (unlikely(reset_ts)) { | |
669 | err = st_lsm6dsx_reset_hw_ts(hw); | |
670 | if (err < 0) | |
671 | return err; | |
672 | } | |
673 | return read_len; | |
674 | } | |
675 | ||
535de397 | 676 | int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw) |
290a6ce1 LB |
677 | { |
678 | int err; | |
679 | ||
a912ee4c LB |
680 | if (!hw->settings->fifo_ops.read_fifo) |
681 | return -ENOTSUPP; | |
682 | ||
290a6ce1 LB |
683 | mutex_lock(&hw->fifo_lock); |
684 | ||
50ff457d | 685 | hw->settings->fifo_ops.read_fifo(hw); |
290a6ce1 LB |
686 | err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS); |
687 | ||
688 | mutex_unlock(&hw->fifo_lock); | |
689 | ||
690 | return err; | |
691 | } | |
692 | ||
db3c4905 LB |
693 | static void |
694 | st_lsm6dsx_update_samples_to_discard(struct st_lsm6dsx_sensor *sensor) | |
695 | { | |
696 | const struct st_lsm6dsx_samples_to_discard *data; | |
697 | struct st_lsm6dsx_hw *hw = sensor->hw; | |
698 | int i; | |
699 | ||
700 | if (sensor->id != ST_LSM6DSX_ID_GYRO && | |
701 | sensor->id != ST_LSM6DSX_ID_ACC) | |
702 | return; | |
703 | ||
704 | /* check if drdy mask is supported in hw */ | |
705 | if (hw->settings->drdy_mask.addr) | |
706 | return; | |
707 | ||
708 | data = &hw->settings->samples_to_discard[sensor->id]; | |
709 | for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) { | |
710 | if (data->val[i].milli_hz == sensor->odr) { | |
711 | sensor->samples_to_discard = data->val[i].samples; | |
712 | return; | |
713 | } | |
714 | } | |
715 | } | |
716 | ||
3b72950d | 717 | int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable) |
290a6ce1 | 718 | { |
290a6ce1 | 719 | struct st_lsm6dsx_hw *hw = sensor->hw; |
c2686eb2 | 720 | u8 fifo_mask; |
290a6ce1 LB |
721 | int err; |
722 | ||
335eaedc LB |
723 | mutex_lock(&hw->conf_lock); |
724 | ||
c2686eb2 LB |
725 | if (enable) |
726 | fifo_mask = hw->fifo_mask | BIT(sensor->id); | |
727 | else | |
728 | fifo_mask = hw->fifo_mask & ~BIT(sensor->id); | |
729 | ||
730 | if (hw->fifo_mask) { | |
290a6ce1 LB |
731 | err = st_lsm6dsx_flush_fifo(hw); |
732 | if (err < 0) | |
335eaedc | 733 | goto out; |
290a6ce1 LB |
734 | } |
735 | ||
db3c4905 LB |
736 | if (enable) |
737 | st_lsm6dsx_update_samples_to_discard(sensor); | |
738 | ||
cd83c5c1 LB |
739 | err = st_lsm6dsx_device_set_enable(sensor, enable); |
740 | if (err < 0) | |
741 | goto out; | |
ff81a933 | 742 | |
e485e2a2 LB |
743 | err = st_lsm6dsx_set_fifo_odr(sensor, enable); |
744 | if (err < 0) | |
745 | goto out; | |
746 | ||
290a6ce1 LB |
747 | err = st_lsm6dsx_update_decimators(hw); |
748 | if (err < 0) | |
335eaedc | 749 | goto out; |
290a6ce1 LB |
750 | |
751 | err = st_lsm6dsx_update_watermark(sensor, sensor->watermark); | |
752 | if (err < 0) | |
335eaedc | 753 | goto out; |
290a6ce1 | 754 | |
c2686eb2 | 755 | if (fifo_mask) { |
a1bab939 | 756 | err = st_lsm6dsx_resume_fifo(hw); |
c2686eb2 LB |
757 | if (err < 0) |
758 | goto out; | |
290a6ce1 LB |
759 | } |
760 | ||
c2686eb2 LB |
761 | hw->fifo_mask = fifo_mask; |
762 | ||
335eaedc LB |
763 | out: |
764 | mutex_unlock(&hw->conf_lock); | |
765 | ||
766 | return err; | |
290a6ce1 LB |
767 | } |
768 | ||
290a6ce1 LB |
769 | static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev) |
770 | { | |
3b72950d LB |
771 | struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); |
772 | struct st_lsm6dsx_hw *hw = sensor->hw; | |
773 | ||
774 | if (!hw->settings->fifo_ops.update_fifo) | |
775 | return -ENOTSUPP; | |
776 | ||
777 | return hw->settings->fifo_ops.update_fifo(sensor, true); | |
290a6ce1 LB |
778 | } |
779 | ||
780 | static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev) | |
781 | { | |
3b72950d LB |
782 | struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); |
783 | struct st_lsm6dsx_hw *hw = sensor->hw; | |
784 | ||
785 | if (!hw->settings->fifo_ops.update_fifo) | |
786 | return -ENOTSUPP; | |
787 | ||
788 | return hw->settings->fifo_ops.update_fifo(sensor, false); | |
290a6ce1 LB |
789 | } |
790 | ||
791 | static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = { | |
792 | .preenable = st_lsm6dsx_buffer_preenable, | |
793 | .postdisable = st_lsm6dsx_buffer_postdisable, | |
794 | }; | |
795 | ||
796 | int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw) | |
797 | { | |
17395ce2 | 798 | int i, ret; |
290a6ce1 LB |
799 | |
800 | for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { | |
6ffb55e5 LB |
801 | if (!hw->iio_devs[i]) |
802 | continue; | |
803 | ||
17395ce2 | 804 | ret = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[i], |
17395ce2 AA |
805 | &st_lsm6dsx_buffer_ops); |
806 | if (ret) | |
807 | return ret; | |
290a6ce1 LB |
808 | } |
809 | ||
810 | return 0; | |
811 | } |