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