drivers/staging/iio/imu/adis_buffer.c

   1 #include <linux/export.h>
   2 #include <linux/interrupt.h>
   3 #include <linux/mutex.h>
   4 #include <linux/kernel.h>
   5 #include <linux/spi/spi.h>
   6 #include <linux/slab.h>
   7
   8 #include <linux/iio/iio.h>
   9 #include <linux/iio/buffer.h>
  10 #include "../ring_sw.h"
  11 #include <linux/iio/trigger_consumer.h>
  12
  13 #include  "adis.h"
  14
  15 #define ADIS_MAX_OUTPUTS 12
  16
  17 static int adis_read_buffer_data(struct adis *adis, struct iio_dev *indio_dev)
  18 {
  19         int n_outputs = indio_dev->num_channels;
  20         struct spi_transfer xfers[ADIS_MAX_OUTPUTS + 1];
  21         struct spi_message msg;
  22         int ret;
  23         int i;
  24
  25         mutex_lock(&adis->txrx_lock);
  26
  27         spi_message_init(&msg);
  28
  29         memset(xfers, 0, sizeof(xfers));
  30         for (i = 0; i <= n_outputs; i++) {
  31                 xfers[i].bits_per_word = 8;
  32                 xfers[i].cs_change = 1;
  33                 xfers[i].len = 2;
  34                 xfers[i].delay_usecs = adis->data->read_delay;
  35                 if (i < n_outputs) {
  36                         xfers[i].tx_buf = adis->tx + 2 * i;
  37                         adis->tx[2 * i] = indio_dev->channels[i].address;
  38                         adis->tx[2 * i + 1] = 0;
  39                 }
  40                 if (i >= 1)
  41                         xfers[i].rx_buf = adis->rx + 2 * (i - 1);
  42                 spi_message_add_tail(&xfers[i], &msg);
  43         }
  44
  45         ret = spi_sync(adis->spi, &msg);
  46         if (ret)
  47                 dev_err(&adis->spi->dev, "Failed to read data: %d", ret);
  48
  49         mutex_unlock(&adis->txrx_lock);
  50
  51         return ret;
  52 }
  53
  54 static irqreturn_t adis_trigger_handler(int irq, void *p)
  55 {
  56         struct iio_poll_func *pf = p;
  57         struct iio_dev *indio_dev = pf->indio_dev;
  58         struct adis *adis = iio_device_get_drvdata(indio_dev);
  59         u16 *data;
  60         int i = 0;
  61
  62         data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
  63         if (data == NULL) {
  64                 dev_err(&adis->spi->dev, "Failed to allocate memory.");
  65                 return -ENOMEM;
  66         }
  67
  68         if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)
  69             && adis_read_buffer_data(adis, indio_dev) >= 0)
  70                 for (; i < bitmap_weight(indio_dev->active_scan_mask,
  71                                          indio_dev->masklength); i++)
  72                         data[i] = be16_to_cpup((__be16 *)&(adis->rx[i*2]));
  73
  74         /* Guaranteed to be aligned with 8 byte boundary */
  75         if (indio_dev->scan_timestamp)
  76                 *((s64 *)(PTR_ALIGN(data, sizeof(s64)))) = pf->timestamp;
  77
  78         iio_push_to_buffers(indio_dev, (u8 *)data);
  79
  80         iio_trigger_notify_done(indio_dev->trig);
  81         kfree(data);
  82
  83         return IRQ_HANDLED;
  84 }
  85
  86 static const struct iio_buffer_setup_ops adis_buffer_setup_ops = {
  87         .preenable = &iio_sw_buffer_preenable,
  88         .postenable = &iio_triggered_buffer_postenable,
  89         .predisable = &iio_triggered_buffer_predisable,
  90 };
  91
  92 static int adis_buffer_setup(struct iio_dev *indio_dev,
  93         irqreturn_t (*trigger_handler)(int, void *))
  94 {
  95         int ret = 0;
  96         struct iio_buffer *buffer;
  97
  98         if (!trigger_handler)
  99                 trigger_handler = &adis_trigger_handler;
 100
 101         buffer = iio_sw_rb_allocate(indio_dev);
 102         if (!buffer) {
 103                 ret = -ENOMEM;
 104                 return ret;
 105         }
 106
 107         indio_dev->buffer = buffer;
 108         indio_dev->setup_ops = &adis_buffer_setup_ops;
 109
 110         indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
 111                                                  trigger_handler,
 112                                                  IRQF_ONESHOT,
 113                                                  indio_dev,
 114                                                  "%s_consumer%d",
 115                                                  indio_dev->name,
 116                                                  indio_dev->id);
 117         if (indio_dev->pollfunc == NULL) {
 118                 ret = -ENOMEM;
 119                 goto error_iio_sw_rb_free;
 120         }
 121
 122         indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
 123         return 0;
 124
 125 error_iio_sw_rb_free:
 126         iio_sw_rb_free(indio_dev->buffer);
 127         return ret;
 128 }
 129
 130 static void adis_buffer_cleanup(struct iio_dev *indio_dev)
 131 {
 132         iio_dealloc_pollfunc(indio_dev->pollfunc);
 133         iio_sw_rb_free(indio_dev->buffer);
 134 }
 135
 136 /**
 137  * adis_setup_buffer_and_trigger() - Sets up buffer and trigger for the adis device
 138  * @adis: The adis device.
 139  * @indio_dev: The IIO device.
 140  * @trigger_handler: Optional trigger handler, may be NULL.
 141  *
 142  * Returns 0 on success, a negative error code otherwise.
 143  *
 144  * This function sets up the buffer and trigger for a adis devices.  If
 145  * 'trigger_handler' is NULL the default trigger handler will be used. The
 146  * default trigger handler will simply read the registers assigned to the
 147  * currently active channels.
 148  *
 149  * adis_cleanup_buffer_and_trigger() should be called to free the resources
 150  * allocated by this function.
 151  */
 152 int adis_setup_buffer_and_trigger(struct adis *adis, struct iio_dev *indio_dev,
 153         irqreturn_t (*trigger_handler)(int, void *))
 154 {
 155         int ret;
 156
 157         ret = adis_buffer_setup(indio_dev, trigger_handler);
 158         if (ret)
 159                 return ret;
 160
 161         ret = iio_buffer_register(indio_dev,
 162                                   indio_dev->channels,
 163                                   indio_dev->num_channels);
 164         if (ret) {
 165                 dev_err(&adis->spi->dev, "Failed to initialize buffer %d\n",
 166                         ret);
 167                 goto error_unreg_buffer_funcs;
 168         }
 169
 170         if (adis->spi->irq) {
 171                 ret = adis_probe_trigger(adis, indio_dev);
 172                 if (ret)
 173                         goto error_uninitialize_buffer;
 174         }
 175         return 0;
 176
 177 error_uninitialize_buffer:
 178         iio_buffer_unregister(indio_dev);
 179 error_unreg_buffer_funcs:
 180         adis_buffer_cleanup(indio_dev);
 181         return ret;
 182 }
 183 EXPORT_SYMBOL_GPL(adis_setup_buffer_and_trigger);
 184
 185 /**
 186  * adis_cleanup_buffer_and_trigger() - Free buffer and trigger resources
 187  * @adis: The adis device.
 188  * @indio_dev: The IIO device.
 189  *
 190  * Frees resources allocated by adis_setup_buffer_and_trigger()
 191  */
 192 void adis_cleanup_buffer_and_trigger(struct adis *adis,
 193         struct iio_dev *indio_dev)
 194 {
 195         if (adis->spi->irq)
 196                 adis_remove_trigger(adis);
 197         iio_buffer_unregister(indio_dev);
 198         adis_buffer_cleanup(indio_dev);
 199 }
 200 EXPORT_SYMBOL_GPL(adis_cleanup_buffer_and_trigger);