]>
Commit | Line | Data |
---|---|---|
14cd9a73 | 1 | #include <linux/interrupt.h> |
14cd9a73 | 2 | #include <linux/gpio.h> |
14cd9a73 | 3 | #include <linux/mutex.h> |
14cd9a73 JC |
4 | #include <linux/kernel.h> |
5 | #include <linux/spi/spi.h> | |
5a0e3ad6 | 6 | #include <linux/slab.h> |
8e336a72 | 7 | #include <linux/export.h> |
14cd9a73 | 8 | |
06458e27 | 9 | #include <linux/iio/iio.h> |
06458e27 JC |
10 | #include <linux/iio/kfifo_buf.h> |
11 | #include <linux/iio/trigger.h> | |
12 | #include <linux/iio/trigger_consumer.h> | |
14cd9a73 JC |
13 | #include "lis3l02dq.h" |
14 | ||
15 | /** | |
91b4171f | 16 | * combine_8_to_16() utility function to munge two u8s into u16 |
14cd9a73 JC |
17 | **/ |
18 | static inline u16 combine_8_to_16(u8 lower, u8 upper) | |
19 | { | |
20 | u16 _lower = lower; | |
21 | u16 _upper = upper; | |
d7b79519 | 22 | |
14cd9a73 JC |
23 | return _lower | (_upper << 8); |
24 | } | |
25 | ||
d731aea0 JC |
26 | /** |
27 | * lis3l02dq_data_rdy_trig_poll() the event handler for the data rdy trig | |
28 | **/ | |
29 | irqreturn_t lis3l02dq_data_rdy_trig_poll(int irq, void *private) | |
30 | { | |
31 | struct iio_dev *indio_dev = private; | |
7b2fdd19 | 32 | struct lis3l02dq_state *st = iio_priv(indio_dev); |
d731aea0 JC |
33 | |
34 | if (st->trigger_on) { | |
398fd22b | 35 | iio_trigger_poll(st->trig); |
d731aea0 | 36 | return IRQ_HANDLED; |
d6be7a02 CR |
37 | } |
38 | ||
39 | return IRQ_WAKE_THREAD; | |
d731aea0 JC |
40 | } |
41 | ||
26de7208 | 42 | static const u8 read_all_tx_array[] = { |
14cd9a73 JC |
43 | LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_X_L_ADDR), 0, |
44 | LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_X_H_ADDR), 0, | |
45 | LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Y_L_ADDR), 0, | |
46 | LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Y_H_ADDR), 0, | |
47 | LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Z_L_ADDR), 0, | |
48 | LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Z_H_ADDR), 0, | |
49 | }; | |
50 | ||
51 | /** | |
52 | * lis3l02dq_read_all() Reads all channels currently selected | |
91b4171f | 53 | * @indio_dev: IIO device state |
25985edc | 54 | * @rx_array: (dma capable) receive array, must be at least |
14cd9a73 JC |
55 | * 4*number of channels |
56 | **/ | |
7b2fdd19 | 57 | static int lis3l02dq_read_all(struct iio_dev *indio_dev, u8 *rx_array) |
14cd9a73 | 58 | { |
7b2fdd19 | 59 | struct lis3l02dq_state *st = iio_priv(indio_dev); |
14cd9a73 JC |
60 | struct spi_transfer *xfers; |
61 | struct spi_message msg; | |
62 | int ret, i, j = 0; | |
63 | ||
550268ca JC |
64 | xfers = kcalloc(bitmap_weight(indio_dev->active_scan_mask, |
65 | indio_dev->masklength) * 2, | |
66 | sizeof(*xfers), GFP_KERNEL); | |
14cd9a73 JC |
67 | if (!xfers) |
68 | return -ENOMEM; | |
69 | ||
70 | mutex_lock(&st->buf_lock); | |
71 | ||
f3736416 | 72 | for (i = 0; i < ARRAY_SIZE(read_all_tx_array)/4; i++) |
550268ca | 73 | if (test_bit(i, indio_dev->active_scan_mask)) { |
14cd9a73 JC |
74 | /* lower byte */ |
75 | xfers[j].tx_buf = st->tx + 2*j; | |
76 | st->tx[2*j] = read_all_tx_array[i*4]; | |
77 | st->tx[2*j + 1] = 0; | |
78 | if (rx_array) | |
79 | xfers[j].rx_buf = rx_array + j*2; | |
80 | xfers[j].bits_per_word = 8; | |
81 | xfers[j].len = 2; | |
82 | xfers[j].cs_change = 1; | |
83 | j++; | |
84 | ||
85 | /* upper byte */ | |
86 | xfers[j].tx_buf = st->tx + 2*j; | |
87 | st->tx[2*j] = read_all_tx_array[i*4 + 2]; | |
88 | st->tx[2*j + 1] = 0; | |
89 | if (rx_array) | |
90 | xfers[j].rx_buf = rx_array + j*2; | |
91 | xfers[j].bits_per_word = 8; | |
92 | xfers[j].len = 2; | |
93 | xfers[j].cs_change = 1; | |
94 | j++; | |
95 | } | |
f3736416 | 96 | |
14cd9a73 JC |
97 | /* After these are transmitted, the rx_buff should have |
98 | * values in alternate bytes | |
99 | */ | |
100 | spi_message_init(&msg); | |
550268ca JC |
101 | for (j = 0; j < bitmap_weight(indio_dev->active_scan_mask, |
102 | indio_dev->masklength) * 2; j++) | |
14cd9a73 JC |
103 | spi_message_add_tail(&xfers[j], &msg); |
104 | ||
105 | ret = spi_sync(st->us, &msg); | |
106 | mutex_unlock(&st->buf_lock); | |
107 | kfree(xfers); | |
108 | ||
109 | return ret; | |
110 | } | |
111 | ||
6ddbb08a | 112 | static int lis3l02dq_get_buffer_element(struct iio_dev *indio_dev, |
9963bce7 | 113 | u8 *buf) |
73bce12e JC |
114 | { |
115 | int ret, i; | |
ffb283e1 | 116 | u8 *rx_array; |
73bce12e | 117 | s16 *data = (s16 *)buf; |
550268ca JC |
118 | int scan_count = bitmap_weight(indio_dev->active_scan_mask, |
119 | indio_dev->masklength); | |
14cd9a73 | 120 | |
3637d77f | 121 | rx_array = kcalloc(4, scan_count, GFP_KERNEL); |
aa5587f3 | 122 | if (!rx_array) |
73bce12e | 123 | return -ENOMEM; |
7b2fdd19 | 124 | ret = lis3l02dq_read_all(indio_dev, rx_array); |
d1dc9c12 PM |
125 | if (ret < 0) { |
126 | kfree(rx_array); | |
73bce12e | 127 | return ret; |
d1dc9c12 | 128 | } |
550268ca | 129 | for (i = 0; i < scan_count; i++) |
73bce12e JC |
130 | data[i] = combine_8_to_16(rx_array[i*4+1], |
131 | rx_array[i*4+3]); | |
14cd9a73 | 132 | kfree(rx_array); |
14cd9a73 | 133 | |
73bce12e | 134 | return i*sizeof(data[0]); |
14cd9a73 | 135 | } |
14cd9a73 | 136 | |
7b2fdd19 JC |
137 | static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p) |
138 | { | |
139 | struct iio_poll_func *pf = p; | |
e65bc6ac | 140 | struct iio_dev *indio_dev = pf->indio_dev; |
7b2fdd19 | 141 | int len = 0; |
420fe2e9 | 142 | char *data; |
7b2fdd19 | 143 | |
420fe2e9 | 144 | data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL); |
aa5587f3 | 145 | if (!data) |
0b30246e | 146 | goto done; |
7b2fdd19 | 147 | |
550268ca | 148 | if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) |
6ddbb08a | 149 | len = lis3l02dq_get_buffer_element(indio_dev, data); |
7b2fdd19 | 150 | |
fdc8020c | 151 | iio_push_to_buffers_with_timestamp(indio_dev, data, pf->timestamp); |
7b2fdd19 | 152 | |
7b2fdd19 | 153 | kfree(data); |
0b30246e LPC |
154 | done: |
155 | iio_trigger_notify_done(indio_dev->trig); | |
7b2fdd19 JC |
156 | return IRQ_HANDLED; |
157 | } | |
158 | ||
14cd9a73 | 159 | /* Caller responsible for locking as necessary. */ |
26de7208 | 160 | static int |
8dc5afd8 | 161 | __lis3l02dq_write_data_ready_config(struct iio_dev *indio_dev, bool state) |
14cd9a73 JC |
162 | { |
163 | int ret; | |
164 | u8 valold; | |
165 | bool currentlyset; | |
7b2fdd19 | 166 | struct lis3l02dq_state *st = iio_priv(indio_dev); |
14cd9a73 | 167 | |
91b4171f | 168 | /* Get the current event mask register */ |
1b076b52 | 169 | ret = lis3l02dq_spi_read_reg_8(indio_dev, |
14cd9a73 JC |
170 | LIS3L02DQ_REG_CTRL_2_ADDR, |
171 | &valold); | |
172 | if (ret) | |
173 | goto error_ret; | |
91b4171f | 174 | /* Find out if data ready is already on */ |
14cd9a73 JC |
175 | currentlyset |
176 | = valold & LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION; | |
177 | ||
91b4171f | 178 | /* Disable requested */ |
14cd9a73 | 179 | if (!state && currentlyset) { |
91b4171f | 180 | /* Disable the data ready signal */ |
14cd9a73 | 181 | valold &= ~LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION; |
1e3345bc | 182 | |
91b4171f | 183 | /* The double write is to overcome a hardware bug? */ |
1b076b52 | 184 | ret = lis3l02dq_spi_write_reg_8(indio_dev, |
14cd9a73 | 185 | LIS3L02DQ_REG_CTRL_2_ADDR, |
7df86302 | 186 | valold); |
14cd9a73 JC |
187 | if (ret) |
188 | goto error_ret; | |
1b076b52 | 189 | ret = lis3l02dq_spi_write_reg_8(indio_dev, |
14cd9a73 | 190 | LIS3L02DQ_REG_CTRL_2_ADDR, |
7df86302 | 191 | valold); |
14cd9a73 JC |
192 | if (ret) |
193 | goto error_ret; | |
d731aea0 | 194 | st->trigger_on = false; |
91b4171f | 195 | /* Enable requested */ |
14cd9a73 | 196 | } else if (state && !currentlyset) { |
91b4171f PM |
197 | /* If not set, enable requested |
198 | * first disable all events */ | |
1e3345bc JC |
199 | ret = lis3l02dq_disable_all_events(indio_dev); |
200 | if (ret < 0) | |
201 | goto error_ret; | |
202 | ||
203 | valold = ret | | |
204 | LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION; | |
1e3345bc | 205 | |
d731aea0 | 206 | st->trigger_on = true; |
1b076b52 | 207 | ret = lis3l02dq_spi_write_reg_8(indio_dev, |
14cd9a73 | 208 | LIS3L02DQ_REG_CTRL_2_ADDR, |
7df86302 | 209 | valold); |
d731aea0 | 210 | if (ret) |
14cd9a73 JC |
211 | goto error_ret; |
212 | } | |
213 | ||
214 | return 0; | |
215 | error_ret: | |
216 | return ret; | |
217 | } | |
218 | ||
219 | /** | |
220 | * lis3l02dq_data_rdy_trigger_set_state() set datardy interrupt state | |
221 | * | |
222 | * If disabling the interrupt also does a final read to ensure it is clear. | |
223 | * This is only important in some cases where the scan enable elements are | |
6ddbb08a | 224 | * switched before the buffer is reenabled. |
14cd9a73 JC |
225 | **/ |
226 | static int lis3l02dq_data_rdy_trigger_set_state(struct iio_trigger *trig, | |
227 | bool state) | |
228 | { | |
1e9663c6 | 229 | struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); |
14cd9a73 JC |
230 | int ret = 0; |
231 | u8 t; | |
1e3345bc | 232 | |
8dc5afd8 | 233 | __lis3l02dq_write_data_ready_config(indio_dev, state); |
6fae58f3 | 234 | if (!state) { |
d1dbf011 | 235 | /* |
4abf6f8b | 236 | * A possible quirk with the handler is currently worked around |
91b4171f | 237 | * by ensuring outstanding read events are cleared. |
d1dbf011 | 238 | */ |
7b2fdd19 | 239 | ret = lis3l02dq_read_all(indio_dev, NULL); |
14cd9a73 | 240 | } |
7b2fdd19 | 241 | lis3l02dq_spi_read_reg_8(indio_dev, |
14cd9a73 JC |
242 | LIS3L02DQ_REG_WAKE_UP_SRC_ADDR, |
243 | &t); | |
244 | return ret; | |
245 | } | |
9dbfb6f1 | 246 | |
14cd9a73 | 247 | /** |
91b4171f | 248 | * lis3l02dq_trig_try_reen() try reenabling irq for data rdy trigger |
14cd9a73 | 249 | * @trig: the datardy trigger |
d1dbf011 | 250 | */ |
14cd9a73 JC |
251 | static int lis3l02dq_trig_try_reen(struct iio_trigger *trig) |
252 | { | |
1e9663c6 | 253 | struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); |
7b2fdd19 | 254 | struct lis3l02dq_state *st = iio_priv(indio_dev); |
d1dbf011 JC |
255 | int i; |
256 | ||
91b4171f PM |
257 | /* If gpio still high (or high again) |
258 | * In theory possible we will need to do this several times */ | |
d1dbf011 | 259 | for (i = 0; i < 5; i++) |
65cb587d | 260 | if (gpio_get_value(st->gpio)) |
7b2fdd19 | 261 | lis3l02dq_read_all(indio_dev, NULL); |
d1dbf011 JC |
262 | else |
263 | break; | |
264 | if (i == 5) | |
b4a051d8 | 265 | pr_info("Failed to clear the interrupt for lis3l02dq\n"); |
d1dbf011 | 266 | |
14cd9a73 JC |
267 | /* irq reenabled so success! */ |
268 | return 0; | |
269 | } | |
270 | ||
d29f73db JC |
271 | static const struct iio_trigger_ops lis3l02dq_trigger_ops = { |
272 | .owner = THIS_MODULE, | |
273 | .set_trigger_state = &lis3l02dq_data_rdy_trigger_set_state, | |
274 | .try_reenable = &lis3l02dq_trig_try_reen, | |
275 | }; | |
276 | ||
14cd9a73 JC |
277 | int lis3l02dq_probe_trigger(struct iio_dev *indio_dev) |
278 | { | |
279 | int ret; | |
7b2fdd19 | 280 | struct lis3l02dq_state *st = iio_priv(indio_dev); |
14cd9a73 | 281 | |
7cbb7537 | 282 | st->trig = iio_trigger_alloc("lis3l02dq-dev%d", indio_dev->id); |
d1dbf011 JC |
283 | if (!st->trig) { |
284 | ret = -ENOMEM; | |
59c85e82 | 285 | goto error_ret; |
14cd9a73 | 286 | } |
3c9bbf58 | 287 | |
1b076b52 | 288 | st->trig->dev.parent = &st->us->dev; |
d29f73db | 289 | st->trig->ops = &lis3l02dq_trigger_ops; |
1e9663c6 | 290 | iio_trigger_set_drvdata(st->trig, indio_dev); |
1b076b52 | 291 | ret = iio_trigger_register(st->trig); |
14cd9a73 | 292 | if (ret) |
d1dbf011 | 293 | goto error_free_trig; |
14cd9a73 JC |
294 | |
295 | return 0; | |
296 | ||
14cd9a73 | 297 | error_free_trig: |
7cbb7537 | 298 | iio_trigger_free(st->trig); |
d1dbf011 | 299 | error_ret: |
14cd9a73 JC |
300 | return ret; |
301 | } | |
302 | ||
303 | void lis3l02dq_remove_trigger(struct iio_dev *indio_dev) | |
304 | { | |
7b2fdd19 | 305 | struct lis3l02dq_state *st = iio_priv(indio_dev); |
14cd9a73 | 306 | |
1b076b52 | 307 | iio_trigger_unregister(st->trig); |
7cbb7537 | 308 | iio_trigger_free(st->trig); |
14cd9a73 JC |
309 | } |
310 | ||
6ddbb08a | 311 | void lis3l02dq_unconfigure_buffer(struct iio_dev *indio_dev) |
14cd9a73 | 312 | { |
0ed731d2 | 313 | iio_dealloc_pollfunc(indio_dev->pollfunc); |
a819a0df | 314 | iio_kfifo_free(indio_dev->buffer); |
14cd9a73 JC |
315 | } |
316 | ||
6ddbb08a | 317 | static int lis3l02dq_buffer_postenable(struct iio_dev *indio_dev) |
f3736416 JC |
318 | { |
319 | /* Disable unwanted channels otherwise the interrupt will not clear */ | |
320 | u8 t; | |
321 | int ret; | |
322 | bool oneenabled = false; | |
323 | ||
1b076b52 | 324 | ret = lis3l02dq_spi_read_reg_8(indio_dev, |
f3736416 JC |
325 | LIS3L02DQ_REG_CTRL_1_ADDR, |
326 | &t); | |
327 | if (ret) | |
328 | goto error_ret; | |
329 | ||
550268ca | 330 | if (test_bit(0, indio_dev->active_scan_mask)) { |
f3736416 JC |
331 | t |= LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE; |
332 | oneenabled = true; | |
e273eb01 | 333 | } else { |
f3736416 | 334 | t &= ~LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE; |
e273eb01 | 335 | } |
550268ca | 336 | if (test_bit(1, indio_dev->active_scan_mask)) { |
f3736416 JC |
337 | t |= LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE; |
338 | oneenabled = true; | |
e273eb01 | 339 | } else { |
f3736416 | 340 | t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE; |
e273eb01 | 341 | } |
550268ca | 342 | if (test_bit(2, indio_dev->active_scan_mask)) { |
f3736416 JC |
343 | t |= LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE; |
344 | oneenabled = true; | |
e273eb01 | 345 | } else { |
f3736416 | 346 | t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE; |
e273eb01 | 347 | } |
f3736416 JC |
348 | if (!oneenabled) /* what happens in this case is unknown */ |
349 | return -EINVAL; | |
1b076b52 | 350 | ret = lis3l02dq_spi_write_reg_8(indio_dev, |
f3736416 | 351 | LIS3L02DQ_REG_CTRL_1_ADDR, |
7df86302 | 352 | t); |
f3736416 JC |
353 | if (ret) |
354 | goto error_ret; | |
355 | ||
3b99fb76 | 356 | return iio_triggered_buffer_postenable(indio_dev); |
f3736416 JC |
357 | error_ret: |
358 | return ret; | |
359 | } | |
360 | ||
361 | /* Turn all channels on again */ | |
6ddbb08a | 362 | static int lis3l02dq_buffer_predisable(struct iio_dev *indio_dev) |
f3736416 JC |
363 | { |
364 | u8 t; | |
365 | int ret; | |
366 | ||
3b99fb76 | 367 | ret = iio_triggered_buffer_predisable(indio_dev); |
f3736416 JC |
368 | if (ret) |
369 | goto error_ret; | |
370 | ||
1b076b52 | 371 | ret = lis3l02dq_spi_read_reg_8(indio_dev, |
f3736416 JC |
372 | LIS3L02DQ_REG_CTRL_1_ADDR, |
373 | &t); | |
374 | if (ret) | |
375 | goto error_ret; | |
376 | t |= LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE | | |
377 | LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE | | |
378 | LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE; | |
379 | ||
1b076b52 | 380 | ret = lis3l02dq_spi_write_reg_8(indio_dev, |
f3736416 | 381 | LIS3L02DQ_REG_CTRL_1_ADDR, |
7df86302 | 382 | t); |
f3736416 JC |
383 | |
384 | error_ret: | |
385 | return ret; | |
386 | } | |
387 | ||
6ddbb08a | 388 | static const struct iio_buffer_setup_ops lis3l02dq_buffer_setup_ops = { |
6ddbb08a JC |
389 | .postenable = &lis3l02dq_buffer_postenable, |
390 | .predisable = &lis3l02dq_buffer_predisable, | |
5565a450 | 391 | }; |
f3736416 | 392 | |
6ddbb08a | 393 | int lis3l02dq_configure_buffer(struct iio_dev *indio_dev) |
14cd9a73 | 394 | { |
73bce12e | 395 | int ret; |
6ddbb08a | 396 | struct iio_buffer *buffer; |
d1dbf011 | 397 | |
7ab374a0 | 398 | buffer = iio_kfifo_allocate(); |
6ddbb08a | 399 | if (!buffer) |
73bce12e JC |
400 | return -ENOMEM; |
401 | ||
9e69c935 | 402 | iio_device_attach_buffer(indio_dev, buffer); |
f3736416 | 403 | |
6ddbb08a | 404 | buffer->scan_timestamp = true; |
1612244f | 405 | indio_dev->setup_ops = &lis3l02dq_buffer_setup_ops; |
14cd9a73 | 406 | |
d1dbf011 | 407 | /* Functions are NULL as we set handler below */ |
0ed731d2 JC |
408 | indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time, |
409 | &lis3l02dq_trigger_handler, | |
410 | 0, | |
411 | indio_dev, | |
412 | "lis3l02dq_consumer%d", | |
413 | indio_dev->id); | |
d1dbf011 | 414 | |
aa5587f3 | 415 | if (!indio_dev->pollfunc) { |
d1dbf011 | 416 | ret = -ENOMEM; |
859171ca | 417 | goto error_iio_sw_rb_free; |
d1dbf011 | 418 | } |
d1dbf011 | 419 | |
ec3afa40 | 420 | indio_dev->modes |= INDIO_BUFFER_TRIGGERED; |
14cd9a73 JC |
421 | return 0; |
422 | ||
423 | error_iio_sw_rb_free: | |
a819a0df | 424 | iio_kfifo_free(indio_dev->buffer); |
14cd9a73 JC |
425 | return ret; |
426 | } |