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         /* All but the timestamp channel */
  30         burst_length = (indio_dev->num_channels - 1) * sizeof(u16);
  31         burst_length += adis->burst->extra_len + adis->burst_extra_len;
  32
  33         if (adis->burst->burst_max_len)
  34                 burst_max_length = adis->burst->burst_max_len;
  35         else
  36                 burst_max_length = burst_length;
  37
  38         adis->xfer = kcalloc(2, sizeof(*adis->xfer), GFP_KERNEL);
  39         if (!adis->xfer)
  40                 return -ENOMEM;
  41
  42         adis->buffer = kzalloc(burst_max_length + sizeof(u16), GFP_KERNEL);
  43         if (!adis->buffer) {
  44                 kfree(adis->xfer);
  45                 adis->xfer = NULL;
  46                 return -ENOMEM;
  47         }
  48
  49         tx = adis->buffer + burst_max_length;
  50         tx[0] = ADIS_READ_REG(adis->burst->reg_cmd);
  51         tx[1] = 0;
  52
  53         adis->xfer[0].tx_buf = tx;
  54         adis->xfer[0].bits_per_word = 8;
  55         adis->xfer[0].len = 2;
  56         adis->xfer[1].rx_buf = adis->buffer;
  57         adis->xfer[1].bits_per_word = 8;
  58         adis->xfer[1].len = burst_length;
  59
  60         spi_message_init(&adis->msg);
  61         spi_message_add_tail(&adis->xfer[0], &adis->msg);
  62         spi_message_add_tail(&adis->xfer[1], &adis->msg);
  63
  64         return 0;
  65 }
  66
  67 int adis_update_scan_mode(struct iio_dev *indio_dev,
  68         const unsigned long *scan_mask)
  69 {
  70         struct adis *adis = iio_device_get_drvdata(indio_dev);
  71         const struct iio_chan_spec *chan;
  72         unsigned int scan_count;
  73         unsigned int i, j;
  74         __be16 *tx, *rx;
  75
  76         kfree(adis->xfer);
  77         kfree(adis->buffer);
  78
  79         if (adis->burst && adis->burst->en)
  80                 return adis_update_scan_mode_burst(indio_dev, scan_mask);
  81
  82         scan_count = indio_dev->scan_bytes / 2;
  83
  84         adis->xfer = kcalloc(scan_count + 1, sizeof(*adis->xfer), GFP_KERNEL);
  85         if (!adis->xfer)
  86                 return -ENOMEM;
  87
  88         adis->buffer = kcalloc(indio_dev->scan_bytes, 2, GFP_KERNEL);
  89         if (!adis->buffer) {
  90                 kfree(adis->xfer);
  91                 adis->xfer = NULL;
  92                 return -ENOMEM;
  93         }
  94
  95         rx = adis->buffer;
  96         tx = rx + scan_count;
  97
  98         spi_message_init(&adis->msg);
  99
 100         for (j = 0; j <= scan_count; j++) {
 101                 adis->xfer[j].bits_per_word = 8;
 102                 if (j != scan_count)
 103                         adis->xfer[j].cs_change = 1;
 104                 adis->xfer[j].len = 2;
 105                 adis->xfer[j].delay.value = adis->data->read_delay;
 106                 adis->xfer[j].delay.unit = SPI_DELAY_UNIT_USECS;
 107                 if (j < scan_count)
 108                         adis->xfer[j].tx_buf = &tx[j];
 109                 if (j >= 1)
 110                         adis->xfer[j].rx_buf = &rx[j - 1];
 111                 spi_message_add_tail(&adis->xfer[j], &adis->msg);
 112         }
 113
 114         chan = indio_dev->channels;
 115         for (i = 0; i < indio_dev->num_channels; i++, chan++) {
 116                 if (!test_bit(chan->scan_index, scan_mask))
 117                         continue;
 118                 if (chan->scan_type.storagebits == 32)
 119                         *tx++ = cpu_to_be16((chan->address + 2) << 8);
 120                 *tx++ = cpu_to_be16(chan->address << 8);
 121         }
 122
 123         return 0;
 124 }
 125 EXPORT_SYMBOL_GPL(adis_update_scan_mode);
 126
 127 static irqreturn_t adis_trigger_handler(int irq, void *p)
 128 {
 129         struct iio_poll_func *pf = p;
 130         struct iio_dev *indio_dev = pf->indio_dev;
 131         struct adis *adis = iio_device_get_drvdata(indio_dev);
 132         int ret;
 133
 134         if (!adis->buffer)
 135                 return -ENOMEM;
 136
 137         if (adis->data->has_paging) {
 138                 mutex_lock(&adis->state_lock);
 139                 if (adis->current_page != 0) {
 140                         adis->tx[0] = ADIS_WRITE_REG(ADIS_REG_PAGE_ID);
 141                         adis->tx[1] = 0;
 142                         spi_write(adis->spi, adis->tx, 2);
 143                 }
 144         }
 145
 146         ret = spi_sync(adis->spi, &adis->msg);
 147         if (ret)
 148                 dev_err(&adis->spi->dev, "Failed to read data: %d", ret);
 149
 150
 151         if (adis->data->has_paging) {
 152                 adis->current_page = 0;
 153                 mutex_unlock(&adis->state_lock);
 154         }
 155
 156         iio_push_to_buffers_with_timestamp(indio_dev, adis->buffer,
 157                 pf->timestamp);
 158
 159         iio_trigger_notify_done(indio_dev->trig);
 160
 161         return IRQ_HANDLED;
 162 }
 163
 164 static void adis_buffer_cleanup(void *arg)
 165 {
 166         struct adis *adis = arg;
 167
 168         kfree(adis->buffer);
 169         kfree(adis->xfer);
 170 }
 171
 172 /**
 173  * adis_setup_buffer_and_trigger() - Sets up buffer and trigger for the adis device
 174  * @adis: The adis device.
 175  * @indio_dev: The IIO device.
 176  * @trigger_handler: Optional trigger handler, may be NULL.
 177  *
 178  * Returns 0 on success, a negative error code otherwise.
 179  *
 180  * This function sets up the buffer and trigger for a adis devices.  If
 181  * 'trigger_handler' is NULL the default trigger handler will be used. The
 182  * default trigger handler will simply read the registers assigned to the
 183  * currently active channels.
 184  *
 185  * adis_cleanup_buffer_and_trigger() should be called to free the resources
 186  * allocated by this function.
 187  */
 188 int adis_setup_buffer_and_trigger(struct adis *adis, struct iio_dev *indio_dev,
 189         irqreturn_t (*trigger_handler)(int, void *))
 190 {
 191         int ret;
 192
 193         if (!trigger_handler)
 194                 trigger_handler = adis_trigger_handler;
 195
 196         ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
 197                 trigger_handler, NULL);
 198         if (ret)
 199                 return ret;
 200
 201         if (adis->spi->irq) {
 202                 ret = adis_probe_trigger(adis, indio_dev);
 203                 if (ret)
 204                         goto error_buffer_cleanup;
 205         }
 206         return 0;
 207
 208 error_buffer_cleanup:
 209         iio_triggered_buffer_cleanup(indio_dev);
 210         return ret;
 211 }
 212 EXPORT_SYMBOL_GPL(adis_setup_buffer_and_trigger);
 213
 214 /**
 215  * devm_adis_setup_buffer_and_trigger() - Sets up buffer and trigger for
 216  *                                        the managed adis device
 217  * @adis: The adis device
 218  * @indio_dev: The IIO device
 219  * @trigger_handler: Optional trigger handler, may be NULL.
 220  *
 221  * Returns 0 on success, a negative error code otherwise.
 222  *
 223  * This function perfoms exactly the same as adis_setup_buffer_and_trigger()
 224  */
 225 int
 226 devm_adis_setup_buffer_and_trigger(struct adis *adis, struct iio_dev *indio_dev,
 227                                    irq_handler_t trigger_handler)
 228 {
 229         int ret;
 230
 231         if (!trigger_handler)
 232                 trigger_handler = adis_trigger_handler;
 233
 234         ret = devm_iio_triggered_buffer_setup(&adis->spi->dev, indio_dev,
 235                                               &iio_pollfunc_store_time,
 236                                               trigger_handler, NULL);
 237         if (ret)
 238                 return ret;
 239
 240         if (adis->spi->irq) {
 241                 ret = devm_adis_probe_trigger(adis, indio_dev);
 242                 if (ret)
 243                         return ret;
 244         }
 245
 246         return devm_add_action_or_reset(&adis->spi->dev, adis_buffer_cleanup,
 247                                         adis);
 248 }
 249 EXPORT_SYMBOL_GPL(devm_adis_setup_buffer_and_trigger);
 250
 251 /**
 252  * adis_cleanup_buffer_and_trigger() - Free buffer and trigger resources
 253  * @adis: The adis device.
 254  * @indio_dev: The IIO device.
 255  *
 256  * Frees resources allocated by adis_setup_buffer_and_trigger()
 257  */
 258 void adis_cleanup_buffer_and_trigger(struct adis *adis,
 259         struct iio_dev *indio_dev)
 260 {
 261         if (adis->spi->irq)
 262                 adis_remove_trigger(adis);
 263         kfree(adis->buffer);
 264         kfree(adis->xfer);
 265         iio_triggered_buffer_cleanup(indio_dev);
 266 }
 267 EXPORT_SYMBOL_GPL(adis_cleanup_buffer_and_trigger);