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
  24 #include <linux/iio/iio.h>
  25 #include "iio_core.h"
  26 #include <linux/iio/sysfs.h>
  27 #include <linux/iio/buffer.h>
  28
  29 static const char * const iio_endian_prefix[] = {
  30         [IIO_BE] = "be",
  31         [IIO_LE] = "le",
  32 };
  33
  34 static bool iio_buffer_is_active(struct iio_dev *indio_dev,
  35                                  struct iio_buffer *buf)
  36 {
  37         struct list_head *p;
  38
  39         list_for_each(p, &indio_dev->buffer_list)
  40                 if (p == &buf->buffer_list)
  41                         return true;
  42
  43         return false;
  44 }
  45
  46 /**
  47  * iio_buffer_read_first_n_outer() - chrdev read for buffer access
  48  *
  49  * This function relies on all buffer implementations having an
  50  * iio_buffer as their first element.
  51  **/
  52 ssize_t iio_buffer_read_first_n_outer(struct file *filp, char __user *buf,
  53                                       size_t n, loff_t *f_ps)
  54 {
  55         struct iio_dev *indio_dev = filp->private_data;
  56         struct iio_buffer *rb = indio_dev->buffer;
  57
  58         if (!rb || !rb->access->read_first_n)
  59                 return -EINVAL;
  60         return rb->access->read_first_n(rb, n, buf);
  61 }
  62
  63 /**
  64  * iio_buffer_poll() - poll the buffer to find out if it has data
  65  */
  66 unsigned int iio_buffer_poll(struct file *filp,
  67                              struct poll_table_struct *wait)
  68 {
  69         struct iio_dev *indio_dev = filp->private_data;
  70         struct iio_buffer *rb = indio_dev->buffer;
  71
  72         poll_wait(filp, &rb->pollq, wait);
  73         if (rb->stufftoread)
  74                 return POLLIN | POLLRDNORM;
  75         /* need a way of knowing if there may be enough data... */
  76         return 0;
  77 }
  78
  79 void iio_buffer_init(struct iio_buffer *buffer)
  80 {
  81         INIT_LIST_HEAD(&buffer->demux_list);
  82         init_waitqueue_head(&buffer->pollq);
  83 }
  84 EXPORT_SYMBOL(iio_buffer_init);
  85
  86 static ssize_t iio_show_scan_index(struct device *dev,
  87                                    struct device_attribute *attr,
  88                                    char *buf)
  89 {
  90         return sprintf(buf, "%u\n", to_iio_dev_attr(attr)->c->scan_index);
  91 }
  92
  93 static ssize_t iio_show_fixed_type(struct device *dev,
  94                                    struct device_attribute *attr,
  95                                    char *buf)
  96 {
  97         struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
  98         u8 type = this_attr->c->scan_type.endianness;
  99
 100         if (type == IIO_CPU) {
 101 #ifdef __LITTLE_ENDIAN
 102                 type = IIO_LE;
 103 #else
 104                 type = IIO_BE;
 105 #endif
 106         }
 107         return sprintf(buf, "%s:%c%d/%d>>%u\n",
 108                        iio_endian_prefix[type],
 109                        this_attr->c->scan_type.sign,
 110                        this_attr->c->scan_type.realbits,
 111                        this_attr->c->scan_type.storagebits,
 112                        this_attr->c->scan_type.shift);
 113 }
 114
 115 static ssize_t iio_scan_el_show(struct device *dev,
 116                                 struct device_attribute *attr,
 117                                 char *buf)
 118 {
 119         int ret;
 120         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 121
 122         /* Ensure ret is 0 or 1. */
 123         ret = !!test_bit(to_iio_dev_attr(attr)->address,
 124                        indio_dev->buffer->scan_mask);
 125
 126         return sprintf(buf, "%d\n", ret);
 127 }
 128
 129 static int iio_scan_mask_clear(struct iio_buffer *buffer, int bit)
 130 {
 131         clear_bit(bit, buffer->scan_mask);
 132         return 0;
 133 }
 134
 135 static ssize_t iio_scan_el_store(struct device *dev,
 136                                  struct device_attribute *attr,
 137                                  const char *buf,
 138                                  size_t len)
 139 {
 140         int ret;
 141         bool state;
 142         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 143         struct iio_buffer *buffer = indio_dev->buffer;
 144         struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 145
 146         ret = strtobool(buf, &state);
 147         if (ret < 0)
 148                 return ret;
 149         mutex_lock(&indio_dev->mlock);
 150         if (iio_buffer_is_active(indio_dev, indio_dev->buffer)) {
 151                 ret = -EBUSY;
 152                 goto error_ret;
 153         }
 154         ret = iio_scan_mask_query(indio_dev, buffer, this_attr->address);
 155         if (ret < 0)
 156                 goto error_ret;
 157         if (!state && ret) {
 158                 ret = iio_scan_mask_clear(buffer, this_attr->address);
 159                 if (ret)
 160                         goto error_ret;
 161         } else if (state && !ret) {
 162                 ret = iio_scan_mask_set(indio_dev, buffer, this_attr->address);
 163                 if (ret)
 164                         goto error_ret;
 165         }
 166
 167 error_ret:
 168         mutex_unlock(&indio_dev->mlock);
 169
 170         return ret < 0 ? ret : len;
 171
 172 }
 173
 174 static ssize_t iio_scan_el_ts_show(struct device *dev,
 175                                    struct device_attribute *attr,
 176                                    char *buf)
 177 {
 178         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 179         return sprintf(buf, "%d\n", indio_dev->buffer->scan_timestamp);
 180 }
 181
 182 static ssize_t iio_scan_el_ts_store(struct device *dev,
 183                                     struct device_attribute *attr,
 184                                     const char *buf,
 185                                     size_t len)
 186 {
 187         int ret;
 188         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 189         bool state;
 190
 191         ret = strtobool(buf, &state);
 192         if (ret < 0)
 193                 return ret;
 194
 195         mutex_lock(&indio_dev->mlock);
 196         if (iio_buffer_is_active(indio_dev, indio_dev->buffer)) {
 197                 ret = -EBUSY;
 198                 goto error_ret;
 199         }
 200         indio_dev->buffer->scan_timestamp = state;
 201 error_ret:
 202         mutex_unlock(&indio_dev->mlock);
 203
 204         return ret ? ret : len;
 205 }
 206
 207 static int iio_buffer_add_channel_sysfs(struct iio_dev *indio_dev,
 208                                         const struct iio_chan_spec *chan)
 209 {
 210         int ret, attrcount = 0;
 211         struct iio_buffer *buffer = indio_dev->buffer;
 212
 213         ret = __iio_add_chan_devattr("index",
 214                                      chan,
 215                                      &iio_show_scan_index,
 216                                      NULL,
 217                                      0,
 218                                      0,
 219                                      &indio_dev->dev,
 220                                      &buffer->scan_el_dev_attr_list);
 221         if (ret)
 222                 goto error_ret;
 223         attrcount++;
 224         ret = __iio_add_chan_devattr("type",
 225                                      chan,
 226                                      &iio_show_fixed_type,
 227                                      NULL,
 228                                      0,
 229                                      0,
 230                                      &indio_dev->dev,
 231                                      &buffer->scan_el_dev_attr_list);
 232         if (ret)
 233                 goto error_ret;
 234         attrcount++;
 235         if (chan->type != IIO_TIMESTAMP)
 236                 ret = __iio_add_chan_devattr("en",
 237                                              chan,
 238                                              &iio_scan_el_show,
 239                                              &iio_scan_el_store,
 240                                              chan->scan_index,
 241                                              0,
 242                                              &indio_dev->dev,
 243                                              &buffer->scan_el_dev_attr_list);
 244         else
 245                 ret = __iio_add_chan_devattr("en",
 246                                              chan,
 247                                              &iio_scan_el_ts_show,
 248                                              &iio_scan_el_ts_store,
 249                                              chan->scan_index,
 250                                              0,
 251                                              &indio_dev->dev,
 252                                              &buffer->scan_el_dev_attr_list);
 253         attrcount++;
 254         ret = attrcount;
 255 error_ret:
 256         return ret;
 257 }
 258
 259 static void iio_buffer_remove_and_free_scan_dev_attr(struct iio_dev *indio_dev,
 260                                                      struct iio_dev_attr *p)
 261 {
 262         kfree(p->dev_attr.attr.name);
 263         kfree(p);
 264 }
 265
 266 static void __iio_buffer_attr_cleanup(struct iio_dev *indio_dev)
 267 {
 268         struct iio_dev_attr *p, *n;
 269         struct iio_buffer *buffer = indio_dev->buffer;
 270
 271         list_for_each_entry_safe(p, n,
 272                                  &buffer->scan_el_dev_attr_list, l)
 273                 iio_buffer_remove_and_free_scan_dev_attr(indio_dev, p);
 274 }
 275
 276 static const char * const iio_scan_elements_group_name = "scan_elements";
 277
 278 int iio_buffer_register(struct iio_dev *indio_dev,
 279                         const struct iio_chan_spec *channels,
 280                         int num_channels)
 281 {
 282         struct iio_dev_attr *p;
 283         struct attribute **attr;
 284         struct iio_buffer *buffer = indio_dev->buffer;
 285         int ret, i, attrn, attrcount, attrcount_orig = 0;
 286
 287         if (buffer->attrs)
 288                 indio_dev->groups[indio_dev->groupcounter++] = buffer->attrs;
 289
 290         if (buffer->scan_el_attrs != NULL) {
 291                 attr = buffer->scan_el_attrs->attrs;
 292                 while (*attr++ != NULL)
 293                         attrcount_orig++;
 294         }
 295         attrcount = attrcount_orig;
 296         INIT_LIST_HEAD(&buffer->scan_el_dev_attr_list);
 297         if (channels) {
 298                 /* new magic */
 299                 for (i = 0; i < num_channels; i++) {
 300                         if (channels[i].scan_index < 0)
 301                                 continue;
 302
 303                         /* Establish necessary mask length */
 304                         if (channels[i].scan_index >
 305                             (int)indio_dev->masklength - 1)
 306                                 indio_dev->masklength
 307                                         = channels[i].scan_index + 1;
 308
 309                         ret = iio_buffer_add_channel_sysfs(indio_dev,
 310                                                          &channels[i]);
 311                         if (ret < 0)
 312                                 goto error_cleanup_dynamic;
 313                         attrcount += ret;
 314                         if (channels[i].type == IIO_TIMESTAMP)
 315                                 indio_dev->scan_index_timestamp =
 316                                         channels[i].scan_index;
 317                 }
 318                 if (indio_dev->masklength && buffer->scan_mask == NULL) {
 319                         buffer->scan_mask = kcalloc(BITS_TO_LONGS(indio_dev->masklength),
 320                                                     sizeof(*buffer->scan_mask),
 321                                                     GFP_KERNEL);
 322                         if (buffer->scan_mask == NULL) {
 323                                 ret = -ENOMEM;
 324                                 goto error_cleanup_dynamic;
 325                         }
 326                 }
 327         }
 328
 329         buffer->scan_el_group.name = iio_scan_elements_group_name;
 330
 331         buffer->scan_el_group.attrs = kcalloc(attrcount + 1,
 332                                               sizeof(buffer->scan_el_group.attrs[0]),
 333                                               GFP_KERNEL);
 334         if (buffer->scan_el_group.attrs == NULL) {
 335                 ret = -ENOMEM;
 336                 goto error_free_scan_mask;
 337         }
 338         if (buffer->scan_el_attrs)
 339                 memcpy(buffer->scan_el_group.attrs, buffer->scan_el_attrs,
 340                        sizeof(buffer->scan_el_group.attrs[0])*attrcount_orig);
 341         attrn = attrcount_orig;
 342
 343         list_for_each_entry(p, &buffer->scan_el_dev_attr_list, l)
 344                 buffer->scan_el_group.attrs[attrn++] = &p->dev_attr.attr;
 345         indio_dev->groups[indio_dev->groupcounter++] = &buffer->scan_el_group;
 346
 347         return 0;
 348
 349 error_free_scan_mask:
 350         kfree(buffer->scan_mask);
 351 error_cleanup_dynamic:
 352         __iio_buffer_attr_cleanup(indio_dev);
 353
 354         return ret;
 355 }
 356 EXPORT_SYMBOL(iio_buffer_register);
 357
 358 void iio_buffer_unregister(struct iio_dev *indio_dev)
 359 {
 360         kfree(indio_dev->buffer->scan_mask);
 361         kfree(indio_dev->buffer->scan_el_group.attrs);
 362         __iio_buffer_attr_cleanup(indio_dev);
 363 }
 364 EXPORT_SYMBOL(iio_buffer_unregister);
 365
 366 ssize_t iio_buffer_read_length(struct device *dev,
 367                                struct device_attribute *attr,
 368                                char *buf)
 369 {
 370         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 371         struct iio_buffer *buffer = indio_dev->buffer;
 372
 373         if (buffer->access->get_length)
 374                 return sprintf(buf, "%d\n",
 375                                buffer->access->get_length(buffer));
 376
 377         return 0;
 378 }
 379 EXPORT_SYMBOL(iio_buffer_read_length);
 380
 381 ssize_t iio_buffer_write_length(struct device *dev,
 382                                 struct device_attribute *attr,
 383                                 const char *buf,
 384                                 size_t len)
 385 {
 386         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 387         struct iio_buffer *buffer = indio_dev->buffer;
 388         unsigned int val;
 389         int ret;
 390
 391         ret = kstrtouint(buf, 10, &val);
 392         if (ret)
 393                 return ret;
 394
 395         if (buffer->access->get_length)
 396                 if (val == buffer->access->get_length(buffer))
 397                         return len;
 398
 399         mutex_lock(&indio_dev->mlock);
 400         if (iio_buffer_is_active(indio_dev, indio_dev->buffer)) {
 401                 ret = -EBUSY;
 402         } else {
 403                 if (buffer->access->set_length)
 404                         buffer->access->set_length(buffer, val);
 405                 ret = 0;
 406         }
 407         mutex_unlock(&indio_dev->mlock);
 408
 409         return ret ? ret : len;
 410 }
 411 EXPORT_SYMBOL(iio_buffer_write_length);
 412
 413 ssize_t iio_buffer_show_enable(struct device *dev,
 414                                struct device_attribute *attr,
 415                                char *buf)
 416 {
 417         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 418         return sprintf(buf, "%d\n",
 419                        iio_buffer_is_active(indio_dev,
 420                                             indio_dev->buffer));
 421 }
 422 EXPORT_SYMBOL(iio_buffer_show_enable);
 423
 424 /* note NULL used as error indicator as it doesn't make sense. */
 425 static const unsigned long *iio_scan_mask_match(const unsigned long *av_masks,
 426                                           unsigned int masklength,
 427                                           const unsigned long *mask)
 428 {
 429         if (bitmap_empty(mask, masklength))
 430                 return NULL;
 431         while (*av_masks) {
 432                 if (bitmap_subset(mask, av_masks, masklength))
 433                         return av_masks;
 434                 av_masks += BITS_TO_LONGS(masklength);
 435         }
 436         return NULL;
 437 }
 438
 439 static int iio_compute_scan_bytes(struct iio_dev *indio_dev, const long *mask,
 440                                   bool timestamp)
 441 {
 442         const struct iio_chan_spec *ch;
 443         unsigned bytes = 0;
 444         int length, i;
 445
 446         /* How much space will the demuxed element take? */
 447         for_each_set_bit(i, mask,
 448                          indio_dev->masklength) {
 449                 ch = iio_find_channel_from_si(indio_dev, i);
 450                 length = ch->scan_type.storagebits / 8;
 451                 bytes = ALIGN(bytes, length);
 452                 bytes += length;
 453         }
 454         if (timestamp) {
 455                 ch = iio_find_channel_from_si(indio_dev,
 456                                               indio_dev->scan_index_timestamp);
 457                 length = ch->scan_type.storagebits / 8;
 458                 bytes = ALIGN(bytes, length);
 459                 bytes += length;
 460         }
 461         return bytes;
 462 }
 463
 464 int iio_update_buffers(struct iio_dev *indio_dev,
 465                        struct iio_buffer *insert_buffer,
 466                        struct iio_buffer *remove_buffer)
 467 {
 468         int ret;
 469         int success = 0;
 470         struct iio_buffer *buffer;
 471         unsigned long *compound_mask;
 472         const unsigned long *old_mask;
 473
 474         /* Wind down existing buffers - iff there are any */
 475         if (!list_empty(&indio_dev->buffer_list)) {
 476                 if (indio_dev->setup_ops->predisable) {
 477                         ret = indio_dev->setup_ops->predisable(indio_dev);
 478                         if (ret)
 479                                 goto error_ret;
 480                 }
 481                 indio_dev->currentmode = INDIO_DIRECT_MODE;
 482                 if (indio_dev->setup_ops->postdisable) {
 483                         ret = indio_dev->setup_ops->postdisable(indio_dev);
 484                         if (ret)
 485                                 goto error_ret;
 486                 }
 487         }
 488         /* Keep a copy of current setup to allow roll back */
 489         old_mask = indio_dev->active_scan_mask;
 490         if (!indio_dev->available_scan_masks)
 491                 indio_dev->active_scan_mask = NULL;
 492
 493         if (remove_buffer)
 494                 list_del(&remove_buffer->buffer_list);
 495         if (insert_buffer)
 496                 list_add(&insert_buffer->buffer_list, &indio_dev->buffer_list);
 497
 498         /* If no buffers in list, we are done */
 499         if (list_empty(&indio_dev->buffer_list)) {
 500                 indio_dev->currentmode = INDIO_DIRECT_MODE;
 501                 if (indio_dev->available_scan_masks == NULL)
 502                         kfree(old_mask);
 503                 return 0;
 504         }
 505
 506         /* What scan mask do we actually have ?*/
 507         compound_mask = kcalloc(BITS_TO_LONGS(indio_dev->masklength),
 508                                 sizeof(long), GFP_KERNEL);
 509         if (compound_mask == NULL) {
 510                 if (indio_dev->available_scan_masks == NULL)
 511                         kfree(old_mask);
 512                 return -ENOMEM;
 513         }
 514         indio_dev->scan_timestamp = 0;
 515
 516         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
 517                 bitmap_or(compound_mask, compound_mask, buffer->scan_mask,
 518                           indio_dev->masklength);
 519                 indio_dev->scan_timestamp |= buffer->scan_timestamp;
 520         }
 521         if (indio_dev->available_scan_masks) {
 522                 indio_dev->active_scan_mask =
 523                         iio_scan_mask_match(indio_dev->available_scan_masks,
 524                                             indio_dev->masklength,
 525                                             compound_mask);
 526                 if (indio_dev->active_scan_mask == NULL) {
 527                         /*
 528                          * Roll back.
 529                          * Note can only occur when adding a buffer.
 530                          */
 531                         list_del(&insert_buffer->buffer_list);
 532                         indio_dev->active_scan_mask = old_mask;
 533                         success = -EINVAL;
 534                 }
 535         } else {
 536                 indio_dev->active_scan_mask = compound_mask;
 537         }
 538
 539         iio_update_demux(indio_dev);
 540
 541         /* Wind up again */
 542         if (indio_dev->setup_ops->preenable) {
 543                 ret = indio_dev->setup_ops->preenable(indio_dev);
 544                 if (ret) {
 545                         printk(KERN_ERR
 546                                "Buffer not started:"
 547                                "buffer preenable failed\n");
 548                         goto error_remove_inserted;
 549                 }
 550         }
 551         indio_dev->scan_bytes =
 552                 iio_compute_scan_bytes(indio_dev,
 553                                        indio_dev->active_scan_mask,
 554                                        indio_dev->scan_timestamp);
 555         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list)
 556                 if (buffer->access->request_update) {
 557                         ret = buffer->access->request_update(buffer);
 558                         if (ret) {
 559                                 printk(KERN_INFO
 560                                        "Buffer not started:"
 561                                        "buffer parameter update failed\n");
 562                                 goto error_run_postdisable;
 563                         }
 564                 }
 565         if (indio_dev->info->update_scan_mode) {
 566                 ret = indio_dev->info
 567                         ->update_scan_mode(indio_dev,
 568                                            indio_dev->active_scan_mask);
 569                 if (ret < 0) {
 570                         printk(KERN_INFO "update scan mode failed\n");
 571                         goto error_run_postdisable;
 572                 }
 573         }
 574         /* Definitely possible for devices to support both of these.*/
 575         if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) {
 576                 if (!indio_dev->trig) {
 577                         printk(KERN_INFO "Buffer not started: no trigger\n");
 578                         ret = -EINVAL;
 579                         /* Can only occur on first buffer */
 580                         goto error_run_postdisable;
 581                 }
 582                 indio_dev->currentmode = INDIO_BUFFER_TRIGGERED;
 583         } else if (indio_dev->modes & INDIO_BUFFER_HARDWARE) {
 584                 indio_dev->currentmode = INDIO_BUFFER_HARDWARE;
 585         } else { /* should never be reached */
 586                 ret = -EINVAL;
 587                 goto error_run_postdisable;
 588         }
 589
 590         if (indio_dev->setup_ops->postenable) {
 591                 ret = indio_dev->setup_ops->postenable(indio_dev);
 592                 if (ret) {
 593                         printk(KERN_INFO
 594                                "Buffer not started: postenable failed\n");
 595                         indio_dev->currentmode = INDIO_DIRECT_MODE;
 596                         if (indio_dev->setup_ops->postdisable)
 597                                 indio_dev->setup_ops->postdisable(indio_dev);
 598                         goto error_disable_all_buffers;
 599                 }
 600         }
 601
 602         if (indio_dev->available_scan_masks)
 603                 kfree(compound_mask);
 604         else
 605                 kfree(old_mask);
 606
 607         return success;
 608
 609 error_disable_all_buffers:
 610         indio_dev->currentmode = INDIO_DIRECT_MODE;
 611 error_run_postdisable:
 612         if (indio_dev->setup_ops->postdisable)
 613                 indio_dev->setup_ops->postdisable(indio_dev);
 614 error_remove_inserted:
 615
 616         if (insert_buffer)
 617                 list_del(&insert_buffer->buffer_list);
 618         indio_dev->active_scan_mask = old_mask;
 619         kfree(compound_mask);
 620 error_ret:
 621
 622         return ret;
 623 }
 624 EXPORT_SYMBOL_GPL(iio_update_buffers);
 625
 626 ssize_t iio_buffer_store_enable(struct device *dev,
 627                                 struct device_attribute *attr,
 628                                 const char *buf,
 629                                 size_t len)
 630 {
 631         int ret;
 632         bool requested_state;
 633         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 634         struct iio_buffer *pbuf = indio_dev->buffer;
 635         bool inlist;
 636
 637         ret = strtobool(buf, &requested_state);
 638         if (ret < 0)
 639                 return ret;
 640
 641         mutex_lock(&indio_dev->mlock);
 642
 643         /* Find out if it is in the list */
 644         inlist = iio_buffer_is_active(indio_dev, pbuf);
 645         /* Already in desired state */
 646         if (inlist == requested_state)
 647                 goto done;
 648
 649         if (requested_state)
 650                 ret = iio_update_buffers(indio_dev,
 651                                          indio_dev->buffer, NULL);
 652         else
 653                 ret = iio_update_buffers(indio_dev,
 654                                          NULL, indio_dev->buffer);
 655
 656         if (ret < 0)
 657                 goto done;
 658 done:
 659         mutex_unlock(&indio_dev->mlock);
 660         return (ret < 0) ? ret : len;
 661 }
 662 EXPORT_SYMBOL(iio_buffer_store_enable);
 663
 664 int iio_sw_buffer_preenable(struct iio_dev *indio_dev)
 665 {
 666         struct iio_buffer *buffer;
 667         unsigned bytes;
 668         dev_dbg(&indio_dev->dev, "%s\n", __func__);
 669
 670         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list)
 671                 if (buffer->access->set_bytes_per_datum) {
 672                         bytes = iio_compute_scan_bytes(indio_dev,
 673                                                        buffer->scan_mask,
 674                                                        buffer->scan_timestamp);
 675
 676                         buffer->access->set_bytes_per_datum(buffer, bytes);
 677                 }
 678         return 0;
 679 }
 680 EXPORT_SYMBOL(iio_sw_buffer_preenable);
 681
 682 /**
 683  * iio_validate_scan_mask_onehot() - Validates that exactly one channel is selected
 684  * @indio_dev: the iio device
 685  * @mask: scan mask to be checked
 686  *
 687  * Return true if exactly one bit is set in the scan mask, false otherwise. It
 688  * can be used for devices where only one channel can be active for sampling at
 689  * a time.
 690  */
 691 bool iio_validate_scan_mask_onehot(struct iio_dev *indio_dev,
 692         const unsigned long *mask)
 693 {
 694         return bitmap_weight(mask, indio_dev->masklength) == 1;
 695 }
 696 EXPORT_SYMBOL_GPL(iio_validate_scan_mask_onehot);
 697
 698 static bool iio_validate_scan_mask(struct iio_dev *indio_dev,
 699         const unsigned long *mask)
 700 {
 701         if (!indio_dev->setup_ops->validate_scan_mask)
 702                 return true;
 703
 704         return indio_dev->setup_ops->validate_scan_mask(indio_dev, mask);
 705 }
 706
 707 /**
 708  * iio_scan_mask_set() - set particular bit in the scan mask
 709  * @buffer: the buffer whose scan mask we are interested in
 710  * @bit: the bit to be set.
 711  *
 712  * Note that at this point we have no way of knowing what other
 713  * buffers might request, hence this code only verifies that the
 714  * individual buffers request is plausible.
 715  */
 716 int iio_scan_mask_set(struct iio_dev *indio_dev,
 717                       struct iio_buffer *buffer, int bit)
 718 {
 719         const unsigned long *mask;
 720         unsigned long *trialmask;
 721
 722         trialmask = kmalloc(sizeof(*trialmask)*
 723                             BITS_TO_LONGS(indio_dev->masklength),
 724                             GFP_KERNEL);
 725
 726         if (trialmask == NULL)
 727                 return -ENOMEM;
 728         if (!indio_dev->masklength) {
 729                 WARN_ON("trying to set scanmask prior to registering buffer\n");
 730                 goto err_invalid_mask;
 731         }
 732         bitmap_copy(trialmask, buffer->scan_mask, indio_dev->masklength);
 733         set_bit(bit, trialmask);
 734
 735         if (!iio_validate_scan_mask(indio_dev, trialmask))
 736                 goto err_invalid_mask;
 737
 738         if (indio_dev->available_scan_masks) {
 739                 mask = iio_scan_mask_match(indio_dev->available_scan_masks,
 740                                            indio_dev->masklength,
 741                                            trialmask);
 742                 if (!mask)
 743                         goto err_invalid_mask;
 744         }
 745         bitmap_copy(buffer->scan_mask, trialmask, indio_dev->masklength);
 746
 747         kfree(trialmask);
 748
 749         return 0;
 750
 751 err_invalid_mask:
 752         kfree(trialmask);
 753         return -EINVAL;
 754 }
 755 EXPORT_SYMBOL_GPL(iio_scan_mask_set);
 756
 757 int iio_scan_mask_query(struct iio_dev *indio_dev,
 758                         struct iio_buffer *buffer, int bit)
 759 {
 760         if (bit > indio_dev->masklength)
 761                 return -EINVAL;
 762
 763         if (!buffer->scan_mask)
 764                 return 0;
 765
 766         /* Ensure return value is 0 or 1. */
 767         return !!test_bit(bit, buffer->scan_mask);
 768 };
 769 EXPORT_SYMBOL_GPL(iio_scan_mask_query);
 770
 771 /**
 772  * struct iio_demux_table() - table describing demux memcpy ops
 773  * @from:       index to copy from
 774  * @to:         index to copy to
 775  * @length:     how many bytes to copy
 776  * @l:          list head used for management
 777  */
 778 struct iio_demux_table {
 779         unsigned from;
 780         unsigned to;
 781         unsigned length;
 782         struct list_head l;
 783 };
 784
 785 static unsigned char *iio_demux(struct iio_buffer *buffer,
 786                                  unsigned char *datain)
 787 {
 788         struct iio_demux_table *t;
 789
 790         if (list_empty(&buffer->demux_list))
 791                 return datain;
 792         list_for_each_entry(t, &buffer->demux_list, l)
 793                 memcpy(buffer->demux_bounce + t->to,
 794                        datain + t->from, t->length);
 795
 796         return buffer->demux_bounce;
 797 }
 798
 799 static int iio_push_to_buffer(struct iio_buffer *buffer, unsigned char *data)
 800 {
 801         unsigned char *dataout = iio_demux(buffer, data);
 802
 803         return buffer->access->store_to(buffer, dataout);
 804 }
 805
 806 static void iio_buffer_demux_free(struct iio_buffer *buffer)
 807 {
 808         struct iio_demux_table *p, *q;
 809         list_for_each_entry_safe(p, q, &buffer->demux_list, l) {
 810                 list_del(&p->l);
 811                 kfree(p);
 812         }
 813 }
 814
 815
 816 int iio_push_to_buffers(struct iio_dev *indio_dev, unsigned char *data)
 817 {
 818         int ret;
 819         struct iio_buffer *buf;
 820
 821         list_for_each_entry(buf, &indio_dev->buffer_list, buffer_list) {
 822                 ret = iio_push_to_buffer(buf, data);
 823                 if (ret < 0)
 824                         return ret;
 825         }
 826
 827         return 0;
 828 }
 829 EXPORT_SYMBOL_GPL(iio_push_to_buffers);
 830
 831 static int iio_buffer_update_demux(struct iio_dev *indio_dev,
 832                                    struct iio_buffer *buffer)
 833 {
 834         const struct iio_chan_spec *ch;
 835         int ret, in_ind = -1, out_ind, length;
 836         unsigned in_loc = 0, out_loc = 0;
 837         struct iio_demux_table *p;
 838
 839         /* Clear out any old demux */
 840         iio_buffer_demux_free(buffer);
 841         kfree(buffer->demux_bounce);
 842         buffer->demux_bounce = NULL;
 843
 844         /* First work out which scan mode we will actually have */
 845         if (bitmap_equal(indio_dev->active_scan_mask,
 846                          buffer->scan_mask,
 847                          indio_dev->masklength))
 848                 return 0;
 849
 850         /* Now we have the two masks, work from least sig and build up sizes */
 851         for_each_set_bit(out_ind,
 852                          buffer->scan_mask,
 853                          indio_dev->masklength) {
 854                 in_ind = find_next_bit(indio_dev->active_scan_mask,
 855                                        indio_dev->masklength,
 856                                        in_ind + 1);
 857                 while (in_ind != out_ind) {
 858                         in_ind = find_next_bit(indio_dev->active_scan_mask,
 859                                                indio_dev->masklength,
 860                                                in_ind + 1);
 861                         ch = iio_find_channel_from_si(indio_dev, in_ind);
 862                         length = ch->scan_type.storagebits/8;
 863                         /* Make sure we are aligned */
 864                         in_loc += length;
 865                         if (in_loc % length)
 866                                 in_loc += length - in_loc % length;
 867                 }
 868                 p = kmalloc(sizeof(*p), GFP_KERNEL);
 869                 if (p == NULL) {
 870                         ret = -ENOMEM;
 871                         goto error_clear_mux_table;
 872                 }
 873                 ch = iio_find_channel_from_si(indio_dev, in_ind);
 874                 length = ch->scan_type.storagebits/8;
 875                 if (out_loc % length)
 876                         out_loc += length - out_loc % length;
 877                 if (in_loc % length)
 878                         in_loc += length - in_loc % length;
 879                 p->from = in_loc;
 880                 p->to = out_loc;
 881                 p->length = length;
 882                 list_add_tail(&p->l, &buffer->demux_list);
 883                 out_loc += length;
 884                 in_loc += length;
 885         }
 886         /* Relies on scan_timestamp being last */
 887         if (buffer->scan_timestamp) {
 888                 p = kmalloc(sizeof(*p), GFP_KERNEL);
 889                 if (p == NULL) {
 890                         ret = -ENOMEM;
 891                         goto error_clear_mux_table;
 892                 }
 893                 ch = iio_find_channel_from_si(indio_dev,
 894                         indio_dev->scan_index_timestamp);
 895                 length = ch->scan_type.storagebits/8;
 896                 if (out_loc % length)
 897                         out_loc += length - out_loc % length;
 898                 if (in_loc % length)
 899                         in_loc += length - in_loc % length;
 900                 p->from = in_loc;
 901                 p->to = out_loc;
 902                 p->length = length;
 903                 list_add_tail(&p->l, &buffer->demux_list);
 904                 out_loc += length;
 905                 in_loc += length;
 906         }
 907         buffer->demux_bounce = kzalloc(out_loc, GFP_KERNEL);
 908         if (buffer->demux_bounce == NULL) {
 909                 ret = -ENOMEM;
 910                 goto error_clear_mux_table;
 911         }
 912         return 0;
 913
 914 error_clear_mux_table:
 915         iio_buffer_demux_free(buffer);
 916
 917         return ret;
 918 }
 919
 920 int iio_update_demux(struct iio_dev *indio_dev)
 921 {
 922         struct iio_buffer *buffer;
 923         int ret;
 924
 925         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
 926                 ret = iio_buffer_update_demux(indio_dev, buffer);
 927                 if (ret < 0)
 928                         goto error_clear_mux_table;
 929         }
 930         return 0;
 931
 932 error_clear_mux_table:
 933         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list)
 934                 iio_buffer_demux_free(buffer);
 935
 936         return ret;
 937 }
 938 EXPORT_SYMBOL_GPL(iio_update_demux);