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 
st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw * hw)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(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(timeout);
166 }
167 
168 /*
169  * st_lsm6dsx_shub_read_output - read i2c controller register
170  *
171  * Read st_lsm6dsx i2c controller register
172  */
st_lsm6dsx_shub_read_output(struct st_lsm6dsx_hw * hw,u8 * data,int len)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, true);
183 		if (err < 0)
184 			goto out;
185 	}
186 
187 	err = regmap_bulk_read(hw->regmap, hub_settings->shub_out.addr,
188 			       data, len);
189 
190 	if (hub_settings->shub_out.sec_page)
191 		st_lsm6dsx_set_page(hw, false);
192 out:
193 	mutex_unlock(&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  */
st_lsm6dsx_shub_write_reg(struct st_lsm6dsx_hw * hw,u8 addr,u8 * data,int len)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, true);
210 	if (err < 0)
211 		goto out;
212 
213 	err = regmap_bulk_write(hw->regmap, addr, data, len);
214 
215 	st_lsm6dsx_set_page(hw, false);
216 out:
217 	mutex_unlock(&hw->page_lock);
218 
219 	return err;
220 }
221 
222 static int
st_lsm6dsx_shub_write_reg_with_mask(struct st_lsm6dsx_hw * hw,u8 addr,u8 mask,u8 val)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, true);
230 	if (err < 0)
231 		goto out;
232 
233 	err = regmap_update_bits(hw->regmap, addr, mask, val);
234 
235 	st_lsm6dsx_set_page(hw, false);
236 out:
237 	mutex_unlock(&hw->page_lock);
238 
239 	return err;
240 }
241 
st_lsm6dsx_shub_master_enable(struct st_lsm6dsx_sensor * sensor,bool enable)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, 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(hw->regmap, hub_settings->master_en.addr,
266 				 hub_settings->master_en.mask, data);
267 
268 	if (hub_settings->master_en.sec_page)
269 		st_lsm6dsx_set_page(hw, false);
270 out:
271 	mutex_unlock(&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
st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor * sensor,u8 addr,u8 * data,int len)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, slv_addr, config,
304 					sizeof(config));
305 	if (err < 0)
306 		return err;
307 
308 	err = st_lsm6dsx_shub_master_enable(sensor, 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 & ST_LS6DSX_READ_OP_MASK);
316 	if (err < 0)
317 		return err;
318 
319 	st_lsm6dsx_shub_master_enable(sensor, 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, slv_addr, config,
325 					 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
st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor * sensor,u8 addr,u8 * data,int len)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 			hub_settings->wr_once.addr,
350 			hub_settings->wr_once.mask,
351 			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, slv_addr, config,
362 						sizeof(config));
363 		if (err < 0)
364 			return err;
365 
366 		err = st_lsm6dsx_shub_write_reg(hw, hub_settings->dw_slv0_addr,
367 						&data[i], 1);
368 		if (err < 0)
369 			return err;
370 
371 		err = st_lsm6dsx_shub_master_enable(sensor, true);
372 		if (err < 0)
373 			return err;
374 
375 		st_lsm6dsx_shub_wait_complete(hw);
376 
377 		st_lsm6dsx_shub_master_enable(sensor, false);
378 	}
379 
380 	config[0] = hub_settings->pause;
381 	config[1] = 0;
382 	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));
383 }
384 
385 static int
st_lsm6dsx_shub_write_with_mask(struct st_lsm6dsx_sensor * sensor,u8 addr,u8 mask,u8 val)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, 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, sizeof(data));
399 }
400 
401 static int
st_lsm6dsx_shub_get_odr_val(struct st_lsm6dsx_sensor * sensor,u32 odr,u16 * val)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
st_lsm6dsx_shub_set_odr(struct st_lsm6dsx_sensor * sensor,u32 odr)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);
429 	if (err < 0)
430 		return err;
431 
432 	settings = sensor->ext_info.settings;
433 	return st_lsm6dsx_shub_write_with_mask(sensor,
434 					       settings->odr_table.reg.addr,
435 					       settings->odr_table.reg.mask,
436 					       val);
437 }
438 
439 /* use SLV{1,2,3} for FIFO read operations */
440 static int
st_lsm6dsx_shub_config_channels(struct st_lsm6dsx_sensor * sensor,bool enable)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(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, slv_addr, config,
475 					 sizeof(config));
476 }
477 
st_lsm6dsx_shub_set_enable(struct st_lsm6dsx_sensor * sensor,bool enable)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 					      sensor->ext_info.slv_odr);
491 		if (err < 0)
492 			return err;
493 	} else {
494 		err = st_lsm6dsx_shub_write_with_mask(sensor,
495 					settings->odr_table.reg.addr,
496 					settings->odr_table.reg.mask, 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 					settings->pwr_table.reg.addr,
508 					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
st_lsm6dsx_shub_read_oneshot(struct st_lsm6dsx_sensor * sensor,struct iio_chan_spec const * ch,int * val)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, true);
525 	if (err < 0)
526 		return err;
527 
528 	delay = 1000000000 / sensor->ext_info.slv_odr;
529 	usleep_range(delay, 2 * delay);
530 
531 	len = min_t(int, sizeof(data), ch->scan_type.realbits >> 3);
532 	err = st_lsm6dsx_shub_read(sensor, ch->address, data, len);
533 	if (err < 0)
534 		return err;
535 
536 	err = st_lsm6dsx_shub_set_enable(sensor, 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
st_lsm6dsx_shub_read_raw(struct iio_dev * iio_dev,struct iio_chan_spec const * ch,int * val,int * val2,long mask)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(iio_dev);
557 	int ret;
558 
559 	switch (mask) {
560 	case IIO_CHAN_INFO_RAW:
561 		ret = iio_device_claim_direct_mode(iio_dev);
562 		if (ret)
563 			break;
564 
565 		ret = st_lsm6dsx_shub_read_oneshot(sensor, ch, val);
566 		iio_device_release_direct_mode(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
st_lsm6dsx_shub_set_full_scale(struct st_lsm6dsx_sensor * sensor,u32 gain)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, fs_table->reg.addr,
606 					      fs_table->reg.mask,
607 					      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
st_lsm6dsx_shub_write_raw(struct iio_dev * iio_dev,struct iio_chan_spec const * chan,int val,int val2,long mask)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(iio_dev);
622 	int err;
623 
624 	err = iio_device_claim_direct_mode(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, 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(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
641 			odr = st_lsm6dsx_check_odr(ref_sensor, 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, val2);
654 		break;
655 	default:
656 		err = -EINVAL;
657 		break;
658 	}
659 
660 release:
661 	iio_device_release_direct_mode(iio_dev);
662 
663 	return err;
664 }
665 
666 static ssize_t
st_lsm6dsx_shub_sampling_freq_avail(struct device * dev,struct device_attribute * attr,char * buf)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(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 + len, PAGE_SIZE - len, "%d.%03d ",
680 				 val / 1000, val % 1000);
681 	}
682 	buf[len - 1] = '\n';
683 
684 	return len;
685 }
686 
st_lsm6dsx_shub_scale_avail(struct device * dev,struct device_attribute * attr,char * buf)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(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 + len, PAGE_SIZE - len, "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 *
st_lsm6dsx_shub_alloc_iiodev(struct st_lsm6dsx_hw * hw,enum st_lsm6dsx_sensor_id id,const struct st_lsm6dsx_ext_dev_settings * info,u8 i2c_addr,const char * name)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(hw->dev, 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(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(hw->dev, 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(sensor->name, sizeof(sensor->name), "%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 
st_lsm6dsx_shub_init_device(struct st_lsm6dsx_sensor * sensor)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 						      settings->bdu.addr,
795 						      settings->bdu.mask, 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 					settings->temp_comp.addr,
803 					settings->temp_comp.mask, 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 					settings->off_canc.addr,
811 					settings->off_canc.mask, 1);
812 		if (err < 0)
813 			return err;
814 	}
815 
816 	return 0;
817 }
818 
819 static int
st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw * hw,u8 * i2c_addr,const struct st_lsm6dsx_ext_dev_settings * settings)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(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, slv_addr, config,
848 						sizeof(config));
849 		if (err < 0)
850 			return err;
851 
852 		err = st_lsm6dsx_shub_master_enable(sensor, 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, sizeof(data));
859 
860 		st_lsm6dsx_shub_master_enable(sensor, 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, slv_addr, config,
878 					sizeof(config));
879 	if (err < 0)
880 		return err;
881 
882 	return found ? 0 : -ENODEV;
883 }
884 
st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw * hw,const char * name)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,
894 					&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 						&st_lsm6dsx_ext_dev_table[i],
902 						i2c_addr, name);
903 		if (!hw->iio_devs[id])
904 			return -ENOMEM;
905 
906 		sensor = iio_priv(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