Commit | Line | Data |
---|---|---|
d5176b41 SY |
1 | /* |
2 | * USB Phidget MotorControl driver | |
3 | * | |
4 | * Copyright (C) 2006 Sean Young <sean@mess.org> | |
5 | * | |
6 | * This program is free software; you can redistribute it and/or modify | |
7 | * it under the terms of the GNU General Public License as published by | |
8 | * the Free Software Foundation; either version 2 of the License, or | |
9 | * (at your option) any later version. | |
10 | */ | |
11 | ||
12 | #include <linux/kernel.h> | |
13 | #include <linux/errno.h> | |
14 | #include <linux/init.h> | |
15 | #include <linux/module.h> | |
16 | #include <linux/usb.h> | |
17 | ||
912b24c3 SY |
18 | #include "phidget.h" |
19 | ||
d5176b41 SY |
20 | #define DRIVER_AUTHOR "Sean Young <sean@mess.org>" |
21 | #define DRIVER_DESC "USB PhidgetMotorControl Driver" | |
22 | ||
23 | #define USB_VENDOR_ID_GLAB 0x06c2 | |
24 | #define USB_DEVICE_ID_MOTORCONTROL 0x0058 | |
25 | ||
26 | #define URB_INT_SIZE 8 | |
27 | ||
912b24c3 SY |
28 | static unsigned long device_no; |
29 | ||
d5176b41 SY |
30 | struct motorcontrol { |
31 | struct usb_device *udev; | |
32 | struct usb_interface *intf; | |
912b24c3 SY |
33 | struct device *dev; |
34 | int dev_no; | |
d5176b41 SY |
35 | u8 inputs[4]; |
36 | s8 desired_speed[2]; | |
37 | s8 speed[2]; | |
38 | s16 _current[2]; | |
39 | s8 acceleration[2]; | |
40 | struct urb *irq; | |
41 | unsigned char *data; | |
42 | dma_addr_t data_dma; | |
43 | ||
c4028958 | 44 | struct delayed_work do_notify; |
d5176b41 SY |
45 | unsigned long input_events; |
46 | unsigned long speed_events; | |
47 | unsigned long exceed_events; | |
48 | }; | |
49 | ||
50 | static struct usb_device_id id_table[] = { | |
51 | { USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_MOTORCONTROL) }, | |
52 | {} | |
53 | }; | |
54 | MODULE_DEVICE_TABLE(usb, id_table); | |
55 | ||
56 | static int set_motor(struct motorcontrol *mc, int motor) | |
57 | { | |
58 | u8 *buffer; | |
59 | int speed, speed2, acceleration; | |
60 | int retval; | |
61 | ||
62 | buffer = kzalloc(8, GFP_KERNEL); | |
63 | if (!buffer) { | |
441b62c1 | 64 | dev_err(&mc->intf->dev, "%s - out of memory\n", __func__); |
d5176b41 SY |
65 | return -ENOMEM; |
66 | } | |
67 | ||
68 | acceleration = mc->acceleration[motor] * 10; | |
69 | /* -127 <= speed <= 127 */ | |
70 | speed = (mc->desired_speed[motor] * 127) / 100; | |
71 | /* -0x7300 <= speed2 <= 0x7300 */ | |
72 | speed2 = (mc->desired_speed[motor] * 230 * 128) / 100; | |
73 | ||
74 | buffer[0] = motor; | |
75 | buffer[1] = speed; | |
76 | buffer[2] = acceleration >> 8; | |
77 | buffer[3] = acceleration; | |
78 | buffer[4] = speed2 >> 8; | |
79 | buffer[5] = speed2; | |
80 | ||
81 | retval = usb_control_msg(mc->udev, | |
82 | usb_sndctrlpipe(mc->udev, 0), | |
83 | 0x09, 0x21, 0x0200, 0x0000, buffer, 8, 2000); | |
84 | ||
85 | if (retval != 8) | |
86 | dev_err(&mc->intf->dev, "usb_control_msg returned %d\n", | |
87 | retval); | |
88 | kfree(buffer); | |
89 | ||
90 | return retval < 0 ? retval : 0; | |
91 | } | |
92 | ||
7d12e780 | 93 | static void motorcontrol_irq(struct urb *urb) |
d5176b41 SY |
94 | { |
95 | struct motorcontrol *mc = urb->context; | |
96 | unsigned char *buffer = mc->data; | |
97 | int i, level; | |
a95a0381 GKH |
98 | int retval; |
99 | int status = urb->status;; | |
d5176b41 | 100 | |
a95a0381 | 101 | switch (status) { |
d5176b41 SY |
102 | case 0: /* success */ |
103 | break; | |
104 | case -ECONNRESET: /* unlink */ | |
105 | case -ENOENT: | |
106 | case -ESHUTDOWN: | |
107 | return; | |
108 | /* -EPIPE: should clear the halt */ | |
109 | default: /* error */ | |
110 | goto resubmit; | |
111 | } | |
112 | ||
113 | /* digital inputs */ | |
114 | for (i=0; i<4; i++) { | |
115 | level = (buffer[0] >> i) & 1; | |
116 | if (mc->inputs[i] != level) { | |
117 | mc->inputs[i] = level; | |
118 | set_bit(i, &mc->input_events); | |
119 | } | |
120 | } | |
121 | ||
122 | /* motor speed */ | |
123 | if (buffer[2] == 0) { | |
124 | for (i=0; i<2; i++) { | |
125 | level = ((s8)buffer[4+i]) * 100 / 127; | |
126 | if (mc->speed[i] != level) { | |
127 | mc->speed[i] = level; | |
128 | set_bit(i, &mc->speed_events); | |
129 | } | |
130 | } | |
131 | } else { | |
132 | int index = buffer[3] & 1; | |
133 | ||
134 | level = ((s8)buffer[4] << 8) | buffer[5]; | |
135 | level = level * 100 / 29440; | |
136 | if (mc->speed[index] != level) { | |
137 | mc->speed[index] = level; | |
138 | set_bit(index, &mc->speed_events); | |
139 | } | |
140 | ||
141 | level = ((s8)buffer[6] << 8) | buffer[7]; | |
142 | mc->_current[index] = level * 100 / 1572; | |
143 | } | |
144 | ||
145 | if (buffer[1] & 1) | |
146 | set_bit(0, &mc->exceed_events); | |
147 | ||
148 | if (buffer[1] & 2) | |
149 | set_bit(1, &mc->exceed_events); | |
150 | ||
151 | if (mc->input_events || mc->exceed_events || mc->speed_events) | |
c4028958 | 152 | schedule_delayed_work(&mc->do_notify, 0); |
d5176b41 SY |
153 | |
154 | resubmit: | |
a95a0381 GKH |
155 | retval = usb_submit_urb(urb, GFP_ATOMIC); |
156 | if (retval) | |
d5176b41 | 157 | dev_err(&mc->intf->dev, |
898eb71c | 158 | "can't resubmit intr, %s-%s/motorcontrol0, retval %d\n", |
d5176b41 | 159 | mc->udev->bus->bus_name, |
a95a0381 | 160 | mc->udev->devpath, retval); |
d5176b41 SY |
161 | } |
162 | ||
c4028958 | 163 | static void do_notify(struct work_struct *work) |
d5176b41 | 164 | { |
c4028958 DH |
165 | struct motorcontrol *mc = |
166 | container_of(work, struct motorcontrol, do_notify.work); | |
d5176b41 SY |
167 | int i; |
168 | char sysfs_file[8]; | |
169 | ||
170 | for (i=0; i<4; i++) { | |
171 | if (test_and_clear_bit(i, &mc->input_events)) { | |
172 | sprintf(sysfs_file, "input%d", i); | |
912b24c3 | 173 | sysfs_notify(&mc->dev->kobj, NULL, sysfs_file); |
d5176b41 SY |
174 | } |
175 | } | |
176 | ||
177 | for (i=0; i<2; i++) { | |
178 | if (test_and_clear_bit(i, &mc->speed_events)) { | |
179 | sprintf(sysfs_file, "speed%d", i); | |
912b24c3 | 180 | sysfs_notify(&mc->dev->kobj, NULL, sysfs_file); |
d5176b41 SY |
181 | } |
182 | } | |
183 | ||
184 | for (i=0; i<2; i++) { | |
185 | if (test_and_clear_bit(i, &mc->exceed_events)) | |
186 | dev_warn(&mc->intf->dev, | |
187 | "motor #%d exceeds 1.5 Amp current limit\n", i); | |
188 | } | |
189 | } | |
190 | ||
191 | #define show_set_speed(value) \ | |
912b24c3 SY |
192 | static ssize_t set_speed##value(struct device *dev, \ |
193 | struct device_attribute *attr, \ | |
194 | const char *buf, size_t count) \ | |
d5176b41 | 195 | { \ |
912b24c3 | 196 | struct motorcontrol *mc = dev_get_drvdata(dev); \ |
d5176b41 SY |
197 | int speed; \ |
198 | int retval; \ | |
199 | \ | |
200 | if (sscanf(buf, "%d", &speed) < 1) \ | |
201 | return -EINVAL; \ | |
202 | \ | |
203 | if (speed < -100 || speed > 100) \ | |
204 | return -EINVAL; \ | |
205 | \ | |
206 | mc->desired_speed[value] = speed; \ | |
207 | \ | |
208 | retval = set_motor(mc, value); \ | |
209 | \ | |
210 | return retval ? retval : count; \ | |
211 | } \ | |
212 | \ | |
912b24c3 SY |
213 | static ssize_t show_speed##value(struct device *dev, \ |
214 | struct device_attribute *attr, \ | |
215 | char *buf) \ | |
d5176b41 | 216 | { \ |
912b24c3 | 217 | struct motorcontrol *mc = dev_get_drvdata(dev); \ |
d5176b41 SY |
218 | \ |
219 | return sprintf(buf, "%d\n", mc->speed[value]); \ | |
da308e8d SY |
220 | } |
221 | ||
222 | #define speed_attr(value) \ | |
223 | __ATTR(speed##value, S_IWUGO | S_IRUGO, \ | |
224 | show_speed##value, set_speed##value) | |
225 | ||
d5176b41 SY |
226 | show_set_speed(0); |
227 | show_set_speed(1); | |
228 | ||
229 | #define show_set_acceleration(value) \ | |
230 | static ssize_t set_acceleration##value(struct device *dev, \ | |
912b24c3 SY |
231 | struct device_attribute *attr, \ |
232 | const char *buf, size_t count) \ | |
d5176b41 | 233 | { \ |
912b24c3 | 234 | struct motorcontrol *mc = dev_get_drvdata(dev); \ |
d5176b41 SY |
235 | int acceleration; \ |
236 | int retval; \ | |
237 | \ | |
238 | if (sscanf(buf, "%d", &acceleration) < 1) \ | |
239 | return -EINVAL; \ | |
240 | \ | |
241 | if (acceleration < 0 || acceleration > 100) \ | |
242 | return -EINVAL; \ | |
243 | \ | |
244 | mc->acceleration[value] = acceleration; \ | |
245 | \ | |
246 | retval = set_motor(mc, value); \ | |
247 | \ | |
248 | return retval ? retval : count; \ | |
249 | } \ | |
250 | \ | |
912b24c3 SY |
251 | static ssize_t show_acceleration##value(struct device *dev, \ |
252 | struct device_attribute *attr, \ | |
253 | char *buf) \ | |
d5176b41 | 254 | { \ |
912b24c3 | 255 | struct motorcontrol *mc = dev_get_drvdata(dev); \ |
d5176b41 SY |
256 | \ |
257 | return sprintf(buf, "%d\n", mc->acceleration[value]); \ | |
da308e8d SY |
258 | } |
259 | ||
260 | #define acceleration_attr(value) \ | |
261 | __ATTR(acceleration##value, S_IWUGO | S_IRUGO, \ | |
262 | show_acceleration##value, set_acceleration##value) | |
263 | ||
d5176b41 SY |
264 | show_set_acceleration(0); |
265 | show_set_acceleration(1); | |
266 | ||
267 | #define show_current(value) \ | |
912b24c3 SY |
268 | static ssize_t show_current##value(struct device *dev, \ |
269 | struct device_attribute *attr, \ | |
270 | char *buf) \ | |
d5176b41 | 271 | { \ |
912b24c3 | 272 | struct motorcontrol *mc = dev_get_drvdata(dev); \ |
d5176b41 SY |
273 | \ |
274 | return sprintf(buf, "%dmA\n", (int)mc->_current[value]); \ | |
da308e8d SY |
275 | } |
276 | ||
277 | #define current_attr(value) \ | |
278 | __ATTR(current##value, S_IRUGO, show_current##value, NULL) | |
d5176b41 SY |
279 | |
280 | show_current(0); | |
281 | show_current(1); | |
282 | ||
283 | #define show_input(value) \ | |
912b24c3 SY |
284 | static ssize_t show_input##value(struct device *dev, \ |
285 | struct device_attribute *attr, \ | |
286 | char *buf) \ | |
d5176b41 | 287 | { \ |
912b24c3 | 288 | struct motorcontrol *mc = dev_get_drvdata(dev); \ |
d5176b41 SY |
289 | \ |
290 | return sprintf(buf, "%d\n", (int)mc->inputs[value]); \ | |
da308e8d SY |
291 | } |
292 | ||
293 | #define input_attr(value) \ | |
294 | __ATTR(input##value, S_IRUGO, show_input##value, NULL) | |
d5176b41 SY |
295 | |
296 | show_input(0); | |
297 | show_input(1); | |
298 | show_input(2); | |
299 | show_input(3); | |
300 | ||
da308e8d SY |
301 | static struct device_attribute dev_attrs[] = { |
302 | input_attr(0), | |
303 | input_attr(1), | |
304 | input_attr(2), | |
305 | input_attr(3), | |
306 | speed_attr(0), | |
307 | speed_attr(1), | |
308 | acceleration_attr(0), | |
309 | acceleration_attr(1), | |
310 | current_attr(0), | |
311 | current_attr(1) | |
312 | }; | |
313 | ||
d5176b41 SY |
314 | static int motorcontrol_probe(struct usb_interface *intf, const struct usb_device_id *id) |
315 | { | |
316 | struct usb_device *dev = interface_to_usbdev(intf); | |
317 | struct usb_host_interface *interface; | |
318 | struct usb_endpoint_descriptor *endpoint; | |
319 | struct motorcontrol *mc; | |
320 | int pipe, maxp, rc = -ENOMEM; | |
da308e8d | 321 | int bit, value, i; |
d5176b41 SY |
322 | |
323 | interface = intf->cur_altsetting; | |
324 | if (interface->desc.bNumEndpoints != 1) | |
325 | return -ENODEV; | |
326 | ||
327 | endpoint = &interface->endpoint[0].desc; | |
a742e5a7 | 328 | if (!usb_endpoint_dir_in(endpoint)) |
d5176b41 SY |
329 | return -ENODEV; |
330 | ||
331 | /* | |
332 | * bmAttributes | |
333 | */ | |
334 | pipe = usb_rcvintpipe(dev, endpoint->bEndpointAddress); | |
335 | maxp = usb_maxpacket(dev, pipe, usb_pipeout(pipe)); | |
336 | ||
337 | mc = kzalloc(sizeof(*mc), GFP_KERNEL); | |
338 | if (!mc) | |
339 | goto out; | |
340 | ||
912b24c3 | 341 | mc->dev_no = -1; |
54e6ecb2 | 342 | mc->data = usb_buffer_alloc(dev, URB_INT_SIZE, GFP_ATOMIC, &mc->data_dma); |
d5176b41 SY |
343 | if (!mc->data) |
344 | goto out; | |
345 | ||
346 | mc->irq = usb_alloc_urb(0, GFP_KERNEL); | |
347 | if (!mc->irq) | |
348 | goto out; | |
349 | ||
350 | mc->udev = usb_get_dev(dev); | |
351 | mc->intf = intf; | |
352 | mc->acceleration[0] = mc->acceleration[1] = 10; | |
c4028958 | 353 | INIT_DELAYED_WORK(&mc->do_notify, do_notify); |
d5176b41 SY |
354 | usb_fill_int_urb(mc->irq, mc->udev, pipe, mc->data, |
355 | maxp > URB_INT_SIZE ? URB_INT_SIZE : maxp, | |
356 | motorcontrol_irq, mc, endpoint->bInterval); | |
357 | mc->irq->transfer_dma = mc->data_dma; | |
358 | mc->irq->transfer_flags |= URB_NO_TRANSFER_DMA_MAP; | |
359 | ||
360 | usb_set_intfdata(intf, mc); | |
361 | ||
912b24c3 SY |
362 | do { |
363 | bit = find_first_zero_bit(&device_no, sizeof(device_no)); | |
364 | value = test_and_set_bit(bit, &device_no); | |
365 | } while(value); | |
366 | mc->dev_no = bit; | |
367 | ||
bfd3a5a9 GKH |
368 | mc->dev = device_create_drvdata(phidget_class, &mc->udev->dev, |
369 | MKDEV(0, 0), mc, | |
370 | "motorcontrol%d", mc->dev_no); | |
912b24c3 SY |
371 | if (IS_ERR(mc->dev)) { |
372 | rc = PTR_ERR(mc->dev); | |
373 | mc->dev = NULL; | |
374 | goto out; | |
375 | } | |
376 | ||
d5176b41 SY |
377 | if (usb_submit_urb(mc->irq, GFP_KERNEL)) { |
378 | rc = -EIO; | |
379 | goto out; | |
380 | } | |
381 | ||
da308e8d SY |
382 | for (i=0; i<ARRAY_SIZE(dev_attrs); i++) { |
383 | rc = device_create_file(mc->dev, &dev_attrs[i]); | |
384 | if (rc) | |
385 | goto out2; | |
386 | } | |
d5176b41 | 387 | |
912b24c3 | 388 | dev_info(&intf->dev, "USB PhidgetMotorControl attached\n"); |
d5176b41 SY |
389 | |
390 | return 0; | |
da308e8d SY |
391 | out2: |
392 | while (i-- > 0) | |
393 | device_remove_file(mc->dev, &dev_attrs[i]); | |
d5176b41 SY |
394 | out: |
395 | if (mc) { | |
5483eb13 | 396 | usb_free_urb(mc->irq); |
d5176b41 SY |
397 | if (mc->data) |
398 | usb_buffer_free(dev, URB_INT_SIZE, mc->data, mc->data_dma); | |
912b24c3 SY |
399 | if (mc->dev) |
400 | device_unregister(mc->dev); | |
401 | if (mc->dev_no >= 0) | |
402 | clear_bit(mc->dev_no, &device_no); | |
403 | ||
d5176b41 SY |
404 | kfree(mc); |
405 | } | |
406 | ||
407 | return rc; | |
408 | } | |
409 | ||
410 | static void motorcontrol_disconnect(struct usb_interface *interface) | |
411 | { | |
412 | struct motorcontrol *mc; | |
da308e8d | 413 | int i; |
d5176b41 SY |
414 | |
415 | mc = usb_get_intfdata(interface); | |
416 | usb_set_intfdata(interface, NULL); | |
417 | if (!mc) | |
418 | return; | |
419 | ||
420 | usb_kill_urb(mc->irq); | |
421 | usb_free_urb(mc->irq); | |
422 | usb_buffer_free(mc->udev, URB_INT_SIZE, mc->data, mc->data_dma); | |
423 | ||
424 | cancel_delayed_work(&mc->do_notify); | |
425 | ||
da308e8d SY |
426 | for (i=0; i<ARRAY_SIZE(dev_attrs); i++) |
427 | device_remove_file(mc->dev, &dev_attrs[i]); | |
d5176b41 | 428 | |
912b24c3 | 429 | device_unregister(mc->dev); |
d5176b41 SY |
430 | |
431 | usb_put_dev(mc->udev); | |
912b24c3 | 432 | clear_bit(mc->dev_no, &device_no); |
d5176b41 | 433 | kfree(mc); |
912b24c3 SY |
434 | |
435 | dev_info(&interface->dev, "USB PhidgetMotorControl detached\n"); | |
d5176b41 SY |
436 | } |
437 | ||
438 | static struct usb_driver motorcontrol_driver = { | |
439 | .name = "phidgetmotorcontrol", | |
440 | .probe = motorcontrol_probe, | |
441 | .disconnect = motorcontrol_disconnect, | |
442 | .id_table = id_table | |
443 | }; | |
444 | ||
445 | static int __init motorcontrol_init(void) | |
446 | { | |
447 | int retval = 0; | |
448 | ||
449 | retval = usb_register(&motorcontrol_driver); | |
450 | if (retval) | |
451 | err("usb_register failed. Error number %d", retval); | |
452 | ||
453 | return retval; | |
454 | } | |
455 | ||
456 | static void __exit motorcontrol_exit(void) | |
457 | { | |
458 | usb_deregister(&motorcontrol_driver); | |
459 | } | |
460 | ||
461 | module_init(motorcontrol_init); | |
462 | module_exit(motorcontrol_exit); | |
463 | ||
464 | MODULE_AUTHOR(DRIVER_AUTHOR); | |
465 | MODULE_DESCRIPTION(DRIVER_DESC); | |
466 | MODULE_LICENSE("GPL"); |