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 <asm/unaligned.h>
18 #include <linux/iio/common/st_sensors.h>
21 #define ST_SENSORS_WAI_ADDRESS 0x0f
23 static int st_sensors_write_data_with_mask(struct iio_dev
*indio_dev
,
24 u8 reg_addr
, u8 mask
, u8 data
)
28 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
30 err
= sdata
->tf
->read_byte(&sdata
->tb
, sdata
->dev
, reg_addr
, &new_data
);
32 goto st_sensors_write_data_with_mask_error
;
34 new_data
= ((new_data
& (~mask
)) | ((data
<< __ffs(mask
)) & mask
));
35 err
= sdata
->tf
->write_byte(&sdata
->tb
, sdata
->dev
, reg_addr
, new_data
);
37 st_sensors_write_data_with_mask_error
:
41 static int st_sensors_match_odr(struct st_sensors
*sensor
,
42 unsigned int odr
, struct st_sensor_odr_avl
*odr_out
)
46 for (i
= 0; i
< ST_SENSORS_ODR_LIST_MAX
; i
++) {
47 if (sensor
->odr
.odr_avl
[i
].hz
== 0)
48 goto st_sensors_match_odr_error
;
50 if (sensor
->odr
.odr_avl
[i
].hz
== odr
) {
51 odr_out
->hz
= sensor
->odr
.odr_avl
[i
].hz
;
52 odr_out
->value
= sensor
->odr
.odr_avl
[i
].value
;
58 st_sensors_match_odr_error
:
62 int st_sensors_set_odr(struct iio_dev
*indio_dev
, unsigned int odr
)
65 struct st_sensor_odr_avl odr_out
;
66 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
68 err
= st_sensors_match_odr(sdata
->sensor
, odr
, &odr_out
);
70 goto st_sensors_match_odr_error
;
72 if ((sdata
->sensor
->odr
.addr
== sdata
->sensor
->pw
.addr
) &&
73 (sdata
->sensor
->odr
.mask
== sdata
->sensor
->pw
.mask
)) {
74 if (sdata
->enabled
== true) {
75 err
= st_sensors_write_data_with_mask(indio_dev
,
76 sdata
->sensor
->odr
.addr
,
77 sdata
->sensor
->odr
.mask
,
83 err
= st_sensors_write_data_with_mask(indio_dev
,
84 sdata
->sensor
->odr
.addr
, sdata
->sensor
->odr
.mask
,
88 sdata
->odr
= odr_out
.hz
;
90 st_sensors_match_odr_error
:
93 EXPORT_SYMBOL(st_sensors_set_odr
);
95 static int st_sensors_match_fs(struct st_sensors
*sensor
,
96 unsigned int fs
, int *index_fs_avl
)
100 for (i
= 0; i
< ST_SENSORS_FULLSCALE_AVL_MAX
; i
++) {
101 if (sensor
->fs
.fs_avl
[i
].num
== 0)
102 goto st_sensors_match_odr_error
;
104 if (sensor
->fs
.fs_avl
[i
].num
== fs
) {
111 st_sensors_match_odr_error
:
115 static int st_sensors_set_fullscale(struct iio_dev
*indio_dev
, unsigned int fs
)
118 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
120 err
= st_sensors_match_fs(sdata
->sensor
, fs
, &i
);
122 goto st_accel_set_fullscale_error
;
124 err
= st_sensors_write_data_with_mask(indio_dev
,
125 sdata
->sensor
->fs
.addr
,
126 sdata
->sensor
->fs
.mask
,
127 sdata
->sensor
->fs
.fs_avl
[i
].value
);
129 goto st_accel_set_fullscale_error
;
131 sdata
->current_fullscale
= (struct st_sensor_fullscale_avl
*)
132 &sdata
->sensor
->fs
.fs_avl
[i
];
135 st_accel_set_fullscale_error
:
136 dev_err(&indio_dev
->dev
, "failed to set new fullscale.\n");
140 int st_sensors_set_enable(struct iio_dev
*indio_dev
, bool enable
)
145 struct st_sensor_odr_avl odr_out
;
146 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
150 tmp_value
= sdata
->sensor
->pw
.value_on
;
151 if ((sdata
->sensor
->odr
.addr
== sdata
->sensor
->pw
.addr
) &&
152 (sdata
->sensor
->odr
.mask
== sdata
->sensor
->pw
.mask
)) {
153 err
= st_sensors_match_odr(sdata
->sensor
,
154 sdata
->odr
, &odr_out
);
156 goto set_enable_error
;
157 tmp_value
= odr_out
.value
;
160 err
= st_sensors_write_data_with_mask(indio_dev
,
161 sdata
->sensor
->pw
.addr
,
162 sdata
->sensor
->pw
.mask
, tmp_value
);
164 goto set_enable_error
;
166 sdata
->enabled
= true;
169 sdata
->odr
= odr_out
.hz
;
171 err
= st_sensors_write_data_with_mask(indio_dev
,
172 sdata
->sensor
->pw
.addr
,
173 sdata
->sensor
->pw
.mask
,
174 sdata
->sensor
->pw
.value_off
);
176 goto set_enable_error
;
178 sdata
->enabled
= false;
184 EXPORT_SYMBOL(st_sensors_set_enable
);
186 int st_sensors_set_axis_enable(struct iio_dev
*indio_dev
, u8 axis_enable
)
188 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
190 return st_sensors_write_data_with_mask(indio_dev
,
191 sdata
->sensor
->enable_axis
.addr
,
192 sdata
->sensor
->enable_axis
.mask
, axis_enable
);
194 EXPORT_SYMBOL(st_sensors_set_axis_enable
);
196 int st_sensors_init_sensor(struct iio_dev
*indio_dev
)
199 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
201 mutex_init(&sdata
->tb
.buf_lock
);
203 err
= st_sensors_set_enable(indio_dev
, false);
207 err
= st_sensors_set_fullscale(indio_dev
,
208 sdata
->current_fullscale
->num
);
212 err
= st_sensors_set_odr(indio_dev
, sdata
->odr
);
217 err
= st_sensors_write_data_with_mask(indio_dev
,
218 sdata
->sensor
->bdu
.addr
, sdata
->sensor
->bdu
.mask
, true);
222 err
= st_sensors_set_axis_enable(indio_dev
, ST_SENSORS_ENABLE_ALL_AXIS
);
227 EXPORT_SYMBOL(st_sensors_init_sensor
);
229 int st_sensors_set_dataready_irq(struct iio_dev
*indio_dev
, bool enable
)
232 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
234 /* Enable/Disable the interrupt generator 1. */
235 if (sdata
->sensor
->drdy_irq
.ig1
.en_addr
> 0) {
236 err
= st_sensors_write_data_with_mask(indio_dev
,
237 sdata
->sensor
->drdy_irq
.ig1
.en_addr
,
238 sdata
->sensor
->drdy_irq
.ig1
.en_mask
, (int)enable
);
240 goto st_accel_set_dataready_irq_error
;
243 /* Enable/Disable the interrupt generator for data ready. */
244 err
= st_sensors_write_data_with_mask(indio_dev
,
245 sdata
->sensor
->drdy_irq
.addr
,
246 sdata
->sensor
->drdy_irq
.mask
, (int)enable
);
248 st_accel_set_dataready_irq_error
:
251 EXPORT_SYMBOL(st_sensors_set_dataready_irq
);
253 int st_sensors_set_fullscale_by_gain(struct iio_dev
*indio_dev
, int scale
)
255 int err
= -EINVAL
, i
;
256 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
258 for (i
= 0; i
< ST_SENSORS_FULLSCALE_AVL_MAX
; i
++) {
259 if ((sdata
->sensor
->fs
.fs_avl
[i
].gain
== scale
) &&
260 (sdata
->sensor
->fs
.fs_avl
[i
].gain
!= 0)) {
266 goto st_sensors_match_scale_error
;
268 err
= st_sensors_set_fullscale(indio_dev
,
269 sdata
->sensor
->fs
.fs_avl
[i
].num
);
271 st_sensors_match_scale_error
:
274 EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain
);
276 static int st_sensors_read_axis_data(struct iio_dev
*indio_dev
,
277 u8 ch_addr
, int *data
)
280 u8 outdata
[ST_SENSORS_BYTE_FOR_CHANNEL
];
281 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
283 err
= sdata
->tf
->read_multiple_byte(&sdata
->tb
, sdata
->dev
,
284 ch_addr
, ST_SENSORS_BYTE_FOR_CHANNEL
,
285 outdata
, sdata
->multiread_bit
);
289 *data
= (s16
)get_unaligned_le16(outdata
);
295 int st_sensors_read_info_raw(struct iio_dev
*indio_dev
,
296 struct iio_chan_spec
const *ch
, int *val
)
299 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
301 mutex_lock(&indio_dev
->mlock
);
302 if (indio_dev
->currentmode
== INDIO_BUFFER_TRIGGERED
) {
306 err
= st_sensors_set_enable(indio_dev
, true);
310 msleep((sdata
->sensor
->bootime
* 1000) / sdata
->odr
);
311 err
= st_sensors_read_axis_data(indio_dev
, ch
->address
, val
);
315 *val
= *val
>> ch
->scan_type
.shift
;
317 mutex_unlock(&indio_dev
->mlock
);
322 mutex_unlock(&indio_dev
->mlock
);
325 EXPORT_SYMBOL(st_sensors_read_info_raw
);
327 int st_sensors_check_device_support(struct iio_dev
*indio_dev
,
328 int num_sensors_list
, const struct st_sensors
*sensors
)
332 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
334 err
= sdata
->tf
->read_byte(&sdata
->tb
, sdata
->dev
,
335 ST_SENSORS_DEFAULT_WAI_ADDRESS
, &wai
);
337 dev_err(&indio_dev
->dev
, "failed to read Who-Am-I register.\n");
341 for (i
= 0; i
< num_sensors_list
; i
++) {
342 if (sensors
[i
].wai
== wai
)
345 if (i
== num_sensors_list
)
346 goto device_not_supported
;
348 for (n
= 0; n
< ARRAY_SIZE(sensors
[i
].sensors_supported
); n
++) {
349 if (strcmp(indio_dev
->name
,
350 &sensors
[i
].sensors_supported
[n
][0]) == 0)
353 if (n
== ARRAY_SIZE(sensors
[i
].sensors_supported
)) {
354 dev_err(&indio_dev
->dev
, "device name and WhoAmI mismatch.\n");
355 goto sensor_name_mismatch
;
358 sdata
->sensor
= (struct st_sensors
*)&sensors
[i
];
362 device_not_supported
:
363 dev_err(&indio_dev
->dev
, "device not supported: WhoAmI (0x%x).\n", wai
);
364 sensor_name_mismatch
:
369 EXPORT_SYMBOL(st_sensors_check_device_support
);
371 ssize_t
st_sensors_sysfs_get_sampling_frequency(struct device
*dev
,
372 struct device_attribute
*attr
, char *buf
)
374 struct st_sensor_data
*adata
= iio_priv(dev_get_drvdata(dev
));
376 return sprintf(buf
, "%d\n", adata
->odr
);
378 EXPORT_SYMBOL(st_sensors_sysfs_get_sampling_frequency
);
380 ssize_t
st_sensors_sysfs_set_sampling_frequency(struct device
*dev
,
381 struct device_attribute
*attr
, const char *buf
, size_t size
)
385 struct iio_dev
*indio_dev
= dev_get_drvdata(dev
);
387 err
= kstrtoint(buf
, 10, &odr
);
389 goto conversion_error
;
391 mutex_lock(&indio_dev
->mlock
);
392 err
= st_sensors_set_odr(indio_dev
, odr
);
393 mutex_unlock(&indio_dev
->mlock
);
396 return err
< 0 ? err
: size
;
398 EXPORT_SYMBOL(st_sensors_sysfs_set_sampling_frequency
);
400 ssize_t
st_sensors_sysfs_sampling_frequency_avail(struct device
*dev
,
401 struct device_attribute
*attr
, char *buf
)
404 struct iio_dev
*indio_dev
= dev_get_drvdata(dev
);
405 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
407 mutex_lock(&indio_dev
->mlock
);
408 for (i
= 0; i
< ST_SENSORS_ODR_LIST_MAX
; i
++) {
409 if (sdata
->sensor
->odr
.odr_avl
[i
].hz
== 0)
412 len
+= scnprintf(buf
+ len
, PAGE_SIZE
- len
, "%d ",
413 sdata
->sensor
->odr
.odr_avl
[i
].hz
);
415 mutex_unlock(&indio_dev
->mlock
);
420 EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail
);
422 ssize_t
st_sensors_sysfs_scale_avail(struct device
*dev
,
423 struct device_attribute
*attr
, char *buf
)
426 struct iio_dev
*indio_dev
= dev_get_drvdata(dev
);
427 struct st_sensor_data
*sdata
= iio_priv(indio_dev
);
429 mutex_lock(&indio_dev
->mlock
);
430 for (i
= 0; i
< ST_SENSORS_FULLSCALE_AVL_MAX
; i
++) {
431 if (sdata
->sensor
->fs
.fs_avl
[i
].num
== 0)
434 len
+= scnprintf(buf
+ len
, PAGE_SIZE
- len
, "0.%06u ",
435 sdata
->sensor
->fs
.fs_avl
[i
].gain
);
437 mutex_unlock(&indio_dev
->mlock
);
442 EXPORT_SYMBOL(st_sensors_sysfs_scale_avail
);
444 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
445 MODULE_DESCRIPTION("STMicroelectronics ST-sensors core");
446 MODULE_LICENSE("GPL v2");