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