Merge tag 'gcc-plugins-v4.9-rc4' of git://git.kernel.org/pub/scm/linux/kernel/git...
[cascardo/linux.git] / drivers / iio / common / st_sensors / st_sensors_core.c
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 #include "st_sensors_core.h"
22
23 static inline u32 st_sensors_get_unaligned_le24(const u8 *p)
24 {
25         return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8;
26 }
27
28 int st_sensors_write_data_with_mask(struct iio_dev *indio_dev,
29                                     u8 reg_addr, u8 mask, u8 data)
30 {
31         int err;
32         u8 new_data;
33         struct st_sensor_data *sdata = iio_priv(indio_dev);
34
35         err = sdata->tf->read_byte(&sdata->tb, sdata->dev, reg_addr, &new_data);
36         if (err < 0)
37                 goto st_sensors_write_data_with_mask_error;
38
39         new_data = ((new_data & (~mask)) | ((data << __ffs(mask)) & mask));
40         err = sdata->tf->write_byte(&sdata->tb, sdata->dev, reg_addr, new_data);
41
42 st_sensors_write_data_with_mask_error:
43         return err;
44 }
45
46 int st_sensors_debugfs_reg_access(struct iio_dev *indio_dev,
47                                   unsigned reg, unsigned writeval,
48                                   unsigned *readval)
49 {
50         struct st_sensor_data *sdata = iio_priv(indio_dev);
51         u8 readdata;
52         int err;
53
54         if (!readval)
55                 return sdata->tf->write_byte(&sdata->tb, sdata->dev,
56                                              (u8)reg, (u8)writeval);
57
58         err = sdata->tf->read_byte(&sdata->tb, sdata->dev, (u8)reg, &readdata);
59         if (err < 0)
60                 return err;
61
62         *readval = (unsigned)readdata;
63
64         return 0;
65 }
66 EXPORT_SYMBOL(st_sensors_debugfs_reg_access);
67
68 static int st_sensors_match_odr(struct st_sensor_settings *sensor_settings,
69                         unsigned int odr, struct st_sensor_odr_avl *odr_out)
70 {
71         int i, ret = -EINVAL;
72
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;
76
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;
80                         ret = 0;
81                         break;
82                 }
83         }
84
85 st_sensors_match_odr_error:
86         return ret;
87 }
88
89 int st_sensors_set_odr(struct iio_dev *indio_dev, unsigned int odr)
90 {
91         int err;
92         struct st_sensor_odr_avl odr_out = {0, 0};
93         struct st_sensor_data *sdata = iio_priv(indio_dev);
94
95         err = st_sensors_match_odr(sdata->sensor_settings, odr, &odr_out);
96         if (err < 0)
97                 goto st_sensors_match_odr_error;
98
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,
107                                 odr_out.value);
108                 } else {
109                         err = 0;
110                 }
111         } else {
112                 err = st_sensors_write_data_with_mask(indio_dev,
113                         sdata->sensor_settings->odr.addr,
114                         sdata->sensor_settings->odr.mask,
115                         odr_out.value);
116         }
117         if (err >= 0)
118                 sdata->odr = odr_out.hz;
119
120 st_sensors_match_odr_error:
121         return err;
122 }
123 EXPORT_SYMBOL(st_sensors_set_odr);
124
125 static int st_sensors_match_fs(struct st_sensor_settings *sensor_settings,
126                                         unsigned int fs, int *index_fs_avl)
127 {
128         int i, ret = -EINVAL;
129
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;
133
134                 if (sensor_settings->fs.fs_avl[i].num == fs) {
135                         *index_fs_avl = i;
136                         ret = 0;
137                         break;
138                 }
139         }
140
141 st_sensors_match_odr_error:
142         return ret;
143 }
144
145 static int st_sensors_set_fullscale(struct iio_dev *indio_dev, unsigned int fs)
146 {
147         int err, i = 0;
148         struct st_sensor_data *sdata = iio_priv(indio_dev);
149
150         if (sdata->sensor_settings->fs.addr == 0)
151                 return 0;
152
153         err = st_sensors_match_fs(sdata->sensor_settings, fs, &i);
154         if (err < 0)
155                 goto st_accel_set_fullscale_error;
156
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);
161         if (err < 0)
162                 goto st_accel_set_fullscale_error;
163
164         sdata->current_fullscale = (struct st_sensor_fullscale_avl *)
165                                         &sdata->sensor_settings->fs.fs_avl[i];
166         return err;
167
168 st_accel_set_fullscale_error:
169         dev_err(&indio_dev->dev, "failed to set new fullscale.\n");
170         return err;
171 }
172
173 int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable)
174 {
175         u8 tmp_value;
176         int err = -EINVAL;
177         bool found = false;
178         struct st_sensor_odr_avl odr_out = {0, 0};
179         struct st_sensor_data *sdata = iio_priv(indio_dev);
180
181         if (enable) {
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);
189                         if (err < 0)
190                                 goto set_enable_error;
191                         tmp_value = odr_out.value;
192                         found = true;
193                 }
194                 err = st_sensors_write_data_with_mask(indio_dev,
195                                 sdata->sensor_settings->pw.addr,
196                                 sdata->sensor_settings->pw.mask, tmp_value);
197                 if (err < 0)
198                         goto set_enable_error;
199
200                 sdata->enabled = true;
201
202                 if (found)
203                         sdata->odr = odr_out.hz;
204         } else {
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);
209                 if (err < 0)
210                         goto set_enable_error;
211
212                 sdata->enabled = false;
213         }
214
215 set_enable_error:
216         return err;
217 }
218 EXPORT_SYMBOL(st_sensors_set_enable);
219
220 int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable)
221 {
222         struct st_sensor_data *sdata = iio_priv(indio_dev);
223
224         return st_sensors_write_data_with_mask(indio_dev,
225                                 sdata->sensor_settings->enable_axis.addr,
226                                 sdata->sensor_settings->enable_axis.mask,
227                                 axis_enable);
228 }
229 EXPORT_SYMBOL(st_sensors_set_axis_enable);
230
231 int st_sensors_power_enable(struct iio_dev *indio_dev)
232 {
233         struct st_sensor_data *pdata = iio_priv(indio_dev);
234         int err;
235
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);
241         }
242         err = regulator_enable(pdata->vdd);
243         if (err != 0) {
244                 dev_warn(&indio_dev->dev,
245                          "Failed to enable specified Vdd supply\n");
246                 return err;
247         }
248
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;
254         }
255         err = regulator_enable(pdata->vdd_io);
256         if (err != 0) {
257                 dev_warn(&indio_dev->dev,
258                          "Failed to enable specified Vdd_IO supply\n");
259                 goto st_sensors_disable_vdd;
260         }
261
262         return 0;
263
264 st_sensors_disable_vdd:
265         regulator_disable(pdata->vdd);
266         return err;
267 }
268 EXPORT_SYMBOL(st_sensors_power_enable);
269
270 void st_sensors_power_disable(struct iio_dev *indio_dev)
271 {
272         struct st_sensor_data *pdata = iio_priv(indio_dev);
273
274         regulator_disable(pdata->vdd);
275         regulator_disable(pdata->vdd_io);
276 }
277 EXPORT_SYMBOL(st_sensors_power_disable);
278
279 static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev,
280                                         struct st_sensors_platform_data *pdata)
281 {
282         struct st_sensor_data *sdata = iio_priv(indio_dev);
283
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);
291                 return 0;
292         }
293
294         switch (pdata->drdy_int_pin) {
295         case 1:
296                 if (sdata->sensor_settings->drdy_irq.mask_int1 == 0) {
297                         dev_err(&indio_dev->dev,
298                                         "DRDY on INT1 not available.\n");
299                         return -EINVAL;
300                 }
301                 sdata->drdy_int_pin = 1;
302                 break;
303         case 2:
304                 if (sdata->sensor_settings->drdy_irq.mask_int2 == 0) {
305                         dev_err(&indio_dev->dev,
306                                         "DRDY on INT2 not available.\n");
307                         return -EINVAL;
308                 }
309                 sdata->drdy_int_pin = 2;
310                 break;
311         default:
312                 dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n");
313                 return -EINVAL;
314         }
315
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");
320                 else
321                         sdata->int_pin_open_drain = true;
322         }
323
324         return 0;
325 }
326
327 #ifdef CONFIG_OF
328 static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev,
329                 struct st_sensors_platform_data *defdata)
330 {
331         struct st_sensors_platform_data *pdata;
332         struct device_node *np = dev->of_node;
333         u32 val;
334
335         if (!np)
336                 return NULL;
337
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;
341         else
342                 pdata->drdy_int_pin = defdata ? defdata->drdy_int_pin : 0;
343
344         pdata->open_drain = of_property_read_bool(np, "drive-open-drain");
345
346         return pdata;
347 }
348 #else
349 static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev,
350                 struct st_sensors_platform_data *defdata)
351 {
352         return NULL;
353 }
354 #endif
355
356 int st_sensors_init_sensor(struct iio_dev *indio_dev,
357                                         struct st_sensors_platform_data *pdata)
358 {
359         struct st_sensor_data *sdata = iio_priv(indio_dev);
360         struct st_sensors_platform_data *of_pdata;
361         int err = 0;
362
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);
365         if (of_pdata)
366                 pdata = of_pdata;
367
368         if (pdata) {
369                 err = st_sensors_set_drdy_int_pin(indio_dev, pdata);
370                 if (err < 0)
371                         return err;
372         }
373
374         err = st_sensors_set_enable(indio_dev, false);
375         if (err < 0)
376                 return err;
377
378         /* Disable DRDY, this might be still be enabled after reboot. */
379         err = st_sensors_set_dataready_irq(indio_dev, false);
380         if (err < 0)
381                 return err;
382
383         if (sdata->current_fullscale) {
384                 err = st_sensors_set_fullscale(indio_dev,
385                                                 sdata->current_fullscale->num);
386                 if (err < 0)
387                         return err;
388         } else
389                 dev_info(&indio_dev->dev, "Full-scale not possible\n");
390
391         err = st_sensors_set_odr(indio_dev, sdata->odr);
392         if (err < 0)
393                 return err;
394
395         /* set BDU */
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);
400                 if (err < 0)
401                         return err;
402         }
403
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);
410                 if (err < 0)
411                         return err;
412         }
413
414         err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);
415
416         return err;
417 }
418 EXPORT_SYMBOL(st_sensors_init_sensor);
419
420 int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
421 {
422         int err;
423         u8 drdy_mask;
424         struct st_sensor_data *sdata = iio_priv(indio_dev);
425
426         if (!sdata->sensor_settings->drdy_irq.addr)
427                 return 0;
428
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,
434                                 (int)enable);
435                 if (err < 0)
436                         goto st_accel_set_dataready_irq_error;
437         }
438
439         if (sdata->drdy_int_pin == 1)
440                 drdy_mask = sdata->sensor_settings->drdy_irq.mask_int1;
441         else
442                 drdy_mask = sdata->sensor_settings->drdy_irq.mask_int2;
443
444         /* Flag to the poll function that the hardware trigger is in use */
445         sdata->hw_irq_trigger = enable;
446
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);
451
452 st_accel_set_dataready_irq_error:
453         return err;
454 }
455 EXPORT_SYMBOL(st_sensors_set_dataready_irq);
456
457 int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale)
458 {
459         int err = -EINVAL, i;
460         struct st_sensor_data *sdata = iio_priv(indio_dev);
461
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)) {
465                         err = 0;
466                         break;
467                 }
468         }
469         if (err < 0)
470                 goto st_sensors_match_scale_error;
471
472         err = st_sensors_set_fullscale(indio_dev,
473                                 sdata->sensor_settings->fs.fs_avl[i].num);
474
475 st_sensors_match_scale_error:
476         return err;
477 }
478 EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain);
479
480 static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
481                                 struct iio_chan_spec const *ch, int *data)
482 {
483         int err;
484         u8 *outdata;
485         struct st_sensor_data *sdata = iio_priv(indio_dev);
486         unsigned int byte_for_channel = ch->scan_type.realbits >> 3;
487
488         outdata = kmalloc(byte_for_channel, GFP_KERNEL);
489         if (!outdata)
490                 return -ENOMEM;
491
492         err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
493                                 ch->address, byte_for_channel,
494                                 outdata, sdata->multiread_bit);
495         if (err < 0)
496                 goto st_sensors_free_memory;
497
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);
504
505 st_sensors_free_memory:
506         kfree(outdata);
507
508         return err;
509 }
510
511 int st_sensors_read_info_raw(struct iio_dev *indio_dev,
512                                 struct iio_chan_spec const *ch, int *val)
513 {
514         int err;
515         struct st_sensor_data *sdata = iio_priv(indio_dev);
516
517         mutex_lock(&indio_dev->mlock);
518         if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
519                 err = -EBUSY;
520                 goto out;
521         } else {
522                 err = st_sensors_set_enable(indio_dev, true);
523                 if (err < 0)
524                         goto out;
525
526                 msleep((sdata->sensor_settings->bootime * 1000) / sdata->odr);
527                 err = st_sensors_read_axis_data(indio_dev, ch, val);
528                 if (err < 0)
529                         goto out;
530
531                 *val = *val >> ch->scan_type.shift;
532
533                 err = st_sensors_set_enable(indio_dev, false);
534         }
535 out:
536         mutex_unlock(&indio_dev->mlock);
537
538         return err;
539 }
540 EXPORT_SYMBOL(st_sensors_read_info_raw);
541
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)
545 {
546         int i, n, err = 0;
547         u8 wai;
548         struct st_sensor_data *sdata = iio_priv(indio_dev);
549
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) {
554                                 break;
555                         }
556                 }
557                 if (n < ST_SENSORS_MAX_4WAI)
558                         break;
559         }
560         if (i == num_sensors_list) {
561                 dev_err(&indio_dev->dev, "device name %s not recognized.\n",
562                                                         indio_dev->name);
563                 return -ENODEV;
564         }
565
566         if (sensor_settings[i].wai_addr) {
567                 err = sdata->tf->read_byte(&sdata->tb, sdata->dev,
568                                            sensor_settings[i].wai_addr, &wai);
569                 if (err < 0) {
570                         dev_err(&indio_dev->dev,
571                                 "failed to read Who-Am-I register.\n");
572                         return err;
573                 }
574
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);
579                         return -EINVAL;
580                 }
581         }
582
583         sdata->sensor_settings =
584                         (struct st_sensor_settings *)&sensor_settings[i];
585
586         return i;
587 }
588 EXPORT_SYMBOL(st_sensors_check_device_support);
589
590 ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev,
591                                 struct device_attribute *attr, char *buf)
592 {
593         int i, len = 0;
594         struct iio_dev *indio_dev = dev_get_drvdata(dev);
595         struct st_sensor_data *sdata = iio_priv(indio_dev);
596
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)
600                         break;
601
602                 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
603                                 sdata->sensor_settings->odr.odr_avl[i].hz);
604         }
605         mutex_unlock(&indio_dev->mlock);
606         buf[len - 1] = '\n';
607
608         return len;
609 }
610 EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail);
611
612 ssize_t st_sensors_sysfs_scale_avail(struct device *dev,
613                                 struct device_attribute *attr, char *buf)
614 {
615         int i, len = 0;
616         struct iio_dev *indio_dev = dev_get_drvdata(dev);
617         struct st_sensor_data *sdata = iio_priv(indio_dev);
618
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)
622                         break;
623
624                 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
625                                 sdata->sensor_settings->fs.fs_avl[i].gain);
626         }
627         mutex_unlock(&indio_dev->mlock);
628         buf[len - 1] = '\n';
629
630         return len;
631 }
632 EXPORT_SYMBOL(st_sensors_sysfs_scale_avail);
633
634 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
635 MODULE_DESCRIPTION("STMicroelectronics ST-sensors core");
636 MODULE_LICENSE("GPL v2");