]>
Commit | Line | Data |
---|---|---|
290a6ce1 LB |
1 | /* |
2 | * STMicroelectronics st_lsm6dsx FIFO buffer library driver | |
3 | * | |
4 | * LSM6DS3/LSM6DSM: The FIFO buffer can be configured to store data | |
5 | * from gyroscope and accelerometer. Samples are queued without any tag | |
6 | * according to a specific pattern based on 'FIFO data sets' (6 bytes each): | |
7 | * - 1st data set is reserved for gyroscope data | |
8 | * - 2nd data set is reserved for accelerometer data | |
9 | * The FIFO pattern changes depending on the ODRs and decimation factors | |
10 | * assigned to the FIFO data sets. The first sequence of data stored in FIFO | |
11 | * buffer contains the data of all the enabled FIFO data sets | |
12 | * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the | |
13 | * value of the decimation factor and ODR set for each FIFO data set. | |
14 | * FIFO supported modes: | |
15 | * - BYPASS: FIFO disabled | |
16 | * - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index | |
17 | * restarts from the beginning and the oldest sample is overwritten | |
18 | * | |
19 | * Copyright 2016 STMicroelectronics Inc. | |
20 | * | |
21 | * Lorenzo Bianconi <lorenzo.bianconi@st.com> | |
22 | * Denis Ciocca <denis.ciocca@st.com> | |
23 | * | |
24 | * Licensed under the GPL-2. | |
25 | */ | |
26 | #include <linux/module.h> | |
27 | #include <linux/interrupt.h> | |
28 | #include <linux/irq.h> | |
29 | #include <linux/iio/kfifo_buf.h> | |
30 | #include <linux/iio/iio.h> | |
31 | #include <linux/iio/buffer.h> | |
32 | ||
33 | #include "st_lsm6dsx.h" | |
34 | ||
35 | #define ST_LSM6DSX_REG_FIFO_THL_ADDR 0x06 | |
36 | #define ST_LSM6DSX_REG_FIFO_THH_ADDR 0x07 | |
37 | #define ST_LSM6DSX_FIFO_TH_MASK GENMASK(11, 0) | |
38 | #define ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR 0x08 | |
39 | #define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a | |
40 | #define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0) | |
41 | #define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3) | |
42 | #define ST_LSM6DSX_REG_FIFO_DIFFL_ADDR 0x3a | |
43 | #define ST_LSM6DSX_FIFO_DIFF_MASK GENMASK(11, 0) | |
44 | #define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12) | |
45 | #define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e | |
46 | ||
47 | #define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08 | |
48 | ||
49 | struct st_lsm6dsx_decimator_entry { | |
50 | u8 decimator; | |
51 | u8 val; | |
52 | }; | |
53 | ||
54 | static const | |
55 | struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = { | |
56 | { 0, 0x0 }, | |
57 | { 1, 0x1 }, | |
58 | { 2, 0x2 }, | |
59 | { 3, 0x3 }, | |
60 | { 4, 0x4 }, | |
61 | { 8, 0x5 }, | |
62 | { 16, 0x6 }, | |
63 | { 32, 0x7 }, | |
64 | }; | |
65 | ||
66 | static int st_lsm6dsx_get_decimator_val(u8 val) | |
67 | { | |
68 | const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table); | |
69 | int i; | |
70 | ||
71 | for (i = 0; i < max_size; i++) | |
72 | if (st_lsm6dsx_decimator_table[i].decimator == val) | |
73 | break; | |
74 | ||
75 | return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val; | |
76 | } | |
77 | ||
78 | static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw, | |
79 | u16 *max_odr, u16 *min_odr) | |
80 | { | |
81 | struct st_lsm6dsx_sensor *sensor; | |
82 | int i; | |
83 | ||
84 | *max_odr = 0, *min_odr = ~0; | |
85 | for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { | |
86 | sensor = iio_priv(hw->iio_devs[i]); | |
87 | ||
88 | if (!(hw->enable_mask & BIT(sensor->id))) | |
89 | continue; | |
90 | ||
91 | *max_odr = max_t(u16, *max_odr, sensor->odr); | |
92 | *min_odr = min_t(u16, *min_odr, sensor->odr); | |
93 | } | |
94 | } | |
95 | ||
96 | static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw) | |
97 | { | |
98 | struct st_lsm6dsx_sensor *sensor; | |
99 | u16 max_odr, min_odr, sip = 0; | |
100 | int err, i; | |
101 | u8 data; | |
102 | ||
103 | st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr); | |
104 | ||
105 | for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { | |
106 | sensor = iio_priv(hw->iio_devs[i]); | |
107 | ||
108 | /* update fifo decimators and sample in pattern */ | |
109 | if (hw->enable_mask & BIT(sensor->id)) { | |
110 | sensor->sip = sensor->odr / min_odr; | |
111 | sensor->decimator = max_odr / sensor->odr; | |
112 | data = st_lsm6dsx_get_decimator_val(sensor->decimator); | |
113 | } else { | |
114 | sensor->sip = 0; | |
115 | sensor->decimator = 0; | |
116 | data = 0; | |
117 | } | |
118 | ||
119 | err = st_lsm6dsx_write_with_mask(hw, | |
120 | ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR, | |
121 | sensor->decimator_mask, data); | |
122 | if (err < 0) | |
123 | return err; | |
124 | ||
125 | sip += sensor->sip; | |
126 | } | |
127 | hw->sip = sip; | |
128 | ||
129 | return 0; | |
130 | } | |
131 | ||
132 | static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw, | |
133 | enum st_lsm6dsx_fifo_mode fifo_mode) | |
134 | { | |
135 | u8 data; | |
136 | int err; | |
137 | ||
138 | switch (fifo_mode) { | |
139 | case ST_LSM6DSX_FIFO_BYPASS: | |
140 | data = fifo_mode; | |
141 | break; | |
142 | case ST_LSM6DSX_FIFO_CONT: | |
143 | data = (ST_LSM6DSX_MAX_FIFO_ODR_VAL << | |
144 | __ffs(ST_LSM6DSX_FIFO_ODR_MASK)) | fifo_mode; | |
145 | break; | |
146 | default: | |
147 | return -EINVAL; | |
148 | } | |
149 | ||
150 | err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_MODE_ADDR, | |
151 | sizeof(data), &data); | |
152 | if (err < 0) | |
153 | return err; | |
154 | ||
155 | hw->fifo_mode = fifo_mode; | |
156 | ||
157 | return 0; | |
158 | } | |
159 | ||
160 | int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark) | |
161 | { | |
162 | u16 fifo_watermark = ~0, cur_watermark, sip = 0; | |
163 | struct st_lsm6dsx_hw *hw = sensor->hw; | |
164 | struct st_lsm6dsx_sensor *cur_sensor; | |
165 | __le16 wdata; | |
166 | int i, err; | |
167 | u8 data; | |
168 | ||
169 | for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { | |
170 | cur_sensor = iio_priv(hw->iio_devs[i]); | |
171 | ||
172 | if (!(hw->enable_mask & BIT(cur_sensor->id))) | |
173 | continue; | |
174 | ||
175 | cur_watermark = (cur_sensor == sensor) ? watermark | |
176 | : cur_sensor->watermark; | |
177 | ||
178 | fifo_watermark = min_t(u16, fifo_watermark, cur_watermark); | |
179 | sip += cur_sensor->sip; | |
180 | } | |
181 | ||
182 | if (!sip) | |
183 | return 0; | |
184 | ||
185 | fifo_watermark = max_t(u16, fifo_watermark, sip); | |
186 | fifo_watermark = (fifo_watermark / sip) * sip; | |
187 | fifo_watermark = fifo_watermark * ST_LSM6DSX_SAMPLE_DEPTH; | |
188 | ||
189 | mutex_lock(&hw->lock); | |
190 | ||
191 | err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_THH_ADDR, | |
192 | sizeof(data), &data); | |
193 | if (err < 0) | |
194 | goto out; | |
195 | ||
6985bd5e LB |
196 | fifo_watermark = ((data << 8) & ~ST_LSM6DSX_FIFO_TH_MASK) | |
197 | (fifo_watermark & ST_LSM6DSX_FIFO_TH_MASK); | |
290a6ce1 LB |
198 | |
199 | wdata = cpu_to_le16(fifo_watermark); | |
200 | err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_THL_ADDR, | |
201 | sizeof(wdata), (u8 *)&wdata); | |
202 | out: | |
203 | mutex_unlock(&hw->lock); | |
204 | ||
205 | return err < 0 ? err : 0; | |
206 | } | |
207 | ||
208 | /** | |
209 | * st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DSM read FIFO routine | |
210 | * @hw: Pointer to instance of struct st_lsm6dsx_hw. | |
211 | * | |
212 | * Read samples from the hw FIFO and push them to IIO buffers. | |
213 | * | |
214 | * Return: Number of bytes read from the FIFO | |
215 | */ | |
216 | static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) | |
217 | { | |
218 | u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE; | |
219 | int err, acc_sip, gyro_sip, read_len, samples, offset; | |
220 | struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor; | |
221 | s64 acc_ts, acc_delta_ts, gyro_ts, gyro_delta_ts; | |
222 | u8 iio_buff[ALIGN(ST_LSM6DSX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)]; | |
223 | u8 buff[pattern_len]; | |
224 | __le16 fifo_status; | |
225 | ||
226 | err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_DIFFL_ADDR, | |
227 | sizeof(fifo_status), (u8 *)&fifo_status); | |
228 | if (err < 0) | |
229 | return err; | |
230 | ||
231 | if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK)) | |
232 | return 0; | |
233 | ||
234 | fifo_len = (le16_to_cpu(fifo_status) & ST_LSM6DSX_FIFO_DIFF_MASK) * | |
235 | ST_LSM6DSX_CHAN_SIZE; | |
236 | samples = fifo_len / ST_LSM6DSX_SAMPLE_SIZE; | |
237 | fifo_len = (fifo_len / pattern_len) * pattern_len; | |
238 | ||
239 | /* | |
240 | * compute delta timestamp between two consecutive samples | |
241 | * in order to estimate queueing time of data generated | |
242 | * by the sensor | |
243 | */ | |
244 | acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]); | |
245 | acc_ts = acc_sensor->ts - acc_sensor->delta_ts; | |
246 | acc_delta_ts = div_s64(acc_sensor->delta_ts * acc_sensor->decimator, | |
247 | samples); | |
248 | ||
249 | gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]); | |
250 | gyro_ts = gyro_sensor->ts - gyro_sensor->delta_ts; | |
251 | gyro_delta_ts = div_s64(gyro_sensor->delta_ts * gyro_sensor->decimator, | |
252 | samples); | |
253 | ||
254 | for (read_len = 0; read_len < fifo_len; read_len += pattern_len) { | |
255 | err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_OUTL_ADDR, | |
256 | sizeof(buff), buff); | |
257 | if (err < 0) | |
258 | return err; | |
259 | ||
260 | /* | |
261 | * Data are written to the FIFO with a specific pattern | |
262 | * depending on the configured ODRs. The first sequence of data | |
263 | * stored in FIFO contains the data of all enabled sensors | |
264 | * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated | |
265 | * depending on the value of the decimation factor set for each | |
266 | * sensor. | |
267 | * | |
268 | * Supposing the FIFO is storing data from gyroscope and | |
269 | * accelerometer at different ODRs: | |
270 | * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz | |
271 | * Since the gyroscope ODR is twice the accelerometer one, the | |
272 | * following pattern is repeated every 9 samples: | |
273 | * - Gx, Gy, Gz, Ax, Ay, Az, Gx, Gy, Gz | |
274 | */ | |
275 | gyro_sip = gyro_sensor->sip; | |
276 | acc_sip = acc_sensor->sip; | |
277 | offset = 0; | |
278 | ||
279 | while (acc_sip > 0 || gyro_sip > 0) { | |
280 | if (gyro_sip-- > 0) { | |
281 | memcpy(iio_buff, &buff[offset], | |
282 | ST_LSM6DSX_SAMPLE_SIZE); | |
283 | iio_push_to_buffers_with_timestamp( | |
284 | hw->iio_devs[ST_LSM6DSX_ID_GYRO], | |
285 | iio_buff, gyro_ts); | |
286 | offset += ST_LSM6DSX_SAMPLE_SIZE; | |
287 | gyro_ts += gyro_delta_ts; | |
288 | } | |
289 | ||
290 | if (acc_sip-- > 0) { | |
291 | memcpy(iio_buff, &buff[offset], | |
292 | ST_LSM6DSX_SAMPLE_SIZE); | |
293 | iio_push_to_buffers_with_timestamp( | |
294 | hw->iio_devs[ST_LSM6DSX_ID_ACC], | |
295 | iio_buff, acc_ts); | |
296 | offset += ST_LSM6DSX_SAMPLE_SIZE; | |
297 | acc_ts += acc_delta_ts; | |
298 | } | |
299 | } | |
300 | } | |
301 | ||
302 | return read_len; | |
303 | } | |
304 | ||
305 | static int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw) | |
306 | { | |
307 | int err; | |
308 | ||
309 | mutex_lock(&hw->fifo_lock); | |
310 | ||
311 | st_lsm6dsx_read_fifo(hw); | |
312 | err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS); | |
313 | ||
314 | mutex_unlock(&hw->fifo_lock); | |
315 | ||
316 | return err; | |
317 | } | |
318 | ||
319 | static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable) | |
320 | { | |
321 | struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); | |
322 | struct st_lsm6dsx_hw *hw = sensor->hw; | |
323 | int err; | |
324 | ||
325 | if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) { | |
326 | err = st_lsm6dsx_flush_fifo(hw); | |
327 | if (err < 0) | |
328 | return err; | |
329 | } | |
330 | ||
331 | if (enable) { | |
332 | err = st_lsm6dsx_sensor_enable(sensor); | |
333 | if (err < 0) | |
334 | return err; | |
335 | } else { | |
336 | err = st_lsm6dsx_sensor_disable(sensor); | |
337 | if (err < 0) | |
338 | return err; | |
339 | } | |
340 | ||
341 | err = st_lsm6dsx_update_decimators(hw); | |
342 | if (err < 0) | |
343 | return err; | |
344 | ||
345 | err = st_lsm6dsx_update_watermark(sensor, sensor->watermark); | |
346 | if (err < 0) | |
347 | return err; | |
348 | ||
349 | if (hw->enable_mask) { | |
350 | err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT); | |
351 | if (err < 0) | |
352 | return err; | |
353 | ||
354 | /* | |
355 | * store enable buffer timestamp as reference to compute | |
356 | * first delta timestamp | |
357 | */ | |
358 | sensor->ts = iio_get_time_ns(iio_dev); | |
359 | } | |
360 | ||
361 | return 0; | |
362 | } | |
363 | ||
364 | static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private) | |
365 | { | |
366 | struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private; | |
367 | struct st_lsm6dsx_sensor *sensor; | |
368 | int i; | |
369 | ||
370 | if (!hw->sip) | |
371 | return IRQ_NONE; | |
372 | ||
373 | for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { | |
374 | sensor = iio_priv(hw->iio_devs[i]); | |
375 | ||
376 | if (sensor->sip > 0) { | |
377 | s64 timestamp; | |
378 | ||
379 | timestamp = iio_get_time_ns(hw->iio_devs[i]); | |
380 | sensor->delta_ts = timestamp - sensor->ts; | |
381 | sensor->ts = timestamp; | |
382 | } | |
383 | } | |
384 | ||
385 | return IRQ_WAKE_THREAD; | |
386 | } | |
387 | ||
388 | static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private) | |
389 | { | |
390 | struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private; | |
391 | int count; | |
392 | ||
393 | mutex_lock(&hw->fifo_lock); | |
394 | count = st_lsm6dsx_read_fifo(hw); | |
395 | mutex_unlock(&hw->fifo_lock); | |
396 | ||
397 | return !count ? IRQ_NONE : IRQ_HANDLED; | |
398 | } | |
399 | ||
400 | static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev) | |
401 | { | |
402 | return st_lsm6dsx_update_fifo(iio_dev, true); | |
403 | } | |
404 | ||
405 | static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev) | |
406 | { | |
407 | return st_lsm6dsx_update_fifo(iio_dev, false); | |
408 | } | |
409 | ||
410 | static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = { | |
411 | .preenable = st_lsm6dsx_buffer_preenable, | |
412 | .postdisable = st_lsm6dsx_buffer_postdisable, | |
413 | }; | |
414 | ||
415 | int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw) | |
416 | { | |
417 | struct iio_buffer *buffer; | |
418 | unsigned long irq_type; | |
419 | int i, err; | |
420 | ||
421 | irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq)); | |
422 | ||
423 | switch (irq_type) { | |
424 | case IRQF_TRIGGER_HIGH: | |
425 | case IRQF_TRIGGER_RISING: | |
426 | break; | |
427 | default: | |
428 | dev_info(hw->dev, "mode %lx unsupported\n", irq_type); | |
429 | return -EINVAL; | |
430 | } | |
431 | ||
432 | err = devm_request_threaded_irq(hw->dev, hw->irq, | |
433 | st_lsm6dsx_handler_irq, | |
434 | st_lsm6dsx_handler_thread, | |
435 | irq_type | IRQF_ONESHOT, | |
436 | "lsm6dsx", hw); | |
437 | if (err) { | |
438 | dev_err(hw->dev, "failed to request trigger irq %d\n", | |
439 | hw->irq); | |
440 | return err; | |
441 | } | |
442 | ||
443 | for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { | |
444 | buffer = devm_iio_kfifo_allocate(hw->dev); | |
445 | if (!buffer) | |
446 | return -ENOMEM; | |
447 | ||
448 | iio_device_attach_buffer(hw->iio_devs[i], buffer); | |
449 | hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE; | |
450 | hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops; | |
451 | } | |
452 | ||
453 | return 0; | |
454 | } |