Commit | Line | Data |
---|---|---|
a9d26f00 | 1 | #include <linux/interrupt.h> |
a9d26f00 | 2 | #include <linux/mutex.h> |
a9d26f00 BS |
3 | #include <linux/kernel.h> |
4 | #include <linux/spi/spi.h> | |
1cb6c1f5 | 5 | #include <linux/slab.h> |
2a29a90b | 6 | #include <linux/bitops.h> |
8e336a72 | 7 | #include <linux/export.h> |
a9d26f00 BS |
8 | |
9 | #include "../iio.h" | |
a9d26f00 | 10 | #include "../ring_sw.h" |
3f72395e | 11 | #include "../trigger_consumer.h" |
a9d26f00 BS |
12 | #include "adis16400.h" |
13 | ||
3fd66da1 BS |
14 | /** |
15 | * adis16400_spi_read_burst() - read all data registers | |
16 | * @dev: device associated with child of actual device (iio_dev or iio_trig) | |
17 | * @rx: somewhere to pass back the value read (min size is 24 bytes) | |
18 | **/ | |
19 | static int adis16400_spi_read_burst(struct device *dev, u8 *rx) | |
20 | { | |
21 | struct spi_message msg; | |
22 | struct iio_dev *indio_dev = dev_get_drvdata(dev); | |
38d15f06 | 23 | struct adis16400_state *st = iio_priv(indio_dev); |
3fd66da1 BS |
24 | u32 old_speed_hz = st->us->max_speed_hz; |
25 | int ret; | |
26 | ||
27 | struct spi_transfer xfers[] = { | |
28 | { | |
29 | .tx_buf = st->tx, | |
30 | .bits_per_word = 8, | |
31 | .len = 2, | |
3fd66da1 BS |
32 | }, { |
33 | .rx_buf = rx, | |
34 | .bits_per_word = 8, | |
35 | .len = 24, | |
3fd66da1 BS |
36 | }, |
37 | }; | |
38 | ||
39 | mutex_lock(&st->buf_lock); | |
40 | st->tx[0] = ADIS16400_READ_REG(ADIS16400_GLOB_CMD); | |
41 | st->tx[1] = 0; | |
42 | ||
43 | spi_message_init(&msg); | |
44 | spi_message_add_tail(&xfers[0], &msg); | |
45 | spi_message_add_tail(&xfers[1], &msg); | |
46 | ||
47 | st->us->max_speed_hz = min(ADIS16400_SPI_BURST, old_speed_hz); | |
48 | spi_setup(st->us); | |
49 | ||
50 | ret = spi_sync(st->us, &msg); | |
51 | if (ret) | |
52 | dev_err(&st->us->dev, "problem when burst reading"); | |
53 | ||
54 | st->us->max_speed_hz = old_speed_hz; | |
55 | spi_setup(st->us); | |
56 | mutex_unlock(&st->buf_lock); | |
57 | return ret; | |
58 | } | |
59 | ||
2a29a90b JC |
60 | static const u16 read_all_tx_array[] = { |
61 | cpu_to_be16(ADIS16400_READ_REG(ADIS16400_SUPPLY_OUT)), | |
62 | cpu_to_be16(ADIS16400_READ_REG(ADIS16400_XGYRO_OUT)), | |
63 | cpu_to_be16(ADIS16400_READ_REG(ADIS16400_YGYRO_OUT)), | |
64 | cpu_to_be16(ADIS16400_READ_REG(ADIS16400_ZGYRO_OUT)), | |
65 | cpu_to_be16(ADIS16400_READ_REG(ADIS16400_XACCL_OUT)), | |
66 | cpu_to_be16(ADIS16400_READ_REG(ADIS16400_YACCL_OUT)), | |
67 | cpu_to_be16(ADIS16400_READ_REG(ADIS16400_ZACCL_OUT)), | |
68 | cpu_to_be16(ADIS16400_READ_REG(ADIS16350_XTEMP_OUT)), | |
69 | cpu_to_be16(ADIS16400_READ_REG(ADIS16350_YTEMP_OUT)), | |
70 | cpu_to_be16(ADIS16400_READ_REG(ADIS16350_ZTEMP_OUT)), | |
71 | cpu_to_be16(ADIS16400_READ_REG(ADIS16400_AUX_ADC)), | |
72 | }; | |
73 | ||
74 | static int adis16350_spi_read_all(struct device *dev, u8 *rx) | |
75 | { | |
76 | struct iio_dev *indio_dev = dev_get_drvdata(dev); | |
38d15f06 | 77 | struct adis16400_state *st = iio_priv(indio_dev); |
2a29a90b JC |
78 | |
79 | struct spi_message msg; | |
80 | int i, j = 0, ret; | |
81 | struct spi_transfer *xfers; | |
550268ca JC |
82 | int scan_count = bitmap_weight(indio_dev->active_scan_mask, |
83 | indio_dev->masklength); | |
2a29a90b | 84 | |
550268ca | 85 | xfers = kzalloc(sizeof(*xfers)*(scan_count + 1), |
38d15f06 | 86 | GFP_KERNEL); |
2a29a90b JC |
87 | if (xfers == NULL) |
88 | return -ENOMEM; | |
89 | ||
90 | for (i = 0; i < ARRAY_SIZE(read_all_tx_array); i++) | |
550268ca | 91 | if (test_bit(i, indio_dev->active_scan_mask)) { |
2a29a90b JC |
92 | xfers[j].tx_buf = &read_all_tx_array[i]; |
93 | xfers[j].bits_per_word = 16; | |
94 | xfers[j].len = 2; | |
95 | xfers[j + 1].rx_buf = rx + j*2; | |
96 | j++; | |
97 | } | |
98 | xfers[j].bits_per_word = 16; | |
99 | xfers[j].len = 2; | |
100 | ||
101 | spi_message_init(&msg); | |
550268ca | 102 | for (j = 0; j < scan_count + 1; j++) |
2a29a90b JC |
103 | spi_message_add_tail(&xfers[j], &msg); |
104 | ||
105 | ret = spi_sync(st->us, &msg); | |
106 | kfree(xfers); | |
107 | ||
108 | return ret; | |
109 | } | |
110 | ||
a9d26f00 BS |
111 | /* Whilst this makes a lot of calls to iio_sw_ring functions - it is to device |
112 | * specific to be rolled into the core. | |
113 | */ | |
e7854845 | 114 | static irqreturn_t adis16400_trigger_handler(int irq, void *p) |
a9d26f00 | 115 | { |
e7854845 | 116 | struct iio_poll_func *pf = p; |
e65bc6ac | 117 | struct iio_dev *indio_dev = pf->indio_dev; |
38d15f06 | 118 | struct adis16400_state *st = iio_priv(indio_dev); |
14555b14 | 119 | struct iio_buffer *ring = indio_dev->buffer; |
2a29a90b | 120 | int i = 0, j, ret = 0; |
a9d26f00 | 121 | s16 *data; |
5565a450 | 122 | size_t datasize = ring->access->get_bytes_per_datum(ring); |
32b5eeca | 123 | /* Asumption that long is enough for maximum channels */ |
550268ca JC |
124 | unsigned long mask = *indio_dev->active_scan_mask; |
125 | int scan_count = bitmap_weight(indio_dev->active_scan_mask, | |
126 | indio_dev->masklength); | |
a9d26f00 BS |
127 | data = kmalloc(datasize , GFP_KERNEL); |
128 | if (data == NULL) { | |
129 | dev_err(&st->us->dev, "memory alloc failed in ring bh"); | |
e7854845 | 130 | return -ENOMEM; |
a9d26f00 BS |
131 | } |
132 | ||
550268ca | 133 | if (scan_count) { |
2a29a90b JC |
134 | if (st->variant->flags & ADIS16400_NO_BURST) { |
135 | ret = adis16350_spi_read_all(&indio_dev->dev, st->rx); | |
136 | if (ret < 0) | |
53aebb53 | 137 | goto err; |
550268ca | 138 | for (; i < scan_count; i++) |
2a29a90b JC |
139 | data[i] = *(s16 *)(st->rx + i*2); |
140 | } else { | |
141 | ret = adis16400_spi_read_burst(&indio_dev->dev, st->rx); | |
142 | if (ret < 0) | |
53aebb53 | 143 | goto err; |
550268ca | 144 | for (; i < scan_count; i++) { |
0fea4d61 MH |
145 | j = __ffs(mask); |
146 | mask &= ~(1 << j); | |
2a29a90b | 147 | data[i] = be16_to_cpup( |
0fea4d61 MH |
148 | (__be16 *)&(st->rx[j*2])); |
149 | } | |
2a29a90b JC |
150 | } |
151 | } | |
a9d26f00 | 152 | /* Guaranteed to be aligned with 8 byte boundary */ |
bf32963c | 153 | if (ring->scan_timestamp) |
e7854845 | 154 | *((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp; |
14555b14 | 155 | ring->access->store_to(indio_dev->buffer, (u8 *) data, pf->timestamp); |
a9d26f00 | 156 | |
e7854845 | 157 | iio_trigger_notify_done(indio_dev->trig); |
a9d26f00 | 158 | |
53aebb53 | 159 | kfree(data); |
e7854845 | 160 | return IRQ_HANDLED; |
53aebb53 AB |
161 | |
162 | err: | |
163 | kfree(data); | |
164 | return ret; | |
a9d26f00 | 165 | } |
a9d26f00 | 166 | |
a9d26f00 BS |
167 | void adis16400_unconfigure_ring(struct iio_dev *indio_dev) |
168 | { | |
21b185f8 | 169 | iio_dealloc_pollfunc(indio_dev->pollfunc); |
14555b14 | 170 | iio_sw_rb_free(indio_dev->buffer); |
a9d26f00 BS |
171 | } |
172 | ||
14555b14 JC |
173 | static const struct iio_buffer_setup_ops adis16400_ring_setup_ops = { |
174 | .preenable = &iio_sw_buffer_preenable, | |
3b99fb76 JC |
175 | .postenable = &iio_triggered_buffer_postenable, |
176 | .predisable = &iio_triggered_buffer_predisable, | |
5565a450 JC |
177 | }; |
178 | ||
a9d26f00 BS |
179 | int adis16400_configure_ring(struct iio_dev *indio_dev) |
180 | { | |
181 | int ret = 0; | |
14555b14 | 182 | struct iio_buffer *ring; |
a9d26f00 BS |
183 | |
184 | ring = iio_sw_rb_allocate(indio_dev); | |
185 | if (!ring) { | |
186 | ret = -ENOMEM; | |
187 | return ret; | |
188 | } | |
14555b14 | 189 | indio_dev->buffer = ring; |
a9d26f00 | 190 | /* Effectively select the ring buffer implementation */ |
5565a450 | 191 | ring->access = &ring_sw_access_funcs; |
bf32963c | 192 | ring->scan_timestamp = true; |
1612244f | 193 | indio_dev->setup_ops = &adis16400_ring_setup_ops; |
a9d26f00 | 194 | ring->owner = THIS_MODULE; |
e7854845 | 195 | |
21b185f8 JC |
196 | indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time, |
197 | &adis16400_trigger_handler, | |
198 | IRQF_ONESHOT, | |
199 | indio_dev, | |
200 | "%s_consumer%d", | |
201 | indio_dev->name, | |
202 | indio_dev->id); | |
e7854845 JC |
203 | if (indio_dev->pollfunc == NULL) { |
204 | ret = -ENOMEM; | |
15744090 | 205 | goto error_iio_sw_rb_free; |
e7854845 | 206 | } |
15744090 | 207 | |
ec3afa40 | 208 | indio_dev->modes |= INDIO_BUFFER_TRIGGERED; |
a9d26f00 | 209 | return 0; |
a9d26f00 | 210 | error_iio_sw_rb_free: |
14555b14 | 211 | iio_sw_rb_free(indio_dev->buffer); |
a9d26f00 BS |
212 | return ret; |
213 | } |