diff options
Diffstat (limited to 'drivers/staging/iio/imu/adis16400_ring.c')
-rw-r--r-- | drivers/staging/iio/imu/adis16400_ring.c | 245 |
1 files changed, 245 insertions, 0 deletions
diff --git a/drivers/staging/iio/imu/adis16400_ring.c b/drivers/staging/iio/imu/adis16400_ring.c new file mode 100644 index 00000000000..5529b32bd2e --- /dev/null +++ b/drivers/staging/iio/imu/adis16400_ring.c | |||
@@ -0,0 +1,245 @@ | |||
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/sysfs.h> | ||
10 | #include <linux/list.h> | ||
11 | |||
12 | #include "../iio.h" | ||
13 | #include "../sysfs.h" | ||
14 | #include "../ring_sw.h" | ||
15 | #include "../accel/accel.h" | ||
16 | #include "../trigger.h" | ||
17 | #include "adis16400.h" | ||
18 | |||
19 | /** | ||
20 | * combine_8_to_16() utility function to munge to u8s into u16 | ||
21 | **/ | ||
22 | static inline u16 combine_8_to_16(u8 lower, u8 upper) | ||
23 | { | ||
24 | u16 _lower = lower; | ||
25 | u16 _upper = upper; | ||
26 | return _lower | (_upper << 8); | ||
27 | } | ||
28 | |||
29 | static IIO_SCAN_EL_C(supply, ADIS16400_SCAN_SUPPLY, IIO_SIGNED(14), | ||
30 | ADIS16400_SUPPLY_OUT, NULL); | ||
31 | |||
32 | static IIO_SCAN_EL_C(gyro_x, ADIS16400_SCAN_GYRO_X, IIO_SIGNED(14), | ||
33 | ADIS16400_XGYRO_OUT, NULL); | ||
34 | static IIO_SCAN_EL_C(gyro_y, ADIS16400_SCAN_GYRO_Y, IIO_SIGNED(14), | ||
35 | ADIS16400_YGYRO_OUT, NULL); | ||
36 | static IIO_SCAN_EL_C(gyro_z, ADIS16400_SCAN_GYRO_Z, IIO_SIGNED(14), | ||
37 | ADIS16400_ZGYRO_OUT, NULL); | ||
38 | |||
39 | static IIO_SCAN_EL_C(accel_x, ADIS16400_SCAN_ACC_X, IIO_SIGNED(14), | ||
40 | ADIS16400_XACCL_OUT, NULL); | ||
41 | static IIO_SCAN_EL_C(accel_y, ADIS16400_SCAN_ACC_Y, IIO_SIGNED(14), | ||
42 | ADIS16400_YACCL_OUT, NULL); | ||
43 | static IIO_SCAN_EL_C(accel_z, ADIS16400_SCAN_ACC_Z, IIO_SIGNED(14), | ||
44 | ADIS16400_ZACCL_OUT, NULL); | ||
45 | |||
46 | static IIO_SCAN_EL_C(magn_x, ADIS16400_SCAN_MAGN_X, IIO_SIGNED(14), | ||
47 | ADIS16400_XMAGN_OUT, NULL); | ||
48 | static IIO_SCAN_EL_C(magn_y, ADIS16400_SCAN_MAGN_Y, IIO_SIGNED(14), | ||
49 | ADIS16400_YMAGN_OUT, NULL); | ||
50 | static IIO_SCAN_EL_C(magn_z, ADIS16400_SCAN_MAGN_Z, IIO_SIGNED(14), | ||
51 | ADIS16400_ZMAGN_OUT, NULL); | ||
52 | |||
53 | static IIO_SCAN_EL_C(temp, ADIS16400_SCAN_TEMP, IIO_SIGNED(12), | ||
54 | ADIS16400_TEMP_OUT, NULL); | ||
55 | static IIO_SCAN_EL_C(adc_0, ADIS16400_SCAN_ADC_0, IIO_SIGNED(12), | ||
56 | ADIS16400_AUX_ADC, NULL); | ||
57 | |||
58 | static IIO_SCAN_EL_TIMESTAMP(12); | ||
59 | |||
60 | static struct attribute *adis16400_scan_el_attrs[] = { | ||
61 | &iio_scan_el_supply.dev_attr.attr, | ||
62 | &iio_scan_el_gyro_x.dev_attr.attr, | ||
63 | &iio_scan_el_gyro_y.dev_attr.attr, | ||
64 | &iio_scan_el_gyro_z.dev_attr.attr, | ||
65 | &iio_scan_el_accel_x.dev_attr.attr, | ||
66 | &iio_scan_el_accel_y.dev_attr.attr, | ||
67 | &iio_scan_el_accel_z.dev_attr.attr, | ||
68 | &iio_scan_el_magn_x.dev_attr.attr, | ||
69 | &iio_scan_el_magn_y.dev_attr.attr, | ||
70 | &iio_scan_el_magn_z.dev_attr.attr, | ||
71 | &iio_scan_el_temp.dev_attr.attr, | ||
72 | &iio_scan_el_adc_0.dev_attr.attr, | ||
73 | &iio_scan_el_timestamp.dev_attr.attr, | ||
74 | NULL, | ||
75 | }; | ||
76 | |||
77 | static struct attribute_group adis16400_scan_el_group = { | ||
78 | .attrs = adis16400_scan_el_attrs, | ||
79 | .name = "scan_elements", | ||
80 | }; | ||
81 | |||
82 | /** | ||
83 | * adis16400_poll_func_th() top half interrupt handler called by trigger | ||
84 | * @private_data: iio_dev | ||
85 | **/ | ||
86 | static void adis16400_poll_func_th(struct iio_dev *indio_dev) | ||
87 | { | ||
88 | struct adis16400_state *st = iio_dev_get_devdata(indio_dev); | ||
89 | st->last_timestamp = indio_dev->trig->timestamp; | ||
90 | schedule_work(&st->work_trigger_to_ring); | ||
91 | /* Indicate that this interrupt is being handled */ | ||
92 | |||
93 | /* Technically this is trigger related, but without this | ||
94 | * handler running there is currently no way for the interrupt | ||
95 | * to clear. | ||
96 | */ | ||
97 | } | ||
98 | |||
99 | /* Whilst this makes a lot of calls to iio_sw_ring functions - it is to device | ||
100 | * specific to be rolled into the core. | ||
101 | */ | ||
102 | static void adis16400_trigger_bh_to_ring(struct work_struct *work_s) | ||
103 | { | ||
104 | struct adis16400_state *st | ||
105 | = container_of(work_s, struct adis16400_state, | ||
106 | work_trigger_to_ring); | ||
107 | |||
108 | int i = 0; | ||
109 | s16 *data; | ||
110 | size_t datasize = st->indio_dev | ||
111 | ->ring->access.get_bpd(st->indio_dev->ring); | ||
112 | |||
113 | data = kmalloc(datasize , GFP_KERNEL); | ||
114 | if (data == NULL) { | ||
115 | dev_err(&st->us->dev, "memory alloc failed in ring bh"); | ||
116 | return; | ||
117 | } | ||
118 | |||
119 | if (st->indio_dev->scan_count) | ||
120 | if (adis16400_spi_read_burst(&st->indio_dev->dev, st->rx) >= 0) | ||
121 | for (; i < st->indio_dev->scan_count; i++) { | ||
122 | data[i] = combine_8_to_16(st->rx[i*2+1], | ||
123 | st->rx[i*2]); | ||
124 | } | ||
125 | |||
126 | /* Guaranteed to be aligned with 8 byte boundary */ | ||
127 | if (st->indio_dev->scan_timestamp) | ||
128 | *((s64 *)(data + ((i + 3)/4)*4)) = st->last_timestamp; | ||
129 | |||
130 | st->indio_dev->ring->access.store_to(st->indio_dev->ring, | ||
131 | (u8 *)data, | ||
132 | st->last_timestamp); | ||
133 | |||
134 | iio_trigger_notify_done(st->indio_dev->trig); | ||
135 | kfree(data); | ||
136 | |||
137 | return; | ||
138 | } | ||
139 | /* in these circumstances is it better to go with unaligned packing and | ||
140 | * deal with the cost?*/ | ||
141 | static int adis16400_data_rdy_ring_preenable(struct iio_dev *indio_dev) | ||
142 | { | ||
143 | size_t size; | ||
144 | dev_dbg(&indio_dev->dev, "%s\n", __func__); | ||
145 | /* Check if there are any scan elements enabled, if not fail*/ | ||
146 | if (!(indio_dev->scan_count || indio_dev->scan_timestamp)) | ||
147 | return -EINVAL; | ||
148 | |||
149 | if (indio_dev->ring->access.set_bpd) { | ||
150 | if (indio_dev->scan_timestamp) | ||
151 | if (indio_dev->scan_count) /* Timestamp and data */ | ||
152 | size = 6*sizeof(s64); | ||
153 | else /* Timestamp only */ | ||
154 | size = sizeof(s64); | ||
155 | else /* Data only */ | ||
156 | size = indio_dev->scan_count*sizeof(s16); | ||
157 | indio_dev->ring->access.set_bpd(indio_dev->ring, size); | ||
158 | } | ||
159 | |||
160 | return 0; | ||
161 | } | ||
162 | |||
163 | static int adis16400_data_rdy_ring_postenable(struct iio_dev *indio_dev) | ||
164 | { | ||
165 | return indio_dev->trig | ||
166 | ? iio_trigger_attach_poll_func(indio_dev->trig, | ||
167 | indio_dev->pollfunc) | ||
168 | : 0; | ||
169 | } | ||
170 | |||
171 | static int adis16400_data_rdy_ring_predisable(struct iio_dev *indio_dev) | ||
172 | { | ||
173 | return indio_dev->trig | ||
174 | ? iio_trigger_dettach_poll_func(indio_dev->trig, | ||
175 | indio_dev->pollfunc) | ||
176 | : 0; | ||
177 | } | ||
178 | |||
179 | void adis16400_unconfigure_ring(struct iio_dev *indio_dev) | ||
180 | { | ||
181 | kfree(indio_dev->pollfunc); | ||
182 | iio_sw_rb_free(indio_dev->ring); | ||
183 | } | ||
184 | |||
185 | int adis16400_configure_ring(struct iio_dev *indio_dev) | ||
186 | { | ||
187 | int ret = 0; | ||
188 | struct adis16400_state *st = indio_dev->dev_data; | ||
189 | struct iio_ring_buffer *ring; | ||
190 | INIT_WORK(&st->work_trigger_to_ring, adis16400_trigger_bh_to_ring); | ||
191 | /* Set default scan mode */ | ||
192 | |||
193 | iio_scan_mask_set(indio_dev, iio_scan_el_supply.number); | ||
194 | iio_scan_mask_set(indio_dev, iio_scan_el_gyro_x.number); | ||
195 | iio_scan_mask_set(indio_dev, iio_scan_el_gyro_y.number); | ||
196 | iio_scan_mask_set(indio_dev, iio_scan_el_gyro_z.number); | ||
197 | iio_scan_mask_set(indio_dev, iio_scan_el_accel_x.number); | ||
198 | iio_scan_mask_set(indio_dev, iio_scan_el_accel_y.number); | ||
199 | iio_scan_mask_set(indio_dev, iio_scan_el_accel_z.number); | ||
200 | iio_scan_mask_set(indio_dev, iio_scan_el_magn_x.number); | ||
201 | iio_scan_mask_set(indio_dev, iio_scan_el_magn_y.number); | ||
202 | iio_scan_mask_set(indio_dev, iio_scan_el_magn_z.number); | ||
203 | iio_scan_mask_set(indio_dev, iio_scan_el_temp.number); | ||
204 | iio_scan_mask_set(indio_dev, iio_scan_el_adc_0.number); | ||
205 | indio_dev->scan_timestamp = true; | ||
206 | |||
207 | indio_dev->scan_el_attrs = &adis16400_scan_el_group; | ||
208 | |||
209 | ring = iio_sw_rb_allocate(indio_dev); | ||
210 | if (!ring) { | ||
211 | ret = -ENOMEM; | ||
212 | return ret; | ||
213 | } | ||
214 | indio_dev->ring = ring; | ||
215 | /* Effectively select the ring buffer implementation */ | ||
216 | iio_ring_sw_register_funcs(&ring->access); | ||
217 | ring->preenable = &adis16400_data_rdy_ring_preenable; | ||
218 | ring->postenable = &adis16400_data_rdy_ring_postenable; | ||
219 | ring->predisable = &adis16400_data_rdy_ring_predisable; | ||
220 | ring->owner = THIS_MODULE; | ||
221 | |||
222 | indio_dev->pollfunc = kzalloc(sizeof(*indio_dev->pollfunc), GFP_KERNEL); | ||
223 | if (indio_dev->pollfunc == NULL) { | ||
224 | ret = -ENOMEM; | ||
225 | goto error_iio_sw_rb_free;; | ||
226 | } | ||
227 | indio_dev->pollfunc->poll_func_main = &adis16400_poll_func_th; | ||
228 | indio_dev->pollfunc->private_data = indio_dev; | ||
229 | indio_dev->modes |= INDIO_RING_TRIGGERED; | ||
230 | return 0; | ||
231 | |||
232 | error_iio_sw_rb_free: | ||
233 | iio_sw_rb_free(indio_dev->ring); | ||
234 | return ret; | ||
235 | } | ||
236 | |||
237 | int adis16400_initialize_ring(struct iio_ring_buffer *ring) | ||
238 | { | ||
239 | return iio_ring_buffer_register(ring, 0); | ||
240 | } | ||
241 | |||
242 | void adis16400_uninitialize_ring(struct iio_ring_buffer *ring) | ||
243 | { | ||
244 | iio_ring_buffer_unregister(ring); | ||
245 | } | ||