Upgrade to 4.4.50-rt62
[kvmfornfv.git] / kernel / drivers / iio / industrialio-buffer.c
1 /* 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>
17 #include <linux/export.h>
18 #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 #include <linux/sched.h>
24
25 #include <linux/iio/iio.h>
26 #include "iio_core.h"
27 #include <linux/iio/sysfs.h>
28 #include <linux/iio/buffer.h>
29
30 static const char * const iio_endian_prefix[] = {
31         [IIO_BE] = "be",
32         [IIO_LE] = "le",
33 };
34
35 static bool iio_buffer_is_active(struct iio_buffer *buf)
36 {
37         return !list_empty(&buf->buffer_list);
38 }
39
40 static size_t iio_buffer_data_available(struct iio_buffer *buf)
41 {
42         return buf->access->data_available(buf);
43 }
44
45 static int iio_buffer_flush_hwfifo(struct iio_dev *indio_dev,
46                                    struct iio_buffer *buf, size_t required)
47 {
48         if (!indio_dev->info->hwfifo_flush_to_buffer)
49                 return -ENODEV;
50
51         return indio_dev->info->hwfifo_flush_to_buffer(indio_dev, required);
52 }
53
54 static bool iio_buffer_ready(struct iio_dev *indio_dev, struct iio_buffer *buf,
55                              size_t to_wait, int to_flush)
56 {
57         size_t avail;
58         int flushed = 0;
59
60         /* wakeup if the device was unregistered */
61         if (!indio_dev->info)
62                 return true;
63
64         /* drain the buffer if it was disabled */
65         if (!iio_buffer_is_active(buf)) {
66                 to_wait = min_t(size_t, to_wait, 1);
67                 to_flush = 0;
68         }
69
70         avail = iio_buffer_data_available(buf);
71
72         if (avail >= to_wait) {
73                 /* force a flush for non-blocking reads */
74                 if (!to_wait && avail < to_flush)
75                         iio_buffer_flush_hwfifo(indio_dev, buf,
76                                                 to_flush - avail);
77                 return true;
78         }
79
80         if (to_flush)
81                 flushed = iio_buffer_flush_hwfifo(indio_dev, buf,
82                                                   to_wait - avail);
83         if (flushed <= 0)
84                 return false;
85
86         if (avail + flushed >= to_wait)
87                 return true;
88
89         return false;
90 }
91
92 /**
93  * iio_buffer_read_first_n_outer() - chrdev read for buffer access
94  * @filp:       File structure pointer for the char device
95  * @buf:        Destination buffer for iio buffer read
96  * @n:          First n bytes to read
97  * @f_ps:       Long offset provided by the user as a seek position
98  *
99  * This function relies on all buffer implementations having an
100  * iio_buffer as their first element.
101  *
102  * Return: negative values corresponding to error codes or ret != 0
103  *         for ending the reading activity
104  **/
105 ssize_t iio_buffer_read_first_n_outer(struct file *filp, char __user *buf,
106                                       size_t n, loff_t *f_ps)
107 {
108         struct iio_dev *indio_dev = filp->private_data;
109         struct iio_buffer *rb = indio_dev->buffer;
110         DEFINE_WAIT_FUNC(wait, woken_wake_function);
111         size_t datum_size;
112         size_t to_wait;
113         int ret = 0;
114
115         if (!indio_dev->info)
116                 return -ENODEV;
117
118         if (!rb || !rb->access->read_first_n)
119                 return -EINVAL;
120
121         datum_size = rb->bytes_per_datum;
122
123         /*
124          * If datum_size is 0 there will never be anything to read from the
125          * buffer, so signal end of file now.
126          */
127         if (!datum_size)
128                 return 0;
129
130         if (filp->f_flags & O_NONBLOCK)
131                 to_wait = 0;
132         else
133                 to_wait = min_t(size_t, n / datum_size, rb->watermark);
134
135         add_wait_queue(&rb->pollq, &wait);
136         do {
137                 if (!indio_dev->info) {
138                         ret = -ENODEV;
139                         break;
140                 }
141
142                 if (!iio_buffer_ready(indio_dev, rb, to_wait, n / datum_size)) {
143                         if (signal_pending(current)) {
144                                 ret = -ERESTARTSYS;
145                                 break;
146                         }
147
148                         wait_woken(&wait, TASK_INTERRUPTIBLE,
149                                    MAX_SCHEDULE_TIMEOUT);
150                         continue;
151                 }
152
153                 ret = rb->access->read_first_n(rb, n, buf);
154                 if (ret == 0 && (filp->f_flags & O_NONBLOCK))
155                         ret = -EAGAIN;
156         } while (ret == 0);
157         remove_wait_queue(&rb->pollq, &wait);
158
159         return ret;
160 }
161
162 /**
163  * iio_buffer_poll() - poll the buffer to find out if it has data
164  * @filp:       File structure pointer for device access
165  * @wait:       Poll table structure pointer for which the driver adds
166  *              a wait queue
167  *
168  * Return: (POLLIN | POLLRDNORM) if data is available for reading
169  *         or 0 for other cases
170  */
171 unsigned int iio_buffer_poll(struct file *filp,
172                              struct poll_table_struct *wait)
173 {
174         struct iio_dev *indio_dev = filp->private_data;
175         struct iio_buffer *rb = indio_dev->buffer;
176
177         if (!indio_dev->info)
178                 return 0;
179
180         poll_wait(filp, &rb->pollq, wait);
181         if (iio_buffer_ready(indio_dev, rb, rb->watermark, 0))
182                 return POLLIN | POLLRDNORM;
183         return 0;
184 }
185
186 /**
187  * iio_buffer_wakeup_poll - Wakes up the buffer waitqueue
188  * @indio_dev: The IIO device
189  *
190  * Wakes up the event waitqueue used for poll(). Should usually
191  * be called when the device is unregistered.
192  */
193 void iio_buffer_wakeup_poll(struct iio_dev *indio_dev)
194 {
195         if (!indio_dev->buffer)
196                 return;
197
198         wake_up(&indio_dev->buffer->pollq);
199 }
200
201 void iio_buffer_init(struct iio_buffer *buffer)
202 {
203         INIT_LIST_HEAD(&buffer->demux_list);
204         INIT_LIST_HEAD(&buffer->buffer_list);
205         init_waitqueue_head(&buffer->pollq);
206         kref_init(&buffer->ref);
207         buffer->watermark = 1;
208 }
209 EXPORT_SYMBOL(iio_buffer_init);
210
211 static ssize_t iio_show_scan_index(struct device *dev,
212                                    struct device_attribute *attr,
213                                    char *buf)
214 {
215         return sprintf(buf, "%u\n", to_iio_dev_attr(attr)->c->scan_index);
216 }
217
218 static ssize_t iio_show_fixed_type(struct device *dev,
219                                    struct device_attribute *attr,
220                                    char *buf)
221 {
222         struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
223         u8 type = this_attr->c->scan_type.endianness;
224
225         if (type == IIO_CPU) {
226 #ifdef __LITTLE_ENDIAN
227                 type = IIO_LE;
228 #else
229                 type = IIO_BE;
230 #endif
231         }
232         if (this_attr->c->scan_type.repeat > 1)
233                 return sprintf(buf, "%s:%c%d/%dX%d>>%u\n",
234                        iio_endian_prefix[type],
235                        this_attr->c->scan_type.sign,
236                        this_attr->c->scan_type.realbits,
237                        this_attr->c->scan_type.storagebits,
238                        this_attr->c->scan_type.repeat,
239                        this_attr->c->scan_type.shift);
240                 else
241                         return sprintf(buf, "%s:%c%d/%d>>%u\n",
242                        iio_endian_prefix[type],
243                        this_attr->c->scan_type.sign,
244                        this_attr->c->scan_type.realbits,
245                        this_attr->c->scan_type.storagebits,
246                        this_attr->c->scan_type.shift);
247 }
248
249 static ssize_t iio_scan_el_show(struct device *dev,
250                                 struct device_attribute *attr,
251                                 char *buf)
252 {
253         int ret;
254         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
255
256         /* Ensure ret is 0 or 1. */
257         ret = !!test_bit(to_iio_dev_attr(attr)->address,
258                        indio_dev->buffer->scan_mask);
259
260         return sprintf(buf, "%d\n", ret);
261 }
262
263 /* Note NULL used as error indicator as it doesn't make sense. */
264 static const unsigned long *iio_scan_mask_match(const unsigned long *av_masks,
265                                           unsigned int masklength,
266                                           const unsigned long *mask,
267                                           bool strict)
268 {
269         if (bitmap_empty(mask, masklength))
270                 return NULL;
271         while (*av_masks) {
272                 if (strict) {
273                         if (bitmap_equal(mask, av_masks, masklength))
274                                 return av_masks;
275                 } else {
276                         if (bitmap_subset(mask, av_masks, masklength))
277                                 return av_masks;
278                 }
279                 av_masks += BITS_TO_LONGS(masklength);
280         }
281         return NULL;
282 }
283
284 static bool iio_validate_scan_mask(struct iio_dev *indio_dev,
285         const unsigned long *mask)
286 {
287         if (!indio_dev->setup_ops->validate_scan_mask)
288                 return true;
289
290         return indio_dev->setup_ops->validate_scan_mask(indio_dev, mask);
291 }
292
293 /**
294  * iio_scan_mask_set() - set particular bit in the scan mask
295  * @indio_dev: the iio device
296  * @buffer: the buffer whose scan mask we are interested in
297  * @bit: the bit to be set.
298  *
299  * Note that at this point we have no way of knowing what other
300  * buffers might request, hence this code only verifies that the
301  * individual buffers request is plausible.
302  */
303 static int iio_scan_mask_set(struct iio_dev *indio_dev,
304                       struct iio_buffer *buffer, int bit)
305 {
306         const unsigned long *mask;
307         unsigned long *trialmask;
308
309         trialmask = kmalloc(sizeof(*trialmask)*
310                             BITS_TO_LONGS(indio_dev->masklength),
311                             GFP_KERNEL);
312
313         if (trialmask == NULL)
314                 return -ENOMEM;
315         if (!indio_dev->masklength) {
316                 WARN(1, "Trying to set scanmask prior to registering buffer\n");
317                 goto err_invalid_mask;
318         }
319         bitmap_copy(trialmask, buffer->scan_mask, indio_dev->masklength);
320         set_bit(bit, trialmask);
321
322         if (!iio_validate_scan_mask(indio_dev, trialmask))
323                 goto err_invalid_mask;
324
325         if (indio_dev->available_scan_masks) {
326                 mask = iio_scan_mask_match(indio_dev->available_scan_masks,
327                                            indio_dev->masklength,
328                                            trialmask, false);
329                 if (!mask)
330                         goto err_invalid_mask;
331         }
332         bitmap_copy(buffer->scan_mask, trialmask, indio_dev->masklength);
333
334         kfree(trialmask);
335
336         return 0;
337
338 err_invalid_mask:
339         kfree(trialmask);
340         return -EINVAL;
341 }
342
343 static int iio_scan_mask_clear(struct iio_buffer *buffer, int bit)
344 {
345         clear_bit(bit, buffer->scan_mask);
346         return 0;
347 }
348
349 static ssize_t iio_scan_el_store(struct device *dev,
350                                  struct device_attribute *attr,
351                                  const char *buf,
352                                  size_t len)
353 {
354         int ret;
355         bool state;
356         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
357         struct iio_buffer *buffer = indio_dev->buffer;
358         struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
359
360         ret = strtobool(buf, &state);
361         if (ret < 0)
362                 return ret;
363         mutex_lock(&indio_dev->mlock);
364         if (iio_buffer_is_active(indio_dev->buffer)) {
365                 ret = -EBUSY;
366                 goto error_ret;
367         }
368         ret = iio_scan_mask_query(indio_dev, buffer, this_attr->address);
369         if (ret < 0)
370                 goto error_ret;
371         if (!state && ret) {
372                 ret = iio_scan_mask_clear(buffer, this_attr->address);
373                 if (ret)
374                         goto error_ret;
375         } else if (state && !ret) {
376                 ret = iio_scan_mask_set(indio_dev, buffer, this_attr->address);
377                 if (ret)
378                         goto error_ret;
379         }
380
381 error_ret:
382         mutex_unlock(&indio_dev->mlock);
383
384         return ret < 0 ? ret : len;
385
386 }
387
388 static ssize_t iio_scan_el_ts_show(struct device *dev,
389                                    struct device_attribute *attr,
390                                    char *buf)
391 {
392         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
393         return sprintf(buf, "%d\n", indio_dev->buffer->scan_timestamp);
394 }
395
396 static ssize_t iio_scan_el_ts_store(struct device *dev,
397                                     struct device_attribute *attr,
398                                     const char *buf,
399                                     size_t len)
400 {
401         int ret;
402         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
403         bool state;
404
405         ret = strtobool(buf, &state);
406         if (ret < 0)
407                 return ret;
408
409         mutex_lock(&indio_dev->mlock);
410         if (iio_buffer_is_active(indio_dev->buffer)) {
411                 ret = -EBUSY;
412                 goto error_ret;
413         }
414         indio_dev->buffer->scan_timestamp = state;
415 error_ret:
416         mutex_unlock(&indio_dev->mlock);
417
418         return ret ? ret : len;
419 }
420
421 static int iio_buffer_add_channel_sysfs(struct iio_dev *indio_dev,
422                                         const struct iio_chan_spec *chan)
423 {
424         int ret, attrcount = 0;
425         struct iio_buffer *buffer = indio_dev->buffer;
426
427         ret = __iio_add_chan_devattr("index",
428                                      chan,
429                                      &iio_show_scan_index,
430                                      NULL,
431                                      0,
432                                      IIO_SEPARATE,
433                                      &indio_dev->dev,
434                                      &buffer->scan_el_dev_attr_list);
435         if (ret)
436                 return ret;
437         attrcount++;
438         ret = __iio_add_chan_devattr("type",
439                                      chan,
440                                      &iio_show_fixed_type,
441                                      NULL,
442                                      0,
443                                      0,
444                                      &indio_dev->dev,
445                                      &buffer->scan_el_dev_attr_list);
446         if (ret)
447                 return ret;
448         attrcount++;
449         if (chan->type != IIO_TIMESTAMP)
450                 ret = __iio_add_chan_devattr("en",
451                                              chan,
452                                              &iio_scan_el_show,
453                                              &iio_scan_el_store,
454                                              chan->scan_index,
455                                              0,
456                                              &indio_dev->dev,
457                                              &buffer->scan_el_dev_attr_list);
458         else
459                 ret = __iio_add_chan_devattr("en",
460                                              chan,
461                                              &iio_scan_el_ts_show,
462                                              &iio_scan_el_ts_store,
463                                              chan->scan_index,
464                                              0,
465                                              &indio_dev->dev,
466                                              &buffer->scan_el_dev_attr_list);
467         if (ret)
468                 return ret;
469         attrcount++;
470         ret = attrcount;
471         return ret;
472 }
473
474 static ssize_t iio_buffer_read_length(struct device *dev,
475                                       struct device_attribute *attr,
476                                       char *buf)
477 {
478         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
479         struct iio_buffer *buffer = indio_dev->buffer;
480
481         return sprintf(buf, "%d\n", buffer->length);
482 }
483
484 static ssize_t iio_buffer_write_length(struct device *dev,
485                                        struct device_attribute *attr,
486                                        const char *buf, size_t len)
487 {
488         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
489         struct iio_buffer *buffer = indio_dev->buffer;
490         unsigned int val;
491         int ret;
492
493         ret = kstrtouint(buf, 10, &val);
494         if (ret)
495                 return ret;
496
497         if (val == buffer->length)
498                 return len;
499
500         mutex_lock(&indio_dev->mlock);
501         if (iio_buffer_is_active(indio_dev->buffer)) {
502                 ret = -EBUSY;
503         } else {
504                 buffer->access->set_length(buffer, val);
505                 ret = 0;
506         }
507         if (ret)
508                 goto out;
509         if (buffer->length && buffer->length < buffer->watermark)
510                 buffer->watermark = buffer->length;
511 out:
512         mutex_unlock(&indio_dev->mlock);
513
514         return ret ? ret : len;
515 }
516
517 static ssize_t iio_buffer_show_enable(struct device *dev,
518                                       struct device_attribute *attr,
519                                       char *buf)
520 {
521         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
522         return sprintf(buf, "%d\n", iio_buffer_is_active(indio_dev->buffer));
523 }
524
525 static int iio_compute_scan_bytes(struct iio_dev *indio_dev,
526                                 const unsigned long *mask, bool timestamp)
527 {
528         const struct iio_chan_spec *ch;
529         unsigned bytes = 0;
530         int length, i;
531
532         /* How much space will the demuxed element take? */
533         for_each_set_bit(i, mask,
534                          indio_dev->masklength) {
535                 ch = iio_find_channel_from_si(indio_dev, i);
536                 if (ch->scan_type.repeat > 1)
537                         length = ch->scan_type.storagebits / 8 *
538                                 ch->scan_type.repeat;
539                 else
540                         length = ch->scan_type.storagebits / 8;
541                 bytes = ALIGN(bytes, length);
542                 bytes += length;
543         }
544         if (timestamp) {
545                 ch = iio_find_channel_from_si(indio_dev,
546                                               indio_dev->scan_index_timestamp);
547                 if (ch->scan_type.repeat > 1)
548                         length = ch->scan_type.storagebits / 8 *
549                                 ch->scan_type.repeat;
550                 else
551                         length = ch->scan_type.storagebits / 8;
552                 bytes = ALIGN(bytes, length);
553                 bytes += length;
554         }
555         return bytes;
556 }
557
558 static void iio_buffer_activate(struct iio_dev *indio_dev,
559         struct iio_buffer *buffer)
560 {
561         iio_buffer_get(buffer);
562         list_add(&buffer->buffer_list, &indio_dev->buffer_list);
563 }
564
565 static void iio_buffer_deactivate(struct iio_buffer *buffer)
566 {
567         list_del_init(&buffer->buffer_list);
568         wake_up_interruptible(&buffer->pollq);
569         iio_buffer_put(buffer);
570 }
571
572 static void iio_buffer_deactivate_all(struct iio_dev *indio_dev)
573 {
574         struct iio_buffer *buffer, *_buffer;
575
576         list_for_each_entry_safe(buffer, _buffer,
577                         &indio_dev->buffer_list, buffer_list)
578                 iio_buffer_deactivate(buffer);
579 }
580
581 static void iio_buffer_update_bytes_per_datum(struct iio_dev *indio_dev,
582         struct iio_buffer *buffer)
583 {
584         unsigned int bytes;
585
586         if (!buffer->access->set_bytes_per_datum)
587                 return;
588
589         bytes = iio_compute_scan_bytes(indio_dev, buffer->scan_mask,
590                 buffer->scan_timestamp);
591
592         buffer->access->set_bytes_per_datum(buffer, bytes);
593 }
594
595 static int iio_buffer_request_update(struct iio_dev *indio_dev,
596         struct iio_buffer *buffer)
597 {
598         int ret;
599
600         iio_buffer_update_bytes_per_datum(indio_dev, buffer);
601         if (buffer->access->request_update) {
602                 ret = buffer->access->request_update(buffer);
603                 if (ret) {
604                         dev_dbg(&indio_dev->dev,
605                                "Buffer not started: buffer parameter update failed (%d)\n",
606                                 ret);
607                         return ret;
608                 }
609         }
610
611         return 0;
612 }
613
614 static void iio_free_scan_mask(struct iio_dev *indio_dev,
615         const unsigned long *mask)
616 {
617         /* If the mask is dynamically allocated free it, otherwise do nothing */
618         if (!indio_dev->available_scan_masks)
619                 kfree(mask);
620 }
621
622 struct iio_device_config {
623         unsigned int mode;
624         const unsigned long *scan_mask;
625         unsigned int scan_bytes;
626         bool scan_timestamp;
627 };
628
629 static int iio_verify_update(struct iio_dev *indio_dev,
630         struct iio_buffer *insert_buffer, struct iio_buffer *remove_buffer,
631         struct iio_device_config *config)
632 {
633         unsigned long *compound_mask;
634         const unsigned long *scan_mask;
635         bool strict_scanmask = false;
636         struct iio_buffer *buffer;
637         bool scan_timestamp;
638         unsigned int modes;
639
640         memset(config, 0, sizeof(*config));
641
642         /*
643          * If there is just one buffer and we are removing it there is nothing
644          * to verify.
645          */
646         if (remove_buffer && !insert_buffer &&
647                 list_is_singular(&indio_dev->buffer_list))
648                         return 0;
649
650         modes = indio_dev->modes;
651
652         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
653                 if (buffer == remove_buffer)
654                         continue;
655                 modes &= buffer->access->modes;
656         }
657
658         if (insert_buffer)
659                 modes &= insert_buffer->access->modes;
660
661         /* Definitely possible for devices to support both of these. */
662         if ((modes & INDIO_BUFFER_TRIGGERED) && indio_dev->trig) {
663                 config->mode = INDIO_BUFFER_TRIGGERED;
664         } else if (modes & INDIO_BUFFER_HARDWARE) {
665                 /*
666                  * Keep things simple for now and only allow a single buffer to
667                  * be connected in hardware mode.
668                  */
669                 if (insert_buffer && !list_empty(&indio_dev->buffer_list))
670                         return -EINVAL;
671                 config->mode = INDIO_BUFFER_HARDWARE;
672                 strict_scanmask = true;
673         } else if (modes & INDIO_BUFFER_SOFTWARE) {
674                 config->mode = INDIO_BUFFER_SOFTWARE;
675         } else {
676                 /* Can only occur on first buffer */
677                 if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
678                         dev_dbg(&indio_dev->dev, "Buffer not started: no trigger\n");
679                 return -EINVAL;
680         }
681
682         /* What scan mask do we actually have? */
683         compound_mask = kcalloc(BITS_TO_LONGS(indio_dev->masklength),
684                                 sizeof(long), GFP_KERNEL);
685         if (compound_mask == NULL)
686                 return -ENOMEM;
687
688         scan_timestamp = false;
689
690         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
691                 if (buffer == remove_buffer)
692                         continue;
693                 bitmap_or(compound_mask, compound_mask, buffer->scan_mask,
694                           indio_dev->masklength);
695                 scan_timestamp |= buffer->scan_timestamp;
696         }
697
698         if (insert_buffer) {
699                 bitmap_or(compound_mask, compound_mask,
700                           insert_buffer->scan_mask, indio_dev->masklength);
701                 scan_timestamp |= insert_buffer->scan_timestamp;
702         }
703
704         if (indio_dev->available_scan_masks) {
705                 scan_mask = iio_scan_mask_match(indio_dev->available_scan_masks,
706                                     indio_dev->masklength,
707                                     compound_mask,
708                                     strict_scanmask);
709                 kfree(compound_mask);
710                 if (scan_mask == NULL)
711                         return -EINVAL;
712         } else {
713             scan_mask = compound_mask;
714         }
715
716         config->scan_bytes = iio_compute_scan_bytes(indio_dev,
717                                     scan_mask, scan_timestamp);
718         config->scan_mask = scan_mask;
719         config->scan_timestamp = scan_timestamp;
720
721         return 0;
722 }
723
724 static int iio_enable_buffers(struct iio_dev *indio_dev,
725         struct iio_device_config *config)
726 {
727         int ret;
728
729         indio_dev->active_scan_mask = config->scan_mask;
730         indio_dev->scan_timestamp = config->scan_timestamp;
731         indio_dev->scan_bytes = config->scan_bytes;
732
733         iio_update_demux(indio_dev);
734
735         /* Wind up again */
736         if (indio_dev->setup_ops->preenable) {
737                 ret = indio_dev->setup_ops->preenable(indio_dev);
738                 if (ret) {
739                         dev_dbg(&indio_dev->dev,
740                                "Buffer not started: buffer preenable failed (%d)\n", ret);
741                         goto err_undo_config;
742                 }
743         }
744
745         if (indio_dev->info->update_scan_mode) {
746                 ret = indio_dev->info
747                         ->update_scan_mode(indio_dev,
748                                            indio_dev->active_scan_mask);
749                 if (ret < 0) {
750                         dev_dbg(&indio_dev->dev,
751                                 "Buffer not started: update scan mode failed (%d)\n",
752                                 ret);
753                         goto err_run_postdisable;
754                 }
755         }
756
757         indio_dev->currentmode = config->mode;
758
759         if (indio_dev->setup_ops->postenable) {
760                 ret = indio_dev->setup_ops->postenable(indio_dev);
761                 if (ret) {
762                         dev_dbg(&indio_dev->dev,
763                                "Buffer not started: postenable failed (%d)\n", ret);
764                         goto err_run_postdisable;
765                 }
766         }
767
768         return 0;
769
770 err_run_postdisable:
771         indio_dev->currentmode = INDIO_DIRECT_MODE;
772         if (indio_dev->setup_ops->postdisable)
773                 indio_dev->setup_ops->postdisable(indio_dev);
774 err_undo_config:
775         indio_dev->active_scan_mask = NULL;
776
777         return ret;
778 }
779
780 static int iio_disable_buffers(struct iio_dev *indio_dev)
781 {
782         int ret = 0;
783         int ret2;
784
785         /* Wind down existing buffers - iff there are any */
786         if (list_empty(&indio_dev->buffer_list))
787                 return 0;
788
789         /*
790          * If things go wrong at some step in disable we still need to continue
791          * to perform the other steps, otherwise we leave the device in a
792          * inconsistent state. We return the error code for the first error we
793          * encountered.
794          */
795
796         if (indio_dev->setup_ops->predisable) {
797                 ret2 = indio_dev->setup_ops->predisable(indio_dev);
798                 if (ret2 && !ret)
799                         ret = ret2;
800         }
801
802         indio_dev->currentmode = INDIO_DIRECT_MODE;
803
804         if (indio_dev->setup_ops->postdisable) {
805                 ret2 = indio_dev->setup_ops->postdisable(indio_dev);
806                 if (ret2 && !ret)
807                         ret = ret2;
808         }
809
810         iio_free_scan_mask(indio_dev, indio_dev->active_scan_mask);
811         indio_dev->active_scan_mask = NULL;
812
813         return ret;
814 }
815
816 static int __iio_update_buffers(struct iio_dev *indio_dev,
817                        struct iio_buffer *insert_buffer,
818                        struct iio_buffer *remove_buffer)
819 {
820         struct iio_device_config new_config;
821         int ret;
822
823         ret = iio_verify_update(indio_dev, insert_buffer, remove_buffer,
824                 &new_config);
825         if (ret)
826                 return ret;
827
828         if (insert_buffer) {
829                 ret = iio_buffer_request_update(indio_dev, insert_buffer);
830                 if (ret)
831                         goto err_free_config;
832         }
833
834         ret = iio_disable_buffers(indio_dev);
835         if (ret)
836                 goto err_deactivate_all;
837
838         if (remove_buffer)
839                 iio_buffer_deactivate(remove_buffer);
840         if (insert_buffer)
841                 iio_buffer_activate(indio_dev, insert_buffer);
842
843         /* If no buffers in list, we are done */
844         if (list_empty(&indio_dev->buffer_list))
845                 return 0;
846
847         ret = iio_enable_buffers(indio_dev, &new_config);
848         if (ret)
849                 goto err_deactivate_all;
850
851         return 0;
852
853 err_deactivate_all:
854         /*
855          * We've already verified that the config is valid earlier. If things go
856          * wrong in either enable or disable the most likely reason is an IO
857          * error from the device. In this case there is no good recovery
858          * strategy. Just make sure to disable everything and leave the device
859          * in a sane state.  With a bit of luck the device might come back to
860          * life again later and userspace can try again.
861          */
862         iio_buffer_deactivate_all(indio_dev);
863
864 err_free_config:
865         iio_free_scan_mask(indio_dev, new_config.scan_mask);
866         return ret;
867 }
868
869 int iio_update_buffers(struct iio_dev *indio_dev,
870                        struct iio_buffer *insert_buffer,
871                        struct iio_buffer *remove_buffer)
872 {
873         int ret;
874
875         if (insert_buffer == remove_buffer)
876                 return 0;
877
878         mutex_lock(&indio_dev->info_exist_lock);
879         mutex_lock(&indio_dev->mlock);
880
881         if (insert_buffer && iio_buffer_is_active(insert_buffer))
882                 insert_buffer = NULL;
883
884         if (remove_buffer && !iio_buffer_is_active(remove_buffer))
885                 remove_buffer = NULL;
886
887         if (!insert_buffer && !remove_buffer) {
888                 ret = 0;
889                 goto out_unlock;
890         }
891
892         if (indio_dev->info == NULL) {
893                 ret = -ENODEV;
894                 goto out_unlock;
895         }
896
897         ret = __iio_update_buffers(indio_dev, insert_buffer, remove_buffer);
898
899 out_unlock:
900         mutex_unlock(&indio_dev->mlock);
901         mutex_unlock(&indio_dev->info_exist_lock);
902
903         return ret;
904 }
905 EXPORT_SYMBOL_GPL(iio_update_buffers);
906
907 void iio_disable_all_buffers(struct iio_dev *indio_dev)
908 {
909         iio_disable_buffers(indio_dev);
910         iio_buffer_deactivate_all(indio_dev);
911 }
912
913 static ssize_t iio_buffer_store_enable(struct device *dev,
914                                        struct device_attribute *attr,
915                                        const char *buf,
916                                        size_t len)
917 {
918         int ret;
919         bool requested_state;
920         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
921         bool inlist;
922
923         ret = strtobool(buf, &requested_state);
924         if (ret < 0)
925                 return ret;
926
927         mutex_lock(&indio_dev->mlock);
928
929         /* Find out if it is in the list */
930         inlist = iio_buffer_is_active(indio_dev->buffer);
931         /* Already in desired state */
932         if (inlist == requested_state)
933                 goto done;
934
935         if (requested_state)
936                 ret = __iio_update_buffers(indio_dev,
937                                          indio_dev->buffer, NULL);
938         else
939                 ret = __iio_update_buffers(indio_dev,
940                                          NULL, indio_dev->buffer);
941
942 done:
943         mutex_unlock(&indio_dev->mlock);
944         return (ret < 0) ? ret : len;
945 }
946
947 static const char * const iio_scan_elements_group_name = "scan_elements";
948
949 static ssize_t iio_buffer_show_watermark(struct device *dev,
950                                          struct device_attribute *attr,
951                                          char *buf)
952 {
953         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
954         struct iio_buffer *buffer = indio_dev->buffer;
955
956         return sprintf(buf, "%u\n", buffer->watermark);
957 }
958
959 static ssize_t iio_buffer_store_watermark(struct device *dev,
960                                           struct device_attribute *attr,
961                                           const char *buf,
962                                           size_t len)
963 {
964         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
965         struct iio_buffer *buffer = indio_dev->buffer;
966         unsigned int val;
967         int ret;
968
969         ret = kstrtouint(buf, 10, &val);
970         if (ret)
971                 return ret;
972         if (!val)
973                 return -EINVAL;
974
975         mutex_lock(&indio_dev->mlock);
976
977         if (val > buffer->length) {
978                 ret = -EINVAL;
979                 goto out;
980         }
981
982         if (iio_buffer_is_active(indio_dev->buffer)) {
983                 ret = -EBUSY;
984                 goto out;
985         }
986
987         buffer->watermark = val;
988
989         if (indio_dev->info->hwfifo_set_watermark)
990                 indio_dev->info->hwfifo_set_watermark(indio_dev, val);
991 out:
992         mutex_unlock(&indio_dev->mlock);
993
994         return ret ? ret : len;
995 }
996
997 static DEVICE_ATTR(length, S_IRUGO | S_IWUSR, iio_buffer_read_length,
998                    iio_buffer_write_length);
999 static struct device_attribute dev_attr_length_ro = __ATTR(length,
1000         S_IRUGO, iio_buffer_read_length, NULL);
1001 static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR,
1002                    iio_buffer_show_enable, iio_buffer_store_enable);
1003 static DEVICE_ATTR(watermark, S_IRUGO | S_IWUSR,
1004                    iio_buffer_show_watermark, iio_buffer_store_watermark);
1005
1006 static struct attribute *iio_buffer_attrs[] = {
1007         &dev_attr_length.attr,
1008         &dev_attr_enable.attr,
1009         &dev_attr_watermark.attr,
1010 };
1011
1012 int iio_buffer_alloc_sysfs_and_mask(struct iio_dev *indio_dev)
1013 {
1014         struct iio_dev_attr *p;
1015         struct attribute **attr;
1016         struct iio_buffer *buffer = indio_dev->buffer;
1017         int ret, i, attrn, attrcount, attrcount_orig = 0;
1018         const struct iio_chan_spec *channels;
1019
1020         channels = indio_dev->channels;
1021         if (channels) {
1022                 int ml = indio_dev->masklength;
1023
1024                 for (i = 0; i < indio_dev->num_channels; i++)
1025                         ml = max(ml, channels[i].scan_index + 1);
1026                 indio_dev->masklength = ml;
1027         }
1028
1029         if (!buffer)
1030                 return 0;
1031
1032         attrcount = 0;
1033         if (buffer->attrs) {
1034                 while (buffer->attrs[attrcount] != NULL)
1035                         attrcount++;
1036         }
1037
1038         attr = kcalloc(attrcount + ARRAY_SIZE(iio_buffer_attrs) + 1,
1039                        sizeof(struct attribute *), GFP_KERNEL);
1040         if (!attr)
1041                 return -ENOMEM;
1042
1043         memcpy(attr, iio_buffer_attrs, sizeof(iio_buffer_attrs));
1044         if (!buffer->access->set_length)
1045                 attr[0] = &dev_attr_length_ro.attr;
1046
1047         if (buffer->attrs)
1048                 memcpy(&attr[ARRAY_SIZE(iio_buffer_attrs)], buffer->attrs,
1049                        sizeof(struct attribute *) * attrcount);
1050
1051         attr[attrcount + ARRAY_SIZE(iio_buffer_attrs)] = NULL;
1052
1053         buffer->buffer_group.name = "buffer";
1054         buffer->buffer_group.attrs = attr;
1055
1056         indio_dev->groups[indio_dev->groupcounter++] = &buffer->buffer_group;
1057
1058         if (buffer->scan_el_attrs != NULL) {
1059                 attr = buffer->scan_el_attrs->attrs;
1060                 while (*attr++ != NULL)
1061                         attrcount_orig++;
1062         }
1063         attrcount = attrcount_orig;
1064         INIT_LIST_HEAD(&buffer->scan_el_dev_attr_list);
1065         channels = indio_dev->channels;
1066         if (channels) {
1067                 /* new magic */
1068                 for (i = 0; i < indio_dev->num_channels; i++) {
1069                         if (channels[i].scan_index < 0)
1070                                 continue;
1071
1072                         ret = iio_buffer_add_channel_sysfs(indio_dev,
1073                                                          &channels[i]);
1074                         if (ret < 0)
1075                                 goto error_cleanup_dynamic;
1076                         attrcount += ret;
1077                         if (channels[i].type == IIO_TIMESTAMP)
1078                                 indio_dev->scan_index_timestamp =
1079                                         channels[i].scan_index;
1080                 }
1081                 if (indio_dev->masklength && buffer->scan_mask == NULL) {
1082                         buffer->scan_mask = kcalloc(BITS_TO_LONGS(indio_dev->masklength),
1083                                                     sizeof(*buffer->scan_mask),
1084                                                     GFP_KERNEL);
1085                         if (buffer->scan_mask == NULL) {
1086                                 ret = -ENOMEM;
1087                                 goto error_cleanup_dynamic;
1088                         }
1089                 }
1090         }
1091
1092         buffer->scan_el_group.name = iio_scan_elements_group_name;
1093
1094         buffer->scan_el_group.attrs = kcalloc(attrcount + 1,
1095                                               sizeof(buffer->scan_el_group.attrs[0]),
1096                                               GFP_KERNEL);
1097         if (buffer->scan_el_group.attrs == NULL) {
1098                 ret = -ENOMEM;
1099                 goto error_free_scan_mask;
1100         }
1101         if (buffer->scan_el_attrs)
1102                 memcpy(buffer->scan_el_group.attrs, buffer->scan_el_attrs,
1103                        sizeof(buffer->scan_el_group.attrs[0])*attrcount_orig);
1104         attrn = attrcount_orig;
1105
1106         list_for_each_entry(p, &buffer->scan_el_dev_attr_list, l)
1107                 buffer->scan_el_group.attrs[attrn++] = &p->dev_attr.attr;
1108         indio_dev->groups[indio_dev->groupcounter++] = &buffer->scan_el_group;
1109
1110         return 0;
1111
1112 error_free_scan_mask:
1113         kfree(buffer->scan_mask);
1114 error_cleanup_dynamic:
1115         iio_free_chan_devattr_list(&buffer->scan_el_dev_attr_list);
1116         kfree(indio_dev->buffer->buffer_group.attrs);
1117
1118         return ret;
1119 }
1120
1121 void iio_buffer_free_sysfs_and_mask(struct iio_dev *indio_dev)
1122 {
1123         if (!indio_dev->buffer)
1124                 return;
1125
1126         kfree(indio_dev->buffer->scan_mask);
1127         kfree(indio_dev->buffer->buffer_group.attrs);
1128         kfree(indio_dev->buffer->scan_el_group.attrs);
1129         iio_free_chan_devattr_list(&indio_dev->buffer->scan_el_dev_attr_list);
1130 }
1131
1132 /**
1133  * iio_validate_scan_mask_onehot() - Validates that exactly one channel is selected
1134  * @indio_dev: the iio device
1135  * @mask: scan mask to be checked
1136  *
1137  * Return true if exactly one bit is set in the scan mask, false otherwise. It
1138  * can be used for devices where only one channel can be active for sampling at
1139  * a time.
1140  */
1141 bool iio_validate_scan_mask_onehot(struct iio_dev *indio_dev,
1142         const unsigned long *mask)
1143 {
1144         return bitmap_weight(mask, indio_dev->masklength) == 1;
1145 }
1146 EXPORT_SYMBOL_GPL(iio_validate_scan_mask_onehot);
1147
1148 int iio_scan_mask_query(struct iio_dev *indio_dev,
1149                         struct iio_buffer *buffer, int bit)
1150 {
1151         if (bit > indio_dev->masklength)
1152                 return -EINVAL;
1153
1154         if (!buffer->scan_mask)
1155                 return 0;
1156
1157         /* Ensure return value is 0 or 1. */
1158         return !!test_bit(bit, buffer->scan_mask);
1159 };
1160 EXPORT_SYMBOL_GPL(iio_scan_mask_query);
1161
1162 /**
1163  * struct iio_demux_table - table describing demux memcpy ops
1164  * @from:       index to copy from
1165  * @to:         index to copy to
1166  * @length:     how many bytes to copy
1167  * @l:          list head used for management
1168  */
1169 struct iio_demux_table {
1170         unsigned from;
1171         unsigned to;
1172         unsigned length;
1173         struct list_head l;
1174 };
1175
1176 static const void *iio_demux(struct iio_buffer *buffer,
1177                                  const void *datain)
1178 {
1179         struct iio_demux_table *t;
1180
1181         if (list_empty(&buffer->demux_list))
1182                 return datain;
1183         list_for_each_entry(t, &buffer->demux_list, l)
1184                 memcpy(buffer->demux_bounce + t->to,
1185                        datain + t->from, t->length);
1186
1187         return buffer->demux_bounce;
1188 }
1189
1190 static int iio_push_to_buffer(struct iio_buffer *buffer, const void *data)
1191 {
1192         const void *dataout = iio_demux(buffer, data);
1193         int ret;
1194
1195         ret = buffer->access->store_to(buffer, dataout);
1196         if (ret)
1197                 return ret;
1198
1199         /*
1200          * We can't just test for watermark to decide if we wake the poll queue
1201          * because read may request less samples than the watermark.
1202          */
1203         wake_up_interruptible_poll(&buffer->pollq, POLLIN | POLLRDNORM);
1204         return 0;
1205 }
1206
1207 static void iio_buffer_demux_free(struct iio_buffer *buffer)
1208 {
1209         struct iio_demux_table *p, *q;
1210         list_for_each_entry_safe(p, q, &buffer->demux_list, l) {
1211                 list_del(&p->l);
1212                 kfree(p);
1213         }
1214 }
1215
1216
1217 int iio_push_to_buffers(struct iio_dev *indio_dev, const void *data)
1218 {
1219         int ret;
1220         struct iio_buffer *buf;
1221
1222         list_for_each_entry(buf, &indio_dev->buffer_list, buffer_list) {
1223                 ret = iio_push_to_buffer(buf, data);
1224                 if (ret < 0)
1225                         return ret;
1226         }
1227
1228         return 0;
1229 }
1230 EXPORT_SYMBOL_GPL(iio_push_to_buffers);
1231
1232 static int iio_buffer_add_demux(struct iio_buffer *buffer,
1233         struct iio_demux_table **p, unsigned int in_loc, unsigned int out_loc,
1234         unsigned int length)
1235 {
1236
1237         if (*p && (*p)->from + (*p)->length == in_loc &&
1238                 (*p)->to + (*p)->length == out_loc) {
1239                 (*p)->length += length;
1240         } else {
1241                 *p = kmalloc(sizeof(**p), GFP_KERNEL);
1242                 if (*p == NULL)
1243                         return -ENOMEM;
1244                 (*p)->from = in_loc;
1245                 (*p)->to = out_loc;
1246                 (*p)->length = length;
1247                 list_add_tail(&(*p)->l, &buffer->demux_list);
1248         }
1249
1250         return 0;
1251 }
1252
1253 static int iio_buffer_update_demux(struct iio_dev *indio_dev,
1254                                    struct iio_buffer *buffer)
1255 {
1256         const struct iio_chan_spec *ch;
1257         int ret, in_ind = -1, out_ind, length;
1258         unsigned in_loc = 0, out_loc = 0;
1259         struct iio_demux_table *p = NULL;
1260
1261         /* Clear out any old demux */
1262         iio_buffer_demux_free(buffer);
1263         kfree(buffer->demux_bounce);
1264         buffer->demux_bounce = NULL;
1265
1266         /* First work out which scan mode we will actually have */
1267         if (bitmap_equal(indio_dev->active_scan_mask,
1268                          buffer->scan_mask,
1269                          indio_dev->masklength))
1270                 return 0;
1271
1272         /* Now we have the two masks, work from least sig and build up sizes */
1273         for_each_set_bit(out_ind,
1274                          buffer->scan_mask,
1275                          indio_dev->masklength) {
1276                 in_ind = find_next_bit(indio_dev->active_scan_mask,
1277                                        indio_dev->masklength,
1278                                        in_ind + 1);
1279                 while (in_ind != out_ind) {
1280                         in_ind = find_next_bit(indio_dev->active_scan_mask,
1281                                                indio_dev->masklength,
1282                                                in_ind + 1);
1283                         ch = iio_find_channel_from_si(indio_dev, in_ind);
1284                         if (ch->scan_type.repeat > 1)
1285                                 length = ch->scan_type.storagebits / 8 *
1286                                         ch->scan_type.repeat;
1287                         else
1288                                 length = ch->scan_type.storagebits / 8;
1289                         /* Make sure we are aligned */
1290                         in_loc = roundup(in_loc, length) + length;
1291                 }
1292                 ch = iio_find_channel_from_si(indio_dev, in_ind);
1293                 if (ch->scan_type.repeat > 1)
1294                         length = ch->scan_type.storagebits / 8 *
1295                                 ch->scan_type.repeat;
1296                 else
1297                         length = ch->scan_type.storagebits / 8;
1298                 out_loc = roundup(out_loc, length);
1299                 in_loc = roundup(in_loc, length);
1300                 ret = iio_buffer_add_demux(buffer, &p, in_loc, out_loc, length);
1301                 if (ret)
1302                         goto error_clear_mux_table;
1303                 out_loc += length;
1304                 in_loc += length;
1305         }
1306         /* Relies on scan_timestamp being last */
1307         if (buffer->scan_timestamp) {
1308                 ch = iio_find_channel_from_si(indio_dev,
1309                         indio_dev->scan_index_timestamp);
1310                 if (ch->scan_type.repeat > 1)
1311                         length = ch->scan_type.storagebits / 8 *
1312                                 ch->scan_type.repeat;
1313                 else
1314                         length = ch->scan_type.storagebits / 8;
1315                 out_loc = roundup(out_loc, length);
1316                 in_loc = roundup(in_loc, length);
1317                 ret = iio_buffer_add_demux(buffer, &p, in_loc, out_loc, length);
1318                 if (ret)
1319                         goto error_clear_mux_table;
1320                 out_loc += length;
1321                 in_loc += length;
1322         }
1323         buffer->demux_bounce = kzalloc(out_loc, GFP_KERNEL);
1324         if (buffer->demux_bounce == NULL) {
1325                 ret = -ENOMEM;
1326                 goto error_clear_mux_table;
1327         }
1328         return 0;
1329
1330 error_clear_mux_table:
1331         iio_buffer_demux_free(buffer);
1332
1333         return ret;
1334 }
1335
1336 int iio_update_demux(struct iio_dev *indio_dev)
1337 {
1338         struct iio_buffer *buffer;
1339         int ret;
1340
1341         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
1342                 ret = iio_buffer_update_demux(indio_dev, buffer);
1343                 if (ret < 0)
1344                         goto error_clear_mux_table;
1345         }
1346         return 0;
1347
1348 error_clear_mux_table:
1349         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list)
1350                 iio_buffer_demux_free(buffer);
1351
1352         return ret;
1353 }
1354 EXPORT_SYMBOL_GPL(iio_update_demux);
1355
1356 /**
1357  * iio_buffer_release() - Free a buffer's resources
1358  * @ref: Pointer to the kref embedded in the iio_buffer struct
1359  *
1360  * This function is called when the last reference to the buffer has been
1361  * dropped. It will typically free all resources allocated by the buffer. Do not
1362  * call this function manually, always use iio_buffer_put() when done using a
1363  * buffer.
1364  */
1365 static void iio_buffer_release(struct kref *ref)
1366 {
1367         struct iio_buffer *buffer = container_of(ref, struct iio_buffer, ref);
1368
1369         buffer->access->release(buffer);
1370 }
1371
1372 /**
1373  * iio_buffer_get() - Grab a reference to the buffer
1374  * @buffer: The buffer to grab a reference for, may be NULL
1375  *
1376  * Returns the pointer to the buffer that was passed into the function.
1377  */
1378 struct iio_buffer *iio_buffer_get(struct iio_buffer *buffer)
1379 {
1380         if (buffer)
1381                 kref_get(&buffer->ref);
1382
1383         return buffer;
1384 }
1385 EXPORT_SYMBOL_GPL(iio_buffer_get);
1386
1387 /**
1388  * iio_buffer_put() - Release the reference to the buffer
1389  * @buffer: The buffer to release the reference for, may be NULL
1390  */
1391 void iio_buffer_put(struct iio_buffer *buffer)
1392 {
1393         if (buffer)
1394                 kref_put(&buffer->ref, iio_buffer_release);
1395 }
1396 EXPORT_SYMBOL_GPL(iio_buffer_put);