]>
Commit | Line | Data |
---|---|---|
23491b51 DC |
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> | |
ea7e586b | 16 | #include <linux/regulator/consumer.h> |
3ce85cc4 | 17 | #include <linux/of.h> |
23491b51 | 18 | #include <asm/unaligned.h> |
23491b51 DC |
19 | #include <linux/iio/common/st_sensors.h> |
20 | ||
a9fd053b LW |
21 | #include "st_sensors_core.h" |
22 | ||
607a568a DC |
23 | static inline u32 st_sensors_get_unaligned_le24(const u8 *p) |
24 | { | |
23cde4d6 | 25 | return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8; |
607a568a DC |
26 | } |
27 | ||
a9fd053b LW |
28 | int st_sensors_write_data_with_mask(struct iio_dev *indio_dev, |
29 | u8 reg_addr, u8 mask, u8 data) | |
23491b51 DC |
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 | ||
a0175b9c LW |
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 | ||
a7ee8839 | 68 | static int st_sensors_match_odr(struct st_sensor_settings *sensor_settings, |
23491b51 DC |
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++) { | |
a7ee8839 | 74 | if (sensor_settings->odr.odr_avl[i].hz == 0) |
23491b51 DC |
75 | goto st_sensors_match_odr_error; |
76 | ||
a7ee8839 DC |
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; | |
23491b51 DC |
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; | |
852afe99 | 92 | struct st_sensor_odr_avl odr_out = {0, 0}; |
23491b51 DC |
93 | struct st_sensor_data *sdata = iio_priv(indio_dev); |
94 | ||
a7ee8839 | 95 | err = st_sensors_match_odr(sdata->sensor_settings, odr, &odr_out); |
23491b51 DC |
96 | if (err < 0) |
97 | goto st_sensors_match_odr_error; | |
98 | ||
a7ee8839 DC |
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)) { | |
23491b51 DC |
103 | if (sdata->enabled == true) { |
104 | err = st_sensors_write_data_with_mask(indio_dev, | |
a7ee8839 DC |
105 | sdata->sensor_settings->odr.addr, |
106 | sdata->sensor_settings->odr.mask, | |
23491b51 DC |
107 | odr_out.value); |
108 | } else { | |
109 | err = 0; | |
110 | } | |
111 | } else { | |
112 | err = st_sensors_write_data_with_mask(indio_dev, | |
a7ee8839 DC |
113 | sdata->sensor_settings->odr.addr, |
114 | sdata->sensor_settings->odr.mask, | |
23491b51 DC |
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 | ||
a7ee8839 | 125 | static int st_sensors_match_fs(struct st_sensor_settings *sensor_settings, |
23491b51 DC |
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++) { | |
a7ee8839 | 131 | if (sensor_settings->fs.fs_avl[i].num == 0) |
23491b51 DC |
132 | goto st_sensors_match_odr_error; |
133 | ||
a7ee8839 | 134 | if (sensor_settings->fs.fs_avl[i].num == fs) { |
23491b51 DC |
135 | *index_fs_avl = i; |
136 | ret = 0; | |
137 | break; | |
138 | } | |
139 | } | |
140 | ||
141 | st_sensors_match_odr_error: | |
142 | return ret; | |
143 | } | |
144 | ||
a7ee8839 | 145 | static int st_sensors_set_fullscale(struct iio_dev *indio_dev, unsigned int fs) |
23491b51 | 146 | { |
852afe99 | 147 | int err, i = 0; |
23491b51 DC |
148 | struct st_sensor_data *sdata = iio_priv(indio_dev); |
149 | ||
bb602f8c GB |
150 | if (sdata->sensor_settings->fs.addr == 0) |
151 | return 0; | |
152 | ||
a7ee8839 | 153 | err = st_sensors_match_fs(sdata->sensor_settings, fs, &i); |
23491b51 DC |
154 | if (err < 0) |
155 | goto st_accel_set_fullscale_error; | |
156 | ||
157 | err = st_sensors_write_data_with_mask(indio_dev, | |
a7ee8839 DC |
158 | sdata->sensor_settings->fs.addr, |
159 | sdata->sensor_settings->fs.mask, | |
160 | sdata->sensor_settings->fs.fs_avl[i].value); | |
23491b51 DC |
161 | if (err < 0) |
162 | goto st_accel_set_fullscale_error; | |
163 | ||
164 | sdata->current_fullscale = (struct st_sensor_fullscale_avl *) | |
a7ee8839 | 165 | &sdata->sensor_settings->fs.fs_avl[i]; |
23491b51 DC |
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 | { | |
23491b51 DC |
175 | u8 tmp_value; |
176 | int err = -EINVAL; | |
852afe99 DC |
177 | bool found = false; |
178 | struct st_sensor_odr_avl odr_out = {0, 0}; | |
23491b51 DC |
179 | struct st_sensor_data *sdata = iio_priv(indio_dev); |
180 | ||
181 | if (enable) { | |
a7ee8839 DC |
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, | |
23491b51 DC |
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, | |
a7ee8839 DC |
195 | sdata->sensor_settings->pw.addr, |
196 | sdata->sensor_settings->pw.mask, tmp_value); | |
23491b51 DC |
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, | |
a7ee8839 DC |
206 | sdata->sensor_settings->pw.addr, |
207 | sdata->sensor_settings->pw.mask, | |
208 | sdata->sensor_settings->pw.value_off); | |
23491b51 DC |
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, | |
a7ee8839 DC |
225 | sdata->sensor_settings->enable_axis.addr, |
226 | sdata->sensor_settings->enable_axis.mask, | |
227 | axis_enable); | |
23491b51 DC |
228 | } |
229 | EXPORT_SYMBOL(st_sensors_set_axis_enable); | |
230 | ||
14f295c8 | 231 | int st_sensors_power_enable(struct iio_dev *indio_dev) |
ea7e586b LW |
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. */ | |
aeb55fff LW |
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; | |
ea7e586b LW |
247 | } |
248 | ||
aeb55fff | 249 | pdata->vdd_io = devm_regulator_get(indio_dev->dev.parent, "vddio"); |
1b246fca | 250 | if (IS_ERR(pdata->vdd_io)) { |
aeb55fff | 251 | dev_err(&indio_dev->dev, "unable to get Vdd_IO supply\n"); |
1b246fca | 252 | err = PTR_ERR(pdata->vdd_io); |
aeb55fff LW |
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; | |
ea7e586b | 260 | } |
14f295c8 GB |
261 | |
262 | return 0; | |
263 | ||
264 | st_sensors_disable_vdd: | |
aeb55fff | 265 | regulator_disable(pdata->vdd); |
14f295c8 | 266 | return err; |
ea7e586b LW |
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 | ||
aeb55fff LW |
274 | regulator_disable(pdata->vdd); |
275 | regulator_disable(pdata->vdd_io); | |
ea7e586b LW |
276 | } |
277 | EXPORT_SYMBOL(st_sensors_power_disable); | |
278 | ||
38d1c6a9 | 279 | static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev, |
a7ee8839 | 280 | struct st_sensors_platform_data *pdata) |
23491b51 | 281 | { |
23491b51 DC |
282 | struct st_sensor_data *sdata = iio_priv(indio_dev); |
283 | ||
d2bc4318 LW |
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 | ||
23cde4d6 DC |
294 | switch (pdata->drdy_int_pin) { |
295 | case 1: | |
a7ee8839 | 296 | if (sdata->sensor_settings->drdy_irq.mask_int1 == 0) { |
23cde4d6 DC |
297 | dev_err(&indio_dev->dev, |
298 | "DRDY on INT1 not available.\n"); | |
38d1c6a9 | 299 | return -EINVAL; |
23cde4d6 DC |
300 | } |
301 | sdata->drdy_int_pin = 1; | |
302 | break; | |
303 | case 2: | |
a7ee8839 | 304 | if (sdata->sensor_settings->drdy_irq.mask_int2 == 0) { |
23cde4d6 DC |
305 | dev_err(&indio_dev->dev, |
306 | "DRDY on INT2 not available.\n"); | |
38d1c6a9 | 307 | return -EINVAL; |
23cde4d6 DC |
308 | } |
309 | sdata->drdy_int_pin = 2; | |
310 | break; | |
311 | default: | |
312 | dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n"); | |
38d1c6a9 | 313 | return -EINVAL; |
23cde4d6 DC |
314 | } |
315 | ||
0e6f6871 LW |
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 | ||
38d1c6a9 LJ |
324 | return 0; |
325 | } | |
326 | ||
3ce85cc4 LW |
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 | |
d2bc4318 | 342 | pdata->drdy_int_pin = defdata ? defdata->drdy_int_pin : 0; |
3ce85cc4 | 343 | |
0e6f6871 LW |
344 | pdata->open_drain = of_property_read_bool(np, "drive-open-drain"); |
345 | ||
3ce85cc4 LW |
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 | ||
38d1c6a9 LJ |
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); | |
3ce85cc4 | 360 | struct st_sensors_platform_data *of_pdata; |
38d1c6a9 LJ |
361 | int err = 0; |
362 | ||
3ce85cc4 LW |
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 | ||
3c8bf223 | 368 | if (pdata) { |
38d1c6a9 | 369 | err = st_sensors_set_drdy_int_pin(indio_dev, pdata); |
3c8bf223 LJ |
370 | if (err < 0) |
371 | return err; | |
372 | } | |
38d1c6a9 | 373 | |
23491b51 DC |
374 | err = st_sensors_set_enable(indio_dev, false); |
375 | if (err < 0) | |
efd9566f | 376 | return err; |
23491b51 | 377 | |
99147606 CDL |
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 | ||
362f2f86 LJ |
383 | if (sdata->current_fullscale) { |
384 | err = st_sensors_set_fullscale(indio_dev, | |
a7ee8839 | 385 | sdata->current_fullscale->num); |
362f2f86 | 386 | if (err < 0) |
efd9566f | 387 | return err; |
362f2f86 LJ |
388 | } else |
389 | dev_info(&indio_dev->dev, "Full-scale not possible\n"); | |
23491b51 DC |
390 | |
391 | err = st_sensors_set_odr(indio_dev, sdata->odr); | |
392 | if (err < 0) | |
efd9566f | 393 | return err; |
23491b51 DC |
394 | |
395 | /* set BDU */ | |
bb60646c LW |
396 | if (sdata->sensor_settings->bdu.addr) { |
397 | err = st_sensors_write_data_with_mask(indio_dev, | |
a7ee8839 DC |
398 | sdata->sensor_settings->bdu.addr, |
399 | sdata->sensor_settings->bdu.mask, true); | |
bb60646c LW |
400 | if (err < 0) |
401 | return err; | |
402 | } | |
23491b51 | 403 | |
65e4345c LW |
404 | /* set DAS */ |
405 | if (sdata->sensor_settings->das.addr) { | |
406 | err = st_sensors_write_data_with_mask(indio_dev, | |
407 | sdata->sensor_settings->das.addr, | |
408 | sdata->sensor_settings->das.mask, 1); | |
409 | if (err < 0) | |
410 | return err; | |
411 | } | |
412 | ||
0e6f6871 LW |
413 | if (sdata->int_pin_open_drain) { |
414 | dev_info(&indio_dev->dev, | |
415 | "set interrupt line to open drain mode\n"); | |
416 | err = st_sensors_write_data_with_mask(indio_dev, | |
417 | sdata->sensor_settings->drdy_irq.addr_od, | |
418 | sdata->sensor_settings->drdy_irq.mask_od, 1); | |
419 | if (err < 0) | |
420 | return err; | |
421 | } | |
422 | ||
23491b51 DC |
423 | err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS); |
424 | ||
23491b51 DC |
425 | return err; |
426 | } | |
427 | EXPORT_SYMBOL(st_sensors_init_sensor); | |
428 | ||
429 | int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable) | |
430 | { | |
431 | int err; | |
23cde4d6 | 432 | u8 drdy_mask; |
23491b51 DC |
433 | struct st_sensor_data *sdata = iio_priv(indio_dev); |
434 | ||
a7ee8839 | 435 | if (!sdata->sensor_settings->drdy_irq.addr) |
38d1c6a9 LJ |
436 | return 0; |
437 | ||
23491b51 | 438 | /* Enable/Disable the interrupt generator 1. */ |
a7ee8839 | 439 | if (sdata->sensor_settings->drdy_irq.ig1.en_addr > 0) { |
23491b51 | 440 | err = st_sensors_write_data_with_mask(indio_dev, |
a7ee8839 DC |
441 | sdata->sensor_settings->drdy_irq.ig1.en_addr, |
442 | sdata->sensor_settings->drdy_irq.ig1.en_mask, | |
443 | (int)enable); | |
23491b51 DC |
444 | if (err < 0) |
445 | goto st_accel_set_dataready_irq_error; | |
446 | } | |
447 | ||
23cde4d6 | 448 | if (sdata->drdy_int_pin == 1) |
a7ee8839 | 449 | drdy_mask = sdata->sensor_settings->drdy_irq.mask_int1; |
23cde4d6 | 450 | else |
a7ee8839 | 451 | drdy_mask = sdata->sensor_settings->drdy_irq.mask_int2; |
23cde4d6 | 452 | |
65925b65 LW |
453 | /* Flag to the poll function that the hardware trigger is in use */ |
454 | sdata->hw_irq_trigger = enable; | |
455 | ||
23491b51 DC |
456 | /* Enable/Disable the interrupt generator for data ready. */ |
457 | err = st_sensors_write_data_with_mask(indio_dev, | |
a7ee8839 DC |
458 | sdata->sensor_settings->drdy_irq.addr, |
459 | drdy_mask, (int)enable); | |
23491b51 DC |
460 | |
461 | st_accel_set_dataready_irq_error: | |
462 | return err; | |
463 | } | |
464 | EXPORT_SYMBOL(st_sensors_set_dataready_irq); | |
465 | ||
466 | int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale) | |
467 | { | |
468 | int err = -EINVAL, i; | |
469 | struct st_sensor_data *sdata = iio_priv(indio_dev); | |
470 | ||
471 | for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) { | |
a7ee8839 DC |
472 | if ((sdata->sensor_settings->fs.fs_avl[i].gain == scale) && |
473 | (sdata->sensor_settings->fs.fs_avl[i].gain != 0)) { | |
23491b51 DC |
474 | err = 0; |
475 | break; | |
476 | } | |
477 | } | |
478 | if (err < 0) | |
479 | goto st_sensors_match_scale_error; | |
480 | ||
481 | err = st_sensors_set_fullscale(indio_dev, | |
a7ee8839 | 482 | sdata->sensor_settings->fs.fs_avl[i].num); |
23491b51 DC |
483 | |
484 | st_sensors_match_scale_error: | |
485 | return err; | |
486 | } | |
487 | EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain); | |
488 | ||
489 | static int st_sensors_read_axis_data(struct iio_dev *indio_dev, | |
607a568a | 490 | struct iio_chan_spec const *ch, int *data) |
23491b51 DC |
491 | { |
492 | int err; | |
607a568a | 493 | u8 *outdata; |
23491b51 | 494 | struct st_sensor_data *sdata = iio_priv(indio_dev); |
65c8aea0 | 495 | unsigned int byte_for_channel; |
607a568a | 496 | |
65c8aea0 LB |
497 | byte_for_channel = DIV_ROUND_UP(ch->scan_type.realbits + |
498 | ch->scan_type.shift, 8); | |
607a568a | 499 | outdata = kmalloc(byte_for_channel, GFP_KERNEL); |
caf5ca12 LJ |
500 | if (!outdata) |
501 | return -ENOMEM; | |
23491b51 DC |
502 | |
503 | err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev, | |
607a568a | 504 | ch->address, byte_for_channel, |
23491b51 DC |
505 | outdata, sdata->multiread_bit); |
506 | if (err < 0) | |
607a568a | 507 | goto st_sensors_free_memory; |
23491b51 | 508 | |
4861a007 LW |
509 | if (byte_for_channel == 1) |
510 | *data = (s8)*outdata; | |
511 | else if (byte_for_channel == 2) | |
607a568a DC |
512 | *data = (s16)get_unaligned_le16(outdata); |
513 | else if (byte_for_channel == 3) | |
514 | *data = (s32)st_sensors_get_unaligned_le24(outdata); | |
23491b51 | 515 | |
607a568a DC |
516 | st_sensors_free_memory: |
517 | kfree(outdata); | |
caf5ca12 | 518 | |
23491b51 DC |
519 | return err; |
520 | } | |
521 | ||
522 | int st_sensors_read_info_raw(struct iio_dev *indio_dev, | |
523 | struct iio_chan_spec const *ch, int *val) | |
524 | { | |
525 | int err; | |
526 | struct st_sensor_data *sdata = iio_priv(indio_dev); | |
527 | ||
528 | mutex_lock(&indio_dev->mlock); | |
529 | if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { | |
530 | err = -EBUSY; | |
5bb8e72d | 531 | goto out; |
23491b51 DC |
532 | } else { |
533 | err = st_sensors_set_enable(indio_dev, true); | |
534 | if (err < 0) | |
5bb8e72d | 535 | goto out; |
23491b51 | 536 | |
a7ee8839 | 537 | msleep((sdata->sensor_settings->bootime * 1000) / sdata->odr); |
607a568a | 538 | err = st_sensors_read_axis_data(indio_dev, ch, val); |
23491b51 | 539 | if (err < 0) |
5bb8e72d | 540 | goto out; |
23491b51 DC |
541 | |
542 | *val = *val >> ch->scan_type.shift; | |
d61a04dc DC |
543 | |
544 | err = st_sensors_set_enable(indio_dev, false); | |
23491b51 | 545 | } |
5bb8e72d | 546 | out: |
23491b51 DC |
547 | mutex_unlock(&indio_dev->mlock); |
548 | ||
549 | return err; | |
23491b51 DC |
550 | } |
551 | EXPORT_SYMBOL(st_sensors_read_info_raw); | |
552 | ||
553 | int st_sensors_check_device_support(struct iio_dev *indio_dev, | |
a7ee8839 DC |
554 | int num_sensors_list, |
555 | const struct st_sensor_settings *sensor_settings) | |
23491b51 | 556 | { |
4e68cfbf | 557 | int i, n, err = 0; |
bc27381e | 558 | u8 wai; |
23491b51 DC |
559 | struct st_sensor_data *sdata = iio_priv(indio_dev); |
560 | ||
23491b51 | 561 | for (i = 0; i < num_sensors_list; i++) { |
bc27381e GB |
562 | for (n = 0; n < ST_SENSORS_MAX_4WAI; n++) { |
563 | if (strcmp(indio_dev->name, | |
564 | sensor_settings[i].sensors_supported[n]) == 0) { | |
565 | break; | |
566 | } | |
567 | } | |
568 | if (n < ST_SENSORS_MAX_4WAI) | |
23491b51 DC |
569 | break; |
570 | } | |
bc27381e GB |
571 | if (i == num_sensors_list) { |
572 | dev_err(&indio_dev->dev, "device name %s not recognized.\n", | |
573 | indio_dev->name); | |
574 | return -ENODEV; | |
575 | } | |
23491b51 | 576 | |
4e68cfbf JC |
577 | if (sensor_settings[i].wai_addr) { |
578 | err = sdata->tf->read_byte(&sdata->tb, sdata->dev, | |
579 | sensor_settings[i].wai_addr, &wai); | |
580 | if (err < 0) { | |
581 | dev_err(&indio_dev->dev, | |
582 | "failed to read Who-Am-I register.\n"); | |
583 | return err; | |
584 | } | |
bc27381e | 585 | |
4e68cfbf JC |
586 | if (sensor_settings[i].wai != wai) { |
587 | dev_err(&indio_dev->dev, | |
588 | "%s: WhoAmI mismatch (0x%x).\n", | |
589 | indio_dev->name, wai); | |
590 | return -EINVAL; | |
591 | } | |
23491b51 DC |
592 | } |
593 | ||
a7ee8839 DC |
594 | sdata->sensor_settings = |
595 | (struct st_sensor_settings *)&sensor_settings[i]; | |
23491b51 DC |
596 | |
597 | return i; | |
23491b51 DC |
598 | } |
599 | EXPORT_SYMBOL(st_sensors_check_device_support); | |
600 | ||
23491b51 DC |
601 | ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev, |
602 | struct device_attribute *attr, char *buf) | |
603 | { | |
4d2e4fc2 | 604 | int i, len = 0; |
23491b51 | 605 | struct iio_dev *indio_dev = dev_get_drvdata(dev); |
4d2e4fc2 DC |
606 | struct st_sensor_data *sdata = iio_priv(indio_dev); |
607 | ||
608 | mutex_lock(&indio_dev->mlock); | |
609 | for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) { | |
a7ee8839 | 610 | if (sdata->sensor_settings->odr.odr_avl[i].hz == 0) |
4d2e4fc2 DC |
611 | break; |
612 | ||
613 | len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", | |
a7ee8839 | 614 | sdata->sensor_settings->odr.odr_avl[i].hz); |
4d2e4fc2 DC |
615 | } |
616 | mutex_unlock(&indio_dev->mlock); | |
617 | buf[len - 1] = '\n'; | |
23491b51 | 618 | |
4d2e4fc2 | 619 | return len; |
23491b51 DC |
620 | } |
621 | EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail); | |
622 | ||
623 | ssize_t st_sensors_sysfs_scale_avail(struct device *dev, | |
624 | struct device_attribute *attr, char *buf) | |
625 | { | |
d304286a | 626 | int i, len = 0, q, r; |
23491b51 | 627 | struct iio_dev *indio_dev = dev_get_drvdata(dev); |
4d2e4fc2 DC |
628 | struct st_sensor_data *sdata = iio_priv(indio_dev); |
629 | ||
630 | mutex_lock(&indio_dev->mlock); | |
631 | for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) { | |
a7ee8839 | 632 | if (sdata->sensor_settings->fs.fs_avl[i].num == 0) |
4d2e4fc2 DC |
633 | break; |
634 | ||
d304286a LB |
635 | q = sdata->sensor_settings->fs.fs_avl[i].gain / 1000000; |
636 | r = sdata->sensor_settings->fs.fs_avl[i].gain % 1000000; | |
637 | ||
638 | len += scnprintf(buf + len, PAGE_SIZE - len, "%u.%06u ", q, r); | |
4d2e4fc2 DC |
639 | } |
640 | mutex_unlock(&indio_dev->mlock); | |
641 | buf[len - 1] = '\n'; | |
23491b51 | 642 | |
4d2e4fc2 | 643 | return len; |
23491b51 DC |
644 | } |
645 | EXPORT_SYMBOL(st_sensors_sysfs_scale_avail); | |
646 | ||
647 | MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); | |
648 | MODULE_DESCRIPTION("STMicroelectronics ST-sensors core"); | |
649 | MODULE_LICENSE("GPL v2"); |