tjh.dev
Linux kernel mirror (for testing)
git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git
kernel
os
linux
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
28static unsigned long device_no;
29
30struct 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 work_struct do_notify;
45 unsigned long input_events;
46 unsigned long speed_events;
47 unsigned long exceed_events;
48};
49
50static struct usb_device_id id_table[] = {
51 { USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_MOTORCONTROL) },
52 {}
53};
54MODULE_DEVICE_TABLE(usb, id_table);
55
56static 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", __FUNCTION__);
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
93static 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 status;
99
100 switch (urb->status) {
101 case 0: /* success */
102 break;
103 case -ECONNRESET: /* unlink */
104 case -ENOENT:
105 case -ESHUTDOWN:
106 return;
107 /* -EPIPE: should clear the halt */
108 default: /* error */
109 goto resubmit;
110 }
111
112 /* digital inputs */
113 for (i=0; i<4; i++) {
114 level = (buffer[0] >> i) & 1;
115 if (mc->inputs[i] != level) {
116 mc->inputs[i] = level;
117 set_bit(i, &mc->input_events);
118 }
119 }
120
121 /* motor speed */
122 if (buffer[2] == 0) {
123 for (i=0; i<2; i++) {
124 level = ((s8)buffer[4+i]) * 100 / 127;
125 if (mc->speed[i] != level) {
126 mc->speed[i] = level;
127 set_bit(i, &mc->speed_events);
128 }
129 }
130 } else {
131 int index = buffer[3] & 1;
132
133 level = ((s8)buffer[4] << 8) | buffer[5];
134 level = level * 100 / 29440;
135 if (mc->speed[index] != level) {
136 mc->speed[index] = level;
137 set_bit(index, &mc->speed_events);
138 }
139
140 level = ((s8)buffer[6] << 8) | buffer[7];
141 mc->_current[index] = level * 100 / 1572;
142 }
143
144 if (buffer[1] & 1)
145 set_bit(0, &mc->exceed_events);
146
147 if (buffer[1] & 2)
148 set_bit(1, &mc->exceed_events);
149
150 if (mc->input_events || mc->exceed_events || mc->speed_events)
151 schedule_work(&mc->do_notify);
152
153resubmit:
154 status = usb_submit_urb(urb, SLAB_ATOMIC);
155 if (status)
156 dev_err(&mc->intf->dev,
157 "can't resubmit intr, %s-%s/motorcontrol0, status %d",
158 mc->udev->bus->bus_name,
159 mc->udev->devpath, status);
160}
161
162static void do_notify(void *data)
163{
164 struct motorcontrol *mc = data;
165 int i;
166 char sysfs_file[8];
167
168 for (i=0; i<4; i++) {
169 if (test_and_clear_bit(i, &mc->input_events)) {
170 sprintf(sysfs_file, "input%d", i);
171 sysfs_notify(&mc->dev->kobj, NULL, sysfs_file);
172 }
173 }
174
175 for (i=0; i<2; i++) {
176 if (test_and_clear_bit(i, &mc->speed_events)) {
177 sprintf(sysfs_file, "speed%d", i);
178 sysfs_notify(&mc->dev->kobj, NULL, sysfs_file);
179 }
180 }
181
182 for (i=0; i<2; i++) {
183 if (test_and_clear_bit(i, &mc->exceed_events))
184 dev_warn(&mc->intf->dev,
185 "motor #%d exceeds 1.5 Amp current limit\n", i);
186 }
187}
188
189#define show_set_speed(value) \
190static ssize_t set_speed##value(struct device *dev, \
191 struct device_attribute *attr, \
192 const char *buf, size_t count) \
193{ \
194 struct motorcontrol *mc = dev_get_drvdata(dev); \
195 int speed; \
196 int retval; \
197 \
198 if (sscanf(buf, "%d", &speed) < 1) \
199 return -EINVAL; \
200 \
201 if (speed < -100 || speed > 100) \
202 return -EINVAL; \
203 \
204 mc->desired_speed[value] = speed; \
205 \
206 retval = set_motor(mc, value); \
207 \
208 return retval ? retval : count; \
209} \
210 \
211static ssize_t show_speed##value(struct device *dev, \
212 struct device_attribute *attr, \
213 char *buf) \
214{ \
215 struct motorcontrol *mc = dev_get_drvdata(dev); \
216 \
217 return sprintf(buf, "%d\n", mc->speed[value]); \
218}
219
220#define speed_attr(value) \
221 __ATTR(speed##value, S_IWUGO | S_IRUGO, \
222 show_speed##value, set_speed##value)
223
224show_set_speed(0);
225show_set_speed(1);
226
227#define show_set_acceleration(value) \
228static ssize_t set_acceleration##value(struct device *dev, \
229 struct device_attribute *attr, \
230 const char *buf, size_t count) \
231{ \
232 struct motorcontrol *mc = dev_get_drvdata(dev); \
233 int acceleration; \
234 int retval; \
235 \
236 if (sscanf(buf, "%d", &acceleration) < 1) \
237 return -EINVAL; \
238 \
239 if (acceleration < 0 || acceleration > 100) \
240 return -EINVAL; \
241 \
242 mc->acceleration[value] = acceleration; \
243 \
244 retval = set_motor(mc, value); \
245 \
246 return retval ? retval : count; \
247} \
248 \
249static ssize_t show_acceleration##value(struct device *dev, \
250 struct device_attribute *attr, \
251 char *buf) \
252{ \
253 struct motorcontrol *mc = dev_get_drvdata(dev); \
254 \
255 return sprintf(buf, "%d\n", mc->acceleration[value]); \
256}
257
258#define acceleration_attr(value) \
259 __ATTR(acceleration##value, S_IWUGO | S_IRUGO, \
260 show_acceleration##value, set_acceleration##value)
261
262show_set_acceleration(0);
263show_set_acceleration(1);
264
265#define show_current(value) \
266static ssize_t show_current##value(struct device *dev, \
267 struct device_attribute *attr, \
268 char *buf) \
269{ \
270 struct motorcontrol *mc = dev_get_drvdata(dev); \
271 \
272 return sprintf(buf, "%dmA\n", (int)mc->_current[value]); \
273}
274
275#define current_attr(value) \
276 __ATTR(current##value, S_IRUGO, show_current##value, NULL)
277
278show_current(0);
279show_current(1);
280
281#define show_input(value) \
282static ssize_t show_input##value(struct device *dev, \
283 struct device_attribute *attr, \
284 char *buf) \
285{ \
286 struct motorcontrol *mc = dev_get_drvdata(dev); \
287 \
288 return sprintf(buf, "%d\n", (int)mc->inputs[value]); \
289}
290
291#define input_attr(value) \
292 __ATTR(input##value, S_IRUGO, show_input##value, NULL)
293
294show_input(0);
295show_input(1);
296show_input(2);
297show_input(3);
298
299static struct device_attribute dev_attrs[] = {
300 input_attr(0),
301 input_attr(1),
302 input_attr(2),
303 input_attr(3),
304 speed_attr(0),
305 speed_attr(1),
306 acceleration_attr(0),
307 acceleration_attr(1),
308 current_attr(0),
309 current_attr(1)
310};
311
312static int motorcontrol_probe(struct usb_interface *intf, const struct usb_device_id *id)
313{
314 struct usb_device *dev = interface_to_usbdev(intf);
315 struct usb_host_interface *interface;
316 struct usb_endpoint_descriptor *endpoint;
317 struct motorcontrol *mc;
318 int pipe, maxp, rc = -ENOMEM;
319 int bit, value, i;
320
321 interface = intf->cur_altsetting;
322 if (interface->desc.bNumEndpoints != 1)
323 return -ENODEV;
324
325 endpoint = &interface->endpoint[0].desc;
326 if (!(endpoint->bEndpointAddress & 0x80))
327 return -ENODEV;
328
329 /*
330 * bmAttributes
331 */
332 pipe = usb_rcvintpipe(dev, endpoint->bEndpointAddress);
333 maxp = usb_maxpacket(dev, pipe, usb_pipeout(pipe));
334
335 mc = kzalloc(sizeof(*mc), GFP_KERNEL);
336 if (!mc)
337 goto out;
338
339 mc->dev_no = -1;
340 mc->data = usb_buffer_alloc(dev, URB_INT_SIZE, SLAB_ATOMIC, &mc->data_dma);
341 if (!mc->data)
342 goto out;
343
344 mc->irq = usb_alloc_urb(0, GFP_KERNEL);
345 if (!mc->irq)
346 goto out;
347
348 mc->udev = usb_get_dev(dev);
349 mc->intf = intf;
350 mc->acceleration[0] = mc->acceleration[1] = 10;
351 INIT_WORK(&mc->do_notify, do_notify, mc);
352 usb_fill_int_urb(mc->irq, mc->udev, pipe, mc->data,
353 maxp > URB_INT_SIZE ? URB_INT_SIZE : maxp,
354 motorcontrol_irq, mc, endpoint->bInterval);
355 mc->irq->transfer_dma = mc->data_dma;
356 mc->irq->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
357
358 usb_set_intfdata(intf, mc);
359
360 do {
361 bit = find_first_zero_bit(&device_no, sizeof(device_no));
362 value = test_and_set_bit(bit, &device_no);
363 } while(value);
364 mc->dev_no = bit;
365
366 mc->dev = device_create(phidget_class, &mc->udev->dev, 0,
367 "motorcontrol%d", mc->dev_no);
368 if (IS_ERR(mc->dev)) {
369 rc = PTR_ERR(mc->dev);
370 mc->dev = NULL;
371 goto out;
372 }
373
374 dev_set_drvdata(mc->dev, mc);
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;
390out2:
391 while (i-- > 0)
392 device_remove_file(mc->dev, &dev_attrs[i]);
393out:
394 if (mc) {
395 if (mc->irq)
396 usb_free_urb(mc->irq);
397 if (mc->data)
398 usb_buffer_free(dev, URB_INT_SIZE, mc->data, mc->data_dma);
399 if (mc->dev)
400 device_unregister(mc->dev);
401 if (mc->dev_no >= 0)
402 clear_bit(mc->dev_no, &device_no);
403
404 kfree(mc);
405 }
406
407 return rc;
408}
409
410static void motorcontrol_disconnect(struct usb_interface *interface)
411{
412 struct motorcontrol *mc;
413 int i;
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
426 for (i=0; i<ARRAY_SIZE(dev_attrs); i++)
427 device_remove_file(mc->dev, &dev_attrs[i]);
428
429 device_unregister(mc->dev);
430
431 usb_put_dev(mc->udev);
432 clear_bit(mc->dev_no, &device_no);
433 kfree(mc);
434
435 dev_info(&interface->dev, "USB PhidgetMotorControl detached\n");
436}
437
438static struct usb_driver motorcontrol_driver = {
439 .name = "phidgetmotorcontrol",
440 .probe = motorcontrol_probe,
441 .disconnect = motorcontrol_disconnect,
442 .id_table = id_table
443};
444
445static 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
456static void __exit motorcontrol_exit(void)
457{
458 usb_deregister(&motorcontrol_driver);
459}
460
461module_init(motorcontrol_init);
462module_exit(motorcontrol_exit);
463
464MODULE_AUTHOR(DRIVER_AUTHOR);
465MODULE_DESCRIPTION(DRIVER_DESC);
466MODULE_LICENSE("GPL");