diff options
Diffstat (limited to 'drivers/staging/iio/accel/adis16204_ring.c')
-rw-r--r-- | drivers/staging/iio/accel/adis16204_ring.c | 154 |
1 files changed, 154 insertions, 0 deletions
diff --git a/drivers/staging/iio/accel/adis16204_ring.c b/drivers/staging/iio/accel/adis16204_ring.c new file mode 100644 index 00000000000..852df06684d --- /dev/null +++ b/drivers/staging/iio/accel/adis16204_ring.c | |||
@@ -0,0 +1,154 @@ | |||
1 | #include <linux/interrupt.h> | ||
2 | #include <linux/irq.h> | ||
3 | #include <linux/gpio.h> | ||
4 | #include <linux/workqueue.h> | ||
5 | #include <linux/mutex.h> | ||
6 | #include <linux/device.h> | ||
7 | #include <linux/kernel.h> | ||
8 | #include <linux/spi/spi.h> | ||
9 | #include <linux/slab.h> | ||
10 | #include <linux/sysfs.h> | ||
11 | #include <linux/list.h> | ||
12 | |||
13 | #include "../iio.h" | ||
14 | #include "../sysfs.h" | ||
15 | #include "../ring_sw.h" | ||
16 | #include "accel.h" | ||
17 | #include "../trigger.h" | ||
18 | #include "adis16204.h" | ||
19 | |||
20 | /** | ||
21 | * adis16204_read_ring_data() read data registers which will be placed into ring | ||
22 | * @dev: device associated with child of actual device (iio_dev or iio_trig) | ||
23 | * @rx: somewhere to pass back the value read | ||
24 | **/ | ||
25 | static int adis16204_read_ring_data(struct device *dev, u8 *rx) | ||
26 | { | ||
27 | struct spi_message msg; | ||
28 | struct iio_dev *indio_dev = dev_get_drvdata(dev); | ||
29 | struct adis16204_state *st = iio_priv(indio_dev); | ||
30 | struct spi_transfer xfers[ADIS16204_OUTPUTS + 1]; | ||
31 | int ret; | ||
32 | int i; | ||
33 | |||
34 | mutex_lock(&st->buf_lock); | ||
35 | |||
36 | spi_message_init(&msg); | ||
37 | |||
38 | memset(xfers, 0, sizeof(xfers)); | ||
39 | for (i = 0; i <= ADIS16204_OUTPUTS; i++) { | ||
40 | xfers[i].bits_per_word = 8; | ||
41 | xfers[i].cs_change = 1; | ||
42 | xfers[i].len = 2; | ||
43 | xfers[i].delay_usecs = 20; | ||
44 | xfers[i].tx_buf = st->tx + 2 * i; | ||
45 | st->tx[2 * i] | ||
46 | = ADIS16204_READ_REG(ADIS16204_SUPPLY_OUT + 2 * i); | ||
47 | st->tx[2 * i + 1] = 0; | ||
48 | if (i >= 1) | ||
49 | xfers[i].rx_buf = rx + 2 * (i - 1); | ||
50 | spi_message_add_tail(&xfers[i], &msg); | ||
51 | } | ||
52 | |||
53 | ret = spi_sync(st->us, &msg); | ||
54 | if (ret) | ||
55 | dev_err(&st->us->dev, "problem when burst reading"); | ||
56 | |||
57 | mutex_unlock(&st->buf_lock); | ||
58 | |||
59 | return ret; | ||
60 | } | ||
61 | |||
62 | /* Whilst this makes a lot of calls to iio_sw_ring functions - it is to device | ||
63 | * specific to be rolled into the core. | ||
64 | */ | ||
65 | static irqreturn_t adis16204_trigger_handler(int irq, void *p) | ||
66 | { | ||
67 | struct iio_poll_func *pf = p; | ||
68 | struct iio_dev *indio_dev = pf->private_data; | ||
69 | struct adis16204_state *st = iio_priv(indio_dev); | ||
70 | struct iio_ring_buffer *ring = indio_dev->ring; | ||
71 | int i = 0; | ||
72 | s16 *data; | ||
73 | size_t datasize = ring->access->get_bytes_per_datum(ring); | ||
74 | |||
75 | data = kmalloc(datasize, GFP_KERNEL); | ||
76 | if (data == NULL) { | ||
77 | dev_err(&st->us->dev, "memory alloc failed in ring bh"); | ||
78 | return -ENOMEM; | ||
79 | } | ||
80 | |||
81 | if (ring->scan_count) | ||
82 | if (adis16204_read_ring_data(&indio_dev->dev, st->rx) >= 0) | ||
83 | for (; i < ring->scan_count; i++) | ||
84 | data[i] = be16_to_cpup( | ||
85 | (__be16 *)&(st->rx[i*2])); | ||
86 | |||
87 | /* Guaranteed to be aligned with 8 byte boundary */ | ||
88 | if (ring->scan_timestamp) | ||
89 | *((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp; | ||
90 | |||
91 | ring->access->store_to(ring, (u8 *)data, pf->timestamp); | ||
92 | |||
93 | iio_trigger_notify_done(indio_dev->trig); | ||
94 | kfree(data); | ||
95 | |||
96 | return IRQ_HANDLED; | ||
97 | } | ||
98 | |||
99 | void adis16204_unconfigure_ring(struct iio_dev *indio_dev) | ||
100 | { | ||
101 | iio_dealloc_pollfunc(indio_dev->pollfunc); | ||
102 | iio_sw_rb_free(indio_dev->ring); | ||
103 | } | ||
104 | |||
105 | static const struct iio_ring_setup_ops adis16204_ring_setup_ops = { | ||
106 | .preenable = &iio_sw_ring_preenable, | ||
107 | .postenable = &iio_triggered_ring_postenable, | ||
108 | .predisable = &iio_triggered_ring_predisable, | ||
109 | }; | ||
110 | |||
111 | int adis16204_configure_ring(struct iio_dev *indio_dev) | ||
112 | { | ||
113 | int ret = 0; | ||
114 | struct iio_ring_buffer *ring; | ||
115 | |||
116 | ring = iio_sw_rb_allocate(indio_dev); | ||
117 | if (!ring) { | ||
118 | ret = -ENOMEM; | ||
119 | return ret; | ||
120 | } | ||
121 | indio_dev->ring = ring; | ||
122 | /* Effectively select the ring buffer implementation */ | ||
123 | ring->access = &ring_sw_access_funcs; | ||
124 | ring->bpe = 2; | ||
125 | ring->scan_timestamp = true; | ||
126 | ring->setup_ops = &adis16204_ring_setup_ops; | ||
127 | ring->owner = THIS_MODULE; | ||
128 | |||
129 | /* Set default scan mode */ | ||
130 | iio_scan_mask_set(ring, ADIS16204_SCAN_SUPPLY); | ||
131 | iio_scan_mask_set(ring, ADIS16204_SCAN_ACC_X); | ||
132 | iio_scan_mask_set(ring, ADIS16204_SCAN_ACC_Y); | ||
133 | iio_scan_mask_set(ring, ADIS16204_SCAN_AUX_ADC); | ||
134 | iio_scan_mask_set(ring, ADIS16204_SCAN_TEMP); | ||
135 | |||
136 | indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time, | ||
137 | &adis16204_trigger_handler, | ||
138 | IRQF_ONESHOT, | ||
139 | indio_dev, | ||
140 | "%s_consumer%d", | ||
141 | indio_dev->name, | ||
142 | indio_dev->id); | ||
143 | if (indio_dev->pollfunc == NULL) { | ||
144 | ret = -ENOMEM; | ||
145 | goto error_iio_sw_rb_free; | ||
146 | } | ||
147 | |||
148 | indio_dev->modes |= INDIO_RING_TRIGGERED; | ||
149 | return 0; | ||
150 | |||
151 | error_iio_sw_rb_free: | ||
152 | iio_sw_rb_free(indio_dev->ring); | ||
153 | return ret; | ||
154 | } | ||