1
2
3
4
5
6
7
8
9
10#include <linux/kernel.h>
11#include <linux/module.h>
12#include <linux/slab.h>
13#include <linux/delay.h>
14#include <linux/iio/iio.h>
15#include <linux/mutex.h>
16#include <linux/property.h>
17#include <linux/regulator/consumer.h>
18#include <linux/regmap.h>
19#include <asm/unaligned.h>
20#include <linux/iio/common/st_sensors.h>
21
22#include "st_sensors_core.h"
23
24int st_sensors_write_data_with_mask(struct iio_dev *indio_dev,
25 u8 reg_addr, u8 mask, u8 data)
26{
27 struct st_sensor_data *sdata = iio_priv(indio_dev);
28
29 return regmap_update_bits(sdata->regmap,
30 reg_addr, mask, data << __ffs(mask));
31}
32
33int st_sensors_debugfs_reg_access(struct iio_dev *indio_dev,
34 unsigned reg, unsigned writeval,
35 unsigned *readval)
36{
37 struct st_sensor_data *sdata = iio_priv(indio_dev);
38 int err;
39
40 if (!readval)
41 return regmap_write(sdata->regmap, reg, writeval);
42
43 err = regmap_read(sdata->regmap, reg, readval);
44 if (err < 0)
45 return err;
46
47 return 0;
48}
49EXPORT_SYMBOL(st_sensors_debugfs_reg_access);
50
51static int st_sensors_match_odr(struct st_sensor_settings *sensor_settings,
52 unsigned int odr, struct st_sensor_odr_avl *odr_out)
53{
54 int i, ret = -EINVAL;
55
56 for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
57 if (sensor_settings->odr.odr_avl[i].hz == 0)
58 goto st_sensors_match_odr_error;
59
60 if (sensor_settings->odr.odr_avl[i].hz == odr) {
61 odr_out->hz = sensor_settings->odr.odr_avl[i].hz;
62 odr_out->value = sensor_settings->odr.odr_avl[i].value;
63 ret = 0;
64 break;
65 }
66 }
67
68st_sensors_match_odr_error:
69 return ret;
70}
71
72int st_sensors_set_odr(struct iio_dev *indio_dev, unsigned int odr)
73{
74 int err;
75 struct st_sensor_odr_avl odr_out = {0, 0};
76 struct st_sensor_data *sdata = iio_priv(indio_dev);
77
78 if (!sdata->sensor_settings->odr.mask)
79 return 0;
80
81 err = st_sensors_match_odr(sdata->sensor_settings, odr, &odr_out);
82 if (err < 0)
83 goto st_sensors_match_odr_error;
84
85 if ((sdata->sensor_settings->odr.addr ==
86 sdata->sensor_settings->pw.addr) &&
87 (sdata->sensor_settings->odr.mask ==
88 sdata->sensor_settings->pw.mask)) {
89 if (sdata->enabled == true) {
90 err = st_sensors_write_data_with_mask(indio_dev,
91 sdata->sensor_settings->odr.addr,
92 sdata->sensor_settings->odr.mask,
93 odr_out.value);
94 } else {
95 err = 0;
96 }
97 } else {
98 err = st_sensors_write_data_with_mask(indio_dev,
99 sdata->sensor_settings->odr.addr,
100 sdata->sensor_settings->odr.mask,
101 odr_out.value);
102 }
103 if (err >= 0)
104 sdata->odr = odr_out.hz;
105
106st_sensors_match_odr_error:
107 return err;
108}
109EXPORT_SYMBOL(st_sensors_set_odr);
110
111static int st_sensors_match_fs(struct st_sensor_settings *sensor_settings,
112 unsigned int fs, int *index_fs_avl)
113{
114 int i, ret = -EINVAL;
115
116 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
117 if (sensor_settings->fs.fs_avl[i].num == 0)
118 return ret;
119
120 if (sensor_settings->fs.fs_avl[i].num == fs) {
121 *index_fs_avl = i;
122 ret = 0;
123 break;
124 }
125 }
126
127 return ret;
128}
129
130static int st_sensors_set_fullscale(struct iio_dev *indio_dev, unsigned int fs)
131{
132 int err, i = 0;
133 struct st_sensor_data *sdata = iio_priv(indio_dev);
134
135 if (sdata->sensor_settings->fs.addr == 0)
136 return 0;
137
138 err = st_sensors_match_fs(sdata->sensor_settings, fs, &i);
139 if (err < 0)
140 goto st_accel_set_fullscale_error;
141
142 err = st_sensors_write_data_with_mask(indio_dev,
143 sdata->sensor_settings->fs.addr,
144 sdata->sensor_settings->fs.mask,
145 sdata->sensor_settings->fs.fs_avl[i].value);
146 if (err < 0)
147 goto st_accel_set_fullscale_error;
148
149 sdata->current_fullscale = &sdata->sensor_settings->fs.fs_avl[i];
150 return err;
151
152st_accel_set_fullscale_error:
153 dev_err(&indio_dev->dev, "failed to set new fullscale.\n");
154 return err;
155}
156
157int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable)
158{
159 u8 tmp_value;
160 int err = -EINVAL;
161 bool found = false;
162 struct st_sensor_odr_avl odr_out = {0, 0};
163 struct st_sensor_data *sdata = iio_priv(indio_dev);
164
165 if (enable) {
166 tmp_value = sdata->sensor_settings->pw.value_on;
167 if ((sdata->sensor_settings->odr.addr ==
168 sdata->sensor_settings->pw.addr) &&
169 (sdata->sensor_settings->odr.mask ==
170 sdata->sensor_settings->pw.mask)) {
171 err = st_sensors_match_odr(sdata->sensor_settings,
172 sdata->odr, &odr_out);
173 if (err < 0)
174 goto set_enable_error;
175 tmp_value = odr_out.value;
176 found = true;
177 }
178 err = st_sensors_write_data_with_mask(indio_dev,
179 sdata->sensor_settings->pw.addr,
180 sdata->sensor_settings->pw.mask, tmp_value);
181 if (err < 0)
182 goto set_enable_error;
183
184 sdata->enabled = true;
185
186 if (found)
187 sdata->odr = odr_out.hz;
188 } else {
189 err = st_sensors_write_data_with_mask(indio_dev,
190 sdata->sensor_settings->pw.addr,
191 sdata->sensor_settings->pw.mask,
192 sdata->sensor_settings->pw.value_off);
193 if (err < 0)
194 goto set_enable_error;
195
196 sdata->enabled = false;
197 }
198
199set_enable_error:
200 return err;
201}
202EXPORT_SYMBOL(st_sensors_set_enable);
203
204int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable)
205{
206 struct st_sensor_data *sdata = iio_priv(indio_dev);
207 int err = 0;
208
209 if (sdata->sensor_settings->enable_axis.addr)
210 err = st_sensors_write_data_with_mask(indio_dev,
211 sdata->sensor_settings->enable_axis.addr,
212 sdata->sensor_settings->enable_axis.mask,
213 axis_enable);
214 return err;
215}
216EXPORT_SYMBOL(st_sensors_set_axis_enable);
217
218int st_sensors_power_enable(struct iio_dev *indio_dev)
219{
220 struct st_sensor_data *pdata = iio_priv(indio_dev);
221 int err;
222
223
224 pdata->vdd = devm_regulator_get(indio_dev->dev.parent, "vdd");
225 if (IS_ERR(pdata->vdd)) {
226 dev_err(&indio_dev->dev, "unable to get Vdd supply\n");
227 return PTR_ERR(pdata->vdd);
228 }
229 err = regulator_enable(pdata->vdd);
230 if (err != 0) {
231 dev_warn(&indio_dev->dev,
232 "Failed to enable specified Vdd supply\n");
233 return err;
234 }
235
236 pdata->vdd_io = devm_regulator_get(indio_dev->dev.parent, "vddio");
237 if (IS_ERR(pdata->vdd_io)) {
238 dev_err(&indio_dev->dev, "unable to get Vdd_IO supply\n");
239 err = PTR_ERR(pdata->vdd_io);
240 goto st_sensors_disable_vdd;
241 }
242 err = regulator_enable(pdata->vdd_io);
243 if (err != 0) {
244 dev_warn(&indio_dev->dev,
245 "Failed to enable specified Vdd_IO supply\n");
246 goto st_sensors_disable_vdd;
247 }
248
249 return 0;
250
251st_sensors_disable_vdd:
252 regulator_disable(pdata->vdd);
253 return err;
254}
255EXPORT_SYMBOL(st_sensors_power_enable);
256
257void st_sensors_power_disable(struct iio_dev *indio_dev)
258{
259 struct st_sensor_data *pdata = iio_priv(indio_dev);
260
261 regulator_disable(pdata->vdd);
262 regulator_disable(pdata->vdd_io);
263}
264EXPORT_SYMBOL(st_sensors_power_disable);
265
266static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev,
267 struct st_sensors_platform_data *pdata)
268{
269 struct st_sensor_data *sdata = iio_priv(indio_dev);
270
271
272 if (!sdata->sensor_settings->drdy_irq.int1.addr &&
273 !sdata->sensor_settings->drdy_irq.int2.addr) {
274 if (pdata->drdy_int_pin)
275 dev_info(&indio_dev->dev,
276 "DRDY on pin INT%d specified, but sensor does not support interrupts\n",
277 pdata->drdy_int_pin);
278 return 0;
279 }
280
281 switch (pdata->drdy_int_pin) {
282 case 1:
283 if (!sdata->sensor_settings->drdy_irq.int1.mask) {
284 dev_err(&indio_dev->dev,
285 "DRDY on INT1 not available.\n");
286 return -EINVAL;
287 }
288 sdata->drdy_int_pin = 1;
289 break;
290 case 2:
291 if (!sdata->sensor_settings->drdy_irq.int2.mask) {
292 dev_err(&indio_dev->dev,
293 "DRDY on INT2 not available.\n");
294 return -EINVAL;
295 }
296 sdata->drdy_int_pin = 2;
297 break;
298 default:
299 dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n");
300 return -EINVAL;
301 }
302
303 if (pdata->open_drain) {
304 if (!sdata->sensor_settings->drdy_irq.int1.addr_od &&
305 !sdata->sensor_settings->drdy_irq.int2.addr_od)
306 dev_err(&indio_dev->dev,
307 "open drain requested but unsupported.\n");
308 else
309 sdata->int_pin_open_drain = true;
310 }
311
312 return 0;
313}
314
315static struct st_sensors_platform_data *st_sensors_dev_probe(struct device *dev,
316 struct st_sensors_platform_data *defdata)
317{
318 struct st_sensors_platform_data *pdata;
319 u32 val;
320
321 if (!dev_fwnode(dev))
322 return NULL;
323
324 pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
325 if (!pdata)
326 return ERR_PTR(-ENOMEM);
327 if (!device_property_read_u32(dev, "st,drdy-int-pin", &val) && (val <= 2))
328 pdata->drdy_int_pin = (u8) val;
329 else
330 pdata->drdy_int_pin = defdata ? defdata->drdy_int_pin : 0;
331
332 pdata->open_drain = device_property_read_bool(dev, "drive-open-drain");
333
334 return pdata;
335}
336
337
338
339
340
341
342
343
344
345
346
347
348void st_sensors_dev_name_probe(struct device *dev, char *name, int len)
349{
350 const void *match;
351
352 match = device_get_match_data(dev);
353 if (!match)
354 return;
355
356
357 strlcpy(name, match, len);
358}
359EXPORT_SYMBOL(st_sensors_dev_name_probe);
360
361int st_sensors_init_sensor(struct iio_dev *indio_dev,
362 struct st_sensors_platform_data *pdata)
363{
364 struct st_sensor_data *sdata = iio_priv(indio_dev);
365 struct st_sensors_platform_data *of_pdata;
366 int err = 0;
367
368
369 of_pdata = st_sensors_dev_probe(indio_dev->dev.parent, pdata);
370 if (IS_ERR(of_pdata))
371 return PTR_ERR(of_pdata);
372 if (of_pdata)
373 pdata = of_pdata;
374
375 if (pdata) {
376 err = st_sensors_set_drdy_int_pin(indio_dev, pdata);
377 if (err < 0)
378 return err;
379 }
380
381 err = st_sensors_set_enable(indio_dev, false);
382 if (err < 0)
383 return err;
384
385
386 err = st_sensors_set_dataready_irq(indio_dev, false);
387 if (err < 0)
388 return err;
389
390 if (sdata->current_fullscale) {
391 err = st_sensors_set_fullscale(indio_dev,
392 sdata->current_fullscale->num);
393 if (err < 0)
394 return err;
395 } else
396 dev_info(&indio_dev->dev, "Full-scale not possible\n");
397
398 err = st_sensors_set_odr(indio_dev, sdata->odr);
399 if (err < 0)
400 return err;
401
402
403 if (sdata->sensor_settings->bdu.addr) {
404 err = st_sensors_write_data_with_mask(indio_dev,
405 sdata->sensor_settings->bdu.addr,
406 sdata->sensor_settings->bdu.mask, true);
407 if (err < 0)
408 return err;
409 }
410
411
412 if (sdata->sensor_settings->das.addr) {
413 err = st_sensors_write_data_with_mask(indio_dev,
414 sdata->sensor_settings->das.addr,
415 sdata->sensor_settings->das.mask, 1);
416 if (err < 0)
417 return err;
418 }
419
420 if (sdata->int_pin_open_drain) {
421 u8 addr, mask;
422
423 if (sdata->drdy_int_pin == 1) {
424 addr = sdata->sensor_settings->drdy_irq.int1.addr_od;
425 mask = sdata->sensor_settings->drdy_irq.int1.mask_od;
426 } else {
427 addr = sdata->sensor_settings->drdy_irq.int2.addr_od;
428 mask = sdata->sensor_settings->drdy_irq.int2.mask_od;
429 }
430
431 dev_info(&indio_dev->dev,
432 "set interrupt line to open drain mode on pin %d\n",
433 sdata->drdy_int_pin);
434 err = st_sensors_write_data_with_mask(indio_dev, addr,
435 mask, 1);
436 if (err < 0)
437 return err;
438 }
439
440 err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);
441
442 return err;
443}
444EXPORT_SYMBOL(st_sensors_init_sensor);
445
446int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
447{
448 int err;
449 u8 drdy_addr, drdy_mask;
450 struct st_sensor_data *sdata = iio_priv(indio_dev);
451
452 if (!sdata->sensor_settings->drdy_irq.int1.addr &&
453 !sdata->sensor_settings->drdy_irq.int2.addr) {
454
455
456
457
458
459
460 if (sdata->sensor_settings->drdy_irq.stat_drdy.addr)
461 sdata->hw_irq_trigger = enable;
462 return 0;
463 }
464
465
466 if (sdata->sensor_settings->drdy_irq.ig1.en_addr > 0) {
467 err = st_sensors_write_data_with_mask(indio_dev,
468 sdata->sensor_settings->drdy_irq.ig1.en_addr,
469 sdata->sensor_settings->drdy_irq.ig1.en_mask,
470 (int)enable);
471 if (err < 0)
472 goto st_accel_set_dataready_irq_error;
473 }
474
475 if (sdata->drdy_int_pin == 1) {
476 drdy_addr = sdata->sensor_settings->drdy_irq.int1.addr;
477 drdy_mask = sdata->sensor_settings->drdy_irq.int1.mask;
478 } else {
479 drdy_addr = sdata->sensor_settings->drdy_irq.int2.addr;
480 drdy_mask = sdata->sensor_settings->drdy_irq.int2.mask;
481 }
482
483
484 sdata->hw_irq_trigger = enable;
485
486
487 err = st_sensors_write_data_with_mask(indio_dev, drdy_addr,
488 drdy_mask, (int)enable);
489
490st_accel_set_dataready_irq_error:
491 return err;
492}
493EXPORT_SYMBOL(st_sensors_set_dataready_irq);
494
495int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale)
496{
497 int err = -EINVAL, i;
498 struct st_sensor_data *sdata = iio_priv(indio_dev);
499
500 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
501 if ((sdata->sensor_settings->fs.fs_avl[i].gain == scale) &&
502 (sdata->sensor_settings->fs.fs_avl[i].gain != 0)) {
503 err = 0;
504 break;
505 }
506 }
507 if (err < 0)
508 goto st_sensors_match_scale_error;
509
510 err = st_sensors_set_fullscale(indio_dev,
511 sdata->sensor_settings->fs.fs_avl[i].num);
512
513st_sensors_match_scale_error:
514 return err;
515}
516EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain);
517
518static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
519 struct iio_chan_spec const *ch, int *data)
520{
521 int err;
522 u8 *outdata;
523 struct st_sensor_data *sdata = iio_priv(indio_dev);
524 unsigned int byte_for_channel;
525
526 byte_for_channel = DIV_ROUND_UP(ch->scan_type.realbits +
527 ch->scan_type.shift, 8);
528 outdata = kmalloc(byte_for_channel, GFP_DMA | GFP_KERNEL);
529 if (!outdata)
530 return -ENOMEM;
531
532 err = regmap_bulk_read(sdata->regmap, ch->address,
533 outdata, byte_for_channel);
534 if (err < 0)
535 goto st_sensors_free_memory;
536
537 if (byte_for_channel == 1)
538 *data = (s8)*outdata;
539 else if (byte_for_channel == 2)
540 *data = (s16)get_unaligned_le16(outdata);
541 else if (byte_for_channel == 3)
542 *data = (s32)sign_extend32(get_unaligned_le24(outdata), 23);
543
544st_sensors_free_memory:
545 kfree(outdata);
546
547 return err;
548}
549
550int st_sensors_read_info_raw(struct iio_dev *indio_dev,
551 struct iio_chan_spec const *ch, int *val)
552{
553 int err;
554 struct st_sensor_data *sdata = iio_priv(indio_dev);
555
556 mutex_lock(&indio_dev->mlock);
557 if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
558 err = -EBUSY;
559 goto out;
560 } else {
561 err = st_sensors_set_enable(indio_dev, true);
562 if (err < 0)
563 goto out;
564
565 msleep((sdata->sensor_settings->bootime * 1000) / sdata->odr);
566 err = st_sensors_read_axis_data(indio_dev, ch, val);
567 if (err < 0)
568 goto out;
569
570 *val = *val >> ch->scan_type.shift;
571
572 err = st_sensors_set_enable(indio_dev, false);
573 }
574out:
575 mutex_unlock(&indio_dev->mlock);
576
577 return err;
578}
579EXPORT_SYMBOL(st_sensors_read_info_raw);
580
581
582
583
584
585
586
587
588
589
590
591int st_sensors_get_settings_index(const char *name,
592 const struct st_sensor_settings *list,
593 const int list_length)
594{
595 int i, n;
596
597 for (i = 0; i < list_length; i++) {
598 for (n = 0; n < ST_SENSORS_MAX_4WAI; n++) {
599 if (strcmp(name, list[i].sensors_supported[n]) == 0)
600 return i;
601 }
602 }
603
604 return -ENODEV;
605}
606EXPORT_SYMBOL(st_sensors_get_settings_index);
607
608
609
610
611
612
613
614
615int st_sensors_verify_id(struct iio_dev *indio_dev)
616{
617 struct st_sensor_data *sdata = iio_priv(indio_dev);
618 int wai, err;
619
620 if (sdata->sensor_settings->wai_addr) {
621 err = regmap_read(sdata->regmap,
622 sdata->sensor_settings->wai_addr, &wai);
623 if (err < 0) {
624 dev_err(&indio_dev->dev,
625 "failed to read Who-Am-I register.\n");
626 return err;
627 }
628
629 if (sdata->sensor_settings->wai != wai) {
630 dev_err(&indio_dev->dev,
631 "%s: WhoAmI mismatch (0x%x).\n",
632 indio_dev->name, wai);
633 return -EINVAL;
634 }
635 }
636
637 return 0;
638}
639EXPORT_SYMBOL(st_sensors_verify_id);
640
641ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev,
642 struct device_attribute *attr, char *buf)
643{
644 int i, len = 0;
645 struct iio_dev *indio_dev = dev_get_drvdata(dev);
646 struct st_sensor_data *sdata = iio_priv(indio_dev);
647
648 mutex_lock(&indio_dev->mlock);
649 for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
650 if (sdata->sensor_settings->odr.odr_avl[i].hz == 0)
651 break;
652
653 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
654 sdata->sensor_settings->odr.odr_avl[i].hz);
655 }
656 mutex_unlock(&indio_dev->mlock);
657 buf[len - 1] = '\n';
658
659 return len;
660}
661EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail);
662
663ssize_t st_sensors_sysfs_scale_avail(struct device *dev,
664 struct device_attribute *attr, char *buf)
665{
666 int i, len = 0, q, r;
667 struct iio_dev *indio_dev = dev_get_drvdata(dev);
668 struct st_sensor_data *sdata = iio_priv(indio_dev);
669
670 mutex_lock(&indio_dev->mlock);
671 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
672 if (sdata->sensor_settings->fs.fs_avl[i].num == 0)
673 break;
674
675 q = sdata->sensor_settings->fs.fs_avl[i].gain / 1000000;
676 r = sdata->sensor_settings->fs.fs_avl[i].gain % 1000000;
677
678 len += scnprintf(buf + len, PAGE_SIZE - len, "%u.%06u ", q, r);
679 }
680 mutex_unlock(&indio_dev->mlock);
681 buf[len - 1] = '\n';
682
683 return len;
684}
685EXPORT_SYMBOL(st_sensors_sysfs_scale_avail);
686
687MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
688MODULE_DESCRIPTION("STMicroelectronics ST-sensors core");
689MODULE_LICENSE("GPL v2");
690
