Lars-Peter Clausen | ccd2b52 | 2012-11-13 13:28:00 +0000 | [diff] [blame^] | 1 | #include <linux/export.h> |
| 2 | #include <linux/interrupt.h> |
| 3 | #include <linux/mutex.h> |
| 4 | #include <linux/kernel.h> |
| 5 | #include <linux/spi/spi.h> |
| 6 | #include <linux/slab.h> |
| 7 | |
| 8 | #include <linux/iio/iio.h> |
| 9 | #include <linux/iio/buffer.h> |
| 10 | #include "../ring_sw.h" |
| 11 | #include <linux/iio/trigger_consumer.h> |
| 12 | |
| 13 | #include "adis.h" |
| 14 | |
| 15 | #define ADIS_MAX_OUTPUTS 12 |
| 16 | |
| 17 | static int adis_read_buffer_data(struct adis *adis, struct iio_dev *indio_dev) |
| 18 | { |
| 19 | int n_outputs = indio_dev->num_channels; |
| 20 | struct spi_transfer xfers[ADIS_MAX_OUTPUTS + 1]; |
| 21 | struct spi_message msg; |
| 22 | int ret; |
| 23 | int i; |
| 24 | |
| 25 | mutex_lock(&adis->txrx_lock); |
| 26 | |
| 27 | spi_message_init(&msg); |
| 28 | |
| 29 | memset(xfers, 0, sizeof(xfers)); |
| 30 | for (i = 0; i <= n_outputs; i++) { |
| 31 | xfers[i].bits_per_word = 8; |
| 32 | xfers[i].cs_change = 1; |
| 33 | xfers[i].len = 2; |
| 34 | xfers[i].delay_usecs = adis->data->read_delay; |
| 35 | if (i < n_outputs) { |
| 36 | xfers[i].tx_buf = adis->tx + 2 * i; |
| 37 | adis->tx[2 * i] = indio_dev->channels[i].address; |
| 38 | adis->tx[2 * i + 1] = 0; |
| 39 | } |
| 40 | if (i >= 1) |
| 41 | xfers[i].rx_buf = adis->rx + 2 * (i - 1); |
| 42 | spi_message_add_tail(&xfers[i], &msg); |
| 43 | } |
| 44 | |
| 45 | ret = spi_sync(adis->spi, &msg); |
| 46 | if (ret) |
| 47 | dev_err(&adis->spi->dev, "Failed to read data: %d", ret); |
| 48 | |
| 49 | mutex_unlock(&adis->txrx_lock); |
| 50 | |
| 51 | return ret; |
| 52 | } |
| 53 | |
| 54 | static irqreturn_t adis_trigger_handler(int irq, void *p) |
| 55 | { |
| 56 | struct iio_poll_func *pf = p; |
| 57 | struct iio_dev *indio_dev = pf->indio_dev; |
| 58 | struct adis *adis = iio_device_get_drvdata(indio_dev); |
| 59 | u16 *data; |
| 60 | int i = 0; |
| 61 | |
| 62 | data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL); |
| 63 | if (data == NULL) { |
| 64 | dev_err(&adis->spi->dev, "Failed to allocate memory."); |
| 65 | return -ENOMEM; |
| 66 | } |
| 67 | |
| 68 | if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) |
| 69 | && adis_read_buffer_data(adis, indio_dev) >= 0) |
| 70 | for (; i < bitmap_weight(indio_dev->active_scan_mask, |
| 71 | indio_dev->masklength); i++) |
| 72 | data[i] = be16_to_cpup((__be16 *)&(adis->rx[i*2])); |
| 73 | |
| 74 | /* Guaranteed to be aligned with 8 byte boundary */ |
| 75 | if (indio_dev->scan_timestamp) |
| 76 | *((s64 *)(PTR_ALIGN(data, sizeof(s64)))) = pf->timestamp; |
| 77 | |
| 78 | iio_push_to_buffers(indio_dev, (u8 *)data); |
| 79 | |
| 80 | iio_trigger_notify_done(indio_dev->trig); |
| 81 | kfree(data); |
| 82 | |
| 83 | return IRQ_HANDLED; |
| 84 | } |
| 85 | |
| 86 | static const struct iio_buffer_setup_ops adis_buffer_setup_ops = { |
| 87 | .preenable = &iio_sw_buffer_preenable, |
| 88 | .postenable = &iio_triggered_buffer_postenable, |
| 89 | .predisable = &iio_triggered_buffer_predisable, |
| 90 | }; |
| 91 | |
| 92 | static int adis_buffer_setup(struct iio_dev *indio_dev, |
| 93 | irqreturn_t (*trigger_handler)(int, void *)) |
| 94 | { |
| 95 | int ret = 0; |
| 96 | struct iio_buffer *buffer; |
| 97 | |
| 98 | if (!trigger_handler) |
| 99 | trigger_handler = &adis_trigger_handler; |
| 100 | |
| 101 | buffer = iio_sw_rb_allocate(indio_dev); |
| 102 | if (!buffer) { |
| 103 | ret = -ENOMEM; |
| 104 | return ret; |
| 105 | } |
| 106 | |
| 107 | indio_dev->buffer = buffer; |
| 108 | indio_dev->setup_ops = &adis_buffer_setup_ops; |
| 109 | |
| 110 | indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time, |
| 111 | trigger_handler, |
| 112 | IRQF_ONESHOT, |
| 113 | indio_dev, |
| 114 | "%s_consumer%d", |
| 115 | indio_dev->name, |
| 116 | indio_dev->id); |
| 117 | if (indio_dev->pollfunc == NULL) { |
| 118 | ret = -ENOMEM; |
| 119 | goto error_iio_sw_rb_free; |
| 120 | } |
| 121 | |
| 122 | indio_dev->modes |= INDIO_BUFFER_TRIGGERED; |
| 123 | return 0; |
| 124 | |
| 125 | error_iio_sw_rb_free: |
| 126 | iio_sw_rb_free(indio_dev->buffer); |
| 127 | return ret; |
| 128 | } |
| 129 | |
| 130 | static void adis_buffer_cleanup(struct iio_dev *indio_dev) |
| 131 | { |
| 132 | iio_dealloc_pollfunc(indio_dev->pollfunc); |
| 133 | iio_sw_rb_free(indio_dev->buffer); |
| 134 | } |
| 135 | |
| 136 | /** |
| 137 | * adis_setup_buffer_and_trigger() - Sets up buffer and trigger for the adis device |
| 138 | * @adis: The adis device. |
| 139 | * @indio_dev: The IIO device. |
| 140 | * @trigger_handler: Optional trigger handler, may be NULL. |
| 141 | * |
| 142 | * Returns 0 on success, a negative error code otherwise. |
| 143 | * |
| 144 | * This function sets up the buffer and trigger for a adis devices. If |
| 145 | * 'trigger_handler' is NULL the default trigger handler will be used. The |
| 146 | * default trigger handler will simply read the registers assigned to the |
| 147 | * currently active channels. |
| 148 | * |
| 149 | * adis_cleanup_buffer_and_trigger() should be called to free the resources |
| 150 | * allocated by this function. |
| 151 | */ |
| 152 | int adis_setup_buffer_and_trigger(struct adis *adis, struct iio_dev *indio_dev, |
| 153 | irqreturn_t (*trigger_handler)(int, void *)) |
| 154 | { |
| 155 | int ret; |
| 156 | |
| 157 | ret = adis_buffer_setup(indio_dev, trigger_handler); |
| 158 | if (ret) |
| 159 | return ret; |
| 160 | |
| 161 | ret = iio_buffer_register(indio_dev, |
| 162 | indio_dev->channels, |
| 163 | indio_dev->num_channels); |
| 164 | if (ret) { |
| 165 | dev_err(&adis->spi->dev, "Failed to initialize buffer %d\n", |
| 166 | ret); |
| 167 | goto error_unreg_buffer_funcs; |
| 168 | } |
| 169 | |
| 170 | if (adis->spi->irq) { |
| 171 | ret = adis_probe_trigger(adis, indio_dev); |
| 172 | if (ret) |
| 173 | goto error_uninitialize_buffer; |
| 174 | } |
| 175 | return 0; |
| 176 | |
| 177 | error_uninitialize_buffer: |
| 178 | iio_buffer_unregister(indio_dev); |
| 179 | error_unreg_buffer_funcs: |
| 180 | adis_buffer_cleanup(indio_dev); |
| 181 | return ret; |
| 182 | } |
| 183 | EXPORT_SYMBOL_GPL(adis_setup_buffer_and_trigger); |
| 184 | |
| 185 | /** |
| 186 | * adis_cleanup_buffer_and_trigger() - Free buffer and trigger resources |
| 187 | * @adis: The adis device. |
| 188 | * @indio_dev: The IIO device. |
| 189 | * |
| 190 | * Frees resources allocated by adis_setup_buffer_and_trigger() |
| 191 | */ |
| 192 | void adis_cleanup_buffer_and_trigger(struct adis *adis, |
| 193 | struct iio_dev *indio_dev) |
| 194 | { |
| 195 | if (adis->spi->irq) |
| 196 | adis_remove_trigger(adis); |
| 197 | iio_buffer_unregister(indio_dev); |
| 198 | adis_buffer_cleanup(indio_dev); |
| 199 | } |
| 200 | EXPORT_SYMBOL_GPL(adis_cleanup_buffer_and_trigger); |