1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * STMicroelectronics st_lsm6dsr sensor hub library driver
4 *
5 * Copyright 2020 STMicroelectronics Inc.
6 *
7 * Lorenzo Bianconi <lorenzo.bianconi@st.com>
8 */
9
10 #include <linux/module.h>
11 #include <linux/delay.h>
12 #include <linux/iio/iio.h>
13 #include <linux/iio/sysfs.h>
14 #include <asm/unaligned.h>
15
16 #include "st_lsm6dsr.h"
17
18 #define ST_LSM6DSR_REG_MASTER_CONFIG_ADDR 0x14
19 #define ST_LSM6DSR_REG_WRITE_ONCE_MASK BIT(6)
20 #define ST_LSM6DSR_REG_MASTER_ON_MASK BIT(2)
21
22 #define ST_LSM6DSR_REG_SLV0_ADDR 0x15
23 #define ST_LSM6DSR_REG_SLV0_CFG 0x17
24 #define ST_LSM6DSR_REG_SLV1_ADDR 0x18
25 #define ST_LSM6DSR_REG_SLV2_ADDR 0x1b
26 #define ST_LSM6DSR_REG_SLV3_ADDR 0x1e
27 #define ST_LSM6DSR_REG_DATAWRITE_SLV0_ADDR 0x21
28 #define ST_LSM6DSR_REG_BATCH_EXT_SENS_EN_MASK BIT(3)
29 #define ST_LSM6DSR_REG_SLAVE_NUMOP_MASK GENMASK(2, 0)
30
31 #define ST_LSM6DSR_REG_SLV0_OUT_ADDR 0x02
32 #define ST_LSM6DSR_MAX_SLV_NUM 2
33
34 /**
35 * @struct st_lsm6dsr_ext_pwr
36 * @brief External device Power Management description
37 * reg: Generic sensor register description.
38 * off_val: Value to write into register to power off external sensor.
39 * on_val: Value to write into register for power on external sensor.
40 */
41 struct st_lsm6dsr_ext_pwr {
42 struct st_lsm6dsr_reg reg;
43 u8 off_val;
44 u8 on_val;
45 };
46
47 /**
48 * @struct st_lsm6dsr_ext_dev_settings
49 * @brief External sensor descritor entry
50 * i2c_addr: External I2C device address (max two).
51 * wai_addr: Device ID address.
52 * wai_val: Device ID value.
53 * odr_table: ODR sensor table.
54 * fs_table: Full scale table.
55 * temp_comp_reg: Temperature compensation registers.
56 * pwr_table: External device Power Management description.
57 * off_canc_reg: Offset cancellation registers.
58 * bdu_reg: Block Data Update registers.
59 * ext_available_scan_masks: IIO device scan mask.
60 * ext_channels:IIO device channel specifications.
61 * ext_chan_depth: Max number of IIO device channel specifications.
62 * data_len: Sensor output data len.
63 */
64 struct st_lsm6dsr_ext_dev_settings {
65 u8 i2c_addr[2];
66 u8 wai_addr;
67 u8 wai_val;
68 struct st_lsm6dsr_odr_table_entry odr_table;
69 struct st_lsm6dsr_fs_table_entry fs_table;
70 struct st_lsm6dsr_reg temp_comp_reg;
71 struct st_lsm6dsr_ext_pwr pwr_table;
72 struct st_lsm6dsr_reg off_canc_reg;
73 struct st_lsm6dsr_reg bdu_reg;
74 unsigned long ext_available_scan_masks[2];
75 const struct iio_chan_spec ext_channels[5];
76 u8 ext_chan_depth;
77 u8 data_len;
78 };
79
80 static const struct st_lsm6dsr_ext_dev_settings st_lsm6dsr_ext_dev_table[] = {
81 /* LIS2MDL */
82 {
83 .i2c_addr = { 0x1e },
84 .wai_addr = 0x4f,
85 .wai_val = 0x40,
86 .odr_table = {
87 .odr_size = 5,
88 .reg = {
89 .addr = 0x60,
90 .mask = GENMASK(3, 2),
91 },
92 /*
93 * added 5Hz for CTS coverage, reg value is the same
94 * for 5 and 10 Hz
95 */
96 .odr_avl[0] = { 5, 1, 0x0 },
97 .odr_avl[1] = { 10, 0, 0x0 },
98 .odr_avl[2] = { 20, 0, 0x1 },
99 .odr_avl[3] = { 50, 0, 0x2 },
100 .odr_avl[4] = { 100, 0, 0x3 },
101 },
102 .fs_table = {
103 .size = 1,
104 .fs_avl[0] = {
105 .gain = 1500,
106 .val = 0x0,
107 }, /* 1500 uG/LSB */
108 },
109 .temp_comp_reg = {
110 .addr = 0x60,
111 .mask = BIT(7),
112 },
113 .pwr_table = {
114 .reg = {
115 .addr = 0x60,
116 .mask = GENMASK(1, 0),
117 },
118 .off_val = 0x2,
119 .on_val = 0x0,
120 },
121 .off_canc_reg = {
122 .addr = 0x61,
123 .mask = BIT(1),
124 },
125 .bdu_reg = {
126 .addr = 0x62,
127 .mask = BIT(4),
128 },
129 .ext_available_scan_masks = { 0x7, 0x0 },
130 .ext_channels[0] = ST_LSM6DSR_DATA_CHANNEL(IIO_MAGN, 0x68,
131 1, IIO_MOD_X, 0,
132 16, 16, 's'),
133 .ext_channels[1] = ST_LSM6DSR_DATA_CHANNEL(IIO_MAGN, 0x6a,
134 1, IIO_MOD_Y, 1,
135 16, 16, 's'),
136 .ext_channels[2] = ST_LSM6DSR_DATA_CHANNEL(IIO_MAGN, 0x6c,
137 1, IIO_MOD_Z, 2,
138 16, 16, 's'),
139 .ext_channels[3] = ST_LSM6DSR_EVENT_CHANNEL(IIO_MAGN, flush),
140 .ext_channels[4] = IIO_CHAN_SOFT_TIMESTAMP(3),
141 .ext_chan_depth = 5,
142 .data_len = 6,
143 },
144 /* LPS22HB */
145 {
146 .i2c_addr = { 0x5c, 0x5d },
147 .wai_addr = 0x0f,
148 .wai_val = 0xb1,
149 .odr_table = {
150 .odr_size = 4,
151 .reg = {
152 .addr = 0x10,
153 .mask = GENMASK(6, 4),
154 },
155 .odr_avl[0] = { 1, 0, 0x1 },
156 .odr_avl[1] = { 10, 0, 0x2 },
157 .odr_avl[2] = { 25, 0, 0x3 },
158 .odr_avl[3] = { 50, 0, 0x4 },
159 },
160 .fs_table = {
161 .size = 1,
162 /* hPa miscro scale */
163 .fs_avl[0] = {
164 .gain = 1000000UL/4096UL,
165 .val = 0x0,
166 },
167 },
168 .bdu_reg = {
169 .addr = 0x10,
170 .mask = BIT(1),
171 },
172 .ext_available_scan_masks = { 0x1, 0x0 },
173 .ext_channels[0] = ST_LSM6DSR_DATA_CHANNEL(IIO_PRESSURE, 0x28,
174 0, IIO_NO_MOD, 0,
175 24, 32, 'u'),
176 .ext_channels[1] = ST_LSM6DSR_EVENT_CHANNEL(IIO_PRESSURE,
177 flush),
178 .ext_channels[2] = IIO_CHAN_SOFT_TIMESTAMP(1),
179 .ext_chan_depth = 3,
180 .data_len = 3,
181 },
182 /* LPS22HH */
183 {
184 .i2c_addr = { 0x5c, 0x5d },
185 .wai_addr = 0x0f,
186 .wai_val = 0xb3,
187 .odr_table = {
188 .odr_size = 5,
189 .reg = {
190 .addr = 0x10,
191 .mask = GENMASK(6, 4),
192 },
193 .odr_avl[0] = { 1, 0, 0x1 },
194 .odr_avl[1] = { 10, 0, 0x2 },
195 .odr_avl[2] = { 25, 0, 0x3 },
196 .odr_avl[3] = { 50, 0, 0x4 },
197 .odr_avl[4] = { 100, 0, 0x6 },
198 },
199 .fs_table = {
200 .size = 1,
201 /* hPa miscro scale */
202 .fs_avl[0] = {
203 .gain = 1000000UL/4096UL,
204 .val = 0x0,
205 },
206 },
207 .bdu_reg = {
208 .addr = 0x10,
209 .mask = BIT(1),
210 },
211 .ext_available_scan_masks = { 0x1, 0x0 },
212 .ext_channels[0] = ST_LSM6DSR_DATA_CHANNEL(IIO_PRESSURE, 0x28,
213 0, IIO_NO_MOD, 0,
214 24, 32, 'u'),
215 .ext_channels[1] = ST_LSM6DSR_EVENT_CHANNEL(IIO_PRESSURE,
216 flush),
217 .ext_channels[2] = IIO_CHAN_SOFT_TIMESTAMP(1),
218 .ext_chan_depth = 3,
219 .data_len = 3,
220 },
221 };
222
223 /**
224 * Wait write trigger [SHUB]
225 *
226 * In write on external device register, each operation is triggered
227 * by accel/gyro data ready, this means that wait time depends on ODR
228 * plus i2c time
229 * NOTE: Be sure to enable Acc or Gyro before this operation
230 *
231 * @param hw: ST IMU MEMS hw instance.
232 */
st_lsm6dsr_shub_wait_complete(struct st_lsm6dsr_hw * hw)233 static inline void st_lsm6dsr_shub_wait_complete(struct st_lsm6dsr_hw *hw)
234 {
235 struct st_lsm6dsr_sensor *sensor;
236 u16 odr;
237
238 sensor = iio_priv(hw->iio_devs[ST_LSM6DSR_ID_ACC]);
239 /* check if acc is enabled */
240 odr = (hw->enable_mask & BIT(ST_LSM6DSR_ID_ACC)) ? sensor->odr : 13;
241 msleep((2000U / odr) + 1);
242 }
243
244 /**
245 * Read from sensor hub bank register [SHUB]
246 *
247 * NOTE: uses page_lock
248 *
249 * @param hw: ST IMU MEMS hw instance.
250 * @param addr: Remote address register.
251 * @param data: Data buffer.
252 * @param len: Data read len.
253 * @return 0 if OK, < 0 if ERROR
254 */
st_lsm6dsr_shub_read_reg(struct st_lsm6dsr_hw * hw,u8 addr,u8 * data,int len)255 static int st_lsm6dsr_shub_read_reg(struct st_lsm6dsr_hw *hw, u8 addr,
256 u8 *data, int len)
257 {
258 int err;
259
260 mutex_lock(&hw->page_lock);
261 err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_SHUB_REG_MASK,
262 true);
263 if (err < 0)
264 goto out;
265
266 err = hw->tf->read(hw->dev, addr, len, data);
267
268 st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_SHUB_REG_MASK, false);
269 out:
270 mutex_unlock(&hw->page_lock);
271
272 return err;
273 }
274
275 /**
276 * Write to sensor hub bank register [SHUB]
277 *
278 * NOTE: uses page_lock
279 *
280 * @param hw: ST IMU MEMS hw instance.
281 * @param addr: Remote address register.
282 * @param data: Data buffer.
283 * @param len: Data read len.
284 * @return 0 if OK, < 0 if ERROR
285 */
st_lsm6dsr_shub_write_reg(struct st_lsm6dsr_hw * hw,u8 addr,u8 * data,int len)286 static int st_lsm6dsr_shub_write_reg(struct st_lsm6dsr_hw *hw, u8 addr,
287 u8 *data, int len)
288 {
289 int err;
290
291 mutex_lock(&hw->page_lock);
292 err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_SHUB_REG_MASK,
293 true);
294 if (err < 0)
295 goto out;
296
297 err = hw->tf->write(hw->dev, addr, len, data);
298
299 st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_SHUB_REG_MASK, false);
300 out:
301 mutex_unlock(&hw->page_lock);
302
303 return err;
304 }
305
306 /**
307 * Enable sensor hub interface [SHUB]
308 *
309 * NOTE: uses page_lock
310 *
311 * @param sensor: ST IMU sensor instance
312 * @param enable: Master Enable/Disable.
313 * @return 0 if OK, < 0 if ERROR
314 */
st_lsm6dsr_shub_master_enable(struct st_lsm6dsr_sensor * sensor,bool enable)315 static int st_lsm6dsr_shub_master_enable(struct st_lsm6dsr_sensor *sensor,
316 bool enable)
317 {
318 struct st_lsm6dsr_hw *hw = sensor->hw;
319 int err;
320
321 /* enable acc sensor as trigger */
322 err = st_lsm6dsr_sensor_set_enable(sensor, enable);
323 if (err < 0)
324 return err;
325
326 mutex_lock(&hw->page_lock);
327 err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_SHUB_REG_MASK,
328 true);
329 if (err < 0)
330 goto out;
331
332 err = __st_lsm6dsr_write_with_mask(hw,
333 ST_LSM6DSR_REG_MASTER_CONFIG_ADDR,
334 ST_LSM6DSR_REG_MASTER_ON_MASK,
335 enable);
336
337 st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_SHUB_REG_MASK, false);
338
339 out:
340 mutex_unlock(&hw->page_lock);
341
342 return err;
343 }
344
345 /**
346 * Read sensor data register from shub interface
347 *
348 * NOTE: use SLV3 i2c slave for one-shot read operation
349 *
350 * @param sensor: ST IMU sensor instance
351 * @param addr: Remote address register.
352 * @param data: Data buffer.
353 * @param len: Data read len.
354 * @return 0 if OK, < 0 if ERROR
355 */
st_lsm6dsr_shub_read(struct st_lsm6dsr_sensor * sensor,u8 addr,u8 * data,int len)356 static int st_lsm6dsr_shub_read(struct st_lsm6dsr_sensor *sensor, u8 addr,
357 u8 *data, int len)
358 {
359 struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
360 struct st_lsm6dsr_hw *hw = sensor->hw;
361 u8 out_addr = ST_LSM6DSR_REG_SLV0_OUT_ADDR + hw->ext_data_len;
362 u8 config[3];
363 int err;
364
365 config[0] = (ext_info->ext_dev_i2c_addr << 1) | 1;
366 config[1] = addr;
367 config[2] = len & 0x7;
368
369 err = st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_SLV3_ADDR,
370 config, sizeof(config));
371 if (err < 0)
372 return err;
373
374 err = st_lsm6dsr_shub_master_enable(sensor, true);
375 if (err < 0)
376 return err;
377
378 st_lsm6dsr_shub_wait_complete(hw);
379
380 err = st_lsm6dsr_shub_read_reg(hw, out_addr, data, len & 0x7);
381
382 st_lsm6dsr_shub_master_enable(sensor, false);
383
384 memset(config, 0, sizeof(config));
385 return st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_SLV3_ADDR,
386 config, sizeof(config));
387 }
388
389 /**
390 * Write sensor data register from shub interface
391 *
392 * NOTE: use SLV0 i2c slave for write operation
393 *
394 * @param sensor: ST IMU sensor instance
395 * @param addr: Remote address register.
396 * @param data: Data buffer.
397 * @param len: Data read len.
398 * @return 0 if OK, < 0 if ERROR
399 */
st_lsm6dsr_shub_write(struct st_lsm6dsr_sensor * sensor,u8 addr,u8 * data,int len)400 static int st_lsm6dsr_shub_write(struct st_lsm6dsr_sensor *sensor, u8 addr,
401 u8 *data, int len)
402 {
403 struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
404 struct st_lsm6dsr_hw *hw = sensor->hw;
405 u8 mconfig = ST_LSM6DSR_REG_WRITE_ONCE_MASK | 3;
406 u8 config[3] = {};
407 int err, i;
408
409 /* AuxSens = 3 + wr once */
410 err = st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_MASTER_CONFIG_ADDR,
411 &mconfig, sizeof(mconfig));
412 if (err < 0)
413 return err;
414
415 config[0] = ext_info->ext_dev_i2c_addr << 1;
416 for (i = 0; i < len; i++) {
417 config[1] = addr + i;
418
419 err = st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_SLV0_ADDR,
420 config, sizeof(config));
421 if (err < 0)
422 return err;
423
424 err = st_lsm6dsr_shub_write_reg(hw,
425 ST_LSM6DSR_REG_DATAWRITE_SLV0_ADDR,
426 &data[i], 1);
427 if (err < 0)
428 return err;
429
430 err = st_lsm6dsr_shub_master_enable(sensor, true);
431 if (err < 0)
432 return err;
433
434 st_lsm6dsr_shub_wait_complete(hw);
435
436 st_lsm6dsr_shub_master_enable(sensor, false);
437 }
438
439 return st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_SLV0_ADDR,
440 config, sizeof(config));
441 }
442
443 /**
444 * Write sensor data register from shub interface using register bitmask
445 *
446 * @param sensor: ST IMU sensor instance
447 * @param addr: Remote address register.
448 * @param mask: Register bitmask.
449 * @param val: Data buffer.
450 * @return 0 if OK, < 0 if ERROR
451 */
st_lsm6dsr_shub_write_with_mask(struct st_lsm6dsr_sensor * sensor,u8 addr,u8 mask,u8 val)452 static int st_lsm6dsr_shub_write_with_mask(struct st_lsm6dsr_sensor *sensor,
453 u8 addr, u8 mask, u8 val)
454 {
455 int err;
456 u8 data;
457
458 err = st_lsm6dsr_shub_read(sensor, addr, &data, sizeof(data));
459 if (err < 0)
460 return err;
461
462 data = ((data & ~mask) | (val << __ffs(mask) & mask));
463
464 return st_lsm6dsr_shub_write(sensor, addr, &data, sizeof(data));
465 }
466
467 /**
468 * Configure external sensor connected on master I2C interface
469 *
470 * NOTE: use SLV1/SLV2 i2c slave for FIFO read operation
471 *
472 * @param sensor: ST IMU sensor instance
473 * @param enable: Enable/Disable sensor.
474 * @return 0 if OK, < 0 if ERROR
475 */
st_lsm6dsr_shub_config_channels(struct st_lsm6dsr_sensor * sensor,bool enable)476 static int st_lsm6dsr_shub_config_channels(struct st_lsm6dsr_sensor *sensor,
477 bool enable)
478 {
479 struct st_lsm6dsr_ext_dev_info *ext_info;
480 struct st_lsm6dsr_hw *hw = sensor->hw;
481 struct st_lsm6dsr_sensor *cur_sensor;
482 u8 config[6] = {}, enable_mask;
483 int i, j = 0;
484
485 enable_mask = enable ? hw->enable_mask | BIT(sensor->id)
486 : hw->enable_mask & ~BIT(sensor->id);
487
488 for (i = ST_LSM6DSR_ID_EXT0; i <= ST_LSM6DSR_ID_EXT1; i++) {
489 if (!hw->iio_devs[i])
490 continue;
491
492 cur_sensor = iio_priv(hw->iio_devs[i]);
493 if (!(enable_mask & BIT(cur_sensor->id)))
494 continue;
495
496 ext_info = &cur_sensor->ext_dev_info;
497 config[j] = (ext_info->ext_dev_i2c_addr << 1) | 1;
498 config[j + 1] =
499 ext_info->ext_dev_settings->ext_channels[0].address;
500 config[j + 2] = ST_LSM6DSR_REG_BATCH_EXT_SENS_EN_MASK |
501 (ext_info->ext_dev_settings->data_len &
502 ST_LSM6DSR_REG_SLAVE_NUMOP_MASK);
503 j += 3;
504 }
505
506 return st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_SLV1_ADDR,
507 config, sizeof(config));
508 }
509
510 /**
511 * Get a valid ODR [SHUB]
512 *
513 * Check a valid ODR closest to the passed value
514 *
515 * @param sensor: SST IMU sensor instance.
516 * @param odr: ODR value (in Hz).
517 * @param val: ODR register value data pointer.
518 * @return 0 if OK, negative value for ERROR
519 */
st_lsm6dsr_shub_get_odr_val(struct st_lsm6dsr_sensor * sensor,u16 odr,u8 * val)520 static int st_lsm6dsr_shub_get_odr_val(struct st_lsm6dsr_sensor *sensor,
521 u16 odr, u8 *val)
522 {
523 struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
524 int i;
525
526 for (i = 0; i < ext_info->ext_dev_settings->odr_table.odr_size; i++)
527 if (ext_info->ext_dev_settings->odr_table.odr_avl[i].hz >= odr)
528 break;
529
530 if (i == ext_info->ext_dev_settings->odr_table.odr_size)
531 return -EINVAL;
532
533 *val = ext_info->ext_dev_settings->odr_table.odr_avl[i].val;
534
535 /* set decimator for low ODR */
536 sensor->decimator =
537 ext_info->ext_dev_settings->odr_table.odr_avl[i].uhz;
538 sensor->dec_counter = 0;
539
540 return 0;
541 }
542
543 /**
544 * Set new ODR to sensor [SHUB]
545 *
546 * Set a valid ODR closest to the passed value
547 *
548 * @param sensor: ST IMU sensor instance
549 * @param odr: ODR value (in Hz).
550 * @return 0 if OK, negative value for ERROR
551 */
st_lsm6dsr_shub_set_odr(struct st_lsm6dsr_sensor * sensor,u16 odr)552 static int st_lsm6dsr_shub_set_odr(struct st_lsm6dsr_sensor *sensor, u16 odr)
553 {
554 struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
555 struct st_lsm6dsr_hw *hw = sensor->hw;
556 u8 odr_val;
557 int err;
558
559 err = st_lsm6dsr_shub_get_odr_val(sensor, odr, &odr_val);
560 if (err < 0)
561 return err;
562
563 if (sensor->odr == odr && (hw->enable_mask & BIT(sensor->id)))
564 return 0;
565
566 return st_lsm6dsr_shub_write_with_mask(sensor,
567 ext_info->ext_dev_settings->odr_table.reg.addr,
568 ext_info->ext_dev_settings->odr_table.reg.mask,
569 odr_val);
570 }
571
572 /**
573 * Enable or Disable sensor [SHUB]
574 *
575 * @param sensor: ST IMU sensor instance
576 * @param enable: Enable or disable the sensor [true,false].
577 * @return 0 if OK, negative value for ERROR
578 */
st_lsm6dsr_shub_set_enable(struct st_lsm6dsr_sensor * sensor,bool enable)579 int st_lsm6dsr_shub_set_enable(struct st_lsm6dsr_sensor *sensor, bool enable)
580 {
581 struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
582 int err;
583
584 err = st_lsm6dsr_shub_config_channels(sensor, enable);
585 if (err < 0)
586 return err;
587
588 if (enable) {
589 err = st_lsm6dsr_shub_set_odr(sensor, sensor->odr);
590 if (err < 0)
591 return err;
592 } else {
593 err = st_lsm6dsr_shub_write_with_mask(sensor,
594 ext_info->ext_dev_settings->odr_table.reg.addr,
595 ext_info->ext_dev_settings->odr_table.reg.mask,
596 0);
597 if (err < 0)
598 return err;
599 }
600
601 if (ext_info->ext_dev_settings->pwr_table.reg.addr) {
602 u8 val;
603
604 val = enable ? ext_info->ext_dev_settings->pwr_table.on_val
605 : ext_info->ext_dev_settings->pwr_table.off_val;
606 err = st_lsm6dsr_shub_write_with_mask(sensor,
607 ext_info->ext_dev_settings->pwr_table.reg.addr,
608 ext_info->ext_dev_settings->pwr_table.reg.mask,
609 val);
610 if (err < 0)
611 return err;
612 }
613
614 return st_lsm6dsr_shub_master_enable(sensor, enable);
615 }
616
st_lsm6dsr_get_unaligned_le24(const u8 * p)617 static inline u32 st_lsm6dsr_get_unaligned_le24(const u8 *p)
618 {
619 return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8;
620 }
621
622 /**
623 * Single sensor read operation [SHUB]
624 *
625 * @param sensor: ST IMU sensor instance
626 * @param ch: IIO Channel.
627 * @param val: Output data register value.
628 * @return IIO_VAL_INT if OK, negative value for ERROR
629 */
st_lsm6dsr_shub_read_oneshot(struct st_lsm6dsr_sensor * sensor,struct iio_chan_spec const * ch,int * val)630 static int st_lsm6dsr_shub_read_oneshot(struct st_lsm6dsr_sensor *sensor,
631 struct iio_chan_spec const *ch,
632 int *val)
633 {
634 int err, delay, len = ch->scan_type.realbits >> 3;
635 u8 data[ST_LSM6DSR_RX_MAX_LENGTH];
636
637 err = st_lsm6dsr_shub_set_enable(sensor, true);
638 if (err < 0)
639 return err;
640
641 delay = 1000000 / sensor->odr;
642 usleep_range(delay, 2 * delay);
643
644 err = st_lsm6dsr_shub_read(sensor, ch->address, data, len);
645 if (err < 0)
646 return err;
647
648 st_lsm6dsr_shub_set_enable(sensor, false);
649
650 switch (len) {
651 case 3:
652 *val = (s32)st_lsm6dsr_get_unaligned_le24(data);
653 break;
654 case 2:
655 *val = (s16)get_unaligned_le16(data);
656 break;
657 default:
658 return -EINVAL;
659 }
660
661 return IIO_VAL_INT;
662 }
663
664 /**
665 * Read Sensor data configuration [SHUB]
666 *
667 * @param iio_dev: IIO Device.
668 * @param ch: IIO Channel.
669 * @param val: Data Buffer (MSB).
670 * @param val2: Data Buffer (LSB).
671 * @param mask: Data Mask.
672 * @return 0 if OK, -EINVAL value for ERROR
673 */
st_lsm6dsr_shub_read_raw(struct iio_dev * iio_dev,struct iio_chan_spec const * ch,int * val,int * val2,long mask)674 static int st_lsm6dsr_shub_read_raw(struct iio_dev *iio_dev,
675 struct iio_chan_spec const *ch,
676 int *val, int *val2, long mask)
677 {
678 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
679 int ret;
680
681 switch (mask) {
682 case IIO_CHAN_INFO_RAW:
683 mutex_lock(&iio_dev->mlock);
684 if (iio_buffer_enabled(iio_dev)) {
685 ret = -EBUSY;
686 mutex_unlock(&iio_dev->mlock);
687 break;
688 }
689 ret = st_lsm6dsr_shub_read_oneshot(sensor, ch, val);
690 mutex_unlock(&iio_dev->mlock);
691 break;
692 case IIO_CHAN_INFO_SAMP_FREQ:
693 *val = sensor->odr;
694 ret = IIO_VAL_INT;
695 break;
696 case IIO_CHAN_INFO_SCALE:
697 *val = 0;
698 *val2 = sensor->gain;
699 ret = IIO_VAL_INT_PLUS_MICRO;
700 break;
701 default:
702 ret = -EINVAL;
703 break;
704 }
705
706 return ret;
707 }
708
709 /**
710 * Write Sensor data configuration [SHUB]
711 *
712 * @param iio_dev: IIO Device.
713 * @param chan: IIO Channel.
714 * @param val: Data Buffer (MSB).
715 * @param val2: Data Buffer (LSB).
716 * @param mask: Data Mask.
717 * @return 0 if OK, -EINVAL value for ERROR
718 */
st_lsm6dsr_shub_write_raw(struct iio_dev * iio_dev,struct iio_chan_spec const * chan,int val,int val2,long mask)719 static int st_lsm6dsr_shub_write_raw(struct iio_dev *iio_dev,
720 struct iio_chan_spec const *chan,
721 int val, int val2, long mask)
722 {
723 struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
724 int err;
725
726 mutex_lock(&iio_dev->mlock);
727
728 switch (mask) {
729 case IIO_CHAN_INFO_SAMP_FREQ: {
730 u8 data;
731
732 err = st_lsm6dsr_shub_get_odr_val(sensor, val, &data);
733 if (!err)
734 sensor->odr = val;
735 break;
736 }
737 case IIO_CHAN_INFO_SCALE:
738 err = 0;
739 break;
740 default:
741 err = -EINVAL;
742 break;
743 }
744
745 mutex_unlock(&iio_dev->mlock);
746
747 return err;
748 }
749
750 /**
751 * Get a list of available sensor ODR [SHUB]
752 *
753 * List of available ODR returned separated by commas
754 *
755 * @param dev: IIO Device.
756 * @param attr: IIO Channel attribute.
757 * @param buf: User buffer.
758 * @return buffer len
759 */
760 static ssize_t
st_lsm6dsr_sysfs_shub_sampling_freq_avail(struct device * dev,struct device_attribute * attr,char * buf)761 st_lsm6dsr_sysfs_shub_sampling_freq_avail(struct device *dev,
762 struct device_attribute *attr,
763 char *buf)
764 {
765 struct st_lsm6dsr_sensor *sensor = iio_priv(dev_get_drvdata(dev));
766 struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
767 int i, len = 0;
768
769 for (i = 0; i < ST_LSM6DSR_ODR_LIST_SIZE; i++) {
770 u16 val = ext_info->ext_dev_settings->odr_table.odr_avl[i].hz;
771
772 if (val > 0)
773 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
774 val);
775 }
776 buf[len - 1] = '\n';
777
778 return len;
779 }
780
781 /**
782 * Get a list of available sensor Full Scale [SHUB]
783 *
784 * List of available Full Scale returned separated by commas
785 *
786 * @param dev: IIO Device.
787 * @param attr: IIO Channel attribute.
788 * @param buf: User buffer.
789 * @return buffer len
790 */
st_lsm6dsr_sysfs_shub_scale_avail(struct device * dev,struct device_attribute * attr,char * buf)791 static ssize_t st_lsm6dsr_sysfs_shub_scale_avail(struct device *dev,
792 struct device_attribute *attr,
793 char *buf)
794 {
795 struct st_lsm6dsr_sensor *sensor = iio_priv(dev_get_drvdata(dev));
796 struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
797 int i, len = 0;
798
799 for (i = 0; i < ext_info->ext_dev_settings->fs_table.size; i++) {
800 u16 val = ext_info->ext_dev_settings->fs_table.fs_avl[i].gain;
801
802 if (val > 0)
803 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
804 val);
805 }
806 buf[len - 1] = '\n';
807
808 return len;
809 }
810
811 static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsr_sysfs_shub_sampling_freq_avail);
812 static IIO_DEVICE_ATTR(in_ext_scale_available, 0444,
813 st_lsm6dsr_sysfs_shub_scale_avail, NULL, 0);
814 static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444,
815 st_lsm6dsr_get_max_watermark, NULL, 0);
816 static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_lsm6dsr_flush_fifo, 0);
817 static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_lsm6dsr_get_watermark,
818 st_lsm6dsr_set_watermark, 0);
819
820 static struct attribute *st_lsm6dsr_ext_attributes[] = {
821 &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
822 &iio_dev_attr_in_ext_scale_available.dev_attr.attr,
823 &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr,
824 &iio_dev_attr_hwfifo_watermark.dev_attr.attr,
825 &iio_dev_attr_hwfifo_flush.dev_attr.attr,
826 NULL,
827 };
828
829 static const struct attribute_group st_lsm6dsr_ext_attribute_group = {
830 .attrs = st_lsm6dsr_ext_attributes,
831 };
832
833 static const struct iio_info st_lsm6dsr_ext_info = {
834 .attrs = &st_lsm6dsr_ext_attribute_group,
835 .read_raw = st_lsm6dsr_shub_read_raw,
836 .write_raw = st_lsm6dsr_shub_write_raw,
837 };
838
839 /**
840 * Allocate IIO device [SHUB]
841 *
842 * @param hw: ST IMU MEMS hw instance.
843 * @param ext_settings: xternal sensor descritor entry.
844 * @param id: Sensor Identifier.
845 * @param i2c_addr: external I2C address on master bus.
846 * @return struct iio_dev *, NULL if ERROR
847 */
st_lsm6dsr_shub_alloc_iio_dev(struct st_lsm6dsr_hw * hw,const struct st_lsm6dsr_ext_dev_settings * ext_settings,enum st_lsm6dsr_sensor_id id,u8 i2c_addr)848 static struct iio_dev *st_lsm6dsr_shub_alloc_iio_dev(struct st_lsm6dsr_hw *hw,
849 const struct st_lsm6dsr_ext_dev_settings *ext_settings,
850 enum st_lsm6dsr_sensor_id id, u8 i2c_addr)
851 {
852 struct st_lsm6dsr_sensor *sensor;
853 struct iio_dev *iio_dev;
854
855 iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
856 if (!iio_dev)
857 return NULL;
858
859 iio_dev->modes = INDIO_DIRECT_MODE;
860 iio_dev->dev.parent = hw->dev;
861 iio_dev->available_scan_masks = ext_settings->ext_available_scan_masks;
862 iio_dev->info = &st_lsm6dsr_ext_info;
863 iio_dev->channels = ext_settings->ext_channels;
864 iio_dev->num_channels = ext_settings->ext_chan_depth;
865
866 switch (iio_dev->channels[0].type) {
867 case IIO_MAGN:
868 iio_dev->name = "lsm6dsr_magn";
869 break;
870 case IIO_PRESSURE:
871 iio_dev->name = "lsm6dsr_press";
872 break;
873 default:
874 iio_dev->name = "lsm6dsr_ext";
875 break;
876 }
877
878 sensor = iio_priv(iio_dev);
879 sensor->id = id;
880 sensor->hw = hw;
881 sensor->odr = ext_settings->odr_table.odr_avl[0].hz;
882 sensor->gain = ext_settings->fs_table.fs_avl[0].gain;
883 sensor->max_watermark = ST_LSM6DSR_MAX_FIFO_DEPTH;
884 sensor->watermark = 1;
885 sensor->ext_dev_info.ext_dev_i2c_addr = i2c_addr;
886 sensor->ext_dev_info.ext_dev_settings = ext_settings;
887 sensor->decimator = 0;
888 sensor->dec_counter = 0;
889
890 return iio_dev;
891 }
892
st_lsm6dsr_shub_init_remote_sensor(struct st_lsm6dsr_sensor * sensor)893 static int st_lsm6dsr_shub_init_remote_sensor(struct st_lsm6dsr_sensor *sensor)
894 {
895 struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
896 int err = 0;
897
898 if (ext_info->ext_dev_settings->bdu_reg.addr)
899 err = st_lsm6dsr_shub_write_with_mask(sensor,
900 ext_info->ext_dev_settings->bdu_reg.addr,
901 ext_info->ext_dev_settings->bdu_reg.mask, 1);
902
903 if (ext_info->ext_dev_settings->temp_comp_reg.addr)
904 err = st_lsm6dsr_shub_write_with_mask(sensor,
905 ext_info->ext_dev_settings->temp_comp_reg.addr,
906 ext_info->ext_dev_settings->temp_comp_reg.mask, 1);
907
908 if (ext_info->ext_dev_settings->off_canc_reg.addr)
909 err = st_lsm6dsr_shub_write_with_mask(sensor,
910 ext_info->ext_dev_settings->off_canc_reg.addr,
911 ext_info->ext_dev_settings->off_canc_reg.mask, 1);
912
913 return err;
914 }
915
916 /**
917 * Probe device function [SHUB]
918 *
919 * @param hw: ST IMU MEMS hw instance.
920 * @return 0 if OK, negative for ERROR
921 */
st_lsm6dsr_shub_probe(struct st_lsm6dsr_hw * hw)922 int st_lsm6dsr_shub_probe(struct st_lsm6dsr_hw *hw)
923 {
924 const struct st_lsm6dsr_ext_dev_settings *settings;
925 struct st_lsm6dsr_sensor *acc_sensor, *sensor;
926 u8 config[3], data, num_ext_dev = 0;
927 enum st_lsm6dsr_sensor_id id;
928 int err, i = 0, j;
929
930 acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSR_ID_ACC]);
931 while (i < ARRAY_SIZE(st_lsm6dsr_ext_dev_table) &&
932 num_ext_dev < ST_LSM6DSR_MAX_SLV_NUM) {
933 settings = &st_lsm6dsr_ext_dev_table[i];
934
935 for (j = 0; j < ARRAY_SIZE(settings->i2c_addr); j++) {
936 if (!settings->i2c_addr[j])
937 continue;
938
939 /* read wai slave register */
940 config[0] = (settings->i2c_addr[j] << 1) | 1;
941 config[1] = settings->wai_addr;
942 config[2] = 1;
943
944 err = st_lsm6dsr_shub_write_reg(hw,
945 ST_LSM6DSR_REG_SLV0_ADDR,
946 config, sizeof(config));
947 if (err < 0)
948 return err;
949
950 err = st_lsm6dsr_shub_master_enable(acc_sensor, true);
951 if (err < 0)
952 return err;
953
954 st_lsm6dsr_shub_wait_complete(hw);
955
956 err = st_lsm6dsr_shub_read_reg(hw,
957 ST_LSM6DSR_REG_SLV0_OUT_ADDR,
958 &data, sizeof(data));
959
960 st_lsm6dsr_shub_master_enable(acc_sensor, false);
961
962 if (err < 0)
963 return err;
964
965 if (data != settings->wai_val)
966 continue;
967
968 id = ST_LSM6DSR_ID_EXT0 + num_ext_dev;
969 hw->iio_devs[id] = st_lsm6dsr_shub_alloc_iio_dev(hw,
970 settings, id,
971 settings->i2c_addr[j]);
972 if (!hw->iio_devs[id])
973 return -ENOMEM;
974
975 sensor = iio_priv(hw->iio_devs[id]);
976 err = st_lsm6dsr_shub_init_remote_sensor(sensor);
977 if (err < 0)
978 return err;
979
980 num_ext_dev++;
981 hw->ext_data_len += settings->data_len;
982 break;
983 }
984
985 i++;
986 }
987
988 if (!num_ext_dev)
989 return 0;
990
991 memset(config, 0, sizeof(config));
992 err = st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_SLV0_ADDR,
993 config, sizeof(config));
994 if (err < 0)
995 return err;
996
997 /* AuxSens = 3 + wr once */
998 data = ST_LSM6DSR_REG_WRITE_ONCE_MASK | 3;
999 return st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_MASTER_CONFIG_ADDR,
1000 &data, sizeof(data));
1001 }
1002