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->buffer)
 133                 return -ENOMEM;
 134
 135         if (adis->data->has_paging) {
 136                 mutex_lock(&adis->state_lock);
 137                 if (adis->current_page != 0) {
 138                         adis->tx[0] = ADIS_WRITE_REG(ADIS_REG_PAGE_ID);
 139                         adis->tx[1] = 0;
 140                         spi_write(adis->spi, adis->tx, 2);
 141                 }
 142         }
 143
 144         ret = spi_sync(adis->spi, &adis->msg);
 145         if (ret)
 146                 dev_err(&adis->spi->dev, "Failed to read data: %d", ret);
 147
 148
 149         if (adis->data->has_paging) {
 150                 adis->current_page = 0;
 151                 mutex_unlock(&adis->state_lock);
 152         }
 153
 154         iio_push_to_buffers_with_timestamp(indio_dev, adis->buffer,
 155                 pf->timestamp);
 156
 157         iio_trigger_notify_done(indio_dev->trig);
 158
 159         return IRQ_HANDLED;
 160 }
 161
 162 static void adis_buffer_cleanup(void *arg)
 163 {
 164         struct adis *adis = arg;
 165
 166         kfree(adis->buffer);
 167         kfree(adis->xfer);
 168 }
 169
 170 /**
 171  * devm_adis_setup_buffer_and_trigger() - Sets up buffer and trigger for
 172  *                                        the managed adis device
 173  * @adis: The adis device
 174  * @indio_dev: The IIO device
 175  * @trigger_handler: Optional trigger handler, may be NULL.
 176  *
 177  * Returns 0 on success, a negative error code otherwise.
 178  *
 179  * This function sets up the buffer and trigger for a adis devices.  If
 180  * 'trigger_handler' is NULL the default trigger handler will be used. The
 181  * default trigger handler will simply read the registers assigned to the
 182  * currently active channels.
 183  */
 184 int
 185 devm_adis_setup_buffer_and_trigger(struct adis *adis, struct iio_dev *indio_dev,
 186                                    irq_handler_t trigger_handler)
 187 {
 188         int ret;
 189
 190         if (!trigger_handler)
 191                 trigger_handler = adis_trigger_handler;
 192
 193         ret = devm_iio_triggered_buffer_setup(&adis->spi->dev, indio_dev,
 194                                               &iio_pollfunc_store_time,
 195                                               trigger_handler, NULL);
 196         if (ret)
 197                 return ret;
 198
 199         if (adis->spi->irq) {
 200                 ret = devm_adis_probe_trigger(adis, indio_dev);
 201                 if (ret)
 202                         return ret;
 203         }
 204
 205         return devm_add_action_or_reset(&adis->spi->dev, adis_buffer_cleanup,
 206                                         adis);
 207 }
 208 EXPORT_SYMBOL_GPL(devm_adis_setup_buffer_and_trigger);
 209