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 <asm/unaligned.h>
19 #include <linux/iio/common/st_sensors.h>
21 #include "st_sensors_core.h"
23 static inline u32
st_sensors_get_unaligned_le24(const u8
*p
)
25 return (s32
)((p
[0] | p
[1] << 8 | p
[2] << 16) << 8) >> 8;
28 int st_sensors_write_data_with_mask(struct iio_dev
*indio_dev
,
29 u8 reg_addr
, u8 mask
, u8 data
)
33 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
35 err
= sdata
->tf
->read_byte(&sdata
->tb
, sdata
->dev
, reg_addr
, &new_data
);
37 goto st_sensors_write_data_with_mask_error
;
39 new_data
= ((new_data
& (~mask
)) | ((data
<< __ffs(mask
)) & mask
));
40 err
= sdata
->tf
->write_byte(&sdata
->tb
, sdata
->dev
, reg_addr
, new_data
);
42 st_sensors_write_data_with_mask_error
:
46 int st_sensors_debugfs_reg_access(struct iio_dev
*indio_dev
,
47 unsigned reg
, unsigned writeval
,
50 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
55 return sdata
->tf
->write_byte(&sdata
->tb
, sdata
->dev
,
56 (u8
)reg
, (u8
)writeval
);
58 err
= sdata
->tf
->read_byte(&sdata
->tb
, sdata
->dev
, (u8
)reg
, &readdata
);
62 *readval
= (unsigned)readdata
;
66 EXPORT_SYMBOL(st_sensors_debugfs_reg_access
);
68 static int st_sensors_match_odr(struct st_sensor_settings
*sensor_settings
,
69 unsigned int odr
, struct st_sensor_odr_avl
*odr_out
)
73 for (i
= 0; i
< ST_SENSORS_ODR_LIST_MAX
; i
++) {
74 if (sensor_settings
->odr
.odr_avl
[i
].hz
== 0)
75 goto st_sensors_match_odr_error
;
77 if (sensor_settings
->odr
.odr_avl
[i
].hz
== odr
) {
78 odr_out
->hz
= sensor_settings
->odr
.odr_avl
[i
].hz
;
79 odr_out
->value
= sensor_settings
->odr
.odr_avl
[i
].value
;
85 st_sensors_match_odr_error
:
89 int st_sensors_set_odr(struct iio_dev
*indio_dev
, unsigned int odr
)
92 struct st_sensor_odr_avl odr_out
= {0, 0};
93 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
95 err
= st_sensors_match_odr(sdata
->sensor_settings
, odr
, &odr_out
);
97 goto st_sensors_match_odr_error
;
99 if ((sdata
->sensor_settings
->odr
.addr
==
100 sdata
->sensor_settings
->pw
.addr
) &&
101 (sdata
->sensor_settings
->odr
.mask
==
102 sdata
->sensor_settings
->pw
.mask
)) {
103 if (sdata
->enabled
== true) {
104 err
= st_sensors_write_data_with_mask(indio_dev
,
105 sdata
->sensor_settings
->odr
.addr
,
106 sdata
->sensor_settings
->odr
.mask
,
112 err
= st_sensors_write_data_with_mask(indio_dev
,
113 sdata
->sensor_settings
->odr
.addr
,
114 sdata
->sensor_settings
->odr
.mask
,
118 sdata
->odr
= odr_out
.hz
;
120 st_sensors_match_odr_error
:
123 EXPORT_SYMBOL(st_sensors_set_odr
);
125 static int st_sensors_match_fs(struct st_sensor_settings
*sensor_settings
,
126 unsigned int fs
, int *index_fs_avl
)
128 int i
, ret
= -EINVAL
;
130 for (i
= 0; i
< ST_SENSORS_FULLSCALE_AVL_MAX
; i
++) {
131 if (sensor_settings
->fs
.fs_avl
[i
].num
== 0)
132 goto st_sensors_match_odr_error
;
134 if (sensor_settings
->fs
.fs_avl
[i
].num
== fs
) {
141 st_sensors_match_odr_error
:
145 static int st_sensors_set_fullscale(struct iio_dev
*indio_dev
, unsigned int fs
)
148 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
150 if (sdata
->sensor_settings
->fs
.addr
== 0)
153 err
= st_sensors_match_fs(sdata
->sensor_settings
, fs
, &i
);
155 goto st_accel_set_fullscale_error
;
157 err
= st_sensors_write_data_with_mask(indio_dev
,
158 sdata
->sensor_settings
->fs
.addr
,
159 sdata
->sensor_settings
->fs
.mask
,
160 sdata
->sensor_settings
->fs
.fs_avl
[i
].value
);
162 goto st_accel_set_fullscale_error
;
164 sdata
->current_fullscale
= (struct st_sensor_fullscale_avl
*)
165 &sdata
->sensor_settings
->fs
.fs_avl
[i
];
168 st_accel_set_fullscale_error
:
169 dev_err(&indio_dev
->dev
, "failed to set new fullscale.\n");
173 int st_sensors_set_enable(struct iio_dev
*indio_dev
, bool enable
)
178 struct st_sensor_odr_avl odr_out
= {0, 0};
179 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
182 tmp_value
= sdata
->sensor_settings
->pw
.value_on
;
183 if ((sdata
->sensor_settings
->odr
.addr
==
184 sdata
->sensor_settings
->pw
.addr
) &&
185 (sdata
->sensor_settings
->odr
.mask
==
186 sdata
->sensor_settings
->pw
.mask
)) {
187 err
= st_sensors_match_odr(sdata
->sensor_settings
,
188 sdata
->odr
, &odr_out
);
190 goto set_enable_error
;
191 tmp_value
= odr_out
.value
;
194 err
= st_sensors_write_data_with_mask(indio_dev
,
195 sdata
->sensor_settings
->pw
.addr
,
196 sdata
->sensor_settings
->pw
.mask
, tmp_value
);
198 goto set_enable_error
;
200 sdata
->enabled
= true;
203 sdata
->odr
= odr_out
.hz
;
205 err
= st_sensors_write_data_with_mask(indio_dev
,
206 sdata
->sensor_settings
->pw
.addr
,
207 sdata
->sensor_settings
->pw
.mask
,
208 sdata
->sensor_settings
->pw
.value_off
);
210 goto set_enable_error
;
212 sdata
->enabled
= false;
218 EXPORT_SYMBOL(st_sensors_set_enable
);
220 int st_sensors_set_axis_enable(struct iio_dev
*indio_dev
, u8 axis_enable
)
222 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
224 return st_sensors_write_data_with_mask(indio_dev
,
225 sdata
->sensor_settings
->enable_axis
.addr
,
226 sdata
->sensor_settings
->enable_axis
.mask
,
229 EXPORT_SYMBOL(st_sensors_set_axis_enable
);
231 int st_sensors_power_enable(struct iio_dev
*indio_dev
)
233 struct st_sensor_data
*pdata
= iio_priv(indio_dev
);
236 /* Regulators not mandatory, but if requested we should enable them. */
237 pdata
->vdd
= devm_regulator_get(indio_dev
->dev
.parent
, "vdd");
238 if (IS_ERR(pdata
->vdd
)) {
239 dev_err(&indio_dev
->dev
, "unable to get Vdd supply\n");
240 return PTR_ERR(pdata
->vdd
);
242 err
= regulator_enable(pdata
->vdd
);
244 dev_warn(&indio_dev
->dev
,
245 "Failed to enable specified Vdd supply\n");
249 pdata
->vdd_io
= devm_regulator_get(indio_dev
->dev
.parent
, "vddio");
250 if (IS_ERR(pdata
->vdd_io
)) {
251 dev_err(&indio_dev
->dev
, "unable to get Vdd_IO supply\n");
252 err
= PTR_ERR(pdata
->vdd_io
);
253 goto st_sensors_disable_vdd
;
255 err
= regulator_enable(pdata
->vdd_io
);
257 dev_warn(&indio_dev
->dev
,
258 "Failed to enable specified Vdd_IO supply\n");
259 goto st_sensors_disable_vdd
;
264 st_sensors_disable_vdd
:
265 regulator_disable(pdata
->vdd
);
268 EXPORT_SYMBOL(st_sensors_power_enable
);
270 void st_sensors_power_disable(struct iio_dev
*indio_dev
)
272 struct st_sensor_data
*pdata
= iio_priv(indio_dev
);
274 regulator_disable(pdata
->vdd
);
275 regulator_disable(pdata
->vdd_io
);
277 EXPORT_SYMBOL(st_sensors_power_disable
);
279 static int st_sensors_set_drdy_int_pin(struct iio_dev
*indio_dev
,
280 struct st_sensors_platform_data
*pdata
)
282 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
284 /* Sensor does not support interrupts */
285 if (sdata
->sensor_settings
->drdy_irq
.addr
== 0) {
286 if (pdata
->drdy_int_pin
)
287 dev_info(&indio_dev
->dev
,
288 "DRDY on pin INT%d specified, but sensor "
289 "does not support interrupts\n",
290 pdata
->drdy_int_pin
);
294 switch (pdata
->drdy_int_pin
) {
296 if (sdata
->sensor_settings
->drdy_irq
.mask_int1
== 0) {
297 dev_err(&indio_dev
->dev
,
298 "DRDY on INT1 not available.\n");
301 sdata
->drdy_int_pin
= 1;
304 if (sdata
->sensor_settings
->drdy_irq
.mask_int2
== 0) {
305 dev_err(&indio_dev
->dev
,
306 "DRDY on INT2 not available.\n");
309 sdata
->drdy_int_pin
= 2;
312 dev_err(&indio_dev
->dev
, "DRDY on pdata not valid.\n");
316 if (pdata
->open_drain
) {
317 if (!sdata
->sensor_settings
->drdy_irq
.addr_od
)
318 dev_err(&indio_dev
->dev
,
319 "open drain requested but unsupported.\n");
321 sdata
->int_pin_open_drain
= true;
328 static struct st_sensors_platform_data
*st_sensors_of_probe(struct device
*dev
,
329 struct st_sensors_platform_data
*defdata
)
331 struct st_sensors_platform_data
*pdata
;
332 struct device_node
*np
= dev
->of_node
;
338 pdata
= devm_kzalloc(dev
, sizeof(*pdata
), GFP_KERNEL
);
339 if (!of_property_read_u32(np
, "st,drdy-int-pin", &val
) && (val
<= 2))
340 pdata
->drdy_int_pin
= (u8
) val
;
342 pdata
->drdy_int_pin
= defdata
? defdata
->drdy_int_pin
: 0;
344 pdata
->open_drain
= of_property_read_bool(np
, "drive-open-drain");
349 static struct st_sensors_platform_data
*st_sensors_of_probe(struct device
*dev
,
350 struct st_sensors_platform_data
*defdata
)
356 int st_sensors_init_sensor(struct iio_dev
*indio_dev
,
357 struct st_sensors_platform_data
*pdata
)
359 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
360 struct st_sensors_platform_data
*of_pdata
;
363 /* If OF/DT pdata exists, it will take precedence of anything else */
364 of_pdata
= st_sensors_of_probe(indio_dev
->dev
.parent
, pdata
);
369 err
= st_sensors_set_drdy_int_pin(indio_dev
, pdata
);
374 err
= st_sensors_set_enable(indio_dev
, false);
378 /* Disable DRDY, this might be still be enabled after reboot. */
379 err
= st_sensors_set_dataready_irq(indio_dev
, false);
383 if (sdata
->current_fullscale
) {
384 err
= st_sensors_set_fullscale(indio_dev
,
385 sdata
->current_fullscale
->num
);
389 dev_info(&indio_dev
->dev
, "Full-scale not possible\n");
391 err
= st_sensors_set_odr(indio_dev
, sdata
->odr
);
396 if (sdata
->sensor_settings
->bdu
.addr
) {
397 err
= st_sensors_write_data_with_mask(indio_dev
,
398 sdata
->sensor_settings
->bdu
.addr
,
399 sdata
->sensor_settings
->bdu
.mask
, true);
404 if (sdata
->int_pin_open_drain
) {
405 dev_info(&indio_dev
->dev
,
406 "set interrupt line to open drain mode\n");
407 err
= st_sensors_write_data_with_mask(indio_dev
,
408 sdata
->sensor_settings
->drdy_irq
.addr_od
,
409 sdata
->sensor_settings
->drdy_irq
.mask_od
, 1);
414 err
= st_sensors_set_axis_enable(indio_dev
, ST_SENSORS_ENABLE_ALL_AXIS
);
418 EXPORT_SYMBOL(st_sensors_init_sensor
);
420 int st_sensors_set_dataready_irq(struct iio_dev
*indio_dev
, bool enable
)
424 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
426 if (!sdata
->sensor_settings
->drdy_irq
.addr
)
429 /* Enable/Disable the interrupt generator 1. */
430 if (sdata
->sensor_settings
->drdy_irq
.ig1
.en_addr
> 0) {
431 err
= st_sensors_write_data_with_mask(indio_dev
,
432 sdata
->sensor_settings
->drdy_irq
.ig1
.en_addr
,
433 sdata
->sensor_settings
->drdy_irq
.ig1
.en_mask
,
436 goto st_accel_set_dataready_irq_error
;
439 if (sdata
->drdy_int_pin
== 1)
440 drdy_mask
= sdata
->sensor_settings
->drdy_irq
.mask_int1
;
442 drdy_mask
= sdata
->sensor_settings
->drdy_irq
.mask_int2
;
444 /* Flag to the poll function that the hardware trigger is in use */
445 sdata
->hw_irq_trigger
= enable
;
447 /* Enable/Disable the interrupt generator for data ready. */
448 err
= st_sensors_write_data_with_mask(indio_dev
,
449 sdata
->sensor_settings
->drdy_irq
.addr
,
450 drdy_mask
, (int)enable
);
452 st_accel_set_dataready_irq_error
:
455 EXPORT_SYMBOL(st_sensors_set_dataready_irq
);
457 int st_sensors_set_fullscale_by_gain(struct iio_dev
*indio_dev
, int scale
)
459 int err
= -EINVAL
, i
;
460 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
462 for (i
= 0; i
< ST_SENSORS_FULLSCALE_AVL_MAX
; i
++) {
463 if ((sdata
->sensor_settings
->fs
.fs_avl
[i
].gain
== scale
) &&
464 (sdata
->sensor_settings
->fs
.fs_avl
[i
].gain
!= 0)) {
470 goto st_sensors_match_scale_error
;
472 err
= st_sensors_set_fullscale(indio_dev
,
473 sdata
->sensor_settings
->fs
.fs_avl
[i
].num
);
475 st_sensors_match_scale_error
:
478 EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain
);
480 static int st_sensors_read_axis_data(struct iio_dev
*indio_dev
,
481 struct iio_chan_spec
const *ch
, int *data
)
485 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
486 unsigned int byte_for_channel
= ch
->scan_type
.realbits
>> 3;
488 outdata
= kmalloc(byte_for_channel
, GFP_KERNEL
);
492 err
= sdata
->tf
->read_multiple_byte(&sdata
->tb
, sdata
->dev
,
493 ch
->address
, byte_for_channel
,
494 outdata
, sdata
->multiread_bit
);
496 goto st_sensors_free_memory
;
498 if (byte_for_channel
== 1)
499 *data
= (s8
)*outdata
;
500 else if (byte_for_channel
== 2)
501 *data
= (s16
)get_unaligned_le16(outdata
);
502 else if (byte_for_channel
== 3)
503 *data
= (s32
)st_sensors_get_unaligned_le24(outdata
);
505 st_sensors_free_memory
:
511 int st_sensors_read_info_raw(struct iio_dev
*indio_dev
,
512 struct iio_chan_spec
const *ch
, int *val
)
515 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
517 mutex_lock(&indio_dev
->mlock
);
518 if (indio_dev
->currentmode
== INDIO_BUFFER_TRIGGERED
) {
522 err
= st_sensors_set_enable(indio_dev
, true);
526 msleep((sdata
->sensor_settings
->bootime
* 1000) / sdata
->odr
);
527 err
= st_sensors_read_axis_data(indio_dev
, ch
, val
);
531 *val
= *val
>> ch
->scan_type
.shift
;
533 err
= st_sensors_set_enable(indio_dev
, false);
536 mutex_unlock(&indio_dev
->mlock
);
540 EXPORT_SYMBOL(st_sensors_read_info_raw
);
542 int st_sensors_check_device_support(struct iio_dev
*indio_dev
,
543 int num_sensors_list
,
544 const struct st_sensor_settings
*sensor_settings
)
548 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
550 for (i
= 0; i
< num_sensors_list
; i
++) {
551 for (n
= 0; n
< ST_SENSORS_MAX_4WAI
; n
++) {
552 if (strcmp(indio_dev
->name
,
553 sensor_settings
[i
].sensors_supported
[n
]) == 0) {
557 if (n
< ST_SENSORS_MAX_4WAI
)
560 if (i
== num_sensors_list
) {
561 dev_err(&indio_dev
->dev
, "device name %s not recognized.\n",
566 if (sensor_settings
[i
].wai_addr
) {
567 err
= sdata
->tf
->read_byte(&sdata
->tb
, sdata
->dev
,
568 sensor_settings
[i
].wai_addr
, &wai
);
570 dev_err(&indio_dev
->dev
,
571 "failed to read Who-Am-I register.\n");
575 if (sensor_settings
[i
].wai
!= wai
) {
576 dev_err(&indio_dev
->dev
,
577 "%s: WhoAmI mismatch (0x%x).\n",
578 indio_dev
->name
, wai
);
583 sdata
->sensor_settings
=
584 (struct st_sensor_settings
*)&sensor_settings
[i
];
588 EXPORT_SYMBOL(st_sensors_check_device_support
);
590 ssize_t
st_sensors_sysfs_sampling_frequency_avail(struct device
*dev
,
591 struct device_attribute
*attr
, char *buf
)
594 struct iio_dev
*indio_dev
= dev_get_drvdata(dev
);
595 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
597 mutex_lock(&indio_dev
->mlock
);
598 for (i
= 0; i
< ST_SENSORS_ODR_LIST_MAX
; i
++) {
599 if (sdata
->sensor_settings
->odr
.odr_avl
[i
].hz
== 0)
602 len
+= scnprintf(buf
+ len
, PAGE_SIZE
- len
, "%d ",
603 sdata
->sensor_settings
->odr
.odr_avl
[i
].hz
);
605 mutex_unlock(&indio_dev
->mlock
);
610 EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail
);
612 ssize_t
st_sensors_sysfs_scale_avail(struct device
*dev
,
613 struct device_attribute
*attr
, char *buf
)
615 int i
, len
= 0, q
, r
;
616 struct iio_dev
*indio_dev
= dev_get_drvdata(dev
);
617 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
619 mutex_lock(&indio_dev
->mlock
);
620 for (i
= 0; i
< ST_SENSORS_FULLSCALE_AVL_MAX
; i
++) {
621 if (sdata
->sensor_settings
->fs
.fs_avl
[i
].num
== 0)
624 q
= sdata
->sensor_settings
->fs
.fs_avl
[i
].gain
/ 1000000;
625 r
= sdata
->sensor_settings
->fs
.fs_avl
[i
].gain
% 1000000;
627 len
+= scnprintf(buf
+ len
, PAGE_SIZE
- len
, "%u.%06u ", q
, r
);
629 mutex_unlock(&indio_dev
->mlock
);
634 EXPORT_SYMBOL(st_sensors_sysfs_scale_avail
);
636 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
637 MODULE_DESCRIPTION("STMicroelectronics ST-sensors core");
638 MODULE_LICENSE("GPL v2");