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