1 /*
2 * STMicroelectronics sensors core library driver
3 *
4 * Copyright 2012-2013 STMicroelectronics Inc.
5 *
6 * Denis Ciocca <denis.ciocca@st.com>
7 *
8 * Licensed under the GPL-2.
9 */
10
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>
17 #include <linux/of.h>
18 #include <asm/unaligned.h>
19 #include <linux/iio/common/st_sensors.h>
20
21
22 #define ST_SENSORS_WAI_ADDRESS 0x0f
23
st_sensors_get_unaligned_le24(const u8 * p)24 static inline u32 st_sensors_get_unaligned_le24(const u8 *p)
25 {
26 return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8;
27 }
28
st_sensors_write_data_with_mask(struct iio_dev * indio_dev,u8 reg_addr,u8 mask,u8 data)29 static int st_sensors_write_data_with_mask(struct iio_dev *indio_dev,
30 u8 reg_addr, u8 mask, u8 data)
31 {
32 int err;
33 u8 new_data;
34 struct st_sensor_data *sdata = iio_priv(indio_dev);
35
36 err = sdata->tf->read_byte(&sdata->tb, sdata->dev, reg_addr, &new_data);
37 if (err < 0)
38 goto st_sensors_write_data_with_mask_error;
39
40 new_data = ((new_data & (~mask)) | ((data << __ffs(mask)) & mask));
41 err = sdata->tf->write_byte(&sdata->tb, sdata->dev, reg_addr, new_data);
42
43 st_sensors_write_data_with_mask_error:
44 return err;
45 }
46
st_sensors_match_odr(struct st_sensor_settings * sensor_settings,unsigned int odr,struct st_sensor_odr_avl * odr_out)47 static int st_sensors_match_odr(struct st_sensor_settings *sensor_settings,
48 unsigned int odr, struct st_sensor_odr_avl *odr_out)
49 {
50 int i, ret = -EINVAL;
51
52 for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
53 if (sensor_settings->odr.odr_avl[i].hz == 0)
54 goto st_sensors_match_odr_error;
55
56 if (sensor_settings->odr.odr_avl[i].hz == odr) {
57 odr_out->hz = sensor_settings->odr.odr_avl[i].hz;
58 odr_out->value = sensor_settings->odr.odr_avl[i].value;
59 ret = 0;
60 break;
61 }
62 }
63
64 st_sensors_match_odr_error:
65 return ret;
66 }
67
st_sensors_set_odr(struct iio_dev * indio_dev,unsigned int odr)68 int st_sensors_set_odr(struct iio_dev *indio_dev, unsigned int odr)
69 {
70 int err;
71 struct st_sensor_odr_avl odr_out = {0, 0};
72 struct st_sensor_data *sdata = iio_priv(indio_dev);
73
74 err = st_sensors_match_odr(sdata->sensor_settings, odr, &odr_out);
75 if (err < 0)
76 goto st_sensors_match_odr_error;
77
78 if ((sdata->sensor_settings->odr.addr ==
79 sdata->sensor_settings->pw.addr) &&
80 (sdata->sensor_settings->odr.mask ==
81 sdata->sensor_settings->pw.mask)) {
82 if (sdata->enabled == true) {
83 err = st_sensors_write_data_with_mask(indio_dev,
84 sdata->sensor_settings->odr.addr,
85 sdata->sensor_settings->odr.mask,
86 odr_out.value);
87 } else {
88 err = 0;
89 }
90 } else {
91 err = st_sensors_write_data_with_mask(indio_dev,
92 sdata->sensor_settings->odr.addr,
93 sdata->sensor_settings->odr.mask,
94 odr_out.value);
95 }
96 if (err >= 0)
97 sdata->odr = odr_out.hz;
98
99 st_sensors_match_odr_error:
100 return err;
101 }
102 EXPORT_SYMBOL(st_sensors_set_odr);
103
st_sensors_match_fs(struct st_sensor_settings * sensor_settings,unsigned int fs,int * index_fs_avl)104 static int st_sensors_match_fs(struct st_sensor_settings *sensor_settings,
105 unsigned int fs, int *index_fs_avl)
106 {
107 int i, ret = -EINVAL;
108
109 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
110 if (sensor_settings->fs.fs_avl[i].num == 0)
111 goto st_sensors_match_odr_error;
112
113 if (sensor_settings->fs.fs_avl[i].num == fs) {
114 *index_fs_avl = i;
115 ret = 0;
116 break;
117 }
118 }
119
120 st_sensors_match_odr_error:
121 return ret;
122 }
123
st_sensors_set_fullscale(struct iio_dev * indio_dev,unsigned int fs)124 static int st_sensors_set_fullscale(struct iio_dev *indio_dev, unsigned int fs)
125 {
126 int err, i = 0;
127 struct st_sensor_data *sdata = iio_priv(indio_dev);
128
129 err = st_sensors_match_fs(sdata->sensor_settings, fs, &i);
130 if (err < 0)
131 goto st_accel_set_fullscale_error;
132
133 err = st_sensors_write_data_with_mask(indio_dev,
134 sdata->sensor_settings->fs.addr,
135 sdata->sensor_settings->fs.mask,
136 sdata->sensor_settings->fs.fs_avl[i].value);
137 if (err < 0)
138 goto st_accel_set_fullscale_error;
139
140 sdata->current_fullscale = (struct st_sensor_fullscale_avl *)
141 &sdata->sensor_settings->fs.fs_avl[i];
142 return err;
143
144 st_accel_set_fullscale_error:
145 dev_err(&indio_dev->dev, "failed to set new fullscale.\n");
146 return err;
147 }
148
st_sensors_set_enable(struct iio_dev * indio_dev,bool enable)149 int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable)
150 {
151 u8 tmp_value;
152 int err = -EINVAL;
153 bool found = false;
154 struct st_sensor_odr_avl odr_out = {0, 0};
155 struct st_sensor_data *sdata = iio_priv(indio_dev);
156
157 if (enable) {
158 tmp_value = sdata->sensor_settings->pw.value_on;
159 if ((sdata->sensor_settings->odr.addr ==
160 sdata->sensor_settings->pw.addr) &&
161 (sdata->sensor_settings->odr.mask ==
162 sdata->sensor_settings->pw.mask)) {
163 err = st_sensors_match_odr(sdata->sensor_settings,
164 sdata->odr, &odr_out);
165 if (err < 0)
166 goto set_enable_error;
167 tmp_value = odr_out.value;
168 found = true;
169 }
170 err = st_sensors_write_data_with_mask(indio_dev,
171 sdata->sensor_settings->pw.addr,
172 sdata->sensor_settings->pw.mask, tmp_value);
173 if (err < 0)
174 goto set_enable_error;
175
176 sdata->enabled = true;
177
178 if (found)
179 sdata->odr = odr_out.hz;
180 } else {
181 err = st_sensors_write_data_with_mask(indio_dev,
182 sdata->sensor_settings->pw.addr,
183 sdata->sensor_settings->pw.mask,
184 sdata->sensor_settings->pw.value_off);
185 if (err < 0)
186 goto set_enable_error;
187
188 sdata->enabled = false;
189 }
190
191 set_enable_error:
192 return err;
193 }
194 EXPORT_SYMBOL(st_sensors_set_enable);
195
st_sensors_set_axis_enable(struct iio_dev * indio_dev,u8 axis_enable)196 int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable)
197 {
198 struct st_sensor_data *sdata = iio_priv(indio_dev);
199
200 return st_sensors_write_data_with_mask(indio_dev,
201 sdata->sensor_settings->enable_axis.addr,
202 sdata->sensor_settings->enable_axis.mask,
203 axis_enable);
204 }
205 EXPORT_SYMBOL(st_sensors_set_axis_enable);
206
st_sensors_power_enable(struct iio_dev * indio_dev)207 void st_sensors_power_enable(struct iio_dev *indio_dev)
208 {
209 struct st_sensor_data *pdata = iio_priv(indio_dev);
210 int err;
211
212 /* Regulators not mandatory, but if requested we should enable them. */
213 pdata->vdd = devm_regulator_get_optional(indio_dev->dev.parent, "vdd");
214 if (!IS_ERR(pdata->vdd)) {
215 err = regulator_enable(pdata->vdd);
216 if (err != 0)
217 dev_warn(&indio_dev->dev,
218 "Failed to enable specified Vdd supply\n");
219 }
220
221 pdata->vdd_io = devm_regulator_get_optional(indio_dev->dev.parent, "vddio");
222 if (!IS_ERR(pdata->vdd_io)) {
223 err = regulator_enable(pdata->vdd_io);
224 if (err != 0)
225 dev_warn(&indio_dev->dev,
226 "Failed to enable specified Vdd_IO supply\n");
227 }
228 }
229 EXPORT_SYMBOL(st_sensors_power_enable);
230
st_sensors_power_disable(struct iio_dev * indio_dev)231 void st_sensors_power_disable(struct iio_dev *indio_dev)
232 {
233 struct st_sensor_data *pdata = iio_priv(indio_dev);
234
235 if (!IS_ERR(pdata->vdd))
236 regulator_disable(pdata->vdd);
237
238 if (!IS_ERR(pdata->vdd_io))
239 regulator_disable(pdata->vdd_io);
240 }
241 EXPORT_SYMBOL(st_sensors_power_disable);
242
st_sensors_set_drdy_int_pin(struct iio_dev * indio_dev,struct st_sensors_platform_data * pdata)243 static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev,
244 struct st_sensors_platform_data *pdata)
245 {
246 struct st_sensor_data *sdata = iio_priv(indio_dev);
247
248 switch (pdata->drdy_int_pin) {
249 case 1:
250 if (sdata->sensor_settings->drdy_irq.mask_int1 == 0) {
251 dev_err(&indio_dev->dev,
252 "DRDY on INT1 not available.\n");
253 return -EINVAL;
254 }
255 sdata->drdy_int_pin = 1;
256 break;
257 case 2:
258 if (sdata->sensor_settings->drdy_irq.mask_int2 == 0) {
259 dev_err(&indio_dev->dev,
260 "DRDY on INT2 not available.\n");
261 return -EINVAL;
262 }
263 sdata->drdy_int_pin = 2;
264 break;
265 default:
266 dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n");
267 return -EINVAL;
268 }
269
270 return 0;
271 }
272
273 #ifdef CONFIG_OF
st_sensors_of_probe(struct device * dev,struct st_sensors_platform_data * defdata)274 static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev,
275 struct st_sensors_platform_data *defdata)
276 {
277 struct st_sensors_platform_data *pdata;
278 struct device_node *np = dev->of_node;
279 u32 val;
280
281 if (!np)
282 return NULL;
283
284 pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
285 if (!of_property_read_u32(np, "st,drdy-int-pin", &val) && (val <= 2))
286 pdata->drdy_int_pin = (u8) val;
287 else
288 pdata->drdy_int_pin = defdata ? defdata->drdy_int_pin : 1;
289
290 return pdata;
291 }
292 #else
st_sensors_of_probe(struct device * dev,struct st_sensors_platform_data * defdata)293 static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev,
294 struct st_sensors_platform_data *defdata)
295 {
296 return NULL;
297 }
298 #endif
299
st_sensors_init_sensor(struct iio_dev * indio_dev,struct st_sensors_platform_data * pdata)300 int st_sensors_init_sensor(struct iio_dev *indio_dev,
301 struct st_sensors_platform_data *pdata)
302 {
303 struct st_sensor_data *sdata = iio_priv(indio_dev);
304 struct st_sensors_platform_data *of_pdata;
305 int err = 0;
306
307 /* If OF/DT pdata exists, it will take precedence of anything else */
308 of_pdata = st_sensors_of_probe(indio_dev->dev.parent, pdata);
309 if (of_pdata)
310 pdata = of_pdata;
311
312 if (pdata) {
313 err = st_sensors_set_drdy_int_pin(indio_dev, pdata);
314 if (err < 0)
315 return err;
316 }
317
318 err = st_sensors_set_enable(indio_dev, false);
319 if (err < 0)
320 return err;
321
322 if (sdata->current_fullscale) {
323 err = st_sensors_set_fullscale(indio_dev,
324 sdata->current_fullscale->num);
325 if (err < 0)
326 return err;
327 } else
328 dev_info(&indio_dev->dev, "Full-scale not possible\n");
329
330 err = st_sensors_set_odr(indio_dev, sdata->odr);
331 if (err < 0)
332 return err;
333
334 /* set BDU */
335 err = st_sensors_write_data_with_mask(indio_dev,
336 sdata->sensor_settings->bdu.addr,
337 sdata->sensor_settings->bdu.mask, true);
338 if (err < 0)
339 return err;
340
341 err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);
342
343 return err;
344 }
345 EXPORT_SYMBOL(st_sensors_init_sensor);
346
st_sensors_set_dataready_irq(struct iio_dev * indio_dev,bool enable)347 int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
348 {
349 int err;
350 u8 drdy_mask;
351 struct st_sensor_data *sdata = iio_priv(indio_dev);
352
353 if (!sdata->sensor_settings->drdy_irq.addr)
354 return 0;
355
356 /* Enable/Disable the interrupt generator 1. */
357 if (sdata->sensor_settings->drdy_irq.ig1.en_addr > 0) {
358 err = st_sensors_write_data_with_mask(indio_dev,
359 sdata->sensor_settings->drdy_irq.ig1.en_addr,
360 sdata->sensor_settings->drdy_irq.ig1.en_mask,
361 (int)enable);
362 if (err < 0)
363 goto st_accel_set_dataready_irq_error;
364 }
365
366 if (sdata->drdy_int_pin == 1)
367 drdy_mask = sdata->sensor_settings->drdy_irq.mask_int1;
368 else
369 drdy_mask = sdata->sensor_settings->drdy_irq.mask_int2;
370
371 /* Enable/Disable the interrupt generator for data ready. */
372 err = st_sensors_write_data_with_mask(indio_dev,
373 sdata->sensor_settings->drdy_irq.addr,
374 drdy_mask, (int)enable);
375
376 st_accel_set_dataready_irq_error:
377 return err;
378 }
379 EXPORT_SYMBOL(st_sensors_set_dataready_irq);
380
st_sensors_set_fullscale_by_gain(struct iio_dev * indio_dev,int scale)381 int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale)
382 {
383 int err = -EINVAL, i;
384 struct st_sensor_data *sdata = iio_priv(indio_dev);
385
386 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
387 if ((sdata->sensor_settings->fs.fs_avl[i].gain == scale) &&
388 (sdata->sensor_settings->fs.fs_avl[i].gain != 0)) {
389 err = 0;
390 break;
391 }
392 }
393 if (err < 0)
394 goto st_sensors_match_scale_error;
395
396 err = st_sensors_set_fullscale(indio_dev,
397 sdata->sensor_settings->fs.fs_avl[i].num);
398
399 st_sensors_match_scale_error:
400 return err;
401 }
402 EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain);
403
st_sensors_read_axis_data(struct iio_dev * indio_dev,struct iio_chan_spec const * ch,int * data)404 static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
405 struct iio_chan_spec const *ch, int *data)
406 {
407 int err;
408 u8 *outdata;
409 struct st_sensor_data *sdata = iio_priv(indio_dev);
410 unsigned int byte_for_channel = ch->scan_type.storagebits >> 3;
411
412 outdata = kmalloc(byte_for_channel, GFP_KERNEL);
413 if (!outdata)
414 return -ENOMEM;
415
416 err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
417 ch->address, byte_for_channel,
418 outdata, sdata->multiread_bit);
419 if (err < 0)
420 goto st_sensors_free_memory;
421
422 if (byte_for_channel == 2)
423 *data = (s16)get_unaligned_le16(outdata);
424 else if (byte_for_channel == 3)
425 *data = (s32)st_sensors_get_unaligned_le24(outdata);
426
427 st_sensors_free_memory:
428 kfree(outdata);
429
430 return err;
431 }
432
st_sensors_read_info_raw(struct iio_dev * indio_dev,struct iio_chan_spec const * ch,int * val)433 int st_sensors_read_info_raw(struct iio_dev *indio_dev,
434 struct iio_chan_spec const *ch, int *val)
435 {
436 int err;
437 struct st_sensor_data *sdata = iio_priv(indio_dev);
438
439 mutex_lock(&indio_dev->mlock);
440 if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
441 err = -EBUSY;
442 goto out;
443 } else {
444 err = st_sensors_set_enable(indio_dev, true);
445 if (err < 0)
446 goto out;
447
448 msleep((sdata->sensor_settings->bootime * 1000) / sdata->odr);
449 err = st_sensors_read_axis_data(indio_dev, ch, val);
450 if (err < 0)
451 goto out;
452
453 *val = *val >> ch->scan_type.shift;
454
455 err = st_sensors_set_enable(indio_dev, false);
456 }
457 out:
458 mutex_unlock(&indio_dev->mlock);
459
460 return err;
461 }
462 EXPORT_SYMBOL(st_sensors_read_info_raw);
463
st_sensors_check_device_support(struct iio_dev * indio_dev,int num_sensors_list,const struct st_sensor_settings * sensor_settings)464 int st_sensors_check_device_support(struct iio_dev *indio_dev,
465 int num_sensors_list,
466 const struct st_sensor_settings *sensor_settings)
467 {
468 u8 wai;
469 int i, n, err;
470 struct st_sensor_data *sdata = iio_priv(indio_dev);
471
472 err = sdata->tf->read_byte(&sdata->tb, sdata->dev,
473 ST_SENSORS_DEFAULT_WAI_ADDRESS, &wai);
474 if (err < 0) {
475 dev_err(&indio_dev->dev, "failed to read Who-Am-I register.\n");
476 goto read_wai_error;
477 }
478
479 for (i = 0; i < num_sensors_list; i++) {
480 if (sensor_settings[i].wai == wai)
481 break;
482 }
483 if (i == num_sensors_list)
484 goto device_not_supported;
485
486 for (n = 0; n < ARRAY_SIZE(sensor_settings[i].sensors_supported); n++) {
487 if (strcmp(indio_dev->name,
488 &sensor_settings[i].sensors_supported[n][0]) == 0)
489 break;
490 }
491 if (n == ARRAY_SIZE(sensor_settings[i].sensors_supported)) {
492 dev_err(&indio_dev->dev, "device name and WhoAmI mismatch.\n");
493 goto sensor_name_mismatch;
494 }
495
496 sdata->sensor_settings =
497 (struct st_sensor_settings *)&sensor_settings[i];
498
499 return i;
500
501 device_not_supported:
502 dev_err(&indio_dev->dev, "device not supported: WhoAmI (0x%x).\n", wai);
503 sensor_name_mismatch:
504 err = -ENODEV;
505 read_wai_error:
506 return err;
507 }
508 EXPORT_SYMBOL(st_sensors_check_device_support);
509
st_sensors_sysfs_sampling_frequency_avail(struct device * dev,struct device_attribute * attr,char * buf)510 ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev,
511 struct device_attribute *attr, char *buf)
512 {
513 int i, len = 0;
514 struct iio_dev *indio_dev = dev_get_drvdata(dev);
515 struct st_sensor_data *sdata = iio_priv(indio_dev);
516
517 mutex_lock(&indio_dev->mlock);
518 for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
519 if (sdata->sensor_settings->odr.odr_avl[i].hz == 0)
520 break;
521
522 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
523 sdata->sensor_settings->odr.odr_avl[i].hz);
524 }
525 mutex_unlock(&indio_dev->mlock);
526 buf[len - 1] = '\n';
527
528 return len;
529 }
530 EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail);
531
st_sensors_sysfs_scale_avail(struct device * dev,struct device_attribute * attr,char * buf)532 ssize_t st_sensors_sysfs_scale_avail(struct device *dev,
533 struct device_attribute *attr, char *buf)
534 {
535 int i, len = 0;
536 struct iio_dev *indio_dev = dev_get_drvdata(dev);
537 struct st_sensor_data *sdata = iio_priv(indio_dev);
538
539 mutex_lock(&indio_dev->mlock);
540 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
541 if (sdata->sensor_settings->fs.fs_avl[i].num == 0)
542 break;
543
544 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
545 sdata->sensor_settings->fs.fs_avl[i].gain);
546 }
547 mutex_unlock(&indio_dev->mlock);
548 buf[len - 1] = '\n';
549
550 return len;
551 }
552 EXPORT_SYMBOL(st_sensors_sysfs_scale_avail);
553
554 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
555 MODULE_DESCRIPTION("STMicroelectronics ST-sensors core");
556 MODULE_LICENSE("GPL v2");
557