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