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 void iio_disable_all_buffers(struct iio_dev *indio_dev)
 465 {
 466         struct iio_buffer *buffer, *_buffer;
 467
 468         if (list_empty(&indio_dev->buffer_list))
 469                 return;
 470
 471         if (indio_dev->setup_ops->predisable)
 472                 indio_dev->setup_ops->predisable(indio_dev);
 473
 474         list_for_each_entry_safe(buffer, _buffer,
 475                         &indio_dev->buffer_list, buffer_list)
 476                 list_del_init(&buffer->buffer_list);
 477
 478         indio_dev->currentmode = INDIO_DIRECT_MODE;
 479         if (indio_dev->setup_ops->postdisable)
 480                 indio_dev->setup_ops->postdisable(indio_dev);
 481
 482         if (indio_dev->available_scan_masks == NULL)
 483                 kfree(indio_dev->active_scan_mask);
 484 }
 485
 486 int iio_update_buffers(struct iio_dev *indio_dev,
 487                        struct iio_buffer *insert_buffer,
 488                        struct iio_buffer *remove_buffer)
 489 {
 490         int ret;
 491         int success = 0;
 492         struct iio_buffer *buffer;
 493         unsigned long *compound_mask;
 494         const unsigned long *old_mask;
 495
 496         /* Wind down existing buffers - iff there are any */
 497         if (!list_empty(&indio_dev->buffer_list)) {
 498                 if (indio_dev->setup_ops->predisable) {
 499                         ret = indio_dev->setup_ops->predisable(indio_dev);
 500                         if (ret)
 501                                 goto error_ret;
 502                 }
 503                 indio_dev->currentmode = INDIO_DIRECT_MODE;
 504                 if (indio_dev->setup_ops->postdisable) {
 505                         ret = indio_dev->setup_ops->postdisable(indio_dev);
 506                         if (ret)
 507                                 goto error_ret;
 508                 }
 509         }
 510         /* Keep a copy of current setup to allow roll back */
 511         old_mask = indio_dev->active_scan_mask;
 512         if (!indio_dev->available_scan_masks)
 513                 indio_dev->active_scan_mask = NULL;
 514
 515         if (remove_buffer)
 516                 list_del(&remove_buffer->buffer_list);
 517         if (insert_buffer)
 518                 list_add(&insert_buffer->buffer_list, &indio_dev->buffer_list);
 519
 520         /* If no buffers in list, we are done */
 521         if (list_empty(&indio_dev->buffer_list)) {
 522                 indio_dev->currentmode = INDIO_DIRECT_MODE;
 523                 if (indio_dev->available_scan_masks == NULL)
 524                         kfree(old_mask);
 525                 return 0;
 526         }
 527
 528         /* What scan mask do we actually have ?*/
 529         compound_mask = kcalloc(BITS_TO_LONGS(indio_dev->masklength),
 530                                 sizeof(long), GFP_KERNEL);
 531         if (compound_mask == NULL) {
 532                 if (indio_dev->available_scan_masks == NULL)
 533                         kfree(old_mask);
 534                 return -ENOMEM;
 535         }
 536         indio_dev->scan_timestamp = 0;
 537
 538         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
 539                 bitmap_or(compound_mask, compound_mask, buffer->scan_mask,
 540                           indio_dev->masklength);
 541                 indio_dev->scan_timestamp |= buffer->scan_timestamp;
 542         }
 543         if (indio_dev->available_scan_masks) {
 544                 indio_dev->active_scan_mask =
 545                         iio_scan_mask_match(indio_dev->available_scan_masks,
 546                                             indio_dev->masklength,
 547                                             compound_mask);
 548                 if (indio_dev->active_scan_mask == NULL) {
 549                         /*
 550                          * Roll back.
 551                          * Note can only occur when adding a buffer.
 552                          */
 553                         list_del(&insert_buffer->buffer_list);
 554                         if (old_mask) {
 555                                 indio_dev->active_scan_mask = old_mask;
 556                                 success = -EINVAL;
 557                         }
 558                         else {
 559                                 kfree(compound_mask);
 560                                 ret = -EINVAL;
 561                                 goto error_ret;
 562                         }
 563                 }
 564         } else {
 565                 indio_dev->active_scan_mask = compound_mask;
 566         }
 567
 568         iio_update_demux(indio_dev);
 569
 570         /* Wind up again */
 571         if (indio_dev->setup_ops->preenable) {
 572                 ret = indio_dev->setup_ops->preenable(indio_dev);
 573                 if (ret) {
 574                         printk(KERN_ERR
 575                                "Buffer not started: buffer preenable failed (%d)\n", ret);
 576                         goto error_remove_inserted;
 577                 }
 578         }
 579         indio_dev->scan_bytes =
 580                 iio_compute_scan_bytes(indio_dev,
 581                                        indio_dev->active_scan_mask,
 582                                        indio_dev->scan_timestamp);
 583         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list)
 584                 if (buffer->access->request_update) {
 585                         ret = buffer->access->request_update(buffer);
 586                         if (ret) {
 587                                 printk(KERN_INFO
 588                                        "Buffer not started: buffer parameter update failed (%d)\n", ret);
 589                                 goto error_run_postdisable;
 590                         }
 591                 }
 592         if (indio_dev->info->update_scan_mode) {
 593                 ret = indio_dev->info
 594                         ->update_scan_mode(indio_dev,
 595                                            indio_dev->active_scan_mask);
 596                 if (ret < 0) {
 597                         printk(KERN_INFO "Buffer not started: update scan mode failed (%d)\n", ret);
 598                         goto error_run_postdisable;
 599                 }
 600         }
 601         /* Definitely possible for devices to support both of these.*/
 602         if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) {
 603                 if (!indio_dev->trig) {
 604                         printk(KERN_INFO "Buffer not started: no trigger\n");
 605                         ret = -EINVAL;
 606                         /* Can only occur on first buffer */
 607                         goto error_run_postdisable;
 608                 }
 609                 indio_dev->currentmode = INDIO_BUFFER_TRIGGERED;
 610         } else if (indio_dev->modes & INDIO_BUFFER_HARDWARE) {
 611                 indio_dev->currentmode = INDIO_BUFFER_HARDWARE;
 612         } else { /* should never be reached */
 613                 ret = -EINVAL;
 614                 goto error_run_postdisable;
 615         }
 616
 617         if (indio_dev->setup_ops->postenable) {
 618                 ret = indio_dev->setup_ops->postenable(indio_dev);
 619                 if (ret) {
 620                         printk(KERN_INFO
 621                                "Buffer not started: postenable failed (%d)\n", ret);
 622                         indio_dev->currentmode = INDIO_DIRECT_MODE;
 623                         if (indio_dev->setup_ops->postdisable)
 624                                 indio_dev->setup_ops->postdisable(indio_dev);
 625                         goto error_disable_all_buffers;
 626                 }
 627         }
 628
 629         if (indio_dev->available_scan_masks)
 630                 kfree(compound_mask);
 631         else
 632                 kfree(old_mask);
 633
 634         return success;
 635
 636 error_disable_all_buffers:
 637         indio_dev->currentmode = INDIO_DIRECT_MODE;
 638 error_run_postdisable:
 639         if (indio_dev->setup_ops->postdisable)
 640                 indio_dev->setup_ops->postdisable(indio_dev);
 641 error_remove_inserted:
 642
 643         if (insert_buffer)
 644                 list_del(&insert_buffer->buffer_list);
 645         indio_dev->active_scan_mask = old_mask;
 646         kfree(compound_mask);
 647 error_ret:
 648
 649         return ret;
 650 }
 651 EXPORT_SYMBOL_GPL(iio_update_buffers);
 652
 653 ssize_t iio_buffer_store_enable(struct device *dev,
 654                                 struct device_attribute *attr,
 655                                 const char *buf,
 656                                 size_t len)
 657 {
 658         int ret;
 659         bool requested_state;
 660         struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 661         struct iio_buffer *pbuf = indio_dev->buffer;
 662         bool inlist;
 663
 664         ret = strtobool(buf, &requested_state);
 665         if (ret < 0)
 666                 return ret;
 667
 668         mutex_lock(&indio_dev->mlock);
 669
 670         /* Find out if it is in the list */
 671         inlist = iio_buffer_is_active(indio_dev, pbuf);
 672         /* Already in desired state */
 673         if (inlist == requested_state)
 674                 goto done;
 675
 676         if (requested_state)
 677                 ret = iio_update_buffers(indio_dev,
 678                                          indio_dev->buffer, NULL);
 679         else
 680                 ret = iio_update_buffers(indio_dev,
 681                                          NULL, indio_dev->buffer);
 682
 683         if (ret < 0)
 684                 goto done;
 685 done:
 686         mutex_unlock(&indio_dev->mlock);
 687         return (ret < 0) ? ret : len;
 688 }
 689 EXPORT_SYMBOL(iio_buffer_store_enable);
 690
 691 int iio_sw_buffer_preenable(struct iio_dev *indio_dev)
 692 {
 693         struct iio_buffer *buffer;
 694         unsigned bytes;
 695         dev_dbg(&indio_dev->dev, "%s\n", __func__);
 696
 697         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list)
 698                 if (buffer->access->set_bytes_per_datum) {
 699                         bytes = iio_compute_scan_bytes(indio_dev,
 700                                                        buffer->scan_mask,
 701                                                        buffer->scan_timestamp);
 702
 703                         buffer->access->set_bytes_per_datum(buffer, bytes);
 704                 }
 705         return 0;
 706 }
 707 EXPORT_SYMBOL(iio_sw_buffer_preenable);
 708
 709 /**
 710  * iio_validate_scan_mask_onehot() - Validates that exactly one channel is selected
 711  * @indio_dev: the iio device
 712  * @mask: scan mask to be checked
 713  *
 714  * Return true if exactly one bit is set in the scan mask, false otherwise. It
 715  * can be used for devices where only one channel can be active for sampling at
 716  * a time.
 717  */
 718 bool iio_validate_scan_mask_onehot(struct iio_dev *indio_dev,
 719         const unsigned long *mask)
 720 {
 721         return bitmap_weight(mask, indio_dev->masklength) == 1;
 722 }
 723 EXPORT_SYMBOL_GPL(iio_validate_scan_mask_onehot);
 724
 725 static bool iio_validate_scan_mask(struct iio_dev *indio_dev,
 726         const unsigned long *mask)
 727 {
 728         if (!indio_dev->setup_ops->validate_scan_mask)
 729                 return true;
 730
 731         return indio_dev->setup_ops->validate_scan_mask(indio_dev, mask);
 732 }
 733
 734 /**
 735  * iio_scan_mask_set() - set particular bit in the scan mask
 736  * @buffer: the buffer whose scan mask we are interested in
 737  * @bit: the bit to be set.
 738  *
 739  * Note that at this point we have no way of knowing what other
 740  * buffers might request, hence this code only verifies that the
 741  * individual buffers request is plausible.
 742  */
 743 int iio_scan_mask_set(struct iio_dev *indio_dev,
 744                       struct iio_buffer *buffer, int bit)
 745 {
 746         const unsigned long *mask;
 747         unsigned long *trialmask;
 748
 749         trialmask = kmalloc(sizeof(*trialmask)*
 750                             BITS_TO_LONGS(indio_dev->masklength),
 751                             GFP_KERNEL);
 752
 753         if (trialmask == NULL)
 754                 return -ENOMEM;
 755         if (!indio_dev->masklength) {
 756                 WARN(1, "trying to set scanmask prior to registering buffer\n");
 757                 goto err_invalid_mask;
 758         }
 759         bitmap_copy(trialmask, buffer->scan_mask, indio_dev->masklength);
 760         set_bit(bit, trialmask);
 761
 762         if (!iio_validate_scan_mask(indio_dev, trialmask))
 763                 goto err_invalid_mask;
 764
 765         if (indio_dev->available_scan_masks) {
 766                 mask = iio_scan_mask_match(indio_dev->available_scan_masks,
 767                                            indio_dev->masklength,
 768                                            trialmask);
 769                 if (!mask)
 770                         goto err_invalid_mask;
 771         }
 772         bitmap_copy(buffer->scan_mask, trialmask, indio_dev->masklength);
 773
 774         kfree(trialmask);
 775
 776         return 0;
 777
 778 err_invalid_mask:
 779         kfree(trialmask);
 780         return -EINVAL;
 781 }
 782 EXPORT_SYMBOL_GPL(iio_scan_mask_set);
 783
 784 int iio_scan_mask_query(struct iio_dev *indio_dev,
 785                         struct iio_buffer *buffer, int bit)
 786 {
 787         if (bit > indio_dev->masklength)
 788                 return -EINVAL;
 789
 790         if (!buffer->scan_mask)
 791                 return 0;
 792
 793         /* Ensure return value is 0 or 1. */
 794         return !!test_bit(bit, buffer->scan_mask);
 795 };
 796 EXPORT_SYMBOL_GPL(iio_scan_mask_query);
 797
 798 /**
 799  * struct iio_demux_table() - table describing demux memcpy ops
 800  * @from:       index to copy from
 801  * @to:         index to copy to
 802  * @length:     how many bytes to copy
 803  * @l:          list head used for management
 804  */
 805 struct iio_demux_table {
 806         unsigned from;
 807         unsigned to;
 808         unsigned length;
 809         struct list_head l;
 810 };
 811
 812 static unsigned char *iio_demux(struct iio_buffer *buffer,
 813                                  unsigned char *datain)
 814 {
 815         struct iio_demux_table *t;
 816
 817         if (list_empty(&buffer->demux_list))
 818                 return datain;
 819         list_for_each_entry(t, &buffer->demux_list, l)
 820                 memcpy(buffer->demux_bounce + t->to,
 821                        datain + t->from, t->length);
 822
 823         return buffer->demux_bounce;
 824 }
 825
 826 static int iio_push_to_buffer(struct iio_buffer *buffer, unsigned char *data)
 827 {
 828         unsigned char *dataout = iio_demux(buffer, data);
 829
 830         return buffer->access->store_to(buffer, dataout);
 831 }
 832
 833 static void iio_buffer_demux_free(struct iio_buffer *buffer)
 834 {
 835         struct iio_demux_table *p, *q;
 836         list_for_each_entry_safe(p, q, &buffer->demux_list, l) {
 837                 list_del(&p->l);
 838                 kfree(p);
 839         }
 840 }
 841
 842
 843 int iio_push_to_buffers(struct iio_dev *indio_dev, unsigned char *data)
 844 {
 845         int ret;
 846         struct iio_buffer *buf;
 847
 848         list_for_each_entry(buf, &indio_dev->buffer_list, buffer_list) {
 849                 ret = iio_push_to_buffer(buf, data);
 850                 if (ret < 0)
 851                         return ret;
 852         }
 853
 854         return 0;
 855 }
 856 EXPORT_SYMBOL_GPL(iio_push_to_buffers);
 857
 858 static int iio_buffer_update_demux(struct iio_dev *indio_dev,
 859                                    struct iio_buffer *buffer)
 860 {
 861         const struct iio_chan_spec *ch;
 862         int ret, in_ind = -1, out_ind, length;
 863         unsigned in_loc = 0, out_loc = 0;
 864         struct iio_demux_table *p;
 865
 866         /* Clear out any old demux */
 867         iio_buffer_demux_free(buffer);
 868         kfree(buffer->demux_bounce);
 869         buffer->demux_bounce = NULL;
 870
 871         /* First work out which scan mode we will actually have */
 872         if (bitmap_equal(indio_dev->active_scan_mask,
 873                          buffer->scan_mask,
 874                          indio_dev->masklength))
 875                 return 0;
 876
 877         /* Now we have the two masks, work from least sig and build up sizes */
 878         for_each_set_bit(out_ind,
 879                          buffer->scan_mask,
 880                          indio_dev->masklength) {
 881                 in_ind = find_next_bit(indio_dev->active_scan_mask,
 882                                        indio_dev->masklength,
 883                                        in_ind + 1);
 884                 while (in_ind != out_ind) {
 885                         in_ind = find_next_bit(indio_dev->active_scan_mask,
 886                                                indio_dev->masklength,
 887                                                in_ind + 1);
 888                         ch = iio_find_channel_from_si(indio_dev, in_ind);
 889                         length = ch->scan_type.storagebits/8;
 890                         /* Make sure we are aligned */
 891                         in_loc += length;
 892                         if (in_loc % length)
 893                                 in_loc += length - in_loc % length;
 894                 }
 895                 p = kmalloc(sizeof(*p), GFP_KERNEL);
 896                 if (p == NULL) {
 897                         ret = -ENOMEM;
 898                         goto error_clear_mux_table;
 899                 }
 900                 ch = iio_find_channel_from_si(indio_dev, in_ind);
 901                 length = ch->scan_type.storagebits/8;
 902                 if (out_loc % length)
 903                         out_loc += length - out_loc % length;
 904                 if (in_loc % length)
 905                         in_loc += length - in_loc % length;
 906                 p->from = in_loc;
 907                 p->to = out_loc;
 908                 p->length = length;
 909                 list_add_tail(&p->l, &buffer->demux_list);
 910                 out_loc += length;
 911                 in_loc += length;
 912         }
 913         /* Relies on scan_timestamp being last */
 914         if (buffer->scan_timestamp) {
 915                 p = kmalloc(sizeof(*p), GFP_KERNEL);
 916                 if (p == NULL) {
 917                         ret = -ENOMEM;
 918                         goto error_clear_mux_table;
 919                 }
 920                 ch = iio_find_channel_from_si(indio_dev,
 921                         indio_dev->scan_index_timestamp);
 922                 length = ch->scan_type.storagebits/8;
 923                 if (out_loc % length)
 924                         out_loc += length - out_loc % length;
 925                 if (in_loc % length)
 926                         in_loc += length - in_loc % length;
 927                 p->from = in_loc;
 928                 p->to = out_loc;
 929                 p->length = length;
 930                 list_add_tail(&p->l, &buffer->demux_list);
 931                 out_loc += length;
 932                 in_loc += length;
 933         }
 934         buffer->demux_bounce = kzalloc(out_loc, GFP_KERNEL);
 935         if (buffer->demux_bounce == NULL) {
 936                 ret = -ENOMEM;
 937                 goto error_clear_mux_table;
 938         }
 939         return 0;
 940
 941 error_clear_mux_table:
 942         iio_buffer_demux_free(buffer);
 943
 944         return ret;
 945 }
 946
 947 int iio_update_demux(struct iio_dev *indio_dev)
 948 {
 949         struct iio_buffer *buffer;
 950         int ret;
 951
 952         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
 953                 ret = iio_buffer_update_demux(indio_dev, buffer);
 954                 if (ret < 0)
 955                         goto error_clear_mux_table;
 956         }
 957         return 0;
 958
 959 error_clear_mux_table:
 960         list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list)
 961                 iio_buffer_demux_free(buffer);
 962
 963         return ret;
 964 }
 965 EXPORT_SYMBOL_GPL(iio_update_demux);