tjh.dev
Linux kernel mirror (for testing)
git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git
kernel
os
linux
1/*
2 * USB PhidgetServo driver 1.0
3 *
4 * Copyright (C) 2004 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 * This is a driver for the USB PhidgetServo version 2.0 and 3.0 servo
12 * controllers available at: http://www.phidgets.com/
13 *
14 * Note that the driver takes input as: degrees.minutes
15 *
16 * CAUTION: Generally you should use 0 < degrees < 180 as anything else
17 * is probably beyond the range of your servo and may damage it.
18 *
19 * Jun 16, 2004: Sean Young <sean@mess.org>
20 * - cleanups
21 * - was using memory after kfree()
22 * Aug 8, 2004: Sean Young <sean@mess.org>
23 * - set the highest angle as high as the hardware allows, there are
24 * some odd servos out there
25 *
26 */
27
28#include <linux/config.h>
29#ifdef CONFIG_USB_DEBUG
30#define DEBUG 1
31#endif
32#include <linux/kernel.h>
33#include <linux/errno.h>
34#include <linux/init.h>
35#include <linux/slab.h>
36#include <linux/module.h>
37#include <linux/usb.h>
38
39#define DRIVER_AUTHOR "Sean Young <sean@mess.org>"
40#define DRIVER_DESC "USB PhidgetServo Driver"
41
42#define VENDOR_ID_GLAB 0x06c2
43#define DEVICE_ID_GLAB_PHIDGETSERVO_QUAD 0x0038
44#define DEVICE_ID_GLAB_PHIDGETSERVO_UNI 0x0039
45
46#define VENDOR_ID_WISEGROUP 0x0925
47#define VENDOR_ID_WISEGROUP_PHIDGETSERVO_QUAD 0x8101
48#define VENDOR_ID_WISEGROUP_PHIDGETSERVO_UNI 0x8104
49
50#define SERVO_VERSION_30 0x01
51#define SERVO_COUNT_QUAD 0x02
52
53static struct usb_device_id id_table[] = {
54 {
55 USB_DEVICE(VENDOR_ID_GLAB, DEVICE_ID_GLAB_PHIDGETSERVO_QUAD),
56 .driver_info = SERVO_VERSION_30 | SERVO_COUNT_QUAD
57 },
58 {
59 USB_DEVICE(VENDOR_ID_GLAB, DEVICE_ID_GLAB_PHIDGETSERVO_UNI),
60 .driver_info = SERVO_VERSION_30
61 },
62 {
63 USB_DEVICE(VENDOR_ID_WISEGROUP,
64 VENDOR_ID_WISEGROUP_PHIDGETSERVO_QUAD),
65 .driver_info = SERVO_COUNT_QUAD
66 },
67 {
68 USB_DEVICE(VENDOR_ID_WISEGROUP,
69 VENDOR_ID_WISEGROUP_PHIDGETSERVO_UNI),
70 .driver_info = 0
71 },
72 {}
73};
74
75MODULE_DEVICE_TABLE(usb, id_table);
76
77struct phidget_servo {
78 struct usb_device *udev;
79 ulong type;
80 int pulse[4];
81 int degrees[4];
82 int minutes[4];
83};
84
85static int
86change_position_v30(struct phidget_servo *servo, int servo_no, int degrees,
87 int minutes)
88{
89 int retval;
90 unsigned char *buffer;
91
92 if (degrees < -23 || degrees > 362)
93 return -EINVAL;
94
95 buffer = kmalloc(6, GFP_KERNEL);
96 if (!buffer) {
97 dev_err(&servo->udev->dev, "%s - out of memory\n",
98 __FUNCTION__);
99 return -ENOMEM;
100 }
101
102 /*
103 * pulse = 0 - 4095
104 * angle = 0 - 180 degrees
105 *
106 * pulse = angle * 10.6 + 243.8
107 */
108 servo->pulse[servo_no] = ((degrees*60 + minutes)*106 + 2438*60)/600;
109 servo->degrees[servo_no]= degrees;
110 servo->minutes[servo_no]= minutes;
111
112 /*
113 * The PhidgetServo v3.0 is controlled by sending 6 bytes,
114 * 4 * 12 bits for each servo.
115 *
116 * low = lower 8 bits pulse
117 * high = higher 4 bits pulse
118 *
119 * offset bits
120 * +---+-----------------+
121 * | 0 | low 0 |
122 * +---+--------+--------+
123 * | 1 | high 1 | high 0 |
124 * +---+--------+--------+
125 * | 2 | low 1 |
126 * +---+-----------------+
127 * | 3 | low 2 |
128 * +---+--------+--------+
129 * | 4 | high 3 | high 2 |
130 * +---+--------+--------+
131 * | 5 | low 3 |
132 * +---+-----------------+
133 */
134
135 buffer[0] = servo->pulse[0] & 0xff;
136 buffer[1] = (servo->pulse[0] >> 8 & 0x0f)
137 | (servo->pulse[1] >> 4 & 0xf0);
138 buffer[2] = servo->pulse[1] & 0xff;
139 buffer[3] = servo->pulse[2] & 0xff;
140 buffer[4] = (servo->pulse[2] >> 8 & 0x0f)
141 | (servo->pulse[3] >> 4 & 0xf0);
142 buffer[5] = servo->pulse[3] & 0xff;
143
144 dev_dbg(&servo->udev->dev,
145 "data: %02x %02x %02x %02x %02x %02x\n",
146 buffer[0], buffer[1], buffer[2],
147 buffer[3], buffer[4], buffer[5]);
148
149 retval = usb_control_msg(servo->udev,
150 usb_sndctrlpipe(servo->udev, 0),
151 0x09, 0x21, 0x0200, 0x0000, buffer, 6, 2000);
152
153 kfree(buffer);
154
155 return retval;
156}
157
158static int
159change_position_v20(struct phidget_servo *servo, int servo_no, int degrees,
160 int minutes)
161{
162 int retval;
163 unsigned char *buffer;
164
165 if (degrees < -23 || degrees > 278)
166 return -EINVAL;
167
168 buffer = kmalloc(2, GFP_KERNEL);
169 if (!buffer) {
170 dev_err(&servo->udev->dev, "%s - out of memory\n",
171 __FUNCTION__);
172 return -ENOMEM;
173 }
174
175 /*
176 * angle = 0 - 180 degrees
177 * pulse = angle + 23
178 */
179 servo->pulse[servo_no]= degrees + 23;
180 servo->degrees[servo_no]= degrees;
181 servo->minutes[servo_no]= 0;
182
183 /*
184 * The PhidgetServo v2.0 is controlled by sending two bytes. The
185 * first byte is the servo number xor'ed with 2:
186 *
187 * servo 0 = 2
188 * servo 1 = 3
189 * servo 2 = 0
190 * servo 3 = 1
191 *
192 * The second byte is the position.
193 */
194
195 buffer[0] = servo_no ^ 2;
196 buffer[1] = servo->pulse[servo_no];
197
198 dev_dbg(&servo->udev->dev, "data: %02x %02x\n", buffer[0], buffer[1]);
199
200 retval = usb_control_msg(servo->udev,
201 usb_sndctrlpipe(servo->udev, 0),
202 0x09, 0x21, 0x0200, 0x0000, buffer, 2, 2000);
203
204 kfree(buffer);
205
206 return retval;
207}
208
209#define show_set(value) \
210static ssize_t set_servo##value (struct device *dev, struct device_attribute *attr, \
211 const char *buf, size_t count) \
212{ \
213 int degrees, minutes, retval; \
214 struct usb_interface *intf = to_usb_interface (dev); \
215 struct phidget_servo *servo = usb_get_intfdata (intf); \
216 \
217 minutes = 0; \
218 /* must at least convert degrees */ \
219 if (sscanf (buf, "%d.%d", °rees, &minutes) < 1) { \
220 return -EINVAL; \
221 } \
222 \
223 if (minutes < 0 || minutes > 59) \
224 return -EINVAL; \
225 \
226 if (servo->type & SERVO_VERSION_30) \
227 retval = change_position_v30 (servo, value, degrees, \
228 minutes); \
229 else \
230 retval = change_position_v20 (servo, value, degrees, \
231 minutes); \
232 \
233 return retval < 0 ? retval : count; \
234} \
235 \
236static ssize_t show_servo##value (struct device *dev, struct device_attribute *attr, char *buf) \
237{ \
238 struct usb_interface *intf = to_usb_interface (dev); \
239 struct phidget_servo *servo = usb_get_intfdata (intf); \
240 \
241 return sprintf (buf, "%d.%02d\n", servo->degrees[value], \
242 servo->minutes[value]); \
243} \
244static DEVICE_ATTR(servo##value, S_IWUGO | S_IRUGO, \
245 show_servo##value, set_servo##value);
246
247show_set(0);
248show_set(1);
249show_set(2);
250show_set(3);
251
252static int
253servo_probe(struct usb_interface *interface, const struct usb_device_id *id)
254{
255 struct usb_device *udev = interface_to_usbdev(interface);
256 struct phidget_servo *dev;
257
258 dev = kmalloc(sizeof (struct phidget_servo), GFP_KERNEL);
259 if (dev == NULL) {
260 dev_err(&interface->dev, "%s - out of memory\n", __FUNCTION__);
261 return -ENOMEM;
262 }
263 memset(dev, 0x00, sizeof (*dev));
264
265 dev->udev = usb_get_dev(udev);
266 dev->type = id->driver_info;
267 usb_set_intfdata(interface, dev);
268
269 device_create_file(&interface->dev, &dev_attr_servo0);
270 if (dev->type & SERVO_COUNT_QUAD) {
271 device_create_file(&interface->dev, &dev_attr_servo1);
272 device_create_file(&interface->dev, &dev_attr_servo2);
273 device_create_file(&interface->dev, &dev_attr_servo3);
274 }
275
276 dev_info(&interface->dev, "USB %d-Motor PhidgetServo v%d.0 attached\n",
277 dev->type & SERVO_COUNT_QUAD ? 4 : 1,
278 dev->type & SERVO_VERSION_30 ? 3 : 2);
279
280 if(!(dev->type & SERVO_VERSION_30))
281 dev_info(&interface->dev,
282 "WARNING: v2.0 not tested! Please report if it works.\n");
283
284 return 0;
285}
286
287static void
288servo_disconnect(struct usb_interface *interface)
289{
290 struct phidget_servo *dev;
291
292 dev = usb_get_intfdata(interface);
293 usb_set_intfdata(interface, NULL);
294
295 device_remove_file(&interface->dev, &dev_attr_servo0);
296 if (dev->type & SERVO_COUNT_QUAD) {
297 device_remove_file(&interface->dev, &dev_attr_servo1);
298 device_remove_file(&interface->dev, &dev_attr_servo2);
299 device_remove_file(&interface->dev, &dev_attr_servo3);
300 }
301
302 usb_put_dev(dev->udev);
303
304 dev_info(&interface->dev, "USB %d-Motor PhidgetServo v%d.0 detached\n",
305 dev->type & SERVO_COUNT_QUAD ? 4 : 1,
306 dev->type & SERVO_VERSION_30 ? 3 : 2);
307
308 kfree(dev);
309}
310
311static struct usb_driver servo_driver = {
312 .owner = THIS_MODULE,
313 .name = "phidgetservo",
314 .probe = servo_probe,
315 .disconnect = servo_disconnect,
316 .id_table = id_table
317};
318
319static int __init
320phidget_servo_init(void)
321{
322 int retval;
323
324 retval = usb_register(&servo_driver);
325 if (retval)
326 err("usb_register failed. Error number %d", retval);
327
328 return retval;
329}
330
331static void __exit
332phidget_servo_exit(void)
333{
334 usb_deregister(&servo_driver);
335}
336
337module_init(phidget_servo_init);
338module_exit(phidget_servo_exit);
339
340MODULE_AUTHOR(DRIVER_AUTHOR);
341MODULE_DESCRIPTION(DRIVER_DESC);
342MODULE_LICENSE("GPL");