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