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