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 = {0, 0};
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 = {0, 0};
146 struct st_sensor_data *sdata = iio_priv(indio_dev);
149 tmp_value = sdata->sensor->pw.value_on;
150 if ((sdata->sensor->odr.addr == sdata->sensor->pw.addr) &&
151 (sdata->sensor->odr.mask == sdata->sensor->pw.mask)) {
152 err = st_sensors_match_odr(sdata->sensor,
153 sdata->odr, &odr_out);
155 goto set_enable_error;
156 tmp_value = odr_out.value;
159 err = st_sensors_write_data_with_mask(indio_dev,
160 sdata->sensor->pw.addr,
161 sdata->sensor->pw.mask, tmp_value);
163 goto set_enable_error;
165 sdata->enabled = true;
168 sdata->odr = odr_out.hz;
170 err = st_sensors_write_data_with_mask(indio_dev,
171 sdata->sensor->pw.addr,
172 sdata->sensor->pw.mask,
173 sdata->sensor->pw.value_off);
175 goto set_enable_error;
177 sdata->enabled = false;
183 EXPORT_SYMBOL(st_sensors_set_enable);
185 int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable)
187 struct st_sensor_data *sdata = iio_priv(indio_dev);
189 return st_sensors_write_data_with_mask(indio_dev,
190 sdata->sensor->enable_axis.addr,
191 sdata->sensor->enable_axis.mask, axis_enable);
193 EXPORT_SYMBOL(st_sensors_set_axis_enable);
195 int st_sensors_init_sensor(struct iio_dev *indio_dev)
198 struct st_sensor_data *sdata = iio_priv(indio_dev);
200 mutex_init(&sdata->tb.buf_lock);
202 err = st_sensors_set_enable(indio_dev, false);
206 err = st_sensors_set_fullscale(indio_dev,
207 sdata->current_fullscale->num);
211 err = st_sensors_set_odr(indio_dev, sdata->odr);
216 err = st_sensors_write_data_with_mask(indio_dev,
217 sdata->sensor->bdu.addr, sdata->sensor->bdu.mask, true);
221 err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);
226 EXPORT_SYMBOL(st_sensors_init_sensor);
228 int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
231 struct st_sensor_data *sdata = iio_priv(indio_dev);
233 /* Enable/Disable the interrupt generator 1. */
234 if (sdata->sensor->drdy_irq.ig1.en_addr > 0) {
235 err = st_sensors_write_data_with_mask(indio_dev,
236 sdata->sensor->drdy_irq.ig1.en_addr,
237 sdata->sensor->drdy_irq.ig1.en_mask, (int)enable);
239 goto st_accel_set_dataready_irq_error;
242 /* Enable/Disable the interrupt generator for data ready. */
243 err = st_sensors_write_data_with_mask(indio_dev,
244 sdata->sensor->drdy_irq.addr,
245 sdata->sensor->drdy_irq.mask, (int)enable);
247 st_accel_set_dataready_irq_error:
250 EXPORT_SYMBOL(st_sensors_set_dataready_irq);
252 int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale)
254 int err = -EINVAL, i;
255 struct st_sensor_data *sdata = iio_priv(indio_dev);
257 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
258 if ((sdata->sensor->fs.fs_avl[i].gain == scale) &&
259 (sdata->sensor->fs.fs_avl[i].gain != 0)) {
265 goto st_sensors_match_scale_error;
267 err = st_sensors_set_fullscale(indio_dev,
268 sdata->sensor->fs.fs_avl[i].num);
270 st_sensors_match_scale_error:
273 EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain);
275 static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
276 u8 ch_addr, int *data)
279 u8 outdata[ST_SENSORS_BYTE_FOR_CHANNEL];
280 struct st_sensor_data *sdata = iio_priv(indio_dev);
282 err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
283 ch_addr, ST_SENSORS_BYTE_FOR_CHANNEL,
284 outdata, sdata->multiread_bit);
288 *data = (s16)get_unaligned_le16(outdata);
294 int st_sensors_read_info_raw(struct iio_dev *indio_dev,
295 struct iio_chan_spec const *ch, int *val)
298 struct st_sensor_data *sdata = iio_priv(indio_dev);
300 mutex_lock(&indio_dev->mlock);
301 if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
305 err = st_sensors_set_enable(indio_dev, true);
309 msleep((sdata->sensor->bootime * 1000) / sdata->odr);
310 err = st_sensors_read_axis_data(indio_dev, ch->address, val);
314 *val = *val >> ch->scan_type.shift;
316 mutex_unlock(&indio_dev->mlock);
321 mutex_unlock(&indio_dev->mlock);
324 EXPORT_SYMBOL(st_sensors_read_info_raw);
326 int st_sensors_check_device_support(struct iio_dev *indio_dev,
327 int num_sensors_list, const struct st_sensors *sensors)
331 struct st_sensor_data *sdata = iio_priv(indio_dev);
333 err = sdata->tf->read_byte(&sdata->tb, sdata->dev,
334 ST_SENSORS_DEFAULT_WAI_ADDRESS, &wai);
336 dev_err(&indio_dev->dev, "failed to read Who-Am-I register.\n");
340 for (i = 0; i < num_sensors_list; i++) {
341 if (sensors[i].wai == wai)
344 if (i == num_sensors_list)
345 goto device_not_supported;
347 for (n = 0; n < ARRAY_SIZE(sensors[i].sensors_supported); n++) {
348 if (strcmp(indio_dev->name,
349 &sensors[i].sensors_supported[n][0]) == 0)
352 if (n == ARRAY_SIZE(sensors[i].sensors_supported)) {
353 dev_err(&indio_dev->dev, "device name and WhoAmI mismatch.\n");
354 goto sensor_name_mismatch;
357 sdata->sensor = (struct st_sensors *)&sensors[i];
361 device_not_supported:
362 dev_err(&indio_dev->dev, "device not supported: WhoAmI (0x%x).\n", wai);
363 sensor_name_mismatch:
368 EXPORT_SYMBOL(st_sensors_check_device_support);
370 ssize_t st_sensors_sysfs_get_sampling_frequency(struct device *dev,
371 struct device_attribute *attr, char *buf)
373 struct st_sensor_data *adata = iio_priv(dev_get_drvdata(dev));
375 return sprintf(buf, "%d\n", adata->odr);
377 EXPORT_SYMBOL(st_sensors_sysfs_get_sampling_frequency);
379 ssize_t st_sensors_sysfs_set_sampling_frequency(struct device *dev,
380 struct device_attribute *attr, const char *buf, size_t size)
384 struct iio_dev *indio_dev = dev_get_drvdata(dev);
386 err = kstrtoint(buf, 10, &odr);
388 goto conversion_error;
390 mutex_lock(&indio_dev->mlock);
391 err = st_sensors_set_odr(indio_dev, odr);
392 mutex_unlock(&indio_dev->mlock);
395 return err < 0 ? err : size;
397 EXPORT_SYMBOL(st_sensors_sysfs_set_sampling_frequency);
399 ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev,
400 struct device_attribute *attr, char *buf)
403 struct iio_dev *indio_dev = dev_get_drvdata(dev);
404 struct st_sensor_data *sdata = iio_priv(indio_dev);
406 mutex_lock(&indio_dev->mlock);
407 for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
408 if (sdata->sensor->odr.odr_avl[i].hz == 0)
411 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
412 sdata->sensor->odr.odr_avl[i].hz);
414 mutex_unlock(&indio_dev->mlock);
419 EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail);
421 ssize_t st_sensors_sysfs_scale_avail(struct device *dev,
422 struct device_attribute *attr, char *buf)
425 struct iio_dev *indio_dev = dev_get_drvdata(dev);
426 struct st_sensor_data *sdata = iio_priv(indio_dev);
428 mutex_lock(&indio_dev->mlock);
429 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
430 if (sdata->sensor->fs.fs_avl[i].num == 0)
433 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
434 sdata->sensor->fs.fs_avl[i].gain);
436 mutex_unlock(&indio_dev->mlock);
441 EXPORT_SYMBOL(st_sensors_sysfs_scale_avail);
443 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
444 MODULE_DESCRIPTION("STMicroelectronics ST-sensors core");
445 MODULE_LICENSE("GPL v2");