1 | /* |
2 | * STMicroelectronics st_lsm6dsx i2c controller driver |
3 | * |
4 | * i2c controller embedded in lsm6dx series can connect up to four |
5 | * slave devices using accelerometer sensor as trigger for i2c |
6 | * read/write operations. Current implementation relies on SLV0 channel |
7 | * for slave configuration and SLV{1,2,3} to read data and push them into |
8 | * the hw FIFO |
9 | * |
10 | * Copyright (C) 2018 Lorenzo Bianconi <lorenzo.bianconi83@gmail.com> |
11 | * |
12 | * Permission to use, copy, modify, and/or distribute this software for any |
13 | * purpose with or without fee is hereby granted, provided that the above |
14 | * copyright notice and this permission notice appear in all copies. |
15 | * |
16 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES |
17 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF |
18 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR |
19 | * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES |
20 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN |
21 | * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF |
22 | * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. |
23 | * |
24 | */ |
25 | #include <linux/module.h> |
26 | #include <linux/regmap.h> |
27 | #include <linux/iio/iio.h> |
28 | #include <linux/iio/sysfs.h> |
29 | #include <linux/bitfield.h> |
30 | |
31 | #include "st_lsm6dsx.h" |
32 | |
33 | #define ST_LSM6DSX_SLV_ADDR(n, base) ((base) + (n) * 3) |
34 | #define ST_LSM6DSX_SLV_SUB_ADDR(n, base) ((base) + 1 + (n) * 3) |
35 | #define ST_LSM6DSX_SLV_CONFIG(n, base) ((base) + 2 + (n) * 3) |
36 | |
37 | #define ST_LS6DSX_READ_OP_MASK GENMASK(2, 0) |
38 | |
39 | static const struct st_lsm6dsx_ext_dev_settings st_lsm6dsx_ext_dev_table[] = { |
40 | /* LIS2MDL */ |
41 | { |
42 | .i2c_addr = { 0x1e }, |
43 | .wai = { |
44 | .addr = 0x4f, |
45 | .val = 0x40, |
46 | }, |
47 | .id = ST_LSM6DSX_ID_MAGN, |
48 | .odr_table = { |
49 | .reg = { |
50 | .addr = 0x60, |
51 | .mask = GENMASK(3, 2), |
52 | }, |
53 | .odr_avl[0] = { 10000, 0x0 }, |
54 | .odr_avl[1] = { 20000, 0x1 }, |
55 | .odr_avl[2] = { 50000, 0x2 }, |
56 | .odr_avl[3] = { 100000, 0x3 }, |
57 | .odr_len = 4, |
58 | }, |
59 | .fs_table = { |
60 | .fs_avl[0] = { |
61 | .gain = 1500, |
62 | .val = 0x0, |
63 | }, /* 1500 uG/LSB */ |
64 | .fs_len = 1, |
65 | }, |
66 | .temp_comp = { |
67 | .addr = 0x60, |
68 | .mask = BIT(7), |
69 | }, |
70 | .pwr_table = { |
71 | .reg = { |
72 | .addr = 0x60, |
73 | .mask = GENMASK(1, 0), |
74 | }, |
75 | .off_val = 0x2, |
76 | .on_val = 0x0, |
77 | }, |
78 | .off_canc = { |
79 | .addr = 0x61, |
80 | .mask = BIT(1), |
81 | }, |
82 | .bdu = { |
83 | .addr = 0x62, |
84 | .mask = BIT(4), |
85 | }, |
86 | .out = { |
87 | .addr = 0x68, |
88 | .len = 6, |
89 | }, |
90 | }, |
91 | /* LIS3MDL */ |
92 | { |
93 | .i2c_addr = { 0x1e }, |
94 | .wai = { |
95 | .addr = 0x0f, |
96 | .val = 0x3d, |
97 | }, |
98 | .id = ST_LSM6DSX_ID_MAGN, |
99 | .odr_table = { |
100 | .reg = { |
101 | .addr = 0x20, |
102 | .mask = GENMASK(4, 2), |
103 | }, |
104 | .odr_avl[0] = { 1000, 0x0 }, |
105 | .odr_avl[1] = { 2000, 0x1 }, |
106 | .odr_avl[2] = { 3000, 0x2 }, |
107 | .odr_avl[3] = { 5000, 0x3 }, |
108 | .odr_avl[4] = { 10000, 0x4 }, |
109 | .odr_avl[5] = { 20000, 0x5 }, |
110 | .odr_avl[6] = { 40000, 0x6 }, |
111 | .odr_avl[7] = { 80000, 0x7 }, |
112 | .odr_len = 8, |
113 | }, |
114 | .fs_table = { |
115 | .reg = { |
116 | .addr = 0x21, |
117 | .mask = GENMASK(6, 5), |
118 | }, |
119 | .fs_avl[0] = { |
120 | .gain = 146, |
121 | .val = 0x00, |
122 | }, /* 4000 uG/LSB */ |
123 | .fs_avl[1] = { |
124 | .gain = 292, |
125 | .val = 0x01, |
126 | }, /* 8000 uG/LSB */ |
127 | .fs_avl[2] = { |
128 | .gain = 438, |
129 | .val = 0x02, |
130 | }, /* 12000 uG/LSB */ |
131 | .fs_avl[3] = { |
132 | .gain = 584, |
133 | .val = 0x03, |
134 | }, /* 16000 uG/LSB */ |
135 | .fs_len = 4, |
136 | }, |
137 | .pwr_table = { |
138 | .reg = { |
139 | .addr = 0x22, |
140 | .mask = GENMASK(1, 0), |
141 | }, |
142 | .off_val = 0x2, |
143 | .on_val = 0x0, |
144 | }, |
145 | .bdu = { |
146 | .addr = 0x24, |
147 | .mask = BIT(6), |
148 | }, |
149 | .out = { |
150 | .addr = 0x28, |
151 | .len = 6, |
152 | }, |
153 | }, |
154 | }; |
155 | |
156 | static void st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw *hw) |
157 | { |
158 | struct st_lsm6dsx_sensor *sensor; |
159 | u32 odr, timeout; |
160 | |
161 | sensor = iio_priv(indio_dev: hw->iio_devs[ST_LSM6DSX_ID_ACC]); |
162 | odr = (hw->enable_mask & BIT(ST_LSM6DSX_ID_ACC)) ? sensor->odr : 12500; |
163 | /* set 10ms as minimum timeout for i2c slave configuration */ |
164 | timeout = max_t(u32, 2000000U / odr + 1, 10); |
165 | msleep(msecs: timeout); |
166 | } |
167 | |
168 | /* |
169 | * st_lsm6dsx_shub_read_output - read i2c controller register |
170 | * |
171 | * Read st_lsm6dsx i2c controller register |
172 | */ |
173 | int st_lsm6dsx_shub_read_output(struct st_lsm6dsx_hw *hw, u8 *data, int len) |
174 | { |
175 | const struct st_lsm6dsx_shub_settings *hub_settings; |
176 | int err; |
177 | |
178 | mutex_lock(&hw->page_lock); |
179 | |
180 | hub_settings = &hw->settings->shub_settings; |
181 | if (hub_settings->shub_out.sec_page) { |
182 | err = st_lsm6dsx_set_page(hw, enable: true); |
183 | if (err < 0) |
184 | goto out; |
185 | } |
186 | |
187 | err = regmap_bulk_read(map: hw->regmap, reg: hub_settings->shub_out.addr, |
188 | val: data, val_count: len); |
189 | |
190 | if (hub_settings->shub_out.sec_page) |
191 | st_lsm6dsx_set_page(hw, enable: false); |
192 | out: |
193 | mutex_unlock(lock: &hw->page_lock); |
194 | |
195 | return err; |
196 | } |
197 | |
198 | /* |
199 | * st_lsm6dsx_shub_write_reg - write i2c controller register |
200 | * |
201 | * Write st_lsm6dsx i2c controller register |
202 | */ |
203 | static int st_lsm6dsx_shub_write_reg(struct st_lsm6dsx_hw *hw, u8 addr, |
204 | u8 *data, int len) |
205 | { |
206 | int err; |
207 | |
208 | mutex_lock(&hw->page_lock); |
209 | err = st_lsm6dsx_set_page(hw, enable: true); |
210 | if (err < 0) |
211 | goto out; |
212 | |
213 | err = regmap_bulk_write(map: hw->regmap, reg: addr, val: data, val_count: len); |
214 | |
215 | st_lsm6dsx_set_page(hw, enable: false); |
216 | out: |
217 | mutex_unlock(lock: &hw->page_lock); |
218 | |
219 | return err; |
220 | } |
221 | |
222 | static int |
223 | st_lsm6dsx_shub_write_reg_with_mask(struct st_lsm6dsx_hw *hw, u8 addr, |
224 | u8 mask, u8 val) |
225 | { |
226 | int err; |
227 | |
228 | mutex_lock(&hw->page_lock); |
229 | err = st_lsm6dsx_set_page(hw, enable: true); |
230 | if (err < 0) |
231 | goto out; |
232 | |
233 | err = regmap_update_bits(map: hw->regmap, reg: addr, mask, val); |
234 | |
235 | st_lsm6dsx_set_page(hw, enable: false); |
236 | out: |
237 | mutex_unlock(lock: &hw->page_lock); |
238 | |
239 | return err; |
240 | } |
241 | |
242 | static int st_lsm6dsx_shub_master_enable(struct st_lsm6dsx_sensor *sensor, |
243 | bool enable) |
244 | { |
245 | const struct st_lsm6dsx_shub_settings *hub_settings; |
246 | struct st_lsm6dsx_hw *hw = sensor->hw; |
247 | unsigned int data; |
248 | int err; |
249 | |
250 | /* enable acc sensor as trigger */ |
251 | err = st_lsm6dsx_sensor_set_enable(sensor, enable); |
252 | if (err < 0) |
253 | return err; |
254 | |
255 | mutex_lock(&hw->page_lock); |
256 | |
257 | hub_settings = &hw->settings->shub_settings; |
258 | if (hub_settings->master_en.sec_page) { |
259 | err = st_lsm6dsx_set_page(hw, enable: true); |
260 | if (err < 0) |
261 | goto out; |
262 | } |
263 | |
264 | data = ST_LSM6DSX_SHIFT_VAL(enable, hub_settings->master_en.mask); |
265 | err = regmap_update_bits(map: hw->regmap, reg: hub_settings->master_en.addr, |
266 | mask: hub_settings->master_en.mask, val: data); |
267 | |
268 | if (hub_settings->master_en.sec_page) |
269 | st_lsm6dsx_set_page(hw, enable: false); |
270 | out: |
271 | mutex_unlock(lock: &hw->page_lock); |
272 | |
273 | return err; |
274 | } |
275 | |
276 | /* |
277 | * st_lsm6dsx_shub_read - read data from slave device register |
278 | * |
279 | * Read data from slave device register. SLV0 is used for |
280 | * one-shot read operation |
281 | */ |
282 | static int |
283 | st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr, |
284 | u8 *data, int len) |
285 | { |
286 | const struct st_lsm6dsx_shub_settings *hub_settings; |
287 | u8 config[3], slv_addr, slv_config = 0; |
288 | struct st_lsm6dsx_hw *hw = sensor->hw; |
289 | const struct st_lsm6dsx_reg *aux_sens; |
290 | int err; |
291 | |
292 | hub_settings = &hw->settings->shub_settings; |
293 | slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr); |
294 | aux_sens = &hw->settings->shub_settings.aux_sens; |
295 | /* do not overwrite aux_sens */ |
296 | if (slv_addr + 2 == aux_sens->addr) |
297 | slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask); |
298 | |
299 | config[0] = (sensor->ext_info.addr << 1) | 1; |
300 | config[1] = addr; |
301 | config[2] = (len & ST_LS6DSX_READ_OP_MASK) | slv_config; |
302 | |
303 | err = st_lsm6dsx_shub_write_reg(hw, addr: slv_addr, data: config, |
304 | len: sizeof(config)); |
305 | if (err < 0) |
306 | return err; |
307 | |
308 | err = st_lsm6dsx_shub_master_enable(sensor, enable: true); |
309 | if (err < 0) |
310 | return err; |
311 | |
312 | st_lsm6dsx_shub_wait_complete(hw); |
313 | |
314 | err = st_lsm6dsx_shub_read_output(hw, data, |
315 | len: len & ST_LS6DSX_READ_OP_MASK); |
316 | if (err < 0) |
317 | return err; |
318 | |
319 | st_lsm6dsx_shub_master_enable(sensor, enable: false); |
320 | |
321 | config[0] = hub_settings->pause; |
322 | config[1] = 0; |
323 | config[2] = slv_config; |
324 | return st_lsm6dsx_shub_write_reg(hw, addr: slv_addr, data: config, |
325 | len: sizeof(config)); |
326 | } |
327 | |
328 | /* |
329 | * st_lsm6dsx_shub_write - write data to slave device register |
330 | * |
331 | * Write data from slave device register. SLV0 is used for |
332 | * one-shot write operation |
333 | */ |
334 | static int |
335 | st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr, |
336 | u8 *data, int len) |
337 | { |
338 | const struct st_lsm6dsx_shub_settings *hub_settings; |
339 | struct st_lsm6dsx_hw *hw = sensor->hw; |
340 | u8 config[2], slv_addr; |
341 | int err, i; |
342 | |
343 | hub_settings = &hw->settings->shub_settings; |
344 | if (hub_settings->wr_once.addr) { |
345 | unsigned int data; |
346 | |
347 | data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->wr_once.mask); |
348 | err = st_lsm6dsx_shub_write_reg_with_mask(hw, |
349 | addr: hub_settings->wr_once.addr, |
350 | mask: hub_settings->wr_once.mask, |
351 | val: data); |
352 | if (err < 0) |
353 | return err; |
354 | } |
355 | |
356 | slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr); |
357 | config[0] = sensor->ext_info.addr << 1; |
358 | for (i = 0 ; i < len; i++) { |
359 | config[1] = addr + i; |
360 | |
361 | err = st_lsm6dsx_shub_write_reg(hw, addr: slv_addr, data: config, |
362 | len: sizeof(config)); |
363 | if (err < 0) |
364 | return err; |
365 | |
366 | err = st_lsm6dsx_shub_write_reg(hw, addr: hub_settings->dw_slv0_addr, |
367 | data: &data[i], len: 1); |
368 | if (err < 0) |
369 | return err; |
370 | |
371 | err = st_lsm6dsx_shub_master_enable(sensor, enable: true); |
372 | if (err < 0) |
373 | return err; |
374 | |
375 | st_lsm6dsx_shub_wait_complete(hw); |
376 | |
377 | st_lsm6dsx_shub_master_enable(sensor, enable: false); |
378 | } |
379 | |
380 | config[0] = hub_settings->pause; |
381 | config[1] = 0; |
382 | return st_lsm6dsx_shub_write_reg(hw, addr: slv_addr, data: config, len: sizeof(config)); |
383 | } |
384 | |
385 | static int |
386 | st_lsm6dsx_shub_write_with_mask(struct st_lsm6dsx_sensor *sensor, |
387 | u8 addr, u8 mask, u8 val) |
388 | { |
389 | int err; |
390 | u8 data; |
391 | |
392 | err = st_lsm6dsx_shub_read(sensor, addr, data: &data, len: sizeof(data)); |
393 | if (err < 0) |
394 | return err; |
395 | |
396 | data = ((data & ~mask) | (val << __ffs(mask) & mask)); |
397 | |
398 | return st_lsm6dsx_shub_write(sensor, addr, data: &data, len: sizeof(data)); |
399 | } |
400 | |
401 | static int |
402 | st_lsm6dsx_shub_get_odr_val(struct st_lsm6dsx_sensor *sensor, |
403 | u32 odr, u16 *val) |
404 | { |
405 | const struct st_lsm6dsx_ext_dev_settings *settings; |
406 | int i; |
407 | |
408 | settings = sensor->ext_info.settings; |
409 | for (i = 0; i < settings->odr_table.odr_len; i++) { |
410 | if (settings->odr_table.odr_avl[i].milli_hz == odr) |
411 | break; |
412 | } |
413 | |
414 | if (i == settings->odr_table.odr_len) |
415 | return -EINVAL; |
416 | |
417 | *val = settings->odr_table.odr_avl[i].val; |
418 | return 0; |
419 | } |
420 | |
421 | static int |
422 | st_lsm6dsx_shub_set_odr(struct st_lsm6dsx_sensor *sensor, u32 odr) |
423 | { |
424 | const struct st_lsm6dsx_ext_dev_settings *settings; |
425 | u16 val; |
426 | int err; |
427 | |
428 | err = st_lsm6dsx_shub_get_odr_val(sensor, odr, val: &val); |
429 | if (err < 0) |
430 | return err; |
431 | |
432 | settings = sensor->ext_info.settings; |
433 | return st_lsm6dsx_shub_write_with_mask(sensor, |
434 | addr: settings->odr_table.reg.addr, |
435 | mask: settings->odr_table.reg.mask, |
436 | val); |
437 | } |
438 | |
439 | /* use SLV{1,2,3} for FIFO read operations */ |
440 | static int |
441 | st_lsm6dsx_shub_config_channels(struct st_lsm6dsx_sensor *sensor, |
442 | bool enable) |
443 | { |
444 | const struct st_lsm6dsx_shub_settings *hub_settings; |
445 | const struct st_lsm6dsx_ext_dev_settings *settings; |
446 | u8 config[9] = {}, enable_mask, slv_addr; |
447 | struct st_lsm6dsx_hw *hw = sensor->hw; |
448 | struct st_lsm6dsx_sensor *cur_sensor; |
449 | int i, j = 0; |
450 | |
451 | hub_settings = &hw->settings->shub_settings; |
452 | if (enable) |
453 | enable_mask = hw->enable_mask | BIT(sensor->id); |
454 | else |
455 | enable_mask = hw->enable_mask & ~BIT(sensor->id); |
456 | |
457 | for (i = ST_LSM6DSX_ID_EXT0; i <= ST_LSM6DSX_ID_EXT2; i++) { |
458 | if (!hw->iio_devs[i]) |
459 | continue; |
460 | |
461 | cur_sensor = iio_priv(indio_dev: hw->iio_devs[i]); |
462 | if (!(enable_mask & BIT(cur_sensor->id))) |
463 | continue; |
464 | |
465 | settings = cur_sensor->ext_info.settings; |
466 | config[j] = (sensor->ext_info.addr << 1) | 1; |
467 | config[j + 1] = settings->out.addr; |
468 | config[j + 2] = (settings->out.len & ST_LS6DSX_READ_OP_MASK) | |
469 | hub_settings->batch_en; |
470 | j += 3; |
471 | } |
472 | |
473 | slv_addr = ST_LSM6DSX_SLV_ADDR(1, hub_settings->slv0_addr); |
474 | return st_lsm6dsx_shub_write_reg(hw, addr: slv_addr, data: config, |
475 | len: sizeof(config)); |
476 | } |
477 | |
478 | int st_lsm6dsx_shub_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable) |
479 | { |
480 | const struct st_lsm6dsx_ext_dev_settings *settings; |
481 | int err; |
482 | |
483 | err = st_lsm6dsx_shub_config_channels(sensor, enable); |
484 | if (err < 0) |
485 | return err; |
486 | |
487 | settings = sensor->ext_info.settings; |
488 | if (enable) { |
489 | err = st_lsm6dsx_shub_set_odr(sensor, |
490 | odr: sensor->ext_info.slv_odr); |
491 | if (err < 0) |
492 | return err; |
493 | } else { |
494 | err = st_lsm6dsx_shub_write_with_mask(sensor, |
495 | addr: settings->odr_table.reg.addr, |
496 | mask: settings->odr_table.reg.mask, val: 0); |
497 | if (err < 0) |
498 | return err; |
499 | } |
500 | |
501 | if (settings->pwr_table.reg.addr) { |
502 | u8 val; |
503 | |
504 | val = enable ? settings->pwr_table.on_val |
505 | : settings->pwr_table.off_val; |
506 | err = st_lsm6dsx_shub_write_with_mask(sensor, |
507 | addr: settings->pwr_table.reg.addr, |
508 | mask: settings->pwr_table.reg.mask, val); |
509 | if (err < 0) |
510 | return err; |
511 | } |
512 | |
513 | return st_lsm6dsx_shub_master_enable(sensor, enable); |
514 | } |
515 | |
516 | static int |
517 | st_lsm6dsx_shub_read_oneshot(struct st_lsm6dsx_sensor *sensor, |
518 | struct iio_chan_spec const *ch, |
519 | int *val) |
520 | { |
521 | int err, delay, len; |
522 | u8 data[4]; |
523 | |
524 | err = st_lsm6dsx_shub_set_enable(sensor, enable: true); |
525 | if (err < 0) |
526 | return err; |
527 | |
528 | delay = 1000000000 / sensor->ext_info.slv_odr; |
529 | usleep_range(min: delay, max: 2 * delay); |
530 | |
531 | len = min_t(int, sizeof(data), ch->scan_type.realbits >> 3); |
532 | err = st_lsm6dsx_shub_read(sensor, addr: ch->address, data, len); |
533 | if (err < 0) |
534 | return err; |
535 | |
536 | err = st_lsm6dsx_shub_set_enable(sensor, enable: false); |
537 | if (err < 0) |
538 | return err; |
539 | |
540 | switch (len) { |
541 | case 2: |
542 | *val = (s16)le16_to_cpu(*((__le16 *)data)); |
543 | break; |
544 | default: |
545 | return -EINVAL; |
546 | } |
547 | |
548 | return IIO_VAL_INT; |
549 | } |
550 | |
551 | static int |
552 | st_lsm6dsx_shub_read_raw(struct iio_dev *iio_dev, |
553 | struct iio_chan_spec const *ch, |
554 | int *val, int *val2, long mask) |
555 | { |
556 | struct st_lsm6dsx_sensor *sensor = iio_priv(indio_dev: iio_dev); |
557 | int ret; |
558 | |
559 | switch (mask) { |
560 | case IIO_CHAN_INFO_RAW: |
561 | ret = iio_device_claim_direct_mode(indio_dev: iio_dev); |
562 | if (ret) |
563 | break; |
564 | |
565 | ret = st_lsm6dsx_shub_read_oneshot(sensor, ch, val); |
566 | iio_device_release_direct_mode(indio_dev: iio_dev); |
567 | break; |
568 | case IIO_CHAN_INFO_SAMP_FREQ: |
569 | *val = sensor->ext_info.slv_odr / 1000; |
570 | *val2 = (sensor->ext_info.slv_odr % 1000) * 1000; |
571 | ret = IIO_VAL_INT_PLUS_MICRO; |
572 | break; |
573 | case IIO_CHAN_INFO_SCALE: |
574 | *val = 0; |
575 | *val2 = sensor->gain; |
576 | ret = IIO_VAL_INT_PLUS_MICRO; |
577 | break; |
578 | default: |
579 | ret = -EINVAL; |
580 | break; |
581 | } |
582 | |
583 | return ret; |
584 | } |
585 | |
586 | static int |
587 | st_lsm6dsx_shub_set_full_scale(struct st_lsm6dsx_sensor *sensor, |
588 | u32 gain) |
589 | { |
590 | const struct st_lsm6dsx_fs_table_entry *fs_table; |
591 | int i, err; |
592 | |
593 | fs_table = &sensor->ext_info.settings->fs_table; |
594 | if (!fs_table->reg.addr) |
595 | return -ENOTSUPP; |
596 | |
597 | for (i = 0; i < fs_table->fs_len; i++) { |
598 | if (fs_table->fs_avl[i].gain == gain) |
599 | break; |
600 | } |
601 | |
602 | if (i == fs_table->fs_len) |
603 | return -EINVAL; |
604 | |
605 | err = st_lsm6dsx_shub_write_with_mask(sensor, addr: fs_table->reg.addr, |
606 | mask: fs_table->reg.mask, |
607 | val: fs_table->fs_avl[i].val); |
608 | if (err < 0) |
609 | return err; |
610 | |
611 | sensor->gain = gain; |
612 | |
613 | return 0; |
614 | } |
615 | |
616 | static int |
617 | st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev, |
618 | struct iio_chan_spec const *chan, |
619 | int val, int val2, long mask) |
620 | { |
621 | struct st_lsm6dsx_sensor *sensor = iio_priv(indio_dev: iio_dev); |
622 | int err; |
623 | |
624 | err = iio_device_claim_direct_mode(indio_dev: iio_dev); |
625 | if (err) |
626 | return err; |
627 | |
628 | switch (mask) { |
629 | case IIO_CHAN_INFO_SAMP_FREQ: { |
630 | u16 data; |
631 | |
632 | val = val * 1000 + val2 / 1000; |
633 | err = st_lsm6dsx_shub_get_odr_val(sensor, odr: val, val: &data); |
634 | if (!err) { |
635 | struct st_lsm6dsx_hw *hw = sensor->hw; |
636 | struct st_lsm6dsx_sensor *ref_sensor; |
637 | u8 odr_val; |
638 | int odr; |
639 | |
640 | ref_sensor = iio_priv(indio_dev: hw->iio_devs[ST_LSM6DSX_ID_ACC]); |
641 | odr = st_lsm6dsx_check_odr(sensor: ref_sensor, odr: val, val: &odr_val); |
642 | if (odr < 0) { |
643 | err = odr; |
644 | goto release; |
645 | } |
646 | |
647 | sensor->ext_info.slv_odr = val; |
648 | sensor->odr = odr; |
649 | } |
650 | break; |
651 | } |
652 | case IIO_CHAN_INFO_SCALE: |
653 | err = st_lsm6dsx_shub_set_full_scale(sensor, gain: val2); |
654 | break; |
655 | default: |
656 | err = -EINVAL; |
657 | break; |
658 | } |
659 | |
660 | release: |
661 | iio_device_release_direct_mode(indio_dev: iio_dev); |
662 | |
663 | return err; |
664 | } |
665 | |
666 | static ssize_t |
667 | st_lsm6dsx_shub_sampling_freq_avail(struct device *dev, |
668 | struct device_attribute *attr, |
669 | char *buf) |
670 | { |
671 | struct st_lsm6dsx_sensor *sensor = iio_priv(indio_dev: dev_get_drvdata(dev)); |
672 | const struct st_lsm6dsx_ext_dev_settings *settings; |
673 | int i, len = 0; |
674 | |
675 | settings = sensor->ext_info.settings; |
676 | for (i = 0; i < settings->odr_table.odr_len; i++) { |
677 | u32 val = settings->odr_table.odr_avl[i].milli_hz; |
678 | |
679 | len += scnprintf(buf: buf + len, PAGE_SIZE - len, fmt: "%d.%03d " , |
680 | val / 1000, val % 1000); |
681 | } |
682 | buf[len - 1] = '\n'; |
683 | |
684 | return len; |
685 | } |
686 | |
687 | static ssize_t st_lsm6dsx_shub_scale_avail(struct device *dev, |
688 | struct device_attribute *attr, |
689 | char *buf) |
690 | { |
691 | struct st_lsm6dsx_sensor *sensor = iio_priv(indio_dev: dev_get_drvdata(dev)); |
692 | const struct st_lsm6dsx_ext_dev_settings *settings; |
693 | int i, len = 0; |
694 | |
695 | settings = sensor->ext_info.settings; |
696 | for (i = 0; i < settings->fs_table.fs_len; i++) |
697 | len += scnprintf(buf: buf + len, PAGE_SIZE - len, fmt: "0.%06u " , |
698 | settings->fs_table.fs_avl[i].gain); |
699 | buf[len - 1] = '\n'; |
700 | |
701 | return len; |
702 | } |
703 | |
704 | static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_shub_sampling_freq_avail); |
705 | static IIO_DEVICE_ATTR(in_scale_available, 0444, |
706 | st_lsm6dsx_shub_scale_avail, NULL, 0); |
707 | static struct attribute *st_lsm6dsx_shub_attributes[] = { |
708 | &iio_dev_attr_sampling_frequency_available.dev_attr.attr, |
709 | &iio_dev_attr_in_scale_available.dev_attr.attr, |
710 | NULL, |
711 | }; |
712 | |
713 | static const struct attribute_group st_lsm6dsx_shub_attribute_group = { |
714 | .attrs = st_lsm6dsx_shub_attributes, |
715 | }; |
716 | |
717 | static const struct iio_info st_lsm6dsx_shub_info = { |
718 | .attrs = &st_lsm6dsx_shub_attribute_group, |
719 | .read_raw = st_lsm6dsx_shub_read_raw, |
720 | .write_raw = st_lsm6dsx_shub_write_raw, |
721 | .hwfifo_set_watermark = st_lsm6dsx_set_watermark, |
722 | }; |
723 | |
724 | static struct iio_dev * |
725 | st_lsm6dsx_shub_alloc_iiodev(struct st_lsm6dsx_hw *hw, |
726 | enum st_lsm6dsx_sensor_id id, |
727 | const struct st_lsm6dsx_ext_dev_settings *info, |
728 | u8 i2c_addr, const char *name) |
729 | { |
730 | enum st_lsm6dsx_sensor_id ref_id = ST_LSM6DSX_ID_ACC; |
731 | struct iio_chan_spec *ext_channels; |
732 | struct st_lsm6dsx_sensor *sensor; |
733 | struct iio_dev *iio_dev; |
734 | |
735 | iio_dev = devm_iio_device_alloc(parent: hw->dev, sizeof_priv: sizeof(*sensor)); |
736 | if (!iio_dev) |
737 | return NULL; |
738 | |
739 | iio_dev->modes = INDIO_DIRECT_MODE; |
740 | iio_dev->info = &st_lsm6dsx_shub_info; |
741 | |
742 | sensor = iio_priv(indio_dev: iio_dev); |
743 | sensor->id = id; |
744 | sensor->hw = hw; |
745 | sensor->odr = hw->settings->odr_table[ref_id].odr_avl[0].milli_hz; |
746 | sensor->ext_info.slv_odr = info->odr_table.odr_avl[0].milli_hz; |
747 | sensor->gain = info->fs_table.fs_avl[0].gain; |
748 | sensor->ext_info.settings = info; |
749 | sensor->ext_info.addr = i2c_addr; |
750 | sensor->watermark = 1; |
751 | |
752 | switch (info->id) { |
753 | case ST_LSM6DSX_ID_MAGN: { |
754 | const struct iio_chan_spec magn_channels[] = { |
755 | ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr, |
756 | IIO_MOD_X, 0), |
757 | ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 2, |
758 | IIO_MOD_Y, 1), |
759 | ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 4, |
760 | IIO_MOD_Z, 2), |
761 | IIO_CHAN_SOFT_TIMESTAMP(3), |
762 | }; |
763 | |
764 | ext_channels = devm_kzalloc(dev: hw->dev, size: sizeof(magn_channels), |
765 | GFP_KERNEL); |
766 | if (!ext_channels) |
767 | return NULL; |
768 | |
769 | memcpy(ext_channels, magn_channels, sizeof(magn_channels)); |
770 | iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks; |
771 | iio_dev->channels = ext_channels; |
772 | iio_dev->num_channels = ARRAY_SIZE(magn_channels); |
773 | |
774 | scnprintf(buf: sensor->name, size: sizeof(sensor->name), fmt: "%s_magn" , |
775 | name); |
776 | break; |
777 | } |
778 | default: |
779 | return NULL; |
780 | } |
781 | iio_dev->name = sensor->name; |
782 | |
783 | return iio_dev; |
784 | } |
785 | |
786 | static int st_lsm6dsx_shub_init_device(struct st_lsm6dsx_sensor *sensor) |
787 | { |
788 | const struct st_lsm6dsx_ext_dev_settings *settings; |
789 | int err; |
790 | |
791 | settings = sensor->ext_info.settings; |
792 | if (settings->bdu.addr) { |
793 | err = st_lsm6dsx_shub_write_with_mask(sensor, |
794 | addr: settings->bdu.addr, |
795 | mask: settings->bdu.mask, val: 1); |
796 | if (err < 0) |
797 | return err; |
798 | } |
799 | |
800 | if (settings->temp_comp.addr) { |
801 | err = st_lsm6dsx_shub_write_with_mask(sensor, |
802 | addr: settings->temp_comp.addr, |
803 | mask: settings->temp_comp.mask, val: 1); |
804 | if (err < 0) |
805 | return err; |
806 | } |
807 | |
808 | if (settings->off_canc.addr) { |
809 | err = st_lsm6dsx_shub_write_with_mask(sensor, |
810 | addr: settings->off_canc.addr, |
811 | mask: settings->off_canc.mask, val: 1); |
812 | if (err < 0) |
813 | return err; |
814 | } |
815 | |
816 | return 0; |
817 | } |
818 | |
819 | static int |
820 | st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr, |
821 | const struct st_lsm6dsx_ext_dev_settings *settings) |
822 | { |
823 | const struct st_lsm6dsx_shub_settings *hub_settings; |
824 | u8 config[3], data, slv_addr, slv_config = 0; |
825 | const struct st_lsm6dsx_reg *aux_sens; |
826 | struct st_lsm6dsx_sensor *sensor; |
827 | bool found = false; |
828 | int i, err; |
829 | |
830 | sensor = iio_priv(indio_dev: hw->iio_devs[ST_LSM6DSX_ID_ACC]); |
831 | hub_settings = &hw->settings->shub_settings; |
832 | aux_sens = &hw->settings->shub_settings.aux_sens; |
833 | slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr); |
834 | /* do not overwrite aux_sens */ |
835 | if (slv_addr + 2 == aux_sens->addr) |
836 | slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask); |
837 | |
838 | for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) { |
839 | if (!settings->i2c_addr[i]) |
840 | continue; |
841 | |
842 | /* read wai slave register */ |
843 | config[0] = (settings->i2c_addr[i] << 1) | 0x1; |
844 | config[1] = settings->wai.addr; |
845 | config[2] = 0x1 | slv_config; |
846 | |
847 | err = st_lsm6dsx_shub_write_reg(hw, addr: slv_addr, data: config, |
848 | len: sizeof(config)); |
849 | if (err < 0) |
850 | return err; |
851 | |
852 | err = st_lsm6dsx_shub_master_enable(sensor, enable: true); |
853 | if (err < 0) |
854 | return err; |
855 | |
856 | st_lsm6dsx_shub_wait_complete(hw); |
857 | |
858 | err = st_lsm6dsx_shub_read_output(hw, data: &data, len: sizeof(data)); |
859 | |
860 | st_lsm6dsx_shub_master_enable(sensor, enable: false); |
861 | |
862 | if (err < 0) |
863 | return err; |
864 | |
865 | if (data != settings->wai.val) |
866 | continue; |
867 | |
868 | *i2c_addr = settings->i2c_addr[i]; |
869 | found = true; |
870 | break; |
871 | } |
872 | |
873 | /* reset SLV0 channel */ |
874 | config[0] = hub_settings->pause; |
875 | config[1] = 0; |
876 | config[2] = slv_config; |
877 | err = st_lsm6dsx_shub_write_reg(hw, addr: slv_addr, data: config, |
878 | len: sizeof(config)); |
879 | if (err < 0) |
880 | return err; |
881 | |
882 | return found ? 0 : -ENODEV; |
883 | } |
884 | |
885 | int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name) |
886 | { |
887 | enum st_lsm6dsx_sensor_id id = ST_LSM6DSX_ID_EXT0; |
888 | struct st_lsm6dsx_sensor *sensor; |
889 | int err, i, num_ext_dev = 0; |
890 | u8 i2c_addr = 0; |
891 | |
892 | for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_ext_dev_table); i++) { |
893 | err = st_lsm6dsx_shub_check_wai(hw, i2c_addr: &i2c_addr, |
894 | settings: &st_lsm6dsx_ext_dev_table[i]); |
895 | if (err == -ENODEV) |
896 | continue; |
897 | else if (err < 0) |
898 | return err; |
899 | |
900 | hw->iio_devs[id] = st_lsm6dsx_shub_alloc_iiodev(hw, id, |
901 | info: &st_lsm6dsx_ext_dev_table[i], |
902 | i2c_addr, name); |
903 | if (!hw->iio_devs[id]) |
904 | return -ENOMEM; |
905 | |
906 | sensor = iio_priv(indio_dev: hw->iio_devs[id]); |
907 | err = st_lsm6dsx_shub_init_device(sensor); |
908 | if (err < 0) |
909 | return err; |
910 | |
911 | if (++num_ext_dev >= hw->settings->shub_settings.num_ext_dev) |
912 | break; |
913 | id++; |
914 | } |
915 | |
916 | return 0; |
917 | } |
918 | |