2 * STMicroelectronics sensors core library driver
4 * Copyright 2012-2013 STMicroelectronics Inc.
6 * Denis Ciocca <denis.ciocca@st.com>
8 * Licensed under the GPL-2.
11 #include <linux/kernel.h>
12 #include <linux/module.h>
13 #include <linux/slab.h>
14 #include <linux/delay.h>
15 #include <linux/iio/iio.h>
16 #include <linux/regulator/consumer.h>
18 #include <linux/of_device.h>
19 #include <asm/unaligned.h>
20 #include <linux/iio/common/st_sensors.h>
22 #include "st_sensors_core.h"
24 static inline u32
st_sensors_get_unaligned_le24(const u8
*p
)
26 return (s32
)((p
[0] | p
[1] << 8 | p
[2] << 16) << 8) >> 8;
29 int st_sensors_write_data_with_mask(struct iio_dev
*indio_dev
,
30 u8 reg_addr
, u8 mask
, u8 data
)
34 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
36 err
= sdata
->tf
->read_byte(&sdata
->tb
, sdata
->dev
, reg_addr
, &new_data
);
38 goto st_sensors_write_data_with_mask_error
;
40 new_data
= ((new_data
& (~mask
)) | ((data
<< __ffs(mask
)) & mask
));
41 err
= sdata
->tf
->write_byte(&sdata
->tb
, sdata
->dev
, reg_addr
, new_data
);
43 st_sensors_write_data_with_mask_error
:
47 int st_sensors_debugfs_reg_access(struct iio_dev
*indio_dev
,
48 unsigned reg
, unsigned writeval
,
51 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
56 return sdata
->tf
->write_byte(&sdata
->tb
, sdata
->dev
,
57 (u8
)reg
, (u8
)writeval
);
59 err
= sdata
->tf
->read_byte(&sdata
->tb
, sdata
->dev
, (u8
)reg
, &readdata
);
63 *readval
= (unsigned)readdata
;
67 EXPORT_SYMBOL(st_sensors_debugfs_reg_access
);
69 static int st_sensors_match_odr(struct st_sensor_settings
*sensor_settings
,
70 unsigned int odr
, struct st_sensor_odr_avl
*odr_out
)
74 for (i
= 0; i
< ST_SENSORS_ODR_LIST_MAX
; i
++) {
75 if (sensor_settings
->odr
.odr_avl
[i
].hz
== 0)
76 goto st_sensors_match_odr_error
;
78 if (sensor_settings
->odr
.odr_avl
[i
].hz
== odr
) {
79 odr_out
->hz
= sensor_settings
->odr
.odr_avl
[i
].hz
;
80 odr_out
->value
= sensor_settings
->odr
.odr_avl
[i
].value
;
86 st_sensors_match_odr_error
:
90 int st_sensors_set_odr(struct iio_dev
*indio_dev
, unsigned int odr
)
93 struct st_sensor_odr_avl odr_out
= {0, 0};
94 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
96 err
= st_sensors_match_odr(sdata
->sensor_settings
, odr
, &odr_out
);
98 goto st_sensors_match_odr_error
;
100 if ((sdata
->sensor_settings
->odr
.addr
==
101 sdata
->sensor_settings
->pw
.addr
) &&
102 (sdata
->sensor_settings
->odr
.mask
==
103 sdata
->sensor_settings
->pw
.mask
)) {
104 if (sdata
->enabled
== true) {
105 err
= st_sensors_write_data_with_mask(indio_dev
,
106 sdata
->sensor_settings
->odr
.addr
,
107 sdata
->sensor_settings
->odr
.mask
,
113 err
= st_sensors_write_data_with_mask(indio_dev
,
114 sdata
->sensor_settings
->odr
.addr
,
115 sdata
->sensor_settings
->odr
.mask
,
119 sdata
->odr
= odr_out
.hz
;
121 st_sensors_match_odr_error
:
124 EXPORT_SYMBOL(st_sensors_set_odr
);
126 static int st_sensors_match_fs(struct st_sensor_settings
*sensor_settings
,
127 unsigned int fs
, int *index_fs_avl
)
129 int i
, ret
= -EINVAL
;
131 for (i
= 0; i
< ST_SENSORS_FULLSCALE_AVL_MAX
; i
++) {
132 if (sensor_settings
->fs
.fs_avl
[i
].num
== 0)
133 goto st_sensors_match_odr_error
;
135 if (sensor_settings
->fs
.fs_avl
[i
].num
== fs
) {
142 st_sensors_match_odr_error
:
146 static int st_sensors_set_fullscale(struct iio_dev
*indio_dev
, unsigned int fs
)
149 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
151 if (sdata
->sensor_settings
->fs
.addr
== 0)
154 err
= st_sensors_match_fs(sdata
->sensor_settings
, fs
, &i
);
156 goto st_accel_set_fullscale_error
;
158 err
= st_sensors_write_data_with_mask(indio_dev
,
159 sdata
->sensor_settings
->fs
.addr
,
160 sdata
->sensor_settings
->fs
.mask
,
161 sdata
->sensor_settings
->fs
.fs_avl
[i
].value
);
163 goto st_accel_set_fullscale_error
;
165 sdata
->current_fullscale
= (struct st_sensor_fullscale_avl
*)
166 &sdata
->sensor_settings
->fs
.fs_avl
[i
];
169 st_accel_set_fullscale_error
:
170 dev_err(&indio_dev
->dev
, "failed to set new fullscale.\n");
174 int st_sensors_set_enable(struct iio_dev
*indio_dev
, bool enable
)
179 struct st_sensor_odr_avl odr_out
= {0, 0};
180 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
183 tmp_value
= sdata
->sensor_settings
->pw
.value_on
;
184 if ((sdata
->sensor_settings
->odr
.addr
==
185 sdata
->sensor_settings
->pw
.addr
) &&
186 (sdata
->sensor_settings
->odr
.mask
==
187 sdata
->sensor_settings
->pw
.mask
)) {
188 err
= st_sensors_match_odr(sdata
->sensor_settings
,
189 sdata
->odr
, &odr_out
);
191 goto set_enable_error
;
192 tmp_value
= odr_out
.value
;
195 err
= st_sensors_write_data_with_mask(indio_dev
,
196 sdata
->sensor_settings
->pw
.addr
,
197 sdata
->sensor_settings
->pw
.mask
, tmp_value
);
199 goto set_enable_error
;
201 sdata
->enabled
= true;
204 sdata
->odr
= odr_out
.hz
;
206 err
= st_sensors_write_data_with_mask(indio_dev
,
207 sdata
->sensor_settings
->pw
.addr
,
208 sdata
->sensor_settings
->pw
.mask
,
209 sdata
->sensor_settings
->pw
.value_off
);
211 goto set_enable_error
;
213 sdata
->enabled
= false;
219 EXPORT_SYMBOL(st_sensors_set_enable
);
221 int st_sensors_set_axis_enable(struct iio_dev
*indio_dev
, u8 axis_enable
)
223 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
225 return st_sensors_write_data_with_mask(indio_dev
,
226 sdata
->sensor_settings
->enable_axis
.addr
,
227 sdata
->sensor_settings
->enable_axis
.mask
,
230 EXPORT_SYMBOL(st_sensors_set_axis_enable
);
232 int st_sensors_power_enable(struct iio_dev
*indio_dev
)
234 struct st_sensor_data
*pdata
= iio_priv(indio_dev
);
237 /* Regulators not mandatory, but if requested we should enable them. */
238 pdata
->vdd
= devm_regulator_get(indio_dev
->dev
.parent
, "vdd");
239 if (IS_ERR(pdata
->vdd
)) {
240 dev_err(&indio_dev
->dev
, "unable to get Vdd supply\n");
241 return PTR_ERR(pdata
->vdd
);
243 err
= regulator_enable(pdata
->vdd
);
245 dev_warn(&indio_dev
->dev
,
246 "Failed to enable specified Vdd supply\n");
250 pdata
->vdd_io
= devm_regulator_get(indio_dev
->dev
.parent
, "vddio");
251 if (IS_ERR(pdata
->vdd_io
)) {
252 dev_err(&indio_dev
->dev
, "unable to get Vdd_IO supply\n");
253 err
= PTR_ERR(pdata
->vdd_io
);
254 goto st_sensors_disable_vdd
;
256 err
= regulator_enable(pdata
->vdd_io
);
258 dev_warn(&indio_dev
->dev
,
259 "Failed to enable specified Vdd_IO supply\n");
260 goto st_sensors_disable_vdd
;
265 st_sensors_disable_vdd
:
266 regulator_disable(pdata
->vdd
);
269 EXPORT_SYMBOL(st_sensors_power_enable
);
271 void st_sensors_power_disable(struct iio_dev
*indio_dev
)
273 struct st_sensor_data
*pdata
= iio_priv(indio_dev
);
275 regulator_disable(pdata
->vdd
);
276 regulator_disable(pdata
->vdd_io
);
278 EXPORT_SYMBOL(st_sensors_power_disable
);
280 static int st_sensors_set_drdy_int_pin(struct iio_dev
*indio_dev
,
281 struct st_sensors_platform_data
*pdata
)
283 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
285 /* Sensor does not support interrupts */
286 if (sdata
->sensor_settings
->drdy_irq
.addr
== 0) {
287 if (pdata
->drdy_int_pin
)
288 dev_info(&indio_dev
->dev
,
289 "DRDY on pin INT%d specified, but sensor "
290 "does not support interrupts\n",
291 pdata
->drdy_int_pin
);
295 switch (pdata
->drdy_int_pin
) {
297 if (sdata
->sensor_settings
->drdy_irq
.mask_int1
== 0) {
298 dev_err(&indio_dev
->dev
,
299 "DRDY on INT1 not available.\n");
302 sdata
->drdy_int_pin
= 1;
305 if (sdata
->sensor_settings
->drdy_irq
.mask_int2
== 0) {
306 dev_err(&indio_dev
->dev
,
307 "DRDY on INT2 not available.\n");
310 sdata
->drdy_int_pin
= 2;
313 dev_err(&indio_dev
->dev
, "DRDY on pdata not valid.\n");
317 if (pdata
->open_drain
) {
318 if (!sdata
->sensor_settings
->drdy_irq
.addr_od
)
319 dev_err(&indio_dev
->dev
,
320 "open drain requested but unsupported.\n");
322 sdata
->int_pin_open_drain
= true;
329 static struct st_sensors_platform_data
*st_sensors_of_probe(struct device
*dev
,
330 struct st_sensors_platform_data
*defdata
)
332 struct st_sensors_platform_data
*pdata
;
333 struct device_node
*np
= dev
->of_node
;
339 pdata
= devm_kzalloc(dev
, sizeof(*pdata
), GFP_KERNEL
);
340 if (!of_property_read_u32(np
, "st,drdy-int-pin", &val
) && (val
<= 2))
341 pdata
->drdy_int_pin
= (u8
) val
;
343 pdata
->drdy_int_pin
= defdata
? defdata
->drdy_int_pin
: 0;
345 pdata
->open_drain
= of_property_read_bool(np
, "drive-open-drain");
351 * st_sensors_of_name_probe() - device tree probe for ST sensor name
352 * @dev: driver model representation of the device.
353 * @match: the OF match table for the device, containing compatible strings
354 * but also a .data field with the corresponding internal kernel name
355 * used by this sensor.
356 * @name: device name buffer reference.
357 * @len: device name buffer length.
359 * In effect this function matches a compatible string to an internal kernel
360 * name for a certain sensor device, so that the rest of the autodetection can
361 * rely on that name from this point on. I2C/SPI devices will be renamed
362 * to match the internal kernel convention.
364 void st_sensors_of_name_probe(struct device
*dev
,
365 const struct of_device_id
*match
,
368 const struct of_device_id
*of_id
;
370 of_id
= of_match_device(match
, dev
);
371 if (!of_id
|| !of_id
->data
)
374 /* The name from the OF match takes precedence if present */
375 strncpy(name
, of_id
->data
, len
);
376 name
[len
- 1] = '\0';
378 EXPORT_SYMBOL(st_sensors_of_name_probe
);
380 static struct st_sensors_platform_data
*st_sensors_of_probe(struct device
*dev
,
381 struct st_sensors_platform_data
*defdata
)
387 int st_sensors_init_sensor(struct iio_dev
*indio_dev
,
388 struct st_sensors_platform_data
*pdata
)
390 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
391 struct st_sensors_platform_data
*of_pdata
;
394 /* If OF/DT pdata exists, it will take precedence of anything else */
395 of_pdata
= st_sensors_of_probe(indio_dev
->dev
.parent
, pdata
);
400 err
= st_sensors_set_drdy_int_pin(indio_dev
, pdata
);
405 err
= st_sensors_set_enable(indio_dev
, false);
409 /* Disable DRDY, this might be still be enabled after reboot. */
410 err
= st_sensors_set_dataready_irq(indio_dev
, false);
414 if (sdata
->current_fullscale
) {
415 err
= st_sensors_set_fullscale(indio_dev
,
416 sdata
->current_fullscale
->num
);
420 dev_info(&indio_dev
->dev
, "Full-scale not possible\n");
422 err
= st_sensors_set_odr(indio_dev
, sdata
->odr
);
427 if (sdata
->sensor_settings
->bdu
.addr
) {
428 err
= st_sensors_write_data_with_mask(indio_dev
,
429 sdata
->sensor_settings
->bdu
.addr
,
430 sdata
->sensor_settings
->bdu
.mask
, true);
436 if (sdata
->sensor_settings
->das
.addr
) {
437 err
= st_sensors_write_data_with_mask(indio_dev
,
438 sdata
->sensor_settings
->das
.addr
,
439 sdata
->sensor_settings
->das
.mask
, 1);
444 if (sdata
->int_pin_open_drain
) {
445 dev_info(&indio_dev
->dev
,
446 "set interrupt line to open drain mode\n");
447 err
= st_sensors_write_data_with_mask(indio_dev
,
448 sdata
->sensor_settings
->drdy_irq
.addr_od
,
449 sdata
->sensor_settings
->drdy_irq
.mask_od
, 1);
454 err
= st_sensors_set_axis_enable(indio_dev
, ST_SENSORS_ENABLE_ALL_AXIS
);
458 EXPORT_SYMBOL(st_sensors_init_sensor
);
460 int st_sensors_set_dataready_irq(struct iio_dev
*indio_dev
, bool enable
)
464 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
466 if (!sdata
->sensor_settings
->drdy_irq
.addr
) {
468 * there are some devices (e.g. LIS3MDL) where drdy line is
469 * routed to a given pin and it is not possible to select a
470 * different one. Take into account irq status register
471 * to understand if irq trigger can be properly supported
473 if (sdata
->sensor_settings
->drdy_irq
.addr_stat_drdy
)
474 sdata
->hw_irq_trigger
= enable
;
478 /* Enable/Disable the interrupt generator 1. */
479 if (sdata
->sensor_settings
->drdy_irq
.ig1
.en_addr
> 0) {
480 err
= st_sensors_write_data_with_mask(indio_dev
,
481 sdata
->sensor_settings
->drdy_irq
.ig1
.en_addr
,
482 sdata
->sensor_settings
->drdy_irq
.ig1
.en_mask
,
485 goto st_accel_set_dataready_irq_error
;
488 if (sdata
->drdy_int_pin
== 1)
489 drdy_mask
= sdata
->sensor_settings
->drdy_irq
.mask_int1
;
491 drdy_mask
= sdata
->sensor_settings
->drdy_irq
.mask_int2
;
493 /* Flag to the poll function that the hardware trigger is in use */
494 sdata
->hw_irq_trigger
= enable
;
496 /* Enable/Disable the interrupt generator for data ready. */
497 err
= st_sensors_write_data_with_mask(indio_dev
,
498 sdata
->sensor_settings
->drdy_irq
.addr
,
499 drdy_mask
, (int)enable
);
501 st_accel_set_dataready_irq_error
:
504 EXPORT_SYMBOL(st_sensors_set_dataready_irq
);
506 int st_sensors_set_fullscale_by_gain(struct iio_dev
*indio_dev
, int scale
)
508 int err
= -EINVAL
, i
;
509 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
511 for (i
= 0; i
< ST_SENSORS_FULLSCALE_AVL_MAX
; i
++) {
512 if ((sdata
->sensor_settings
->fs
.fs_avl
[i
].gain
== scale
) &&
513 (sdata
->sensor_settings
->fs
.fs_avl
[i
].gain
!= 0)) {
519 goto st_sensors_match_scale_error
;
521 err
= st_sensors_set_fullscale(indio_dev
,
522 sdata
->sensor_settings
->fs
.fs_avl
[i
].num
);
524 st_sensors_match_scale_error
:
527 EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain
);
529 static int st_sensors_read_axis_data(struct iio_dev
*indio_dev
,
530 struct iio_chan_spec
const *ch
, int *data
)
534 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
535 unsigned int byte_for_channel
;
537 byte_for_channel
= DIV_ROUND_UP(ch
->scan_type
.realbits
+
538 ch
->scan_type
.shift
, 8);
539 outdata
= kmalloc(byte_for_channel
, GFP_KERNEL
);
543 err
= sdata
->tf
->read_multiple_byte(&sdata
->tb
, sdata
->dev
,
544 ch
->address
, byte_for_channel
,
545 outdata
, sdata
->multiread_bit
);
547 goto st_sensors_free_memory
;
549 if (byte_for_channel
== 1)
550 *data
= (s8
)*outdata
;
551 else if (byte_for_channel
== 2)
552 *data
= (s16
)get_unaligned_le16(outdata
);
553 else if (byte_for_channel
== 3)
554 *data
= (s32
)st_sensors_get_unaligned_le24(outdata
);
556 st_sensors_free_memory
:
562 int st_sensors_read_info_raw(struct iio_dev
*indio_dev
,
563 struct iio_chan_spec
const *ch
, int *val
)
566 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
568 mutex_lock(&indio_dev
->mlock
);
569 if (indio_dev
->currentmode
== INDIO_BUFFER_TRIGGERED
) {
573 err
= st_sensors_set_enable(indio_dev
, true);
577 msleep((sdata
->sensor_settings
->bootime
* 1000) / sdata
->odr
);
578 err
= st_sensors_read_axis_data(indio_dev
, ch
, val
);
582 *val
= *val
>> ch
->scan_type
.shift
;
584 err
= st_sensors_set_enable(indio_dev
, false);
587 mutex_unlock(&indio_dev
->mlock
);
591 EXPORT_SYMBOL(st_sensors_read_info_raw
);
593 static int st_sensors_init_interface_mode(struct iio_dev
*indio_dev
,
594 const struct st_sensor_settings
*sensor_settings
)
596 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
597 struct device_node
*np
= sdata
->dev
->of_node
;
598 struct st_sensors_platform_data
*pdata
;
600 pdata
= (struct st_sensors_platform_data
*)sdata
->dev
->platform_data
;
601 if (((np
&& of_property_read_bool(np
, "spi-3wire")) ||
602 (pdata
&& pdata
->spi_3wire
)) && sensor_settings
->sim
.addr
) {
605 err
= sdata
->tf
->write_byte(&sdata
->tb
, sdata
->dev
,
606 sensor_settings
->sim
.addr
,
607 sensor_settings
->sim
.value
);
609 dev_err(&indio_dev
->dev
,
610 "failed to init interface mode\n");
618 int st_sensors_check_device_support(struct iio_dev
*indio_dev
,
619 int num_sensors_list
,
620 const struct st_sensor_settings
*sensor_settings
)
624 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
626 for (i
= 0; i
< num_sensors_list
; i
++) {
627 for (n
= 0; n
< ST_SENSORS_MAX_4WAI
; n
++) {
628 if (strcmp(indio_dev
->name
,
629 sensor_settings
[i
].sensors_supported
[n
]) == 0) {
633 if (n
< ST_SENSORS_MAX_4WAI
)
636 if (i
== num_sensors_list
) {
637 dev_err(&indio_dev
->dev
, "device name %s not recognized.\n",
642 err
= st_sensors_init_interface_mode(indio_dev
, &sensor_settings
[i
]);
646 if (sensor_settings
[i
].wai_addr
) {
647 err
= sdata
->tf
->read_byte(&sdata
->tb
, sdata
->dev
,
648 sensor_settings
[i
].wai_addr
, &wai
);
650 dev_err(&indio_dev
->dev
,
651 "failed to read Who-Am-I register.\n");
655 if (sensor_settings
[i
].wai
!= wai
) {
656 dev_err(&indio_dev
->dev
,
657 "%s: WhoAmI mismatch (0x%x).\n",
658 indio_dev
->name
, wai
);
663 sdata
->sensor_settings
=
664 (struct st_sensor_settings
*)&sensor_settings
[i
];
668 EXPORT_SYMBOL(st_sensors_check_device_support
);
670 ssize_t
st_sensors_sysfs_sampling_frequency_avail(struct device
*dev
,
671 struct device_attribute
*attr
, char *buf
)
674 struct iio_dev
*indio_dev
= dev_get_drvdata(dev
);
675 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
677 mutex_lock(&indio_dev
->mlock
);
678 for (i
= 0; i
< ST_SENSORS_ODR_LIST_MAX
; i
++) {
679 if (sdata
->sensor_settings
->odr
.odr_avl
[i
].hz
== 0)
682 len
+= scnprintf(buf
+ len
, PAGE_SIZE
- len
, "%d ",
683 sdata
->sensor_settings
->odr
.odr_avl
[i
].hz
);
685 mutex_unlock(&indio_dev
->mlock
);
690 EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail
);
692 ssize_t
st_sensors_sysfs_scale_avail(struct device
*dev
,
693 struct device_attribute
*attr
, char *buf
)
695 int i
, len
= 0, q
, r
;
696 struct iio_dev
*indio_dev
= dev_get_drvdata(dev
);
697 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
699 mutex_lock(&indio_dev
->mlock
);
700 for (i
= 0; i
< ST_SENSORS_FULLSCALE_AVL_MAX
; i
++) {
701 if (sdata
->sensor_settings
->fs
.fs_avl
[i
].num
== 0)
704 q
= sdata
->sensor_settings
->fs
.fs_avl
[i
].gain
/ 1000000;
705 r
= sdata
->sensor_settings
->fs
.fs_avl
[i
].gain
% 1000000;
707 len
+= scnprintf(buf
+ len
, PAGE_SIZE
- len
, "%u.%06u ", q
, r
);
709 mutex_unlock(&indio_dev
->mlock
);
714 EXPORT_SYMBOL(st_sensors_sysfs_scale_avail
);
716 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
717 MODULE_DESCRIPTION("STMicroelectronics ST-sensors core");
718 MODULE_LICENSE("GPL v2");