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
18 #include "phidget.h"
19
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
28 static unsigned long device_no;
29
30 struct motorcontrol {
31 struct usb_device *udev;
32 struct usb_interface *intf;
33 struct device *dev;
34 int dev_no;
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
44 struct delayed_work do_notify;
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
set_motor(struct motorcontrol * mc,int motor)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) {
64 dev_err(&mc->intf->dev, "%s - out of memory\n", __func__);
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
motorcontrol_irq(struct urb * urb)93 static void motorcontrol_irq(struct urb *urb)
94 {
95 struct motorcontrol *mc = urb->context;
96 unsigned char *buffer = mc->data;
97 int i, level;
98 int retval;
99 int status = urb->status;;
100
101 switch (status) {
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)
152 schedule_delayed_work(&mc->do_notify, 0);
153
154 resubmit:
155 retval = usb_submit_urb(urb, GFP_ATOMIC);
156 if (retval)
157 dev_err(&mc->intf->dev,
158 "can't resubmit intr, %s-%s/motorcontrol0, retval %d\n",
159 mc->udev->bus->bus_name,
160 mc->udev->devpath, retval);
161 }
162
do_notify(struct work_struct * work)163 static void do_notify(struct work_struct *work)
164 {
165 struct motorcontrol *mc =
166 container_of(work, struct motorcontrol, do_notify.work);
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);
173 sysfs_notify(&mc->dev->kobj, NULL, sysfs_file);
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);
180 sysfs_notify(&mc->dev->kobj, NULL, sysfs_file);
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) \
192 static ssize_t set_speed##value(struct device *dev, \
193 struct device_attribute *attr, \
194 const char *buf, size_t count) \
195 { \
196 struct motorcontrol *mc = dev_get_drvdata(dev); \
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 \
213 static ssize_t show_speed##value(struct device *dev, \
214 struct device_attribute *attr, \
215 char *buf) \
216 { \
217 struct motorcontrol *mc = dev_get_drvdata(dev); \
218 \
219 return sprintf(buf, "%d\n", mc->speed[value]); \
220 }
221
222 #define speed_attr(value) \
223 __ATTR(speed##value, S_IWUGO | S_IRUGO, \
224 show_speed##value, set_speed##value)
225
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, \
231 struct device_attribute *attr, \
232 const char *buf, size_t count) \
233 { \
234 struct motorcontrol *mc = dev_get_drvdata(dev); \
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 \
251 static ssize_t show_acceleration##value(struct device *dev, \
252 struct device_attribute *attr, \
253 char *buf) \
254 { \
255 struct motorcontrol *mc = dev_get_drvdata(dev); \
256 \
257 return sprintf(buf, "%d\n", mc->acceleration[value]); \
258 }
259
260 #define acceleration_attr(value) \
261 __ATTR(acceleration##value, S_IWUGO | S_IRUGO, \
262 show_acceleration##value, set_acceleration##value)
263
264 show_set_acceleration(0);
265 show_set_acceleration(1);
266
267 #define show_current(value) \
268 static ssize_t show_current##value(struct device *dev, \
269 struct device_attribute *attr, \
270 char *buf) \
271 { \
272 struct motorcontrol *mc = dev_get_drvdata(dev); \
273 \
274 return sprintf(buf, "%dmA\n", (int)mc->_current[value]); \
275 }
276
277 #define current_attr(value) \
278 __ATTR(current##value, S_IRUGO, show_current##value, NULL)
279
280 show_current(0);
281 show_current(1);
282
283 #define show_input(value) \
284 static ssize_t show_input##value(struct device *dev, \
285 struct device_attribute *attr, \
286 char *buf) \
287 { \
288 struct motorcontrol *mc = dev_get_drvdata(dev); \
289 \
290 return sprintf(buf, "%d\n", (int)mc->inputs[value]); \
291 }
292
293 #define input_attr(value) \
294 __ATTR(input##value, S_IRUGO, show_input##value, NULL)
295
296 show_input(0);
297 show_input(1);
298 show_input(2);
299 show_input(3);
300
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
motorcontrol_probe(struct usb_interface * intf,const struct usb_device_id * id)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;
321 int bit, value, i;
322
323 interface = intf->cur_altsetting;
324 if (interface->desc.bNumEndpoints != 1)
325 return -ENODEV;
326
327 endpoint = &interface->endpoint[0].desc;
328 if (!usb_endpoint_dir_in(endpoint))
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
341 mc->dev_no = -1;
342 mc->data = usb_buffer_alloc(dev, URB_INT_SIZE, GFP_ATOMIC, &mc->data_dma);
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;
353 INIT_DELAYED_WORK(&mc->do_notify, do_notify);
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
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
368 mc->dev = device_create(phidget_class, &mc->udev->dev, MKDEV(0, 0), mc,
369 "motorcontrol%d", mc->dev_no);
370 if (IS_ERR(mc->dev)) {
371 rc = PTR_ERR(mc->dev);
372 mc->dev = NULL;
373 goto out;
374 }
375
376 if (usb_submit_urb(mc->irq, GFP_KERNEL)) {
377 rc = -EIO;
378 goto out;
379 }
380
381 for (i=0; i<ARRAY_SIZE(dev_attrs); i++) {
382 rc = device_create_file(mc->dev, &dev_attrs[i]);
383 if (rc)
384 goto out2;
385 }
386
387 dev_info(&intf->dev, "USB PhidgetMotorControl attached\n");
388
389 return 0;
390 out2:
391 while (i-- > 0)
392 device_remove_file(mc->dev, &dev_attrs[i]);
393 out:
394 if (mc) {
395 usb_free_urb(mc->irq);
396 if (mc->data)
397 usb_buffer_free(dev, URB_INT_SIZE, mc->data, mc->data_dma);
398 if (mc->dev)
399 device_unregister(mc->dev);
400 if (mc->dev_no >= 0)
401 clear_bit(mc->dev_no, &device_no);
402
403 kfree(mc);
404 }
405
406 return rc;
407 }
408
motorcontrol_disconnect(struct usb_interface * interface)409 static void motorcontrol_disconnect(struct usb_interface *interface)
410 {
411 struct motorcontrol *mc;
412 int i;
413
414 mc = usb_get_intfdata(interface);
415 usb_set_intfdata(interface, NULL);
416 if (!mc)
417 return;
418
419 usb_kill_urb(mc->irq);
420 usb_free_urb(mc->irq);
421 usb_buffer_free(mc->udev, URB_INT_SIZE, mc->data, mc->data_dma);
422
423 cancel_delayed_work(&mc->do_notify);
424
425 for (i=0; i<ARRAY_SIZE(dev_attrs); i++)
426 device_remove_file(mc->dev, &dev_attrs[i]);
427
428 device_unregister(mc->dev);
429
430 usb_put_dev(mc->udev);
431 clear_bit(mc->dev_no, &device_no);
432 kfree(mc);
433
434 dev_info(&interface->dev, "USB PhidgetMotorControl detached\n");
435 }
436
437 static struct usb_driver motorcontrol_driver = {
438 .name = "phidgetmotorcontrol",
439 .probe = motorcontrol_probe,
440 .disconnect = motorcontrol_disconnect,
441 .id_table = id_table
442 };
443
motorcontrol_init(void)444 static int __init motorcontrol_init(void)
445 {
446 int retval = 0;
447
448 retval = usb_register(&motorcontrol_driver);
449 if (retval)
450 err("usb_register failed. Error number %d", retval);
451
452 return retval;
453 }
454
motorcontrol_exit(void)455 static void __exit motorcontrol_exit(void)
456 {
457 usb_deregister(&motorcontrol_driver);
458 }
459
460 module_init(motorcontrol_init);
461 module_exit(motorcontrol_exit);
462
463 MODULE_AUTHOR(DRIVER_AUTHOR);
464 MODULE_DESCRIPTION(DRIVER_DESC);
465 MODULE_LICENSE("GPL");
466