iio/imu/adis_buffer.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * Common library for ADIS16XXX devices
0004  *
0005  * Copyright 2012 Analog Devices Inc.
0006  *   Author: Lars-Peter Clausen <lars@metafoo.de>
0007  */
0008
0009 #include <linux/export.h>
0010 #include <linux/interrupt.h>
0011 #include <linux/mutex.h>
0012 #include <linux/kernel.h>
0013 #include <linux/spi/spi.h>
0014 #include <linux/slab.h>
0015
0016 #include <linux/iio/iio.h>
0017 #include <linux/iio/buffer.h>
0018 #include <linux/iio/trigger_consumer.h>
0019 #include <linux/iio/triggered_buffer.h>
0020 #include <linux/iio/imu/adis.h>
0021
0022 static int adis_update_scan_mode_burst(struct iio_dev *indio_dev,
0023                        const unsigned long *scan_mask)
0024 {
0025     struct adis *adis = iio_device_get_drvdata(indio_dev);
0026     unsigned int burst_length, burst_max_length;
0027     u8 *tx;
0028
0029     burst_length = adis->data->burst_len + adis->burst_extra_len;
0030
0031     if (adis->data->burst_max_len)
0032         burst_max_length = adis->data->burst_max_len;
0033     else
0034         burst_max_length = burst_length;
0035
0036     adis->xfer = kcalloc(2, sizeof(*adis->xfer), GFP_KERNEL);
0037     if (!adis->xfer)
0038         return -ENOMEM;
0039
0040     adis->buffer = kzalloc(burst_max_length + sizeof(u16), GFP_KERNEL);
0041     if (!adis->buffer) {
0042         kfree(adis->xfer);
0043         adis->xfer = NULL;
0044         return -ENOMEM;
0045     }
0046
0047     tx = adis->buffer + burst_max_length;
0048     tx[0] = ADIS_READ_REG(adis->data->burst_reg_cmd);
0049     tx[1] = 0;
0050
0051     adis->xfer[0].tx_buf = tx;
0052     adis->xfer[0].bits_per_word = 8;
0053     adis->xfer[0].len = 2;
0054     if (adis->data->burst_max_speed_hz)
0055         adis->xfer[0].speed_hz = adis->data->burst_max_speed_hz;
0056     adis->xfer[1].rx_buf = adis->buffer;
0057     adis->xfer[1].bits_per_word = 8;
0058     adis->xfer[1].len = burst_length;
0059     if (adis->data->burst_max_speed_hz)
0060         adis->xfer[1].speed_hz = adis->data->burst_max_speed_hz;
0061
0062     spi_message_init(&adis->msg);
0063     spi_message_add_tail(&adis->xfer[0], &adis->msg);
0064     spi_message_add_tail(&adis->xfer[1], &adis->msg);
0065
0066     return 0;
0067 }
0068
0069 int adis_update_scan_mode(struct iio_dev *indio_dev,
0070               const unsigned long *scan_mask)
0071 {
0072     struct adis *adis = iio_device_get_drvdata(indio_dev);
0073     const struct iio_chan_spec *chan;
0074     unsigned int scan_count;
0075     unsigned int i, j;
0076     __be16 *tx, *rx;
0077
0078     kfree(adis->xfer);
0079     kfree(adis->buffer);
0080
0081     if (adis->data->burst_len)
0082         return adis_update_scan_mode_burst(indio_dev, scan_mask);
0083
0084     scan_count = indio_dev->scan_bytes / 2;
0085
0086     adis->xfer = kcalloc(scan_count + 1, sizeof(*adis->xfer), GFP_KERNEL);
0087     if (!adis->xfer)
0088         return -ENOMEM;
0089
0090     adis->buffer = kcalloc(indio_dev->scan_bytes, 2, GFP_KERNEL);
0091     if (!adis->buffer) {
0092         kfree(adis->xfer);
0093         adis->xfer = NULL;
0094         return -ENOMEM;
0095     }
0096
0097     rx = adis->buffer;
0098     tx = rx + scan_count;
0099
0100     spi_message_init(&adis->msg);
0101
0102     for (j = 0; j <= scan_count; j++) {
0103         adis->xfer[j].bits_per_word = 8;
0104         if (j != scan_count)
0105             adis->xfer[j].cs_change = 1;
0106         adis->xfer[j].len = 2;
0107         adis->xfer[j].delay.value = adis->data->read_delay;
0108         adis->xfer[j].delay.unit = SPI_DELAY_UNIT_USECS;
0109         if (j < scan_count)
0110             adis->xfer[j].tx_buf = &tx[j];
0111         if (j >= 1)
0112             adis->xfer[j].rx_buf = &rx[j - 1];
0113         spi_message_add_tail(&adis->xfer[j], &adis->msg);
0114     }
0115
0116     chan = indio_dev->channels;
0117     for (i = 0; i < indio_dev->num_channels; i++, chan++) {
0118         if (!test_bit(chan->scan_index, scan_mask))
0119             continue;
0120         if (chan->scan_type.storagebits == 32)
0121             *tx++ = cpu_to_be16((chan->address + 2) << 8);
0122         *tx++ = cpu_to_be16(chan->address << 8);
0123     }
0124
0125     return 0;
0126 }
0127 EXPORT_SYMBOL_NS_GPL(adis_update_scan_mode, IIO_ADISLIB);
0128
0129 static irqreturn_t adis_trigger_handler(int irq, void *p)
0130 {
0131     struct iio_poll_func *pf = p;
0132     struct iio_dev *indio_dev = pf->indio_dev;
0133     struct adis *adis = iio_device_get_drvdata(indio_dev);
0134     int ret;
0135
0136     if (adis->data->has_paging) {
0137         mutex_lock(&adis->state_lock);
0138         if (adis->current_page != 0) {
0139             adis->tx[0] = ADIS_WRITE_REG(ADIS_REG_PAGE_ID);
0140             adis->tx[1] = 0;
0141             ret = spi_write(adis->spi, adis->tx, 2);
0142             if (ret) {
0143                 dev_err(&adis->spi->dev, "Failed to change device page: %d\n", ret);
0144                 mutex_unlock(&adis->state_lock);
0145                 goto irq_done;
0146             }
0147
0148             adis->current_page = 0;
0149         }
0150     }
0151
0152     ret = spi_sync(adis->spi, &adis->msg);
0153     if (adis->data->has_paging)
0154         mutex_unlock(&adis->state_lock);
0155     if (ret) {
0156         dev_err(&adis->spi->dev, "Failed to read data: %d", ret);
0157         goto irq_done;
0158     }
0159
0160     iio_push_to_buffers_with_timestamp(indio_dev, adis->buffer,
0161                        pf->timestamp);
0162
0163 irq_done:
0164     iio_trigger_notify_done(indio_dev->trig);
0165
0166     return IRQ_HANDLED;
0167 }
0168
0169 static void adis_buffer_cleanup(void *arg)
0170 {
0171     struct adis *adis = arg;
0172
0173     kfree(adis->buffer);
0174     kfree(adis->xfer);
0175 }
0176
0177 /**
0178  * devm_adis_setup_buffer_and_trigger() - Sets up buffer and trigger for
0179  *                    the managed adis device
0180  * @adis: The adis device
0181  * @indio_dev: The IIO device
0182  * @trigger_handler: Optional trigger handler, may be NULL.
0183  *
0184  * Returns 0 on success, a negative error code otherwise.
0185  *
0186  * This function sets up the buffer and trigger for a adis devices.  If
0187  * 'trigger_handler' is NULL the default trigger handler will be used. The
0188  * default trigger handler will simply read the registers assigned to the
0189  * currently active channels.
0190  */
0191 int
0192 devm_adis_setup_buffer_and_trigger(struct adis *adis, struct iio_dev *indio_dev,
0193                    irq_handler_t trigger_handler)
0194 {
0195     int ret;
0196
0197     if (!trigger_handler)
0198         trigger_handler = adis_trigger_handler;
0199
0200     ret = devm_iio_triggered_buffer_setup(&adis->spi->dev, indio_dev,
0201                           &iio_pollfunc_store_time,
0202                           trigger_handler, NULL);
0203     if (ret)
0204         return ret;
0205
0206     if (adis->spi->irq) {
0207         ret = devm_adis_probe_trigger(adis, indio_dev);
0208         if (ret)
0209             return ret;
0210     }
0211
0212     return devm_add_action_or_reset(&adis->spi->dev, adis_buffer_cleanup,
0213                     adis);
0214 }
0215 EXPORT_SYMBOL_NS_GPL(devm_adis_setup_buffer_and_trigger, IIO_ADISLIB);
0216