1#include <linux/interrupt.h>
2#include <linux/gpio.h>
3#include <linux/mutex.h>
4#include <linux/kernel.h>
5#include <linux/spi/spi.h>
6#include <linux/slab.h>
7#include <linux/export.h>
8
9#include "../iio.h"
10#include "../ring_sw.h"
11#include "../kfifo_buf.h"
12#include "../trigger.h"
13#include "../trigger_consumer.h"
14#include "lis3l02dq.h"
15
16
17
18
19static inline u16 combine_8_to_16(u8 lower, u8 upper)
20{
21 u16 _lower = lower;
22 u16 _upper = upper;
23 return _lower | (_upper << 8);
24}
25
26
27
28
29irqreturn_t lis3l02dq_data_rdy_trig_poll(int irq, void *private)
30{
31 struct iio_dev *indio_dev = private;
32 struct lis3l02dq_state *st = iio_priv(indio_dev);
33
34 if (st->trigger_on) {
35 iio_trigger_poll(st->trig, iio_get_time_ns());
36 return IRQ_HANDLED;
37 } else
38 return IRQ_WAKE_THREAD;
39}
40
41
42
43
44ssize_t lis3l02dq_read_accel_from_buffer(struct iio_buffer *buffer,
45 int index,
46 int *val)
47{
48 int ret;
49 s16 *data;
50
51 if (!iio_scan_mask_query(buffer, index))
52 return -EINVAL;
53
54 if (!buffer->access->read_last)
55 return -EBUSY;
56
57 data = kmalloc(buffer->access->get_bytes_per_datum(buffer),
58 GFP_KERNEL);
59 if (data == NULL)
60 return -ENOMEM;
61
62 ret = buffer->access->read_last(buffer, (u8 *)data);
63 if (ret)
64 goto error_free_data;
65 *val = data[bitmap_weight(buffer->scan_mask, index)];
66error_free_data:
67
68 kfree(data);
69
70 return ret;
71}
72
73static const u8 read_all_tx_array[] = {
74 LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_X_L_ADDR), 0,
75 LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_X_H_ADDR), 0,
76 LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Y_L_ADDR), 0,
77 LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Y_H_ADDR), 0,
78 LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Z_L_ADDR), 0,
79 LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Z_H_ADDR), 0,
80};
81
82
83
84
85
86
87
88static int lis3l02dq_read_all(struct iio_dev *indio_dev, u8 *rx_array)
89{
90 struct iio_buffer *buffer = indio_dev->buffer;
91 struct lis3l02dq_state *st = iio_priv(indio_dev);
92 struct spi_transfer *xfers;
93 struct spi_message msg;
94 int ret, i, j = 0;
95
96 xfers = kzalloc((buffer->scan_count) * 2
97 * sizeof(*xfers), GFP_KERNEL);
98 if (!xfers)
99 return -ENOMEM;
100
101 mutex_lock(&st->buf_lock);
102
103 for (i = 0; i < ARRAY_SIZE(read_all_tx_array)/4; i++)
104 if (test_bit(i, buffer->scan_mask)) {
105
106 xfers[j].tx_buf = st->tx + 2*j;
107 st->tx[2*j] = read_all_tx_array[i*4];
108 st->tx[2*j + 1] = 0;
109 if (rx_array)
110 xfers[j].rx_buf = rx_array + j*2;
111 xfers[j].bits_per_word = 8;
112 xfers[j].len = 2;
113 xfers[j].cs_change = 1;
114 j++;
115
116
117 xfers[j].tx_buf = st->tx + 2*j;
118 st->tx[2*j] = read_all_tx_array[i*4 + 2];
119 st->tx[2*j + 1] = 0;
120 if (rx_array)
121 xfers[j].rx_buf = rx_array + j*2;
122 xfers[j].bits_per_word = 8;
123 xfers[j].len = 2;
124 xfers[j].cs_change = 1;
125 j++;
126 }
127
128
129
130
131 spi_message_init(&msg);
132 for (j = 0; j < buffer->scan_count * 2; j++)
133 spi_message_add_tail(&xfers[j], &msg);
134
135 ret = spi_sync(st->us, &msg);
136 mutex_unlock(&st->buf_lock);
137 kfree(xfers);
138
139 return ret;
140}
141
142static int lis3l02dq_get_buffer_element(struct iio_dev *indio_dev,
143 u8 *buf)
144{
145 int ret, i;
146 u8 *rx_array ;
147 s16 *data = (s16 *)buf;
148
149 rx_array = kzalloc(4 * (indio_dev->buffer->scan_count), GFP_KERNEL);
150 if (rx_array == NULL)
151 return -ENOMEM;
152 ret = lis3l02dq_read_all(indio_dev, rx_array);
153 if (ret < 0)
154 return ret;
155 for (i = 0; i < indio_dev->buffer->scan_count; i++)
156 data[i] = combine_8_to_16(rx_array[i*4+1],
157 rx_array[i*4+3]);
158 kfree(rx_array);
159
160 return i*sizeof(data[0]);
161}
162
163static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
164{
165 struct iio_poll_func *pf = p;
166 struct iio_dev *indio_dev = pf->indio_dev;
167 struct iio_buffer *buffer = indio_dev->buffer;
168 int len = 0;
169 size_t datasize = buffer->access->get_bytes_per_datum(buffer);
170 char *data = kmalloc(datasize, GFP_KERNEL);
171
172 if (data == NULL) {
173 dev_err(indio_dev->dev.parent,
174 "memory alloc failed in buffer bh");
175 return -ENOMEM;
176 }
177
178 if (buffer->scan_count)
179 len = lis3l02dq_get_buffer_element(indio_dev, data);
180
181
182 if (buffer->scan_timestamp)
183 *(s64 *)(((phys_addr_t)data + len
184 + sizeof(s64) - 1) & ~(sizeof(s64) - 1))
185 = pf->timestamp;
186 buffer->access->store_to(buffer, (u8 *)data, pf->timestamp);
187
188 iio_trigger_notify_done(indio_dev->trig);
189 kfree(data);
190 return IRQ_HANDLED;
191}
192
193
194static int
195__lis3l02dq_write_data_ready_config(struct device *dev, bool state)
196{
197 int ret;
198 u8 valold;
199 bool currentlyset;
200 struct iio_dev *indio_dev = dev_get_drvdata(dev);
201 struct lis3l02dq_state *st = iio_priv(indio_dev);
202
203
204 ret = lis3l02dq_spi_read_reg_8(indio_dev,
205 LIS3L02DQ_REG_CTRL_2_ADDR,
206 &valold);
207 if (ret)
208 goto error_ret;
209
210 currentlyset
211 = valold & LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
212
213
214 if (!state && currentlyset) {
215
216 valold &= ~LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
217
218
219 ret = lis3l02dq_spi_write_reg_8(indio_dev,
220 LIS3L02DQ_REG_CTRL_2_ADDR,
221 valold);
222 if (ret)
223 goto error_ret;
224 ret = lis3l02dq_spi_write_reg_8(indio_dev,
225 LIS3L02DQ_REG_CTRL_2_ADDR,
226 valold);
227 if (ret)
228 goto error_ret;
229 st->trigger_on = false;
230
231 } else if (state && !currentlyset) {
232
233
234 ret = lis3l02dq_disable_all_events(indio_dev);
235 if (ret < 0)
236 goto error_ret;
237
238 valold = ret |
239 LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
240
241 st->trigger_on = true;
242 ret = lis3l02dq_spi_write_reg_8(indio_dev,
243 LIS3L02DQ_REG_CTRL_2_ADDR,
244 valold);
245 if (ret)
246 goto error_ret;
247 }
248
249 return 0;
250error_ret:
251 return ret;
252}
253
254
255
256
257
258
259
260
261static int lis3l02dq_data_rdy_trigger_set_state(struct iio_trigger *trig,
262 bool state)
263{
264 struct iio_dev *indio_dev = trig->private_data;
265 int ret = 0;
266 u8 t;
267
268 __lis3l02dq_write_data_ready_config(&indio_dev->dev, state);
269 if (state == false) {
270
271
272
273
274 ret = lis3l02dq_read_all(indio_dev, NULL);
275 }
276 lis3l02dq_spi_read_reg_8(indio_dev,
277 LIS3L02DQ_REG_WAKE_UP_SRC_ADDR,
278 &t);
279 return ret;
280}
281
282
283
284
285
286static int lis3l02dq_trig_try_reen(struct iio_trigger *trig)
287{
288 struct iio_dev *indio_dev = trig->private_data;
289 struct lis3l02dq_state *st = iio_priv(indio_dev);
290 int i;
291
292
293
294 for (i = 0; i < 5; i++)
295 if (gpio_get_value(irq_to_gpio(st->us->irq)))
296 lis3l02dq_read_all(indio_dev, NULL);
297 else
298 break;
299 if (i == 5)
300 printk(KERN_INFO
301 "Failed to clear the interrupt for lis3l02dq\n");
302
303
304 return 0;
305}
306
307static const struct iio_trigger_ops lis3l02dq_trigger_ops = {
308 .owner = THIS_MODULE,
309 .set_trigger_state = &lis3l02dq_data_rdy_trigger_set_state,
310 .try_reenable = &lis3l02dq_trig_try_reen,
311};
312
313int lis3l02dq_probe_trigger(struct iio_dev *indio_dev)
314{
315 int ret;
316 struct lis3l02dq_state *st = iio_priv(indio_dev);
317
318 st->trig = iio_allocate_trigger("lis3l02dq-dev%d", indio_dev->id);
319 if (!st->trig) {
320 ret = -ENOMEM;
321 goto error_ret;
322 }
323
324 st->trig->dev.parent = &st->us->dev;
325 st->trig->ops = &lis3l02dq_trigger_ops;
326 st->trig->private_data = indio_dev;
327 ret = iio_trigger_register(st->trig);
328 if (ret)
329 goto error_free_trig;
330
331 return 0;
332
333error_free_trig:
334 iio_free_trigger(st->trig);
335error_ret:
336 return ret;
337}
338
339void lis3l02dq_remove_trigger(struct iio_dev *indio_dev)
340{
341 struct lis3l02dq_state *st = iio_priv(indio_dev);
342
343 iio_trigger_unregister(st->trig);
344 iio_free_trigger(st->trig);
345}
346
347void lis3l02dq_unconfigure_buffer(struct iio_dev *indio_dev)
348{
349 iio_dealloc_pollfunc(indio_dev->pollfunc);
350 lis3l02dq_free_buf(indio_dev->buffer);
351}
352
353static int lis3l02dq_buffer_postenable(struct iio_dev *indio_dev)
354{
355
356 u8 t;
357 int ret;
358 bool oneenabled = false;
359
360 ret = lis3l02dq_spi_read_reg_8(indio_dev,
361 LIS3L02DQ_REG_CTRL_1_ADDR,
362 &t);
363 if (ret)
364 goto error_ret;
365
366 if (iio_scan_mask_query(indio_dev->buffer, 0)) {
367 t |= LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE;
368 oneenabled = true;
369 } else
370 t &= ~LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE;
371 if (iio_scan_mask_query(indio_dev->buffer, 1)) {
372 t |= LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE;
373 oneenabled = true;
374 } else
375 t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE;
376 if (iio_scan_mask_query(indio_dev->buffer, 2)) {
377 t |= LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
378 oneenabled = true;
379 } else
380 t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
381
382 if (!oneenabled)
383 return -EINVAL;
384 ret = lis3l02dq_spi_write_reg_8(indio_dev,
385 LIS3L02DQ_REG_CTRL_1_ADDR,
386 t);
387 if (ret)
388 goto error_ret;
389
390 return iio_triggered_buffer_postenable(indio_dev);
391error_ret:
392 return ret;
393}
394
395
396static int lis3l02dq_buffer_predisable(struct iio_dev *indio_dev)
397{
398 u8 t;
399 int ret;
400
401 ret = iio_triggered_buffer_predisable(indio_dev);
402 if (ret)
403 goto error_ret;
404
405 ret = lis3l02dq_spi_read_reg_8(indio_dev,
406 LIS3L02DQ_REG_CTRL_1_ADDR,
407 &t);
408 if (ret)
409 goto error_ret;
410 t |= LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE |
411 LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE |
412 LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
413
414 ret = lis3l02dq_spi_write_reg_8(indio_dev,
415 LIS3L02DQ_REG_CTRL_1_ADDR,
416 t);
417
418error_ret:
419 return ret;
420}
421
422static const struct iio_buffer_setup_ops lis3l02dq_buffer_setup_ops = {
423 .preenable = &iio_sw_buffer_preenable,
424 .postenable = &lis3l02dq_buffer_postenable,
425 .predisable = &lis3l02dq_buffer_predisable,
426};
427
428int lis3l02dq_configure_buffer(struct iio_dev *indio_dev)
429{
430 int ret;
431 struct iio_buffer *buffer;
432
433 buffer = lis3l02dq_alloc_buf(indio_dev);
434 if (!buffer)
435 return -ENOMEM;
436
437 indio_dev->buffer = buffer;
438
439 indio_dev->buffer->access = &lis3l02dq_access_funcs;
440 buffer->bpe = 2;
441
442 buffer->scan_timestamp = true;
443 buffer->setup_ops = &lis3l02dq_buffer_setup_ops;
444 buffer->owner = THIS_MODULE;
445
446
447 indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
448 &lis3l02dq_trigger_handler,
449 0,
450 indio_dev,
451 "lis3l02dq_consumer%d",
452 indio_dev->id);
453
454 if (indio_dev->pollfunc == NULL) {
455 ret = -ENOMEM;
456 goto error_iio_sw_rb_free;
457 }
458
459 indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
460 return 0;
461
462error_iio_sw_rb_free:
463 lis3l02dq_free_buf(indio_dev->buffer);
464 return ret;
465}
466