blob: 096a6bfe0cdfadf5d49873c6464c86199ee477d3 [file] [log] [blame]
Jonathan Cameron14555b12011-09-21 11:15:57 +01001/* The industrial I/O core
2 *
3 * Copyright (c) 2008 Jonathan Cameron
4 *
5 * This program is free software; you can redistribute it and/or modify it
6 * under the terms of the GNU General Public License version 2 as published by
7 * the Free Software Foundation.
8 *
9 * Handling of buffer allocation / resizing.
10 *
11 *
12 * Things to look at here.
13 * - Better memory allocation techniques?
14 * - Alternative access techniques?
15 */
16#include <linux/kernel.h>
Paul Gortmaker8e336a72011-07-10 13:09:12 -040017#include <linux/export.h>
Jonathan Cameron14555b12011-09-21 11:15:57 +010018#include <linux/device.h>
19#include <linux/fs.h>
20#include <linux/cdev.h>
21#include <linux/slab.h>
22#include <linux/poll.h>
23
Jonathan Cameron06458e22012-04-25 15:54:58 +010024#include <linux/iio/iio.h>
Jonathan Cameron14555b12011-09-21 11:15:57 +010025#include "iio_core.h"
Jonathan Cameron06458e22012-04-25 15:54:58 +010026#include <linux/iio/sysfs.h>
27#include <linux/iio/buffer.h>
Jonathan Cameron14555b12011-09-21 11:15:57 +010028
29static const char * const iio_endian_prefix[] = {
30 [IIO_BE] = "be",
31 [IIO_LE] = "le",
32};
33
34/**
35 * iio_buffer_read_first_n_outer() - chrdev read for buffer access
36 *
37 * This function relies on all buffer implementations having an
38 * iio_buffer as their first element.
39 **/
40ssize_t iio_buffer_read_first_n_outer(struct file *filp, char __user *buf,
41 size_t n, loff_t *f_ps)
42{
43 struct iio_dev *indio_dev = filp->private_data;
44 struct iio_buffer *rb = indio_dev->buffer;
45
Jonathan Cameron96e00f12011-10-26 17:27:45 +010046 if (!rb || !rb->access->read_first_n)
Jonathan Cameron14555b12011-09-21 11:15:57 +010047 return -EINVAL;
48 return rb->access->read_first_n(rb, n, buf);
49}
50
51/**
52 * iio_buffer_poll() - poll the buffer to find out if it has data
53 */
54unsigned int iio_buffer_poll(struct file *filp,
55 struct poll_table_struct *wait)
56{
57 struct iio_dev *indio_dev = filp->private_data;
58 struct iio_buffer *rb = indio_dev->buffer;
59
60 poll_wait(filp, &rb->pollq, wait);
61 if (rb->stufftoread)
62 return POLLIN | POLLRDNORM;
63 /* need a way of knowing if there may be enough data... */
64 return 0;
65}
66
Jonathan Cameronf79a9092011-12-05 22:18:29 +000067void iio_buffer_init(struct iio_buffer *buffer)
Jonathan Cameron14555b12011-09-21 11:15:57 +010068{
Jonathan Cameron5ada4ea2011-12-05 21:37:14 +000069 INIT_LIST_HEAD(&buffer->demux_list);
Jonathan Cameron14555b12011-09-21 11:15:57 +010070 init_waitqueue_head(&buffer->pollq);
71}
72EXPORT_SYMBOL(iio_buffer_init);
73
74static ssize_t iio_show_scan_index(struct device *dev,
75 struct device_attribute *attr,
76 char *buf)
77{
78 return sprintf(buf, "%u\n", to_iio_dev_attr(attr)->c->scan_index);
79}
80
81static ssize_t iio_show_fixed_type(struct device *dev,
82 struct device_attribute *attr,
83 char *buf)
84{
85 struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
86 u8 type = this_attr->c->scan_type.endianness;
87
88 if (type == IIO_CPU) {
Jonathan Cameron9d5d1152011-10-04 16:02:08 +010089#ifdef __LITTLE_ENDIAN
90 type = IIO_LE;
91#else
92 type = IIO_BE;
93#endif
Jonathan Cameron14555b12011-09-21 11:15:57 +010094 }
95 return sprintf(buf, "%s:%c%d/%d>>%u\n",
96 iio_endian_prefix[type],
97 this_attr->c->scan_type.sign,
98 this_attr->c->scan_type.realbits,
99 this_attr->c->scan_type.storagebits,
100 this_attr->c->scan_type.shift);
101}
102
103static ssize_t iio_scan_el_show(struct device *dev,
104 struct device_attribute *attr,
105 char *buf)
106{
107 int ret;
Lars-Peter Clausene53f5ac2012-05-12 15:39:33 +0200108 struct iio_dev *indio_dev = dev_to_iio_dev(dev);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100109
Jonathan Cameron5ada4ea2011-12-05 21:37:14 +0000110 ret = test_bit(to_iio_dev_attr(attr)->address,
111 indio_dev->buffer->scan_mask);
112
Jonathan Cameron14555b12011-09-21 11:15:57 +0100113 return sprintf(buf, "%d\n", ret);
114}
115
116static int iio_scan_mask_clear(struct iio_buffer *buffer, int bit)
117{
118 clear_bit(bit, buffer->scan_mask);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100119 return 0;
120}
121
122static ssize_t iio_scan_el_store(struct device *dev,
123 struct device_attribute *attr,
124 const char *buf,
125 size_t len)
126{
Jonathan Camerona714af22012-04-21 10:09:32 +0100127 int ret;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100128 bool state;
Lars-Peter Clausene53f5ac2012-05-12 15:39:33 +0200129 struct iio_dev *indio_dev = dev_to_iio_dev(dev);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100130 struct iio_buffer *buffer = indio_dev->buffer;
131 struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
132
Jonathan Camerona714af22012-04-21 10:09:32 +0100133 ret = strtobool(buf, &state);
134 if (ret < 0)
135 return ret;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100136 mutex_lock(&indio_dev->mlock);
Lars-Peter Clausen5fd62182011-12-19 15:23:43 +0100137 if (iio_buffer_enabled(indio_dev)) {
Jonathan Cameron14555b12011-09-21 11:15:57 +0100138 ret = -EBUSY;
139 goto error_ret;
140 }
Jonathan Cameronf79a9092011-12-05 22:18:29 +0000141 ret = iio_scan_mask_query(indio_dev, buffer, this_attr->address);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100142 if (ret < 0)
143 goto error_ret;
144 if (!state && ret) {
145 ret = iio_scan_mask_clear(buffer, this_attr->address);
146 if (ret)
147 goto error_ret;
148 } else if (state && !ret) {
Jonathan Cameronf79a9092011-12-05 22:18:29 +0000149 ret = iio_scan_mask_set(indio_dev, buffer, this_attr->address);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100150 if (ret)
151 goto error_ret;
152 }
153
154error_ret:
155 mutex_unlock(&indio_dev->mlock);
156
Lars-Peter Clausen5a2a6e12011-12-08 18:35:53 +0100157 return ret < 0 ? ret : len;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100158
159}
160
161static ssize_t iio_scan_el_ts_show(struct device *dev,
162 struct device_attribute *attr,
163 char *buf)
164{
Lars-Peter Clausene53f5ac2012-05-12 15:39:33 +0200165 struct iio_dev *indio_dev = dev_to_iio_dev(dev);
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100166 return sprintf(buf, "%d\n", indio_dev->buffer->scan_timestamp);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100167}
168
169static ssize_t iio_scan_el_ts_store(struct device *dev,
170 struct device_attribute *attr,
171 const char *buf,
172 size_t len)
173{
Jonathan Camerona714af22012-04-21 10:09:32 +0100174 int ret;
Lars-Peter Clausene53f5ac2012-05-12 15:39:33 +0200175 struct iio_dev *indio_dev = dev_to_iio_dev(dev);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100176 bool state;
177
Jonathan Camerona714af22012-04-21 10:09:32 +0100178 ret = strtobool(buf, &state);
179 if (ret < 0)
180 return ret;
181
Jonathan Cameron14555b12011-09-21 11:15:57 +0100182 mutex_lock(&indio_dev->mlock);
Lars-Peter Clausen5fd62182011-12-19 15:23:43 +0100183 if (iio_buffer_enabled(indio_dev)) {
Jonathan Cameron14555b12011-09-21 11:15:57 +0100184 ret = -EBUSY;
185 goto error_ret;
186 }
187 indio_dev->buffer->scan_timestamp = state;
Jonathan Cameronfd6487f2012-04-21 10:09:44 +0100188 indio_dev->scan_timestamp = state;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100189error_ret:
190 mutex_unlock(&indio_dev->mlock);
191
192 return ret ? ret : len;
193}
194
195static int iio_buffer_add_channel_sysfs(struct iio_dev *indio_dev,
196 const struct iio_chan_spec *chan)
197{
198 int ret, attrcount = 0;
199 struct iio_buffer *buffer = indio_dev->buffer;
200
201 ret = __iio_add_chan_devattr("index",
202 chan,
203 &iio_show_scan_index,
204 NULL,
205 0,
206 0,
207 &indio_dev->dev,
208 &buffer->scan_el_dev_attr_list);
209 if (ret)
210 goto error_ret;
211 attrcount++;
212 ret = __iio_add_chan_devattr("type",
213 chan,
214 &iio_show_fixed_type,
215 NULL,
216 0,
217 0,
218 &indio_dev->dev,
219 &buffer->scan_el_dev_attr_list);
220 if (ret)
221 goto error_ret;
222 attrcount++;
223 if (chan->type != IIO_TIMESTAMP)
224 ret = __iio_add_chan_devattr("en",
225 chan,
226 &iio_scan_el_show,
227 &iio_scan_el_store,
228 chan->scan_index,
229 0,
230 &indio_dev->dev,
231 &buffer->scan_el_dev_attr_list);
232 else
233 ret = __iio_add_chan_devattr("en",
234 chan,
235 &iio_scan_el_ts_show,
236 &iio_scan_el_ts_store,
237 chan->scan_index,
238 0,
239 &indio_dev->dev,
240 &buffer->scan_el_dev_attr_list);
241 attrcount++;
242 ret = attrcount;
243error_ret:
244 return ret;
245}
246
247static void iio_buffer_remove_and_free_scan_dev_attr(struct iio_dev *indio_dev,
248 struct iio_dev_attr *p)
249{
250 kfree(p->dev_attr.attr.name);
251 kfree(p);
252}
253
254static void __iio_buffer_attr_cleanup(struct iio_dev *indio_dev)
255{
256 struct iio_dev_attr *p, *n;
257 struct iio_buffer *buffer = indio_dev->buffer;
258
259 list_for_each_entry_safe(p, n,
260 &buffer->scan_el_dev_attr_list, l)
261 iio_buffer_remove_and_free_scan_dev_attr(indio_dev, p);
262}
263
264static const char * const iio_scan_elements_group_name = "scan_elements";
265
266int iio_buffer_register(struct iio_dev *indio_dev,
267 const struct iio_chan_spec *channels,
268 int num_channels)
269{
270 struct iio_dev_attr *p;
271 struct attribute **attr;
272 struct iio_buffer *buffer = indio_dev->buffer;
273 int ret, i, attrn, attrcount, attrcount_orig = 0;
274
275 if (buffer->attrs)
276 indio_dev->groups[indio_dev->groupcounter++] = buffer->attrs;
277
278 if (buffer->scan_el_attrs != NULL) {
279 attr = buffer->scan_el_attrs->attrs;
280 while (*attr++ != NULL)
281 attrcount_orig++;
282 }
283 attrcount = attrcount_orig;
284 INIT_LIST_HEAD(&buffer->scan_el_dev_attr_list);
285 if (channels) {
286 /* new magic */
287 for (i = 0; i < num_channels; i++) {
Lars-Peter Clausenf5b81dd2012-06-18 18:33:47 +0200288 if (channels[i].scan_index < 0)
289 continue;
290
Jonathan Cameron14555b12011-09-21 11:15:57 +0100291 /* Establish necessary mask length */
292 if (channels[i].scan_index >
293 (int)indio_dev->masklength - 1)
294 indio_dev->masklength
Lars-Peter Clausene1dc7be2012-07-02 14:52:56 +0200295 = channels[i].scan_index + 1;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100296
297 ret = iio_buffer_add_channel_sysfs(indio_dev,
298 &channels[i]);
299 if (ret < 0)
300 goto error_cleanup_dynamic;
301 attrcount += ret;
Jonathan Cameronbeb80602011-12-05 21:37:11 +0000302 if (channels[i].type == IIO_TIMESTAMP)
Jonathan Cameronf1264802012-04-21 10:09:34 +0100303 indio_dev->scan_index_timestamp =
Jonathan Cameronbeb80602011-12-05 21:37:11 +0000304 channels[i].scan_index;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100305 }
306 if (indio_dev->masklength && buffer->scan_mask == NULL) {
Thomas Meyerd83fb182011-11-29 22:08:00 +0100307 buffer->scan_mask = kcalloc(BITS_TO_LONGS(indio_dev->masklength),
308 sizeof(*buffer->scan_mask),
309 GFP_KERNEL);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100310 if (buffer->scan_mask == NULL) {
311 ret = -ENOMEM;
312 goto error_cleanup_dynamic;
313 }
314 }
315 }
316
317 buffer->scan_el_group.name = iio_scan_elements_group_name;
318
Thomas Meyerd83fb182011-11-29 22:08:00 +0100319 buffer->scan_el_group.attrs = kcalloc(attrcount + 1,
320 sizeof(buffer->scan_el_group.attrs[0]),
321 GFP_KERNEL);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100322 if (buffer->scan_el_group.attrs == NULL) {
323 ret = -ENOMEM;
324 goto error_free_scan_mask;
325 }
326 if (buffer->scan_el_attrs)
327 memcpy(buffer->scan_el_group.attrs, buffer->scan_el_attrs,
328 sizeof(buffer->scan_el_group.attrs[0])*attrcount_orig);
329 attrn = attrcount_orig;
330
331 list_for_each_entry(p, &buffer->scan_el_dev_attr_list, l)
332 buffer->scan_el_group.attrs[attrn++] = &p->dev_attr.attr;
333 indio_dev->groups[indio_dev->groupcounter++] = &buffer->scan_el_group;
334
335 return 0;
336
337error_free_scan_mask:
338 kfree(buffer->scan_mask);
339error_cleanup_dynamic:
340 __iio_buffer_attr_cleanup(indio_dev);
341
342 return ret;
343}
344EXPORT_SYMBOL(iio_buffer_register);
345
346void iio_buffer_unregister(struct iio_dev *indio_dev)
347{
348 kfree(indio_dev->buffer->scan_mask);
349 kfree(indio_dev->buffer->scan_el_group.attrs);
350 __iio_buffer_attr_cleanup(indio_dev);
351}
352EXPORT_SYMBOL(iio_buffer_unregister);
353
354ssize_t iio_buffer_read_length(struct device *dev,
355 struct device_attribute *attr,
356 char *buf)
357{
Lars-Peter Clausene53f5ac2012-05-12 15:39:33 +0200358 struct iio_dev *indio_dev = dev_to_iio_dev(dev);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100359 struct iio_buffer *buffer = indio_dev->buffer;
360
361 if (buffer->access->get_length)
362 return sprintf(buf, "%d\n",
363 buffer->access->get_length(buffer));
364
365 return 0;
366}
367EXPORT_SYMBOL(iio_buffer_read_length);
368
369ssize_t iio_buffer_write_length(struct device *dev,
370 struct device_attribute *attr,
371 const char *buf,
372 size_t len)
373{
374 int ret;
375 ulong val;
Lars-Peter Clausene53f5ac2012-05-12 15:39:33 +0200376 struct iio_dev *indio_dev = dev_to_iio_dev(dev);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100377 struct iio_buffer *buffer = indio_dev->buffer;
378
379 ret = strict_strtoul(buf, 10, &val);
380 if (ret)
381 return ret;
382
383 if (buffer->access->get_length)
384 if (val == buffer->access->get_length(buffer))
385 return len;
386
Lars-Peter Clausene38c79e2011-12-19 15:23:44 +0100387 mutex_lock(&indio_dev->mlock);
388 if (iio_buffer_enabled(indio_dev)) {
389 ret = -EBUSY;
390 } else {
Lars-Peter Clausen869871b2011-12-19 15:23:48 +0100391 if (buffer->access->set_length)
Lars-Peter Clausene38c79e2011-12-19 15:23:44 +0100392 buffer->access->set_length(buffer, val);
Lars-Peter Clausene38c79e2011-12-19 15:23:44 +0100393 ret = 0;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100394 }
Lars-Peter Clausene38c79e2011-12-19 15:23:44 +0100395 mutex_unlock(&indio_dev->mlock);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100396
Lars-Peter Clausene38c79e2011-12-19 15:23:44 +0100397 return ret ? ret : len;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100398}
399EXPORT_SYMBOL(iio_buffer_write_length);
400
Jonathan Cameron14555b12011-09-21 11:15:57 +0100401ssize_t iio_buffer_store_enable(struct device *dev,
402 struct device_attribute *attr,
403 const char *buf,
404 size_t len)
405{
406 int ret;
407 bool requested_state, current_state;
408 int previous_mode;
Lars-Peter Clausene53f5ac2012-05-12 15:39:33 +0200409 struct iio_dev *indio_dev = dev_to_iio_dev(dev);
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100410 struct iio_buffer *buffer = indio_dev->buffer;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100411
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100412 mutex_lock(&indio_dev->mlock);
413 previous_mode = indio_dev->currentmode;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100414 requested_state = !(buf[0] == '0');
Lars-Peter Clausend4a68822011-12-19 15:23:42 +0100415 current_state = iio_buffer_enabled(indio_dev);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100416 if (current_state == requested_state) {
417 printk(KERN_INFO "iio-buffer, current state requested again\n");
418 goto done;
419 }
420 if (requested_state) {
Jonathan Cameron16122442011-12-05 22:18:14 +0000421 if (indio_dev->setup_ops->preenable) {
422 ret = indio_dev->setup_ops->preenable(indio_dev);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100423 if (ret) {
424 printk(KERN_ERR
425 "Buffer not started:"
426 "buffer preenable failed\n");
427 goto error_ret;
428 }
429 }
430 if (buffer->access->request_update) {
431 ret = buffer->access->request_update(buffer);
432 if (ret) {
433 printk(KERN_INFO
434 "Buffer not started:"
435 "buffer parameter update failed\n");
436 goto error_ret;
437 }
438 }
Jonathan Cameron14555b12011-09-21 11:15:57 +0100439 /* Definitely possible for devices to support both of these.*/
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100440 if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) {
441 if (!indio_dev->trig) {
Jonathan Cameron14555b12011-09-21 11:15:57 +0100442 printk(KERN_INFO
443 "Buffer not started: no trigger\n");
444 ret = -EINVAL;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100445 goto error_ret;
446 }
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100447 indio_dev->currentmode = INDIO_BUFFER_TRIGGERED;
448 } else if (indio_dev->modes & INDIO_BUFFER_HARDWARE)
449 indio_dev->currentmode = INDIO_BUFFER_HARDWARE;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100450 else { /* should never be reached */
451 ret = -EINVAL;
452 goto error_ret;
453 }
454
Jonathan Cameron16122442011-12-05 22:18:14 +0000455 if (indio_dev->setup_ops->postenable) {
456 ret = indio_dev->setup_ops->postenable(indio_dev);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100457 if (ret) {
458 printk(KERN_INFO
459 "Buffer not started:"
460 "postenable failed\n");
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100461 indio_dev->currentmode = previous_mode;
Jonathan Cameron16122442011-12-05 22:18:14 +0000462 if (indio_dev->setup_ops->postdisable)
463 indio_dev->setup_ops->
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100464 postdisable(indio_dev);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100465 goto error_ret;
466 }
467 }
468 } else {
Jonathan Cameron16122442011-12-05 22:18:14 +0000469 if (indio_dev->setup_ops->predisable) {
470 ret = indio_dev->setup_ops->predisable(indio_dev);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100471 if (ret)
472 goto error_ret;
473 }
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100474 indio_dev->currentmode = INDIO_DIRECT_MODE;
Jonathan Cameron16122442011-12-05 22:18:14 +0000475 if (indio_dev->setup_ops->postdisable) {
476 ret = indio_dev->setup_ops->postdisable(indio_dev);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100477 if (ret)
478 goto error_ret;
479 }
480 }
481done:
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100482 mutex_unlock(&indio_dev->mlock);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100483 return len;
484
485error_ret:
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100486 mutex_unlock(&indio_dev->mlock);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100487 return ret;
488}
489EXPORT_SYMBOL(iio_buffer_store_enable);
490
491ssize_t iio_buffer_show_enable(struct device *dev,
492 struct device_attribute *attr,
493 char *buf)
494{
Lars-Peter Clausene53f5ac2012-05-12 15:39:33 +0200495 struct iio_dev *indio_dev = dev_to_iio_dev(dev);
Lars-Peter Clausend4a68822011-12-19 15:23:42 +0100496 return sprintf(buf, "%d\n", iio_buffer_enabled(indio_dev));
Jonathan Cameron14555b12011-09-21 11:15:57 +0100497}
498EXPORT_SYMBOL(iio_buffer_show_enable);
499
Jonathan Cameron14555b12011-09-21 11:15:57 +0100500/* note NULL used as error indicator as it doesn't make sense. */
Michael Hennerichcd4361c2012-02-22 13:16:49 +0100501static const unsigned long *iio_scan_mask_match(const unsigned long *av_masks,
Jonathan Cameron14555b12011-09-21 11:15:57 +0100502 unsigned int masklength,
Michael Hennerichcd4361c2012-02-22 13:16:49 +0100503 const unsigned long *mask)
Jonathan Cameron14555b12011-09-21 11:15:57 +0100504{
505 if (bitmap_empty(mask, masklength))
506 return NULL;
507 while (*av_masks) {
508 if (bitmap_subset(mask, av_masks, masklength))
509 return av_masks;
510 av_masks += BITS_TO_LONGS(masklength);
511 }
512 return NULL;
513}
514
Jonathan Cameron6b3b58e2012-04-21 10:09:33 +0100515static int iio_compute_scan_bytes(struct iio_dev *indio_dev, const long *mask,
516 bool timestamp)
Jonathan Cameron959d2952011-12-05 21:37:13 +0000517{
Jonathan Cameron959d2952011-12-05 21:37:13 +0000518 const struct iio_chan_spec *ch;
519 unsigned bytes = 0;
520 int length, i;
Jonathan Cameron6b3b58e2012-04-21 10:09:33 +0100521
522 /* How much space will the demuxed element take? */
523 for_each_set_bit(i, mask,
524 indio_dev->masklength) {
525 ch = iio_find_channel_from_si(indio_dev, i);
526 length = ch->scan_type.storagebits / 8;
527 bytes = ALIGN(bytes, length);
528 bytes += length;
529 }
530 if (timestamp) {
531 ch = iio_find_channel_from_si(indio_dev,
Jonathan Cameronf1264802012-04-21 10:09:34 +0100532 indio_dev->scan_index_timestamp);
Jonathan Cameron6b3b58e2012-04-21 10:09:33 +0100533 length = ch->scan_type.storagebits / 8;
534 bytes = ALIGN(bytes, length);
535 bytes += length;
536 }
537 return bytes;
538}
539
540int iio_sw_buffer_preenable(struct iio_dev *indio_dev)
541{
542 struct iio_buffer *buffer = indio_dev->buffer;
Jonathan Cameron959d2952011-12-05 21:37:13 +0000543 dev_dbg(&indio_dev->dev, "%s\n", __func__);
544
545 /* How much space will the demuxed element take? */
Jonathan Cameron420fe2e2012-04-21 10:09:35 +0100546 indio_dev->scan_bytes =
547 iio_compute_scan_bytes(indio_dev, buffer->scan_mask,
Jonathan Cameron6b3b58e2012-04-21 10:09:33 +0100548 buffer->scan_timestamp);
Jonathan Cameron420fe2e2012-04-21 10:09:35 +0100549 buffer->access->set_bytes_per_datum(buffer, indio_dev->scan_bytes);
Jonathan Cameron959d2952011-12-05 21:37:13 +0000550
551 /* What scan mask do we actually have ?*/
552 if (indio_dev->available_scan_masks)
553 indio_dev->active_scan_mask =
554 iio_scan_mask_match(indio_dev->available_scan_masks,
555 indio_dev->masklength,
556 buffer->scan_mask);
557 else
558 indio_dev->active_scan_mask = buffer->scan_mask;
Lars-Peter Clausenaff1eb42012-06-15 18:08:59 +0200559
560 if (indio_dev->active_scan_mask == NULL)
561 return -EINVAL;
562
Jonathan Cameron5ada4ea2011-12-05 21:37:14 +0000563 iio_update_demux(indio_dev);
564
565 if (indio_dev->info->update_scan_mode)
566 return indio_dev->info
567 ->update_scan_mode(indio_dev,
568 indio_dev->active_scan_mask);
Jonathan Cameron959d2952011-12-05 21:37:13 +0000569 return 0;
570}
571EXPORT_SYMBOL(iio_sw_buffer_preenable);
572
Jonathan Cameron14555b12011-09-21 11:15:57 +0100573/**
574 * iio_scan_mask_set() - set particular bit in the scan mask
575 * @buffer: the buffer whose scan mask we are interested in
576 * @bit: the bit to be set.
577 **/
Jonathan Cameronf79a9092011-12-05 22:18:29 +0000578int iio_scan_mask_set(struct iio_dev *indio_dev,
579 struct iio_buffer *buffer, int bit)
Jonathan Cameron14555b12011-09-21 11:15:57 +0100580{
Michael Hennerichcd4361c2012-02-22 13:16:49 +0100581 const unsigned long *mask;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100582 unsigned long *trialmask;
583
584 trialmask = kmalloc(sizeof(*trialmask)*
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100585 BITS_TO_LONGS(indio_dev->masklength),
Jonathan Cameron14555b12011-09-21 11:15:57 +0100586 GFP_KERNEL);
587
588 if (trialmask == NULL)
589 return -ENOMEM;
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100590 if (!indio_dev->masklength) {
Jonathan Cameron14555b12011-09-21 11:15:57 +0100591 WARN_ON("trying to set scanmask prior to registering buffer\n");
592 kfree(trialmask);
593 return -EINVAL;
594 }
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100595 bitmap_copy(trialmask, buffer->scan_mask, indio_dev->masklength);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100596 set_bit(bit, trialmask);
597
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100598 if (indio_dev->available_scan_masks) {
599 mask = iio_scan_mask_match(indio_dev->available_scan_masks,
600 indio_dev->masklength,
Jonathan Cameron14555b12011-09-21 11:15:57 +0100601 trialmask);
602 if (!mask) {
603 kfree(trialmask);
604 return -EINVAL;
605 }
606 }
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100607 bitmap_copy(buffer->scan_mask, trialmask, indio_dev->masklength);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100608
609 kfree(trialmask);
610
611 return 0;
612};
613EXPORT_SYMBOL_GPL(iio_scan_mask_set);
614
Jonathan Cameronf79a9092011-12-05 22:18:29 +0000615int iio_scan_mask_query(struct iio_dev *indio_dev,
616 struct iio_buffer *buffer, int bit)
Jonathan Cameron14555b12011-09-21 11:15:57 +0100617{
Jonathan Cameronf8c6f4e2011-10-06 17:14:35 +0100618 if (bit > indio_dev->masklength)
Jonathan Cameron14555b12011-09-21 11:15:57 +0100619 return -EINVAL;
620
621 if (!buffer->scan_mask)
622 return 0;
Jonathan Cameron14555b12011-09-21 11:15:57 +0100623
Lars-Peter Clausen5a2a6e12011-12-08 18:35:53 +0100624 return test_bit(bit, buffer->scan_mask);
Jonathan Cameron14555b12011-09-21 11:15:57 +0100625};
626EXPORT_SYMBOL_GPL(iio_scan_mask_query);
Jonathan Cameron5ada4ea2011-12-05 21:37:14 +0000627
628/**
629 * struct iio_demux_table() - table describing demux memcpy ops
630 * @from: index to copy from
631 * @to: index to copy to
632 * @length: how many bytes to copy
633 * @l: list head used for management
634 */
635struct iio_demux_table {
636 unsigned from;
637 unsigned to;
638 unsigned length;
639 struct list_head l;
640};
641
642static unsigned char *iio_demux(struct iio_buffer *buffer,
643 unsigned char *datain)
644{
645 struct iio_demux_table *t;
646
647 if (list_empty(&buffer->demux_list))
648 return datain;
649 list_for_each_entry(t, &buffer->demux_list, l)
650 memcpy(buffer->demux_bounce + t->to,
651 datain + t->from, t->length);
652
653 return buffer->demux_bounce;
654}
655
656int iio_push_to_buffer(struct iio_buffer *buffer, unsigned char *data,
657 s64 timestamp)
658{
659 unsigned char *dataout = iio_demux(buffer, data);
660
661 return buffer->access->store_to(buffer, dataout, timestamp);
662}
663EXPORT_SYMBOL_GPL(iio_push_to_buffer);
664
Jonathan Cameron842cd102012-04-21 10:09:45 +0100665static void iio_buffer_demux_free(struct iio_buffer *buffer)
666{
667 struct iio_demux_table *p, *q;
668 list_for_each_entry_safe(p, q, &buffer->demux_list, l) {
669 list_del(&p->l);
670 kfree(p);
671 }
672}
673
Jonathan Cameron5ada4ea2011-12-05 21:37:14 +0000674int iio_update_demux(struct iio_dev *indio_dev)
675{
676 const struct iio_chan_spec *ch;
677 struct iio_buffer *buffer = indio_dev->buffer;
678 int ret, in_ind = -1, out_ind, length;
679 unsigned in_loc = 0, out_loc = 0;
Jonathan Cameron842cd102012-04-21 10:09:45 +0100680 struct iio_demux_table *p;
Jonathan Cameron5ada4ea2011-12-05 21:37:14 +0000681
682 /* Clear out any old demux */
Jonathan Cameron842cd102012-04-21 10:09:45 +0100683 iio_buffer_demux_free(buffer);
Jonathan Cameron5ada4ea2011-12-05 21:37:14 +0000684 kfree(buffer->demux_bounce);
685 buffer->demux_bounce = NULL;
686
687 /* First work out which scan mode we will actually have */
688 if (bitmap_equal(indio_dev->active_scan_mask,
689 buffer->scan_mask,
690 indio_dev->masklength))
691 return 0;
692
693 /* Now we have the two masks, work from least sig and build up sizes */
694 for_each_set_bit(out_ind,
695 indio_dev->active_scan_mask,
696 indio_dev->masklength) {
697 in_ind = find_next_bit(indio_dev->active_scan_mask,
698 indio_dev->masklength,
699 in_ind + 1);
700 while (in_ind != out_ind) {
701 in_ind = find_next_bit(indio_dev->active_scan_mask,
702 indio_dev->masklength,
703 in_ind + 1);
704 ch = iio_find_channel_from_si(indio_dev, in_ind);
705 length = ch->scan_type.storagebits/8;
706 /* Make sure we are aligned */
707 in_loc += length;
708 if (in_loc % length)
709 in_loc += length - in_loc % length;
710 }
711 p = kmalloc(sizeof(*p), GFP_KERNEL);
712 if (p == NULL) {
713 ret = -ENOMEM;
714 goto error_clear_mux_table;
715 }
716 ch = iio_find_channel_from_si(indio_dev, in_ind);
717 length = ch->scan_type.storagebits/8;
718 if (out_loc % length)
719 out_loc += length - out_loc % length;
720 if (in_loc % length)
721 in_loc += length - in_loc % length;
722 p->from = in_loc;
723 p->to = out_loc;
724 p->length = length;
725 list_add_tail(&p->l, &buffer->demux_list);
726 out_loc += length;
727 in_loc += length;
728 }
729 /* Relies on scan_timestamp being last */
730 if (buffer->scan_timestamp) {
731 p = kmalloc(sizeof(*p), GFP_KERNEL);
732 if (p == NULL) {
733 ret = -ENOMEM;
734 goto error_clear_mux_table;
735 }
736 ch = iio_find_channel_from_si(indio_dev,
Jonathan Cameronf1264802012-04-21 10:09:34 +0100737 indio_dev->scan_index_timestamp);
Jonathan Cameron5ada4ea2011-12-05 21:37:14 +0000738 length = ch->scan_type.storagebits/8;
739 if (out_loc % length)
740 out_loc += length - out_loc % length;
741 if (in_loc % length)
742 in_loc += length - in_loc % length;
743 p->from = in_loc;
744 p->to = out_loc;
745 p->length = length;
746 list_add_tail(&p->l, &buffer->demux_list);
747 out_loc += length;
748 in_loc += length;
749 }
750 buffer->demux_bounce = kzalloc(out_loc, GFP_KERNEL);
751 if (buffer->demux_bounce == NULL) {
752 ret = -ENOMEM;
753 goto error_clear_mux_table;
754 }
755 return 0;
756
757error_clear_mux_table:
Jonathan Cameron842cd102012-04-21 10:09:45 +0100758 iio_buffer_demux_free(buffer);
759
Jonathan Cameron5ada4ea2011-12-05 21:37:14 +0000760 return ret;
761}
762EXPORT_SYMBOL_GPL(iio_update_demux);