1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34#include <linux/kernel.h>
35#include <linux/module.h>
36#include <linux/delay.h>
37#include <linux/iio/iio.h>
38#include <linux/iio/sysfs.h>
39#include <linux/pm.h>
40#include <linux/regmap.h>
41#include <linux/bitfield.h>
42
43#include <linux/platform_data/st_sensors_pdata.h>
44
45#include "st_lsm6dsx.h"
46
47#define ST_LSM6DSX_REG_INT1_ADDR 0x0d
48#define ST_LSM6DSX_REG_INT2_ADDR 0x0e
49#define ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK BIT(3)
50#define ST_LSM6DSX_REG_WHOAMI_ADDR 0x0f
51#define ST_LSM6DSX_REG_RESET_ADDR 0x12
52#define ST_LSM6DSX_REG_RESET_MASK BIT(0)
53#define ST_LSM6DSX_REG_BDU_ADDR 0x12
54#define ST_LSM6DSX_REG_BDU_MASK BIT(6)
55#define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR 0x13
56#define ST_LSM6DSX_REG_INT2_ON_INT1_MASK BIT(5)
57
58#define ST_LSM6DSX_REG_ACC_ODR_ADDR 0x10
59#define ST_LSM6DSX_REG_ACC_ODR_MASK GENMASK(7, 4)
60#define ST_LSM6DSX_REG_ACC_FS_ADDR 0x10
61#define ST_LSM6DSX_REG_ACC_FS_MASK GENMASK(3, 2)
62#define ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR 0x28
63#define ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR 0x2a
64#define ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR 0x2c
65
66#define ST_LSM6DSX_REG_GYRO_ODR_ADDR 0x11
67#define ST_LSM6DSX_REG_GYRO_ODR_MASK GENMASK(7, 4)
68#define ST_LSM6DSX_REG_GYRO_FS_ADDR 0x11
69#define ST_LSM6DSX_REG_GYRO_FS_MASK GENMASK(3, 2)
70#define ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR 0x22
71#define ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR 0x24
72#define ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR 0x26
73
74#define ST_LSM6DSX_ACC_FS_2G_GAIN IIO_G_TO_M_S_2(61)
75#define ST_LSM6DSX_ACC_FS_4G_GAIN IIO_G_TO_M_S_2(122)
76#define ST_LSM6DSX_ACC_FS_8G_GAIN IIO_G_TO_M_S_2(244)
77#define ST_LSM6DSX_ACC_FS_16G_GAIN IIO_G_TO_M_S_2(488)
78
79#define ST_LSM6DSX_GYRO_FS_245_GAIN IIO_DEGREE_TO_RAD(8750)
80#define ST_LSM6DSX_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(17500)
81#define ST_LSM6DSX_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(35000)
82#define ST_LSM6DSX_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000)
83
84struct st_lsm6dsx_odr {
85 u16 hz;
86 u8 val;
87};
88
89#define ST_LSM6DSX_ODR_LIST_SIZE 6
90struct st_lsm6dsx_odr_table_entry {
91 struct st_lsm6dsx_reg reg;
92 struct st_lsm6dsx_odr odr_avl[ST_LSM6DSX_ODR_LIST_SIZE];
93};
94
95static const struct st_lsm6dsx_odr_table_entry st_lsm6dsx_odr_table[] = {
96 [ST_LSM6DSX_ID_ACC] = {
97 .reg = {
98 .addr = ST_LSM6DSX_REG_ACC_ODR_ADDR,
99 .mask = ST_LSM6DSX_REG_ACC_ODR_MASK,
100 },
101 .odr_avl[0] = { 13, 0x01 },
102 .odr_avl[1] = { 26, 0x02 },
103 .odr_avl[2] = { 52, 0x03 },
104 .odr_avl[3] = { 104, 0x04 },
105 .odr_avl[4] = { 208, 0x05 },
106 .odr_avl[5] = { 416, 0x06 },
107 },
108 [ST_LSM6DSX_ID_GYRO] = {
109 .reg = {
110 .addr = ST_LSM6DSX_REG_GYRO_ODR_ADDR,
111 .mask = ST_LSM6DSX_REG_GYRO_ODR_MASK,
112 },
113 .odr_avl[0] = { 13, 0x01 },
114 .odr_avl[1] = { 26, 0x02 },
115 .odr_avl[2] = { 52, 0x03 },
116 .odr_avl[3] = { 104, 0x04 },
117 .odr_avl[4] = { 208, 0x05 },
118 .odr_avl[5] = { 416, 0x06 },
119 }
120};
121
122struct st_lsm6dsx_fs {
123 u32 gain;
124 u8 val;
125};
126
127#define ST_LSM6DSX_FS_LIST_SIZE 4
128struct st_lsm6dsx_fs_table_entry {
129 struct st_lsm6dsx_reg reg;
130 struct st_lsm6dsx_fs fs_avl[ST_LSM6DSX_FS_LIST_SIZE];
131};
132
133static const struct st_lsm6dsx_fs_table_entry st_lsm6dsx_fs_table[] = {
134 [ST_LSM6DSX_ID_ACC] = {
135 .reg = {
136 .addr = ST_LSM6DSX_REG_ACC_FS_ADDR,
137 .mask = ST_LSM6DSX_REG_ACC_FS_MASK,
138 },
139 .fs_avl[0] = { ST_LSM6DSX_ACC_FS_2G_GAIN, 0x0 },
140 .fs_avl[1] = { ST_LSM6DSX_ACC_FS_4G_GAIN, 0x2 },
141 .fs_avl[2] = { ST_LSM6DSX_ACC_FS_8G_GAIN, 0x3 },
142 .fs_avl[3] = { ST_LSM6DSX_ACC_FS_16G_GAIN, 0x1 },
143 },
144 [ST_LSM6DSX_ID_GYRO] = {
145 .reg = {
146 .addr = ST_LSM6DSX_REG_GYRO_FS_ADDR,
147 .mask = ST_LSM6DSX_REG_GYRO_FS_MASK,
148 },
149 .fs_avl[0] = { ST_LSM6DSX_GYRO_FS_245_GAIN, 0x0 },
150 .fs_avl[1] = { ST_LSM6DSX_GYRO_FS_500_GAIN, 0x1 },
151 .fs_avl[2] = { ST_LSM6DSX_GYRO_FS_1000_GAIN, 0x2 },
152 .fs_avl[3] = { ST_LSM6DSX_GYRO_FS_2000_GAIN, 0x3 },
153 }
154};
155
156static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
157 {
158 .wai = 0x69,
159 .max_fifo_size = 1365,
160 .id = {
161 [0] = ST_LSM6DS3_ID,
162 },
163 .decimator = {
164 [ST_LSM6DSX_ID_ACC] = {
165 .addr = 0x08,
166 .mask = GENMASK(2, 0),
167 },
168 [ST_LSM6DSX_ID_GYRO] = {
169 .addr = 0x08,
170 .mask = GENMASK(5, 3),
171 },
172 },
173 .fifo_ops = {
174 .fifo_th = {
175 .addr = 0x06,
176 .mask = GENMASK(11, 0),
177 },
178 .fifo_diff = {
179 .addr = 0x3a,
180 .mask = GENMASK(11, 0),
181 },
182 .th_wl = 3,
183 },
184 .ts_settings = {
185 .timer_en = {
186 .addr = 0x58,
187 .mask = BIT(7),
188 },
189 .hr_timer = {
190 .addr = 0x5c,
191 .mask = BIT(4),
192 },
193 .fifo_en = {
194 .addr = 0x07,
195 .mask = BIT(7),
196 },
197 .decimator = {
198 .addr = 0x09,
199 .mask = GENMASK(5, 3),
200 },
201 },
202 },
203 {
204 .wai = 0x69,
205 .max_fifo_size = 682,
206 .id = {
207 [0] = ST_LSM6DS3H_ID,
208 },
209 .decimator = {
210 [ST_LSM6DSX_ID_ACC] = {
211 .addr = 0x08,
212 .mask = GENMASK(2, 0),
213 },
214 [ST_LSM6DSX_ID_GYRO] = {
215 .addr = 0x08,
216 .mask = GENMASK(5, 3),
217 },
218 },
219 .fifo_ops = {
220 .fifo_th = {
221 .addr = 0x06,
222 .mask = GENMASK(11, 0),
223 },
224 .fifo_diff = {
225 .addr = 0x3a,
226 .mask = GENMASK(11, 0),
227 },
228 .th_wl = 3,
229 },
230 .ts_settings = {
231 .timer_en = {
232 .addr = 0x58,
233 .mask = BIT(7),
234 },
235 .hr_timer = {
236 .addr = 0x5c,
237 .mask = BIT(4),
238 },
239 .fifo_en = {
240 .addr = 0x07,
241 .mask = BIT(7),
242 },
243 .decimator = {
244 .addr = 0x09,
245 .mask = GENMASK(5, 3),
246 },
247 },
248 },
249 {
250 .wai = 0x6a,
251 .max_fifo_size = 682,
252 .id = {
253 [0] = ST_LSM6DSL_ID,
254 [1] = ST_LSM6DSM_ID,
255 [2] = ST_ISM330DLC_ID,
256 },
257 .decimator = {
258 [ST_LSM6DSX_ID_ACC] = {
259 .addr = 0x08,
260 .mask = GENMASK(2, 0),
261 },
262 [ST_LSM6DSX_ID_GYRO] = {
263 .addr = 0x08,
264 .mask = GENMASK(5, 3),
265 },
266 },
267 .fifo_ops = {
268 .fifo_th = {
269 .addr = 0x06,
270 .mask = GENMASK(10, 0),
271 },
272 .fifo_diff = {
273 .addr = 0x3a,
274 .mask = GENMASK(10, 0),
275 },
276 .th_wl = 3,
277 },
278 .ts_settings = {
279 .timer_en = {
280 .addr = 0x19,
281 .mask = BIT(5),
282 },
283 .hr_timer = {
284 .addr = 0x5c,
285 .mask = BIT(4),
286 },
287 .fifo_en = {
288 .addr = 0x07,
289 .mask = BIT(7),
290 },
291 .decimator = {
292 .addr = 0x09,
293 .mask = GENMASK(5, 3),
294 },
295 },
296 },
297};
298
299#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx) \
300{ \
301 .type = chan_type, \
302 .address = addr, \
303 .modified = 1, \
304 .channel2 = mod, \
305 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
306 BIT(IIO_CHAN_INFO_SCALE), \
307 .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
308 .scan_index = scan_idx, \
309 .scan_type = { \
310 .sign = 's', \
311 .realbits = 16, \
312 .storagebits = 16, \
313 .endianness = IIO_LE, \
314 }, \
315}
316
317static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
318 ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR,
319 IIO_MOD_X, 0),
320 ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR,
321 IIO_MOD_Y, 1),
322 ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR,
323 IIO_MOD_Z, 2),
324 IIO_CHAN_SOFT_TIMESTAMP(3),
325};
326
327static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = {
328 ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR,
329 IIO_MOD_X, 0),
330 ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR,
331 IIO_MOD_Y, 1),
332 ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR,
333 IIO_MOD_Z, 2),
334 IIO_CHAN_SOFT_TIMESTAMP(3),
335};
336
337static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id)
338{
339 int err, i, j, data;
340
341 for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
342 for (j = 0; j < ST_LSM6DSX_MAX_ID; j++) {
343 if (id == st_lsm6dsx_sensor_settings[i].id[j])
344 break;
345 }
346 if (j < ST_LSM6DSX_MAX_ID)
347 break;
348 }
349
350 if (i == ARRAY_SIZE(st_lsm6dsx_sensor_settings)) {
351 dev_err(hw->dev, "unsupported hw id [%02x]\n", id);
352 return -ENODEV;
353 }
354
355 err = regmap_read(hw->regmap, ST_LSM6DSX_REG_WHOAMI_ADDR, &data);
356 if (err < 0) {
357 dev_err(hw->dev, "failed to read whoami register\n");
358 return err;
359 }
360
361 if (data != st_lsm6dsx_sensor_settings[i].wai) {
362 dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
363 return -ENODEV;
364 }
365
366 hw->settings = &st_lsm6dsx_sensor_settings[i];
367
368 return 0;
369}
370
371static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor,
372 u32 gain)
373{
374 struct st_lsm6dsx_hw *hw = sensor->hw;
375 const struct st_lsm6dsx_reg *reg;
376 int i, err;
377 u8 val;
378
379 for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++)
380 if (st_lsm6dsx_fs_table[sensor->id].fs_avl[i].gain == gain)
381 break;
382
383 if (i == ST_LSM6DSX_FS_LIST_SIZE)
384 return -EINVAL;
385
386 val = st_lsm6dsx_fs_table[sensor->id].fs_avl[i].val;
387 reg = &st_lsm6dsx_fs_table[sensor->id].reg;
388 err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
389 ST_LSM6DSX_SHIFT_VAL(val, reg->mask));
390 if (err < 0)
391 return err;
392
393 sensor->gain = gain;
394
395 return 0;
396}
397
398static int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr,
399 u8 *val)
400{
401 int i;
402
403 for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
404 if (st_lsm6dsx_odr_table[sensor->id].odr_avl[i].hz == odr)
405 break;
406
407 if (i == ST_LSM6DSX_ODR_LIST_SIZE)
408 return -EINVAL;
409
410 *val = st_lsm6dsx_odr_table[sensor->id].odr_avl[i].val;
411
412 return 0;
413}
414
415static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
416{
417 struct st_lsm6dsx_hw *hw = sensor->hw;
418 const struct st_lsm6dsx_reg *reg;
419 int err;
420 u8 val;
421
422 err = st_lsm6dsx_check_odr(sensor, odr, &val);
423 if (err < 0)
424 return err;
425
426 reg = &st_lsm6dsx_odr_table[sensor->id].reg;
427 return regmap_update_bits(hw->regmap, reg->addr, reg->mask,
428 ST_LSM6DSX_SHIFT_VAL(val, reg->mask));
429}
430
431int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor)
432{
433 int err;
434
435 err = st_lsm6dsx_set_odr(sensor, sensor->odr);
436 if (err < 0)
437 return err;
438
439 sensor->hw->enable_mask |= BIT(sensor->id);
440
441 return 0;
442}
443
444int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor)
445{
446 struct st_lsm6dsx_hw *hw = sensor->hw;
447 const struct st_lsm6dsx_reg *reg;
448 int err;
449
450 reg = &st_lsm6dsx_odr_table[sensor->id].reg;
451 err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
452 ST_LSM6DSX_SHIFT_VAL(0, reg->mask));
453 if (err < 0)
454 return err;
455
456 sensor->hw->enable_mask &= ~BIT(sensor->id);
457
458 return 0;
459}
460
461static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor,
462 u8 addr, int *val)
463{
464 struct st_lsm6dsx_hw *hw = sensor->hw;
465 int err, delay;
466 __le16 data;
467
468 err = st_lsm6dsx_sensor_enable(sensor);
469 if (err < 0)
470 return err;
471
472 delay = 1000000 / sensor->odr;
473 usleep_range(delay, 2 * delay);
474
475 err = regmap_bulk_read(hw->regmap, addr, &data, sizeof(data));
476 if (err < 0)
477 return err;
478
479 st_lsm6dsx_sensor_disable(sensor);
480
481 *val = (s16)le16_to_cpu(data);
482
483 return IIO_VAL_INT;
484}
485
486static int st_lsm6dsx_read_raw(struct iio_dev *iio_dev,
487 struct iio_chan_spec const *ch,
488 int *val, int *val2, long mask)
489{
490 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
491 int ret;
492
493 switch (mask) {
494 case IIO_CHAN_INFO_RAW:
495 ret = iio_device_claim_direct_mode(iio_dev);
496 if (ret)
497 break;
498
499 ret = st_lsm6dsx_read_oneshot(sensor, ch->address, val);
500 iio_device_release_direct_mode(iio_dev);
501 break;
502 case IIO_CHAN_INFO_SAMP_FREQ:
503 *val = sensor->odr;
504 ret = IIO_VAL_INT;
505 break;
506 case IIO_CHAN_INFO_SCALE:
507 *val = 0;
508 *val2 = sensor->gain;
509 ret = IIO_VAL_INT_PLUS_MICRO;
510 break;
511 default:
512 ret = -EINVAL;
513 break;
514 }
515
516 return ret;
517}
518
519static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev,
520 struct iio_chan_spec const *chan,
521 int val, int val2, long mask)
522{
523 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
524 int err;
525
526 err = iio_device_claim_direct_mode(iio_dev);
527 if (err)
528 return err;
529
530 switch (mask) {
531 case IIO_CHAN_INFO_SCALE:
532 err = st_lsm6dsx_set_full_scale(sensor, val2);
533 break;
534 case IIO_CHAN_INFO_SAMP_FREQ: {
535 u8 data;
536
537 err = st_lsm6dsx_check_odr(sensor, val, &data);
538 if (!err)
539 sensor->odr = val;
540 break;
541 }
542 default:
543 err = -EINVAL;
544 break;
545 }
546
547 iio_device_release_direct_mode(iio_dev);
548
549 return err;
550}
551
552static int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val)
553{
554 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
555 struct st_lsm6dsx_hw *hw = sensor->hw;
556 int err;
557
558 if (val < 1 || val > hw->settings->max_fifo_size)
559 return -EINVAL;
560
561 mutex_lock(&hw->conf_lock);
562
563 err = st_lsm6dsx_update_watermark(sensor, val);
564
565 mutex_unlock(&hw->conf_lock);
566
567 if (err < 0)
568 return err;
569
570 sensor->watermark = val;
571
572 return 0;
573}
574
575static ssize_t
576st_lsm6dsx_sysfs_sampling_frequency_avail(struct device *dev,
577 struct device_attribute *attr,
578 char *buf)
579{
580 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
581 enum st_lsm6dsx_sensor_id id = sensor->id;
582 int i, len = 0;
583
584 for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
585 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
586 st_lsm6dsx_odr_table[id].odr_avl[i].hz);
587 buf[len - 1] = '\n';
588
589 return len;
590}
591
592static ssize_t st_lsm6dsx_sysfs_scale_avail(struct device *dev,
593 struct device_attribute *attr,
594 char *buf)
595{
596 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
597 enum st_lsm6dsx_sensor_id id = sensor->id;
598 int i, len = 0;
599
600 for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++)
601 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
602 st_lsm6dsx_fs_table[id].fs_avl[i].gain);
603 buf[len - 1] = '\n';
604
605 return len;
606}
607
608static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_sysfs_sampling_frequency_avail);
609static IIO_DEVICE_ATTR(in_accel_scale_available, 0444,
610 st_lsm6dsx_sysfs_scale_avail, NULL, 0);
611static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444,
612 st_lsm6dsx_sysfs_scale_avail, NULL, 0);
613
614static struct attribute *st_lsm6dsx_acc_attributes[] = {
615 &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
616 &iio_dev_attr_in_accel_scale_available.dev_attr.attr,
617 NULL,
618};
619
620static const struct attribute_group st_lsm6dsx_acc_attribute_group = {
621 .attrs = st_lsm6dsx_acc_attributes,
622};
623
624static const struct iio_info st_lsm6dsx_acc_info = {
625 .attrs = &st_lsm6dsx_acc_attribute_group,
626 .read_raw = st_lsm6dsx_read_raw,
627 .write_raw = st_lsm6dsx_write_raw,
628 .hwfifo_set_watermark = st_lsm6dsx_set_watermark,
629};
630
631static struct attribute *st_lsm6dsx_gyro_attributes[] = {
632 &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
633 &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr,
634 NULL,
635};
636
637static const struct attribute_group st_lsm6dsx_gyro_attribute_group = {
638 .attrs = st_lsm6dsx_gyro_attributes,
639};
640
641static const struct iio_info st_lsm6dsx_gyro_info = {
642 .attrs = &st_lsm6dsx_gyro_attribute_group,
643 .read_raw = st_lsm6dsx_read_raw,
644 .write_raw = st_lsm6dsx_write_raw,
645 .hwfifo_set_watermark = st_lsm6dsx_set_watermark,
646};
647
648static const unsigned long st_lsm6dsx_available_scan_masks[] = {0x7, 0x0};
649
650static int st_lsm6dsx_of_get_drdy_pin(struct st_lsm6dsx_hw *hw, int *drdy_pin)
651{
652 struct device_node *np = hw->dev->of_node;
653
654 if (!np)
655 return -EINVAL;
656
657 return of_property_read_u32(np, "st,drdy-int-pin", drdy_pin);
658}
659
660static int st_lsm6dsx_get_drdy_reg(struct st_lsm6dsx_hw *hw, u8 *drdy_reg)
661{
662 int err = 0, drdy_pin;
663
664 if (st_lsm6dsx_of_get_drdy_pin(hw, &drdy_pin) < 0) {
665 struct st_sensors_platform_data *pdata;
666 struct device *dev = hw->dev;
667
668 pdata = (struct st_sensors_platform_data *)dev->platform_data;
669 drdy_pin = pdata ? pdata->drdy_int_pin : 1;
670 }
671
672 switch (drdy_pin) {
673 case 1:
674 *drdy_reg = ST_LSM6DSX_REG_INT1_ADDR;
675 break;
676 case 2:
677 *drdy_reg = ST_LSM6DSX_REG_INT2_ADDR;
678 break;
679 default:
680 dev_err(hw->dev, "unsupported data ready pin\n");
681 err = -EINVAL;
682 break;
683 }
684
685 return err;
686}
687
688static int st_lsm6dsx_init_hw_timer(struct st_lsm6dsx_hw *hw)
689{
690 const struct st_lsm6dsx_hw_ts_settings *ts_settings;
691 int err, val;
692
693 ts_settings = &hw->settings->ts_settings;
694
695 if (ts_settings->timer_en.addr) {
696 val = ST_LSM6DSX_SHIFT_VAL(1, ts_settings->timer_en.mask);
697 err = regmap_update_bits(hw->regmap,
698 ts_settings->timer_en.addr,
699 ts_settings->timer_en.mask, val);
700 if (err < 0)
701 return err;
702 }
703
704
705 if (ts_settings->hr_timer.addr) {
706 val = ST_LSM6DSX_SHIFT_VAL(1, ts_settings->hr_timer.mask);
707 err = regmap_update_bits(hw->regmap,
708 ts_settings->hr_timer.addr,
709 ts_settings->hr_timer.mask, val);
710 if (err < 0)
711 return err;
712 }
713
714
715 if (ts_settings->fifo_en.addr) {
716 val = ST_LSM6DSX_SHIFT_VAL(1, ts_settings->fifo_en.mask);
717 err = regmap_update_bits(hw->regmap,
718 ts_settings->fifo_en.addr,
719 ts_settings->fifo_en.mask, val);
720 if (err < 0)
721 return err;
722 }
723 return 0;
724}
725
726static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
727{
728 u8 drdy_int_reg;
729 int err;
730
731 err = regmap_write(hw->regmap, ST_LSM6DSX_REG_RESET_ADDR,
732 ST_LSM6DSX_REG_RESET_MASK);
733 if (err < 0)
734 return err;
735
736 msleep(200);
737
738
739 err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_BDU_ADDR,
740 ST_LSM6DSX_REG_BDU_MASK,
741 FIELD_PREP(ST_LSM6DSX_REG_BDU_MASK, 1));
742 if (err < 0)
743 return err;
744
745
746 err = st_lsm6dsx_get_drdy_reg(hw, &drdy_int_reg);
747 if (err < 0)
748 return err;
749
750 err = regmap_update_bits(hw->regmap, drdy_int_reg,
751 ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK,
752 FIELD_PREP(ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK,
753 1));
754 if (err < 0)
755 return err;
756
757 return st_lsm6dsx_init_hw_timer(hw);
758}
759
760static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
761 enum st_lsm6dsx_sensor_id id,
762 const char *name)
763{
764 struct st_lsm6dsx_sensor *sensor;
765 struct iio_dev *iio_dev;
766
767 iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
768 if (!iio_dev)
769 return NULL;
770
771 iio_dev->modes = INDIO_DIRECT_MODE;
772 iio_dev->dev.parent = hw->dev;
773 iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks;
774
775 sensor = iio_priv(iio_dev);
776 sensor->id = id;
777 sensor->hw = hw;
778 sensor->odr = st_lsm6dsx_odr_table[id].odr_avl[0].hz;
779 sensor->gain = st_lsm6dsx_fs_table[id].fs_avl[0].gain;
780 sensor->watermark = 1;
781
782 switch (id) {
783 case ST_LSM6DSX_ID_ACC:
784 iio_dev->channels = st_lsm6dsx_acc_channels;
785 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_acc_channels);
786 iio_dev->info = &st_lsm6dsx_acc_info;
787
788 scnprintf(sensor->name, sizeof(sensor->name), "%s_accel",
789 name);
790 break;
791 case ST_LSM6DSX_ID_GYRO:
792 iio_dev->channels = st_lsm6dsx_gyro_channels;
793 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_gyro_channels);
794 iio_dev->info = &st_lsm6dsx_gyro_info;
795
796 scnprintf(sensor->name, sizeof(sensor->name), "%s_gyro",
797 name);
798 break;
799 default:
800 return NULL;
801 }
802 iio_dev->name = sensor->name;
803
804 return iio_dev;
805}
806
807int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
808 struct regmap *regmap)
809{
810 struct st_lsm6dsx_hw *hw;
811 int i, err;
812
813 hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL);
814 if (!hw)
815 return -ENOMEM;
816
817 dev_set_drvdata(dev, (void *)hw);
818
819 mutex_init(&hw->fifo_lock);
820 mutex_init(&hw->conf_lock);
821
822 hw->buff = devm_kzalloc(dev, ST_LSM6DSX_BUFF_SIZE, GFP_KERNEL);
823 if (!hw->buff)
824 return -ENOMEM;
825
826 hw->dev = dev;
827 hw->irq = irq;
828 hw->regmap = regmap;
829
830 err = st_lsm6dsx_check_whoami(hw, hw_id);
831 if (err < 0)
832 return err;
833
834 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
835 hw->iio_devs[i] = st_lsm6dsx_alloc_iiodev(hw, i, name);
836 if (!hw->iio_devs[i])
837 return -ENOMEM;
838 }
839
840 err = st_lsm6dsx_init_device(hw);
841 if (err < 0)
842 return err;
843
844 if (hw->irq > 0) {
845 err = st_lsm6dsx_fifo_setup(hw);
846 if (err < 0)
847 return err;
848 }
849
850 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
851 err = devm_iio_device_register(hw->dev, hw->iio_devs[i]);
852 if (err)
853 return err;
854 }
855
856 return 0;
857}
858EXPORT_SYMBOL(st_lsm6dsx_probe);
859
860static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
861{
862 struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev);
863 struct st_lsm6dsx_sensor *sensor;
864 const struct st_lsm6dsx_reg *reg;
865 int i, err = 0;
866
867 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
868 sensor = iio_priv(hw->iio_devs[i]);
869 if (!(hw->enable_mask & BIT(sensor->id)))
870 continue;
871
872 reg = &st_lsm6dsx_odr_table[sensor->id].reg;
873 err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
874 ST_LSM6DSX_SHIFT_VAL(0, reg->mask));
875 if (err < 0)
876 return err;
877 }
878
879 if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS)
880 err = st_lsm6dsx_flush_fifo(hw);
881
882 return err;
883}
884
885static int __maybe_unused st_lsm6dsx_resume(struct device *dev)
886{
887 struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev);
888 struct st_lsm6dsx_sensor *sensor;
889 int i, err = 0;
890
891 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
892 sensor = iio_priv(hw->iio_devs[i]);
893 if (!(hw->enable_mask & BIT(sensor->id)))
894 continue;
895
896 err = st_lsm6dsx_set_odr(sensor, sensor->odr);
897 if (err < 0)
898 return err;
899 }
900
901 if (hw->enable_mask)
902 err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
903
904 return err;
905}
906
907const struct dev_pm_ops st_lsm6dsx_pm_ops = {
908 SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsx_suspend, st_lsm6dsx_resume)
909};
910EXPORT_SYMBOL(st_lsm6dsx_pm_ops);
911
912MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
913MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
914MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx driver");
915MODULE_LICENSE("GPL v2");
916
