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 <lars@metafoo.de>
   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, burst_max_length;
  27         u8 *tx;
  28
  29         burst_length = adis->data->burst_len + adis->burst_extra_len;
  30
  31         if (adis->data->burst_max_len)
  32                 burst_max_length = adis->data->burst_max_len;
  33         else
  34                 burst_max_length = burst_length;
  35
  36         adis->xfer = kcalloc(2, sizeof(*adis->xfer), GFP_KERNEL);
  37         if (!adis->xfer)
  38                 return -ENOMEM;
  39
  40         adis->buffer = kzalloc(burst_max_length + sizeof(u16), GFP_KERNEL);
  41         if (!adis->buffer) {
  42                 kfree(adis->xfer);
  43                 adis->xfer = NULL;
  44                 return -ENOMEM;
  45         }
  46
  47         tx = adis->buffer + burst_max_length;
  48         tx[0] = ADIS_READ_REG(adis->data->burst_reg_cmd);
  49         tx[1] = 0;
  50
  51         adis->xfer[0].tx_buf = tx;
  52         adis->xfer[0].bits_per_word = 8;
  53         adis->xfer[0].len = 2;
  54         adis->xfer[1].rx_buf = adis->buffer;
  55         adis->xfer[1].bits_per_word = 8;
  56         adis->xfer[1].len = burst_length;
  57
  58         spi_message_init(&adis->msg);
  59         spi_message_add_tail(&adis->xfer[0], &adis->msg);
  60         spi_message_add_tail(&adis->xfer[1], &adis->msg);
  61
  62         return 0;
  63 }
  64
  65 int adis_update_scan_mode(struct iio_dev *indio_dev,
  66         const unsigned long *scan_mask)
  67 {
  68         struct adis *adis = iio_device_get_drvdata(indio_dev);
  69         const struct iio_chan_spec *chan;
  70         unsigned int scan_count;
  71         unsigned int i, j;
  72         __be16 *tx, *rx;
  73
  74         kfree(adis->xfer);
  75         kfree(adis->buffer);
  76
  77         if (adis->data->burst_len)
  78                 return adis_update_scan_mode_burst(indio_dev, scan_mask);
  79
  80         scan_count = indio_dev->scan_bytes / 2;
  81
  82         adis->xfer = kcalloc(scan_count + 1, sizeof(*adis->xfer), GFP_KERNEL);
  83         if (!adis->xfer)
  84                 return -ENOMEM;
  85
  86         adis->buffer = kcalloc(indio_dev->scan_bytes, 2, GFP_KERNEL);
  87         if (!adis->buffer) {
  88                 kfree(adis->xfer);
  89                 adis->xfer = NULL;
  90                 return -ENOMEM;
  91         }
  92
  93         rx = adis->buffer;
  94         tx = rx + scan_count;
  95
  96         spi_message_init(&adis->msg);
  97
  98         for (j = 0; j <= scan_count; j++) {
  99                 adis->xfer[j].bits_per_word = 8;
 100                 if (j != scan_count)
 101                         adis->xfer[j].cs_change = 1;
 102                 adis->xfer[j].len = 2;
 103                 adis->xfer[j].delay.value = adis->data->read_delay;
 104                 adis->xfer[j].delay.unit = SPI_DELAY_UNIT_USECS;
 105                 if (j < scan_count)
 106                         adis->xfer[j].tx_buf = &tx[j];
 107                 if (j >= 1)
 108                         adis->xfer[j].rx_buf = &rx[j - 1];
 109                 spi_message_add_tail(&adis->xfer[j], &adis->msg);
 110         }
 111
 112         chan = indio_dev->channels;
 113         for (i = 0; i < indio_dev->num_channels; i++, chan++) {
 114                 if (!test_bit(chan->scan_index, scan_mask))
 115                         continue;
 116                 if (chan->scan_type.storagebits == 32)
 117                         *tx++ = cpu_to_be16((chan->address + 2) << 8);
 118                 *tx++ = cpu_to_be16(chan->address << 8);
 119         }
 120
 121         return 0;
 122 }
 123 EXPORT_SYMBOL_GPL(adis_update_scan_mode);
 124
 125 static irqreturn_t adis_trigger_handler(int irq, void *p)
 126 {
 127         struct iio_poll_func *pf = p;
 128         struct iio_dev *indio_dev = pf->indio_dev;
 129         struct adis *adis = iio_device_get_drvdata(indio_dev);
 130         int ret;
 131
 132         if (adis->data->has_paging) {
 133                 mutex_lock(&adis->state_lock);
 134                 if (adis->current_page != 0) {
 135                         adis->tx[0] = ADIS_WRITE_REG(ADIS_REG_PAGE_ID);
 136                         adis->tx[1] = 0;
 137                         spi_write(adis->spi, adis->tx, 2);
 138                 }
 139         }
 140
 141         ret = spi_sync(adis->spi, &adis->msg);
 142         if (ret)
 143                 dev_err(&adis->spi->dev, "Failed to read data: %d", ret);
 144
 145
 146         if (adis->data->has_paging) {
 147                 adis->current_page = 0;
 148                 mutex_unlock(&adis->state_lock);
 149         }
 150
 151         iio_push_to_buffers_with_timestamp(indio_dev, adis->buffer,
 152                 pf->timestamp);
 153
 154         iio_trigger_notify_done(indio_dev->trig);
 155
 156         return IRQ_HANDLED;
 157 }
 158
 159 static void adis_buffer_cleanup(void *arg)
 160 {
 161         struct adis *adis = arg;
 162
 163         kfree(adis->buffer);
 164         kfree(adis->xfer);
 165 }
 166
 167 /**
 168  * devm_adis_setup_buffer_and_trigger() - Sets up buffer and trigger for
 169  *                                        the managed adis device
 170  * @adis: The adis device
 171  * @indio_dev: The IIO device
 172  * @trigger_handler: Optional trigger handler, may be NULL.
 173  *
 174  * Returns 0 on success, a negative error code otherwise.
 175  *
 176  * This function sets up the buffer and trigger for a adis devices.  If
 177  * 'trigger_handler' is NULL the default trigger handler will be used. The
 178  * default trigger handler will simply read the registers assigned to the
 179  * currently active channels.
 180  */
 181 int
 182 devm_adis_setup_buffer_and_trigger(struct adis *adis, struct iio_dev *indio_dev,
 183                                    irq_handler_t trigger_handler)
 184 {
 185         int ret;
 186
 187         if (!trigger_handler)
 188                 trigger_handler = adis_trigger_handler;
 189
 190         ret = devm_iio_triggered_buffer_setup(&adis->spi->dev, indio_dev,
 191                                               &iio_pollfunc_store_time,
 192                                               trigger_handler, NULL);
 193         if (ret)
 194                 return ret;
 195
 196         if (adis->spi->irq) {
 197                 ret = devm_adis_probe_trigger(adis, indio_dev);
 198                 if (ret)
 199                         return ret;
 200         }
 201
 202         return devm_add_action_or_reset(&adis->spi->dev, adis_buffer_cleanup,
 203                                         adis);
 204 }
 205 EXPORT_SYMBOL_GPL(devm_adis_setup_buffer_and_trigger);
 206