/drivers/staging/iio/accel/adis16204_ring.c

https://bitbucket.org/cyanogenmod/android_kernel_asus_tf300t · C · 154 lines · 120 code · 23 blank · 11 comment · 12 complexity · 7cd3bc9241c0b18a7a46bd0a87b2f62b MD5 · raw file

  1. #include <linux/interrupt.h>
  2. #include <linux/irq.h>
  3. #include <linux/gpio.h>
  4. #include <linux/workqueue.h>
  5. #include <linux/mutex.h>
  6. #include <linux/device.h>
  7. #include <linux/kernel.h>
  8. #include <linux/spi/spi.h>
  9. #include <linux/slab.h>
  10. #include <linux/sysfs.h>
  11. #include <linux/list.h>
  12. #include "../iio.h"
  13. #include "../sysfs.h"
  14. #include "../ring_sw.h"
  15. #include "accel.h"
  16. #include "../trigger.h"
  17. #include "adis16204.h"
  18. /**
  19. * adis16204_read_ring_data() read data registers which will be placed into ring
  20. * @dev: device associated with child of actual device (iio_dev or iio_trig)
  21. * @rx: somewhere to pass back the value read
  22. **/
  23. static int adis16204_read_ring_data(struct device *dev, u8 *rx)
  24. {
  25. struct spi_message msg;
  26. struct iio_dev *indio_dev = dev_get_drvdata(dev);
  27. struct adis16204_state *st = iio_priv(indio_dev);
  28. struct spi_transfer xfers[ADIS16204_OUTPUTS + 1];
  29. int ret;
  30. int i;
  31. mutex_lock(&st->buf_lock);
  32. spi_message_init(&msg);
  33. memset(xfers, 0, sizeof(xfers));
  34. for (i = 0; i <= ADIS16204_OUTPUTS; i++) {
  35. xfers[i].bits_per_word = 8;
  36. xfers[i].cs_change = 1;
  37. xfers[i].len = 2;
  38. xfers[i].delay_usecs = 20;
  39. xfers[i].tx_buf = st->tx + 2 * i;
  40. st->tx[2 * i]
  41. = ADIS16204_READ_REG(ADIS16204_SUPPLY_OUT + 2 * i);
  42. st->tx[2 * i + 1] = 0;
  43. if (i >= 1)
  44. xfers[i].rx_buf = rx + 2 * (i - 1);
  45. spi_message_add_tail(&xfers[i], &msg);
  46. }
  47. ret = spi_sync(st->us, &msg);
  48. if (ret)
  49. dev_err(&st->us->dev, "problem when burst reading");
  50. mutex_unlock(&st->buf_lock);
  51. return ret;
  52. }
  53. /* Whilst this makes a lot of calls to iio_sw_ring functions - it is to device
  54. * specific to be rolled into the core.
  55. */
  56. static irqreturn_t adis16204_trigger_handler(int irq, void *p)
  57. {
  58. struct iio_poll_func *pf = p;
  59. struct iio_dev *indio_dev = pf->private_data;
  60. struct adis16204_state *st = iio_priv(indio_dev);
  61. struct iio_ring_buffer *ring = indio_dev->ring;
  62. int i = 0;
  63. s16 *data;
  64. size_t datasize = ring->access->get_bytes_per_datum(ring);
  65. data = kmalloc(datasize, GFP_KERNEL);
  66. if (data == NULL) {
  67. dev_err(&st->us->dev, "memory alloc failed in ring bh");
  68. return -ENOMEM;
  69. }
  70. if (ring->scan_count)
  71. if (adis16204_read_ring_data(&indio_dev->dev, st->rx) >= 0)
  72. for (; i < ring->scan_count; i++)
  73. data[i] = be16_to_cpup(
  74. (__be16 *)&(st->rx[i*2]));
  75. /* Guaranteed to be aligned with 8 byte boundary */
  76. if (ring->scan_timestamp)
  77. *((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
  78. ring->access->store_to(ring, (u8 *)data, pf->timestamp);
  79. iio_trigger_notify_done(indio_dev->trig);
  80. kfree(data);
  81. return IRQ_HANDLED;
  82. }
  83. void adis16204_unconfigure_ring(struct iio_dev *indio_dev)
  84. {
  85. iio_dealloc_pollfunc(indio_dev->pollfunc);
  86. iio_sw_rb_free(indio_dev->ring);
  87. }
  88. static const struct iio_ring_setup_ops adis16204_ring_setup_ops = {
  89. .preenable = &iio_sw_ring_preenable,
  90. .postenable = &iio_triggered_ring_postenable,
  91. .predisable = &iio_triggered_ring_predisable,
  92. };
  93. int adis16204_configure_ring(struct iio_dev *indio_dev)
  94. {
  95. int ret = 0;
  96. struct iio_ring_buffer *ring;
  97. ring = iio_sw_rb_allocate(indio_dev);
  98. if (!ring) {
  99. ret = -ENOMEM;
  100. return ret;
  101. }
  102. indio_dev->ring = ring;
  103. /* Effectively select the ring buffer implementation */
  104. ring->access = &ring_sw_access_funcs;
  105. ring->bpe = 2;
  106. ring->scan_timestamp = true;
  107. ring->setup_ops = &adis16204_ring_setup_ops;
  108. ring->owner = THIS_MODULE;
  109. /* Set default scan mode */
  110. iio_scan_mask_set(ring, ADIS16204_SCAN_SUPPLY);
  111. iio_scan_mask_set(ring, ADIS16204_SCAN_ACC_X);
  112. iio_scan_mask_set(ring, ADIS16204_SCAN_ACC_Y);
  113. iio_scan_mask_set(ring, ADIS16204_SCAN_AUX_ADC);
  114. iio_scan_mask_set(ring, ADIS16204_SCAN_TEMP);
  115. indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
  116. &adis16204_trigger_handler,
  117. IRQF_ONESHOT,
  118. indio_dev,
  119. "%s_consumer%d",
  120. indio_dev->name,
  121. indio_dev->id);
  122. if (indio_dev->pollfunc == NULL) {
  123. ret = -ENOMEM;
  124. goto error_iio_sw_rb_free;
  125. }
  126. indio_dev->modes |= INDIO_RING_TRIGGERED;
  127. return 0;
  128. error_iio_sw_rb_free:
  129. iio_sw_rb_free(indio_dev->ring);
  130. return ret;
  131. }