]> git.proxmox.com Git - mirror_ubuntu-jammy-kernel.git/blame - drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
projects / mirror_ubuntu-jammy-kernel.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
iio: imu: add support to lsm6dsx driver
[mirror_ubuntu-jammy-kernel.git] / drivers / iio / imu / st_lsm6dsx / st_lsm6dsx_core.c
CommitLineData
290a6ce1
LB		 1/*
2 * STMicroelectronics st_lsm6dsx sensor driver
3 *
4 * The ST LSM6DSx IMU MEMS series consists of 3D digital accelerometer
5 * and 3D digital gyroscope system-in-package with a digital I2C/SPI serial
6 * interface standard output.
7 * LSM6DSx IMU MEMS series has a dynamic user-selectable full-scale
8 * acceleration range of +-2/+-4/+-8/+-16 g and an angular rate range of
9 * +-125/+-245/+-500/+-1000/+-2000 dps
10 * LSM6DSx series has an integrated First-In-First-Out (FIFO) buffer
11 * allowing dynamic batching of sensor data.
12 *
13 * Supported sensors:
14 * - LSM6DS3:
15 * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
16 * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
17 * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
18 * - FIFO size: 8KB
19 *
20 * - LSM6DSM:
21 * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
22 * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
23 * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
24 * - FIFO size: 4KB
25 *
26 * Copyright 2016 STMicroelectronics Inc.
27 *
28 * Lorenzo Bianconi <lorenzo.bianconi@st.com>
29 * Denis Ciocca <denis.ciocca@st.com>
30 *
31 * Licensed under the GPL-2.
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
40#include "st_lsm6dsx.h"
41
42#define ST_LSM6DSX_REG_ACC_DEC_MASK GENMASK(2, 0)
43#define ST_LSM6DSX_REG_GYRO_DEC_MASK GENMASK(5, 3)
44#define ST_LSM6DSX_REG_INT1_ADDR 0x0d
45#define ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK BIT(3)
46#define ST_LSM6DSX_REG_WHOAMI_ADDR 0x0f
47#define ST_LSM6DSX_REG_RESET_ADDR 0x12
48#define ST_LSM6DSX_REG_RESET_MASK BIT(0)
49#define ST_LSM6DSX_REG_BDU_ADDR 0x12
50#define ST_LSM6DSX_REG_BDU_MASK BIT(6)
51#define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR 0x13
52#define ST_LSM6DSX_REG_INT2_ON_INT1_MASK BIT(5)
53#define ST_LSM6DSX_REG_ROUNDING_ADDR 0x16
54#define ST_LSM6DSX_REG_ROUNDING_MASK BIT(2)
55#define ST_LSM6DSX_REG_LIR_ADDR 0x58
56#define ST_LSM6DSX_REG_LIR_MASK BIT(0)
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_LSM6DS3_WHOAMI 0x69
75#define ST_LSM6DSM_WHOAMI 0x6a
76
77#define ST_LSM6DS3_MAX_FIFO_SIZE 8192
78#define ST_LSM6DSM_MAX_FIFO_SIZE 4096
79
80#define ST_LSM6DSX_ACC_FS_2G_GAIN IIO_G_TO_M_S_2(61)
81#define ST_LSM6DSX_ACC_FS_4G_GAIN IIO_G_TO_M_S_2(122)
82#define ST_LSM6DSX_ACC_FS_8G_GAIN IIO_G_TO_M_S_2(244)
83#define ST_LSM6DSX_ACC_FS_16G_GAIN IIO_G_TO_M_S_2(488)
84
85#define ST_LSM6DSX_GYRO_FS_245_GAIN IIO_DEGREE_TO_RAD(4375)
86#define ST_LSM6DSX_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(8750)
87#define ST_LSM6DSX_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(17500)
88#define ST_LSM6DSX_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000)
89
90struct st_lsm6dsx_odr {
91 u16 hz;
92 u8 val;
93};
94
95#define ST_LSM6DSX_ODR_LIST_SIZE 6
96struct st_lsm6dsx_odr_table_entry {
97 struct st_lsm6dsx_reg reg;
98 struct st_lsm6dsx_odr odr_avl[ST_LSM6DSX_ODR_LIST_SIZE];
99};
100
101static const struct st_lsm6dsx_odr_table_entry st_lsm6dsx_odr_table[] = {
102 [ST_LSM6DSX_ID_ACC] = {
103 .reg = {
104 .addr = ST_LSM6DSX_REG_ACC_ODR_ADDR,
105 .mask = ST_LSM6DSX_REG_ACC_ODR_MASK,
106 },
107 .odr_avl[0] = { 13, 0x01 },
108 .odr_avl[1] = { 26, 0x02 },
109 .odr_avl[2] = { 52, 0x03 },
110 .odr_avl[3] = { 104, 0x04 },
111 .odr_avl[4] = { 208, 0x05 },
112 .odr_avl[5] = { 416, 0x06 },
113 },
114 [ST_LSM6DSX_ID_GYRO] = {
115 .reg = {
116 .addr = ST_LSM6DSX_REG_GYRO_ODR_ADDR,
117 .mask = ST_LSM6DSX_REG_GYRO_ODR_MASK,
118 },
119 .odr_avl[0] = { 13, 0x01 },
120 .odr_avl[1] = { 26, 0x02 },
121 .odr_avl[2] = { 52, 0x03 },
122 .odr_avl[3] = { 104, 0x04 },
123 .odr_avl[4] = { 208, 0x05 },
124 .odr_avl[5] = { 416, 0x06 },
125 }
126};
127
128struct st_lsm6dsx_fs {
129 u32 gain;
130 u8 val;
131};
132
133#define ST_LSM6DSX_FS_LIST_SIZE 4
134struct st_lsm6dsx_fs_table_entry {
135 struct st_lsm6dsx_reg reg;
136 struct st_lsm6dsx_fs fs_avl[ST_LSM6DSX_FS_LIST_SIZE];
137};
138
139static const struct st_lsm6dsx_fs_table_entry st_lsm6dsx_fs_table[] = {
140 [ST_LSM6DSX_ID_ACC] = {
141 .reg = {
142 .addr = ST_LSM6DSX_REG_ACC_FS_ADDR,
143 .mask = ST_LSM6DSX_REG_ACC_FS_MASK,
144 },
145 .fs_avl[0] = { ST_LSM6DSX_ACC_FS_2G_GAIN, 0x0 },
146 .fs_avl[1] = { ST_LSM6DSX_ACC_FS_4G_GAIN, 0x2 },
147 .fs_avl[2] = { ST_LSM6DSX_ACC_FS_8G_GAIN, 0x3 },
148 .fs_avl[3] = { ST_LSM6DSX_ACC_FS_16G_GAIN, 0x1 },
149 },
150 [ST_LSM6DSX_ID_GYRO] = {
151 .reg = {
152 .addr = ST_LSM6DSX_REG_GYRO_FS_ADDR,
153 .mask = ST_LSM6DSX_REG_GYRO_FS_MASK,
154 },
155 .fs_avl[0] = { ST_LSM6DSX_GYRO_FS_245_GAIN, 0x0 },
156 .fs_avl[1] = { ST_LSM6DSX_GYRO_FS_500_GAIN, 0x1 },
157 .fs_avl[2] = { ST_LSM6DSX_GYRO_FS_1000_GAIN, 0x2 },
158 .fs_avl[3] = { ST_LSM6DSX_GYRO_FS_2000_GAIN, 0x3 },
159 }
160};
161
162static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
163 {
164 .wai = ST_LSM6DS3_WHOAMI,
165 .max_fifo_size = ST_LSM6DS3_MAX_FIFO_SIZE,
166 .id = ST_LSM6DS3_ID,
167 },
168 {
169 .wai = ST_LSM6DSM_WHOAMI,
170 .max_fifo_size = ST_LSM6DSM_MAX_FIFO_SIZE,
171 .id = ST_LSM6DSM_ID,
172 },
173};
174
175#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx) \
176{ \
177 .type = chan_type, \
178 .address = addr, \
179 .modified = 1, \
180 .channel2 = mod, \
181 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
182 BIT(IIO_CHAN_INFO_SCALE), \
183 .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
184 .scan_index = scan_idx, \
185 .scan_type = { \
186 .sign = 's', \
187 .realbits = 16, \
188 .storagebits = 16, \
189 .endianness = IIO_LE, \
190 }, \
191}
192
193static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
194 ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR,
195 IIO_MOD_X, 0),
196 ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR,
197 IIO_MOD_Y, 1),
198 ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR,
199 IIO_MOD_Z, 2),
200 IIO_CHAN_SOFT_TIMESTAMP(3),
201};
202
203static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = {
204 ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR,
205 IIO_MOD_X, 0),
206 ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR,
207 IIO_MOD_Y, 1),
208 ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR,
209 IIO_MOD_Z, 2),
210 IIO_CHAN_SOFT_TIMESTAMP(3),
211};
212
213int st_lsm6dsx_write_with_mask(struct st_lsm6dsx_hw *hw, u8 addr, u8 mask,
214 u8 val)
215{
216 u8 data;
217 int err;
218
219 mutex_lock(&hw->lock);
220
221 err = hw->tf->read(hw->dev, addr, sizeof(data), &data);
222 if (err < 0) {
223 dev_err(hw->dev, "failed to read %02x register\n", addr);
224 goto out;
225 }
226
227 data = (data & ~mask) | ((val << __ffs(mask)) & mask);
228
229 err = hw->tf->write(hw->dev, addr, sizeof(data), &data);
230 if (err < 0)
231 dev_err(hw->dev, "failed to write %02x register\n", addr);
232
233out:
234 mutex_unlock(&hw->lock);
235
236 return err;
237}
238
239static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id)
240{
241 int err, i;
242 u8 data;
243
244 for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
245 if (id == st_lsm6dsx_sensor_settings[i].id)
246 break;
247 }
248
249 if (i == ARRAY_SIZE(st_lsm6dsx_sensor_settings)) {
250 dev_err(hw->dev, "unsupported hw id [%02x]\n", id);
251 return -ENODEV;
252 }
253
254 err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_WHOAMI_ADDR, sizeof(data),
255 &data);
256 if (err < 0) {
257 dev_err(hw->dev, "failed to read whoami register\n");
258 return err;
259 }
260
261 if (data != st_lsm6dsx_sensor_settings[i].wai) {
262 dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
263 return -ENODEV;
264 }
265
266 hw->settings = &st_lsm6dsx_sensor_settings[i];
267
268 return 0;
269}
270
271static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor,
272 u32 gain)
273{
274 enum st_lsm6dsx_sensor_id id = sensor->id;
275 int i, err;
276 u8 val;
277
278 for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++)
279 if (st_lsm6dsx_fs_table[id].fs_avl[i].gain == gain)
280 break;
281
282 if (i == ST_LSM6DSX_FS_LIST_SIZE)
283 return -EINVAL;
284
285 val = st_lsm6dsx_fs_table[id].fs_avl[i].val;
286 err = st_lsm6dsx_write_with_mask(sensor->hw,
287 st_lsm6dsx_fs_table[id].reg.addr,
288 st_lsm6dsx_fs_table[id].reg.mask,
289 val);
290 if (err < 0)
291 return err;
292
293 sensor->gain = gain;
294
295 return 0;
296}
297
298static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
299{
300 enum st_lsm6dsx_sensor_id id = sensor->id;
301 int i, err;
302 u8 val;
303
304 for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
305 if (st_lsm6dsx_odr_table[id].odr_avl[i].hz == odr)
306 break;
307
308 if (i == ST_LSM6DSX_ODR_LIST_SIZE)
309 return -EINVAL;
310
311 val = st_lsm6dsx_odr_table[id].odr_avl[i].val;
312 err = st_lsm6dsx_write_with_mask(sensor->hw,
313 st_lsm6dsx_odr_table[id].reg.addr,
314 st_lsm6dsx_odr_table[id].reg.mask,
315 val);
316 if (err < 0)
317 return err;
318
319 sensor->odr = odr;
320
321 return 0;
322}
323
324int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor)
325{
326 int err;
327
328 err = st_lsm6dsx_set_odr(sensor, sensor->odr);
329 if (err < 0)
330 return err;
331
332 sensor->hw->enable_mask |= BIT(sensor->id);
333
334 return 0;
335}
336
337int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor)
338{
339 enum st_lsm6dsx_sensor_id id = sensor->id;
340 int err;
341
342 err = st_lsm6dsx_write_with_mask(sensor->hw,
343 st_lsm6dsx_odr_table[id].reg.addr,
344 st_lsm6dsx_odr_table[id].reg.mask, 0);
345 if (err < 0)
346 return err;
347
348 sensor->hw->enable_mask &= ~BIT(id);
349
350 return 0;
351}
352
353static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor,
354 u8 addr, int *val)
355{
356 int err, delay;
357 __le16 data;
358
359 err = st_lsm6dsx_sensor_enable(sensor);
360 if (err < 0)
361 return err;
362
363 delay = 1000000 / sensor->odr;
364 usleep_range(delay, 2 * delay);
365
366 err = sensor->hw->tf->read(sensor->hw->dev, addr, sizeof(data),
367 (u8 *)&data);
368 if (err < 0)
369 return err;
370
371 st_lsm6dsx_sensor_disable(sensor);
372
373 *val = (s16)data;
374
375 return IIO_VAL_INT;
376}
377
378static int st_lsm6dsx_read_raw(struct iio_dev *iio_dev,
379 struct iio_chan_spec const *ch,
380 int *val, int *val2, long mask)
381{
382 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
383 int ret;
384
385 switch (mask) {
386 case IIO_CHAN_INFO_RAW:
387 ret = iio_device_claim_direct_mode(iio_dev);
388 if (ret)
389 break;
390
391 ret = st_lsm6dsx_read_oneshot(sensor, ch->address, val);
392 iio_device_release_direct_mode(iio_dev);
393 break;
394 case IIO_CHAN_INFO_SAMP_FREQ:
395 *val = sensor->odr;
396 ret = IIO_VAL_INT;
397 break;
398 case IIO_CHAN_INFO_SCALE:
399 *val = 0;
400 *val2 = sensor->gain;
401 ret = IIO_VAL_INT_PLUS_MICRO;
402 break;
403 default:
404 ret = -EINVAL;
405 break;
406 }
407
408 return ret;
409}
410
411static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev,
412 struct iio_chan_spec const *chan,
413 int val, int val2, long mask)
414{
415 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
416 int err;
417
418 err = iio_device_claim_direct_mode(iio_dev);
419 if (err)
420 return err;
421
422 switch (mask) {
423 case IIO_CHAN_INFO_SCALE:
424 err = st_lsm6dsx_set_full_scale(sensor, val2);
425 break;
426 case IIO_CHAN_INFO_SAMP_FREQ:
427 err = st_lsm6dsx_set_odr(sensor, val);
428 break;
429 default:
430 err = -EINVAL;
431 break;
432 }
433
434 iio_device_release_direct_mode(iio_dev);
435
436 return err;
437}
438
439static int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val)
440{
441 struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
442 struct st_lsm6dsx_hw *hw = sensor->hw;
443 int err, max_fifo_len;
444
445 max_fifo_len = hw->settings->max_fifo_size / ST_LSM6DSX_SAMPLE_SIZE;
446 if (val < 1 || val > max_fifo_len)
447 return -EINVAL;
448
449 err = st_lsm6dsx_update_watermark(sensor, val);
450 if (err < 0)
451 return err;
452
453 sensor->watermark = val;
454
455 return 0;
456}
457
458static ssize_t
459st_lsm6dsx_sysfs_sampling_frequency_avail(struct device *dev,
460 struct device_attribute *attr,
461 char *buf)
462{
463 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
464 enum st_lsm6dsx_sensor_id id = sensor->id;
465 int i, len = 0;
466
467 for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
468 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
469 st_lsm6dsx_odr_table[id].odr_avl[i].hz);
470 buf[len - 1] = '\n';
471
472 return len;
473}
474
475static ssize_t st_lsm6dsx_sysfs_scale_avail(struct device *dev,
476 struct device_attribute *attr,
477 char *buf)
478{
479 struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
480 enum st_lsm6dsx_sensor_id id = sensor->id;
481 int i, len = 0;
482
483 for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++)
484 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
485 st_lsm6dsx_fs_table[id].fs_avl[i].gain);
486 buf[len - 1] = '\n';
487
488 return len;
489}
490
491static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_sysfs_sampling_frequency_avail);
492static IIO_DEVICE_ATTR(in_accel_scale_available, 0444,
493 st_lsm6dsx_sysfs_scale_avail, NULL, 0);
494static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444,
495 st_lsm6dsx_sysfs_scale_avail, NULL, 0);
496
497static struct attribute *st_lsm6dsx_acc_attributes[] = {
498 &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
499 &iio_dev_attr_in_accel_scale_available.dev_attr.attr,
500 NULL,
501};
502
503static const struct attribute_group st_lsm6dsx_acc_attribute_group = {
504 .attrs = st_lsm6dsx_acc_attributes,
505};
506
507static const struct iio_info st_lsm6dsx_acc_info = {
508 .driver_module = THIS_MODULE,
509 .attrs = &st_lsm6dsx_acc_attribute_group,
510 .read_raw = st_lsm6dsx_read_raw,
511 .write_raw = st_lsm6dsx_write_raw,
512 .hwfifo_set_watermark = st_lsm6dsx_set_watermark,
513};
514
515static struct attribute *st_lsm6dsx_gyro_attributes[] = {
516 &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
517 &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr,
518 NULL,
519};
520
521static const struct attribute_group st_lsm6dsx_gyro_attribute_group = {
522 .attrs = st_lsm6dsx_gyro_attributes,
523};
524
525static const struct iio_info st_lsm6dsx_gyro_info = {
526 .driver_module = THIS_MODULE,
527 .attrs = &st_lsm6dsx_gyro_attribute_group,
528 .read_raw = st_lsm6dsx_read_raw,
529 .write_raw = st_lsm6dsx_write_raw,
530 .hwfifo_set_watermark = st_lsm6dsx_set_watermark,
531};
532
533static const unsigned long st_lsm6dsx_available_scan_masks[] = {0x7, 0x0};
534
535static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
536{
537 int err;
538 u8 data;
539
540 data = ST_LSM6DSX_REG_RESET_MASK;
541 err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_RESET_ADDR, sizeof(data),
542 &data);
543 if (err < 0)
544 return err;
545
546 msleep(200);
547
548 /* latch interrupts */
549 err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_LIR_ADDR,
550 ST_LSM6DSX_REG_LIR_MASK, 1);
551 if (err < 0)
552 return err;
553
554 /* enable Block Data Update */
555 err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_BDU_ADDR,
556 ST_LSM6DSX_REG_BDU_MASK, 1);
557 if (err < 0)
558 return err;
559
560 err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_ROUNDING_ADDR,
561 ST_LSM6DSX_REG_ROUNDING_MASK, 1);
562 if (err < 0)
563 return err;
564
565 /* enable FIFO watermak interrupt */
566 err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_INT1_ADDR,
567 ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK, 1);
568 if (err < 0)
569 return err;
570
571 /* redirect INT2 on INT1 */
572 return st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_INT2_ON_INT1_ADDR,
573 ST_LSM6DSX_REG_INT2_ON_INT1_MASK, 1);
574}
575
576static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
577 enum st_lsm6dsx_sensor_id id)
578{
579 struct st_lsm6dsx_sensor *sensor;
580 struct iio_dev *iio_dev;
581
582 iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
583 if (!iio_dev)
584 return NULL;
585
586 iio_dev->modes = INDIO_DIRECT_MODE;
587 iio_dev->dev.parent = hw->dev;
588 iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks;
589
590 sensor = iio_priv(iio_dev);
591 sensor->id = id;
592 sensor->hw = hw;
593 sensor->odr = st_lsm6dsx_odr_table[id].odr_avl[0].hz;
594 sensor->gain = st_lsm6dsx_fs_table[id].fs_avl[0].gain;
595 sensor->watermark = 1;
596
597 switch (id) {
598 case ST_LSM6DSX_ID_ACC:
599 iio_dev->channels = st_lsm6dsx_acc_channels;
600 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_acc_channels);
601 iio_dev->name = "lsm6dsx_accel";
602 iio_dev->info = &st_lsm6dsx_acc_info;
603
604 sensor->decimator_mask = ST_LSM6DSX_REG_ACC_DEC_MASK;
605 break;
606 case ST_LSM6DSX_ID_GYRO:
607 iio_dev->channels = st_lsm6dsx_gyro_channels;
608 iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_gyro_channels);
609 iio_dev->name = "lsm6dsx_gyro";
610 iio_dev->info = &st_lsm6dsx_gyro_info;
611
612 sensor->decimator_mask = ST_LSM6DSX_REG_GYRO_DEC_MASK;
613 break;
614 default:
615 return NULL;
616 }
617
618 return iio_dev;
619}
620
621int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id,
622 const struct st_lsm6dsx_transfer_function *tf_ops)
623{
624 struct st_lsm6dsx_hw *hw;
625 int i, err;
626
627 hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL);
628 if (!hw)
629 return -ENOMEM;
630
631 dev_set_drvdata(dev, (void *)hw);
632
633 mutex_init(&hw->lock);
634 mutex_init(&hw->fifo_lock);
635
636 hw->dev = dev;
637 hw->irq = irq;
638 hw->tf = tf_ops;
639
640 err = st_lsm6dsx_check_whoami(hw, hw_id);
641 if (err < 0)
642 return err;
643
644 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
645 hw->iio_devs[i] = st_lsm6dsx_alloc_iiodev(hw, i);
646 if (!hw->iio_devs[i])
647 return -ENOMEM;
648 }
649
650 err = st_lsm6dsx_init_device(hw);
651 if (err < 0)
652 return err;
653
654 if (hw->irq > 0) {
655 err = st_lsm6dsx_fifo_setup(hw);
656 if (err < 0)
657 return err;
658 }
659
660 for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
661 err = devm_iio_device_register(hw->dev, hw->iio_devs[i]);
662 if (err)
663 return err;
664 }
665
666 return 0;
667}
668EXPORT_SYMBOL(st_lsm6dsx_probe);
669
670MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
671MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
672MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx driver");
673MODULE_LICENSE("GPL v2");
Ubuntu Jammy Linux kernel mirror