drivers/iio/imu/adis_buffer.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Common library for ADIS16XXX devices
   4  *
   5  * Copyright 2012 Analog Devices Inc.
   6  *   Author: Lars-Peter Clausen <[email protected]>
   7  */
   8
   9 #include <linux/export.h>
  10 #include <linux/interrupt.h>
  11 #include <linux/mutex.h>
  12 #include <linux/kernel.h>
  13 #include <linux/spi/spi.h>
  14 #include <linux/slab.h>
  15
  16 #include <linux/iio/iio.h>
  17 #include <linux/iio/buffer.h>
  18 #include <linux/iio/trigger_consumer.h>
  19 #include <linux/iio/triggered_buffer.h>
  20 #include <linux/iio/imu/adis.h>
  21
  22 static int adis_update_scan_mode_burst(struct iio_dev *indio_dev,
  23         const unsigned long *scan_mask)
  24 {
  25         struct adis *adis = iio_device_get_drvdata(indio_dev);
  26         unsigned int burst_length;
  27         u8 *tx;
  28
  29         /* All but the timestamp channel */
  30         burst_length = (indio_dev->num_channels - 1) * sizeof(u16);
  31         burst_length += adis->burst->extra_len;
  32
  33         adis->xfer = kcalloc(2, sizeof(*adis->xfer), GFP_KERNEL);
  34         if (!adis->xfer)
  35                 return -ENOMEM;
  36
  37         adis->buffer = kzalloc(burst_length + sizeof(u16), GFP_KERNEL);
  38         if (!adis->buffer) {
  39                 kfree(adis->xfer);
  40                 adis->xfer = NULL;
  41                 return -ENOMEM;
  42         }
  43
  44         tx = adis->buffer + burst_length;
  45         tx[0] = ADIS_READ_REG(adis->burst->reg_cmd);
  46         tx[1] = 0;
  47
  48         adis->xfer[0].tx_buf = tx;
  49         adis->xfer[0].bits_per_word = 8;
  50         adis->xfer[0].len = 2;
  51         adis->xfer[1].rx_buf = adis->buffer;
  52         adis->xfer[1].bits_per_word = 8;
  53         adis->xfer[1].len = burst_length;
  54
  55         spi_message_init(&adis->msg);
  56         spi_message_add_tail(&adis->xfer[0], &adis->msg);
  57         spi_message_add_tail(&adis->xfer[1], &adis->msg);
  58
  59         return 0;
  60 }
  61
  62 int adis_update_scan_mode(struct iio_dev *indio_dev,
  63         const unsigned long *scan_mask)
  64 {
  65         struct adis *adis = iio_device_get_drvdata(indio_dev);
  66         const struct iio_chan_spec *chan;
  67         unsigned int scan_count;
  68         unsigned int i, j;
  69         __be16 *tx, *rx;
  70
  71         kfree(adis->xfer);
  72         kfree(adis->buffer);
  73
  74         if (adis->burst && adis->burst->en)
  75                 return adis_update_scan_mode_burst(indio_dev, scan_mask);
  76
  77         scan_count = indio_dev->scan_bytes / 2;
  78
  79         adis->xfer = kcalloc(scan_count + 1, sizeof(*adis->xfer), GFP_KERNEL);
  80         if (!adis->xfer)
  81                 return -ENOMEM;
  82
  83         adis->buffer = kcalloc(indio_dev->scan_bytes, 2, GFP_KERNEL);
  84         if (!adis->buffer) {
  85                 kfree(adis->xfer);
  86                 adis->xfer = NULL;
  87                 return -ENOMEM;
  88         }
  89
  90         rx = adis->buffer;
  91         tx = rx + scan_count;
  92
  93         spi_message_init(&adis->msg);
  94
  95         for (j = 0; j <= scan_count; j++) {
  96                 adis->xfer[j].bits_per_word = 8;
  97                 if (j != scan_count)
  98                         adis->xfer[j].cs_change = 1;
  99                 adis->xfer[j].len = 2;
 100                 adis->xfer[j].delay_usecs = adis->data->read_delay;
 101                 if (j < scan_count)
 102                         adis->xfer[j].tx_buf = &tx[j];
 103                 if (j >= 1)
 104                         adis->xfer[j].rx_buf = &rx[j - 1];
 105                 spi_message_add_tail(&adis->xfer[j], &adis->msg);
 106         }
 107
 108         chan = indio_dev->channels;
 109         for (i = 0; i < indio_dev->num_channels; i++, chan++) {
 110                 if (!test_bit(chan->scan_index, scan_mask))
 111                         continue;
 112                 if (chan->scan_type.storagebits == 32)
 113                         *tx++ = cpu_to_be16((chan->address + 2) << 8);
 114                 *tx++ = cpu_to_be16(chan->address << 8);
 115         }
 116
 117         return 0;
 118 }
 119 EXPORT_SYMBOL_GPL(adis_update_scan_mode);
 120
 121 static irqreturn_t adis_trigger_handler(int irq, void *p)
 122 {
 123         struct iio_poll_func *pf = p;
 124         struct iio_dev *indio_dev = pf->indio_dev;
 125         struct adis *adis = iio_device_get_drvdata(indio_dev);
 126         int ret;
 127
 128         if (!adis->buffer)
 129                 return -ENOMEM;
 130
 131         if (adis->data->has_paging) {
 132                 mutex_lock(&adis->state_lock);
 133                 if (adis->current_page != 0) {
 134                         adis->tx[0] = ADIS_WRITE_REG(ADIS_REG_PAGE_ID);
 135                         adis->tx[1] = 0;
 136                         spi_write(adis->spi, adis->tx, 2);
 137                 }
 138         }
 139
 140         ret = spi_sync(adis->spi, &adis->msg);
 141         if (ret)
 142                 dev_err(&adis->spi->dev, "Failed to read data: %d", ret);
 143
 144
 145         if (adis->data->has_paging) {
 146                 adis->current_page = 0;
 147                 mutex_unlock(&adis->state_lock);
 148         }
 149
 150         iio_push_to_buffers_with_timestamp(indio_dev, adis->buffer,
 151                 pf->timestamp);
 152
 153         iio_trigger_notify_done(indio_dev->trig);
 154
 155         return IRQ_HANDLED;
 156 }
 157
 158 /**
 159  * adis_setup_buffer_and_trigger() - Sets up buffer and trigger for the adis device
 160  * @adis: The adis device.
 161  * @indio_dev: The IIO device.
 162  * @trigger_handler: Optional trigger handler, may be NULL.
 163  *
 164  * Returns 0 on success, a negative error code otherwise.
 165  *
 166  * This function sets up the buffer and trigger for a adis devices.  If
 167  * 'trigger_handler' is NULL the default trigger handler will be used. The
 168  * default trigger handler will simply read the registers assigned to the
 169  * currently active channels.
 170  *
 171  * adis_cleanup_buffer_and_trigger() should be called to free the resources
 172  * allocated by this function.
 173  */
 174 int adis_setup_buffer_and_trigger(struct adis *adis, struct iio_dev *indio_dev,
 175         irqreturn_t (*trigger_handler)(int, void *))
 176 {
 177         int ret;
 178
 179         if (!trigger_handler)
 180                 trigger_handler = adis_trigger_handler;
 181
 182         ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
 183                 trigger_handler, NULL);
 184         if (ret)
 185                 return ret;
 186
 187         if (adis->spi->irq) {
 188                 ret = adis_probe_trigger(adis, indio_dev);
 189                 if (ret)
 190                         goto error_buffer_cleanup;
 191         }
 192         return 0;
 193
 194 error_buffer_cleanup:
 195         iio_triggered_buffer_cleanup(indio_dev);
 196         return ret;
 197 }
 198 EXPORT_SYMBOL_GPL(adis_setup_buffer_and_trigger);
 199
 200 /**
 201  * adis_cleanup_buffer_and_trigger() - Free buffer and trigger resources
 202  * @adis: The adis device.
 203  * @indio_dev: The IIO device.
 204  *
 205  * Frees resources allocated by adis_setup_buffer_and_trigger()
 206  */
 207 void adis_cleanup_buffer_and_trigger(struct adis *adis,
 208         struct iio_dev *indio_dev)
 209 {
 210         if (adis->spi->irq)
 211                 adis_remove_trigger(adis);
 212         kfree(adis->buffer);
 213         kfree(adis->xfer);
 214         iio_triggered_buffer_cleanup(indio_dev);
 215 }
 216 EXPORT_SYMBOL_GPL(adis_cleanup_buffer_and_trigger);