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>
10 #include "../ring_sw.h"
11 #include "../kfifo_buf.h"
12 #include "../trigger.h"
13 #include "../trigger_consumer.h"
14 #include "lis3l02dq.h"
17 * combine_8_to_16() utility function to munge to u8s into u16
19 static inline u16 combine_8_to_16(u8 lower, u8 upper)
23 return _lower | (_upper << 8);
27 * lis3l02dq_data_rdy_trig_poll() the event handler for the data rdy trig
29 irqreturn_t lis3l02dq_data_rdy_trig_poll(int irq, void *private)
31 struct iio_dev *indio_dev = private;
32 struct lis3l02dq_state *st = iio_priv(indio_dev);
35 iio_trigger_poll(st->trig, iio_get_time_ns());
38 return IRQ_WAKE_THREAD;
42 * lis3l02dq_read_accel_from_buffer() individual acceleration read from buffer
44 ssize_t lis3l02dq_read_accel_from_buffer(struct iio_buffer *buffer,
51 if (!iio_scan_mask_query(buffer, index))
54 if (!buffer->access->read_last)
57 data = kmalloc(buffer->access->get_bytes_per_datum(buffer),
62 ret = buffer->access->read_last(buffer, (u8 *)data);
65 *val = data[bitmap_weight(buffer->scan_mask, index)];
73 static 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,
83 * lis3l02dq_read_all() Reads all channels currently selected
84 * @st: device specific state
85 * @rx_array: (dma capable) receive array, must be at least
86 * 4*number of channels
88 static int lis3l02dq_read_all(struct iio_dev *indio_dev, u8 *rx_array)
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;
96 xfers = kzalloc((buffer->scan_count) * 2
97 * sizeof(*xfers), GFP_KERNEL);
101 mutex_lock(&st->buf_lock);
103 for (i = 0; i < ARRAY_SIZE(read_all_tx_array)/4; i++)
104 if (test_bit(i, buffer->scan_mask)) {
106 xfers[j].tx_buf = st->tx + 2*j;
107 st->tx[2*j] = read_all_tx_array[i*4];
110 xfers[j].rx_buf = rx_array + j*2;
111 xfers[j].bits_per_word = 8;
113 xfers[j].cs_change = 1;
117 xfers[j].tx_buf = st->tx + 2*j;
118 st->tx[2*j] = read_all_tx_array[i*4 + 2];
121 xfers[j].rx_buf = rx_array + j*2;
122 xfers[j].bits_per_word = 8;
124 xfers[j].cs_change = 1;
128 /* After these are transmitted, the rx_buff should have
129 * values in alternate bytes
131 spi_message_init(&msg);
132 for (j = 0; j < buffer->scan_count * 2; j++)
133 spi_message_add_tail(&xfers[j], &msg);
135 ret = spi_sync(st->us, &msg);
136 mutex_unlock(&st->buf_lock);
142 static int lis3l02dq_get_buffer_element(struct iio_dev *indio_dev,
147 s16 *data = (s16 *)buf;
149 rx_array = kzalloc(4 * (indio_dev->buffer->scan_count), GFP_KERNEL);
150 if (rx_array == NULL)
152 ret = lis3l02dq_read_all(indio_dev, rx_array);
155 for (i = 0; i < indio_dev->buffer->scan_count; i++)
156 data[i] = combine_8_to_16(rx_array[i*4+1],
160 return i*sizeof(data[0]);
163 static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
165 struct iio_poll_func *pf = p;
166 struct iio_dev *indio_dev = pf->indio_dev;
167 struct iio_buffer *buffer = indio_dev->buffer;
169 size_t datasize = buffer->access->get_bytes_per_datum(buffer);
170 char *data = kmalloc(datasize, GFP_KERNEL);
173 dev_err(indio_dev->dev.parent,
174 "memory alloc failed in buffer bh");
178 if (buffer->scan_count)
179 len = lis3l02dq_get_buffer_element(indio_dev, data);
181 /* Guaranteed to be aligned with 8 byte boundary */
182 if (buffer->scan_timestamp)
183 *(s64 *)(((phys_addr_t)data + len
184 + sizeof(s64) - 1) & ~(sizeof(s64) - 1))
186 buffer->access->store_to(buffer, (u8 *)data, pf->timestamp);
188 iio_trigger_notify_done(indio_dev->trig);
193 /* Caller responsible for locking as necessary. */
195 __lis3l02dq_write_data_ready_config(struct device *dev, bool state)
200 struct iio_dev *indio_dev = dev_get_drvdata(dev);
201 struct lis3l02dq_state *st = iio_priv(indio_dev);
203 /* Get the current event mask register */
204 ret = lis3l02dq_spi_read_reg_8(indio_dev,
205 LIS3L02DQ_REG_CTRL_2_ADDR,
209 /* Find out if data ready is already on */
211 = valold & LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
213 /* Disable requested */
214 if (!state && currentlyset) {
215 /* disable the data ready signal */
216 valold &= ~LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
218 /* The double write is to overcome a hardware bug?*/
219 ret = lis3l02dq_spi_write_reg_8(indio_dev,
220 LIS3L02DQ_REG_CTRL_2_ADDR,
224 ret = lis3l02dq_spi_write_reg_8(indio_dev,
225 LIS3L02DQ_REG_CTRL_2_ADDR,
229 st->trigger_on = false;
230 /* Enable requested */
231 } else if (state && !currentlyset) {
232 /* if not set, enable requested */
233 /* first disable all events */
234 ret = lis3l02dq_disable_all_events(indio_dev);
239 LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
241 st->trigger_on = true;
242 ret = lis3l02dq_spi_write_reg_8(indio_dev,
243 LIS3L02DQ_REG_CTRL_2_ADDR,
255 * lis3l02dq_data_rdy_trigger_set_state() set datardy interrupt state
257 * If disabling the interrupt also does a final read to ensure it is clear.
258 * This is only important in some cases where the scan enable elements are
259 * switched before the buffer is reenabled.
261 static int lis3l02dq_data_rdy_trigger_set_state(struct iio_trigger *trig,
264 struct iio_dev *indio_dev = trig->private_data;
268 __lis3l02dq_write_data_ready_config(&indio_dev->dev, state);
269 if (state == false) {
271 * A possible quirk with teh handler is currently worked around
272 * by ensuring outstanding read events are cleared.
274 ret = lis3l02dq_read_all(indio_dev, NULL);
276 lis3l02dq_spi_read_reg_8(indio_dev,
277 LIS3L02DQ_REG_WAKE_UP_SRC_ADDR,
283 * lis3l02dq_trig_try_reen() try renabling irq for data rdy trigger
284 * @trig: the datardy trigger
286 static int lis3l02dq_trig_try_reen(struct iio_trigger *trig)
288 struct iio_dev *indio_dev = trig->private_data;
289 struct lis3l02dq_state *st = iio_priv(indio_dev);
292 /* If gpio still high (or high again) */
293 /* In theory possible we will need to do this several times */
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);
301 "Failed to clear the interrupt for lis3l02dq\n");
303 /* irq reenabled so success! */
307 static 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,
313 int lis3l02dq_probe_trigger(struct iio_dev *indio_dev)
316 struct lis3l02dq_state *st = iio_priv(indio_dev);
318 st->trig = iio_allocate_trigger("lis3l02dq-dev%d", indio_dev->id);
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);
329 goto error_free_trig;
334 iio_free_trigger(st->trig);
339 void lis3l02dq_remove_trigger(struct iio_dev *indio_dev)
341 struct lis3l02dq_state *st = iio_priv(indio_dev);
343 iio_trigger_unregister(st->trig);
344 iio_free_trigger(st->trig);
347 void lis3l02dq_unconfigure_buffer(struct iio_dev *indio_dev)
349 iio_dealloc_pollfunc(indio_dev->pollfunc);
350 lis3l02dq_free_buf(indio_dev->buffer);
353 static int lis3l02dq_buffer_postenable(struct iio_dev *indio_dev)
355 /* Disable unwanted channels otherwise the interrupt will not clear */
358 bool oneenabled = false;
360 ret = lis3l02dq_spi_read_reg_8(indio_dev,
361 LIS3L02DQ_REG_CTRL_1_ADDR,
366 if (iio_scan_mask_query(indio_dev->buffer, 0)) {
367 t |= LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE;
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;
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;
380 t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
382 if (!oneenabled) /* what happens in this case is unknown */
384 ret = lis3l02dq_spi_write_reg_8(indio_dev,
385 LIS3L02DQ_REG_CTRL_1_ADDR,
390 return iio_triggered_buffer_postenable(indio_dev);
395 /* Turn all channels on again */
396 static int lis3l02dq_buffer_predisable(struct iio_dev *indio_dev)
401 ret = iio_triggered_buffer_predisable(indio_dev);
405 ret = lis3l02dq_spi_read_reg_8(indio_dev,
406 LIS3L02DQ_REG_CTRL_1_ADDR,
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;
414 ret = lis3l02dq_spi_write_reg_8(indio_dev,
415 LIS3L02DQ_REG_CTRL_1_ADDR,
422 static 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,
428 int lis3l02dq_configure_buffer(struct iio_dev *indio_dev)
431 struct iio_buffer *buffer;
433 buffer = lis3l02dq_alloc_buf(indio_dev);
437 indio_dev->buffer = buffer;
438 /* Effectively select the buffer implementation */
439 indio_dev->buffer->access = &lis3l02dq_access_funcs;
442 buffer->scan_timestamp = true;
443 buffer->setup_ops = &lis3l02dq_buffer_setup_ops;
444 buffer->owner = THIS_MODULE;
446 /* Functions are NULL as we set handler below */
447 indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
448 &lis3l02dq_trigger_handler,
451 "lis3l02dq_consumer%d",
454 if (indio_dev->pollfunc == NULL) {
456 goto error_iio_sw_rb_free;
459 indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
462 error_iio_sw_rb_free:
463 lis3l02dq_free_buf(indio_dev->buffer);