1 /*
2 * USB PhidgetServo driver 1.0
3 *
4 * Copyright (C) 2004, 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 * 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
20 #include <linux/kernel.h>
21 #include <linux/errno.h>
22 #include <linux/init.h>
23 #include <linux/slab.h>
24 #include <linux/module.h>
25 #include <linux/usb.h>
26
27 #include "phidget.h"
28
29 #define DRIVER_AUTHOR "Sean Young <sean@mess.org>"
30 #define DRIVER_DESC "USB PhidgetServo Driver"
31
32 #define VENDOR_ID_GLAB 0x06c2
33 #define DEVICE_ID_GLAB_PHIDGETSERVO_QUAD 0x0038
34 #define DEVICE_ID_GLAB_PHIDGETSERVO_UNI 0x0039
35
36 #define VENDOR_ID_WISEGROUP 0x0925
37 #define VENDOR_ID_WISEGROUP_PHIDGETSERVO_QUAD 0x8101
38 #define VENDOR_ID_WISEGROUP_PHIDGETSERVO_UNI 0x8104
39
40 #define SERVO_VERSION_30 0x01
41 #define SERVO_COUNT_QUAD 0x02
42
43 static struct usb_device_id id_table[] = {
44 {
45 USB_DEVICE(VENDOR_ID_GLAB, DEVICE_ID_GLAB_PHIDGETSERVO_QUAD),
46 .driver_info = SERVO_VERSION_30 | SERVO_COUNT_QUAD
47 },
48 {
49 USB_DEVICE(VENDOR_ID_GLAB, DEVICE_ID_GLAB_PHIDGETSERVO_UNI),
50 .driver_info = SERVO_VERSION_30
51 },
52 {
53 USB_DEVICE(VENDOR_ID_WISEGROUP,
54 VENDOR_ID_WISEGROUP_PHIDGETSERVO_QUAD),
55 .driver_info = SERVO_COUNT_QUAD
56 },
57 {
58 USB_DEVICE(VENDOR_ID_WISEGROUP,
59 VENDOR_ID_WISEGROUP_PHIDGETSERVO_UNI),
60 .driver_info = 0
61 },
62 {}
63 };
64
65 MODULE_DEVICE_TABLE(usb, id_table);
66
67 static int unsigned long device_no;
68
69 struct phidget_servo {
70 struct usb_device *udev;
71 struct device *dev;
72 int dev_no;
73 ulong type;
74 int pulse[4];
75 int degrees[4];
76 int minutes[4];
77 };
78
79 static int
change_position_v30(struct phidget_servo * servo,int servo_no,int degrees,int minutes)80 change_position_v30(struct phidget_servo *servo, int servo_no, int degrees,
81 int minutes)
82 {
83 int retval;
84 unsigned char *buffer;
85
86 if (degrees < -23 || degrees > 362)
87 return -EINVAL;
88
89 buffer = kmalloc(6, GFP_KERNEL);
90 if (!buffer) {
91 dev_err(&servo->udev->dev, "%s - out of memory\n",
92 __func__);
93 return -ENOMEM;
94 }
95
96 /*
97 * pulse = 0 - 4095
98 * angle = 0 - 180 degrees
99 *
100 * pulse = angle * 10.6 + 243.8
101 */
102 servo->pulse[servo_no] = ((degrees*60 + minutes)*106 + 2438*60)/600;
103 servo->degrees[servo_no]= degrees;
104 servo->minutes[servo_no]= minutes;
105
106 /*
107 * The PhidgetServo v3.0 is controlled by sending 6 bytes,
108 * 4 * 12 bits for each servo.
109 *
110 * low = lower 8 bits pulse
111 * high = higher 4 bits pulse
112 *
113 * offset bits
114 * +---+-----------------+
115 * | 0 | low 0 |
116 * +---+--------+--------+
117 * | 1 | high 1 | high 0 |
118 * +---+--------+--------+
119 * | 2 | low 1 |
120 * +---+-----------------+
121 * | 3 | low 2 |
122 * +---+--------+--------+
123 * | 4 | high 3 | high 2 |
124 * +---+--------+--------+
125 * | 5 | low 3 |
126 * +---+-----------------+
127 */
128
129 buffer[0] = servo->pulse[0] & 0xff;
130 buffer[1] = (servo->pulse[0] >> 8 & 0x0f)
131 | (servo->pulse[1] >> 4 & 0xf0);
132 buffer[2] = servo->pulse[1] & 0xff;
133 buffer[3] = servo->pulse[2] & 0xff;
134 buffer[4] = (servo->pulse[2] >> 8 & 0x0f)
135 | (servo->pulse[3] >> 4 & 0xf0);
136 buffer[5] = servo->pulse[3] & 0xff;
137
138 dev_dbg(&servo->udev->dev,
139 "data: %02x %02x %02x %02x %02x %02x\n",
140 buffer[0], buffer[1], buffer[2],
141 buffer[3], buffer[4], buffer[5]);
142
143 retval = usb_control_msg(servo->udev,
144 usb_sndctrlpipe(servo->udev, 0),
145 0x09, 0x21, 0x0200, 0x0000, buffer, 6, 2000);
146
147 kfree(buffer);
148
149 return retval;
150 }
151
152 static int
change_position_v20(struct phidget_servo * servo,int servo_no,int degrees,int minutes)153 change_position_v20(struct phidget_servo *servo, int servo_no, int degrees,
154 int minutes)
155 {
156 int retval;
157 unsigned char *buffer;
158
159 if (degrees < -23 || degrees > 278)
160 return -EINVAL;
161
162 buffer = kmalloc(2, GFP_KERNEL);
163 if (!buffer) {
164 dev_err(&servo->udev->dev, "%s - out of memory\n",
165 __func__);
166 return -ENOMEM;
167 }
168
169 /*
170 * angle = 0 - 180 degrees
171 * pulse = angle + 23
172 */
173 servo->pulse[servo_no]= degrees + 23;
174 servo->degrees[servo_no]= degrees;
175 servo->minutes[servo_no]= 0;
176
177 /*
178 * The PhidgetServo v2.0 is controlled by sending two bytes. The
179 * first byte is the servo number xor'ed with 2:
180 *
181 * servo 0 = 2
182 * servo 1 = 3
183 * servo 2 = 0
184 * servo 3 = 1
185 *
186 * The second byte is the position.
187 */
188
189 buffer[0] = servo_no ^ 2;
190 buffer[1] = servo->pulse[servo_no];
191
192 dev_dbg(&servo->udev->dev, "data: %02x %02x\n", buffer[0], buffer[1]);
193
194 retval = usb_control_msg(servo->udev,
195 usb_sndctrlpipe(servo->udev, 0),
196 0x09, 0x21, 0x0200, 0x0000, buffer, 2, 2000);
197
198 kfree(buffer);
199
200 return retval;
201 }
202
203 #define show_set(value) \
204 static ssize_t set_servo##value (struct device *dev, \
205 struct device_attribute *attr, \
206 const char *buf, size_t count) \
207 { \
208 int degrees, minutes, retval; \
209 struct phidget_servo *servo = dev_get_drvdata(dev); \
210 \
211 minutes = 0; \
212 /* must at least convert degrees */ \
213 if (sscanf(buf, "%d.%d", °rees, &minutes) < 1) { \
214 return -EINVAL; \
215 } \
216 \
217 if (minutes < 0 || minutes > 59) \
218 return -EINVAL; \
219 \
220 if (servo->type & SERVO_VERSION_30) \
221 retval = change_position_v30(servo, value, degrees, \
222 minutes); \
223 else \
224 retval = change_position_v20(servo, value, degrees, \
225 minutes); \
226 \
227 return retval < 0 ? retval : count; \
228 } \
229 \
230 static ssize_t show_servo##value (struct device *dev, \
231 struct device_attribute *attr, \
232 char *buf) \
233 { \
234 struct phidget_servo *servo = dev_get_drvdata(dev); \
235 \
236 return sprintf(buf, "%d.%02d\n", servo->degrees[value], \
237 servo->minutes[value]); \
238 }
239
240 #define servo_attr(value) \
241 __ATTR(servo##value, S_IWUGO | S_IRUGO, \
242 show_servo##value, set_servo##value)
243 show_set(0);
244 show_set(1);
245 show_set(2);
246 show_set(3);
247
248 static struct device_attribute dev_attrs[] = {
249 servo_attr(0), servo_attr(1), servo_attr(2), servo_attr(3)
250 };
251
252 static int
servo_probe(struct usb_interface * interface,const struct usb_device_id * id)253 servo_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 int bit, value, rc;
258 int servo_count, i;
259
260 dev = kzalloc(sizeof (struct phidget_servo), GFP_KERNEL);
261 if (dev == NULL) {
262 dev_err(&interface->dev, "%s - out of memory\n", __func__);
263 rc = -ENOMEM;
264 goto out;
265 }
266
267 dev->udev = usb_get_dev(udev);
268 dev->type = id->driver_info;
269 dev->dev_no = -1;
270 usb_set_intfdata(interface, dev);
271
272 do {
273 bit = find_first_zero_bit(&device_no, sizeof(device_no));
274 value = test_and_set_bit(bit, &device_no);
275 } while (value);
276 dev->dev_no = bit;
277
278 dev->dev = device_create(phidget_class, &dev->udev->dev, MKDEV(0, 0),
279 dev, "servo%d", dev->dev_no);
280 if (IS_ERR(dev->dev)) {
281 rc = PTR_ERR(dev->dev);
282 dev->dev = NULL;
283 goto out;
284 }
285
286 servo_count = dev->type & SERVO_COUNT_QUAD ? 4 : 1;
287
288 for (i=0; i<servo_count; i++) {
289 rc = device_create_file(dev->dev, &dev_attrs[i]);
290 if (rc)
291 goto out2;
292 }
293
294 dev_info(&interface->dev, "USB %d-Motor PhidgetServo v%d.0 attached\n",
295 servo_count, dev->type & SERVO_VERSION_30 ? 3 : 2);
296
297 if (!(dev->type & SERVO_VERSION_30))
298 dev_info(&interface->dev,
299 "WARNING: v2.0 not tested! Please report if it works.\n");
300
301 return 0;
302 out2:
303 while (i-- > 0)
304 device_remove_file(dev->dev, &dev_attrs[i]);
305 out:
306 if (dev) {
307 if (dev->dev)
308 device_unregister(dev->dev);
309 if (dev->dev_no >= 0)
310 clear_bit(dev->dev_no, &device_no);
311
312 kfree(dev);
313 }
314
315 return rc;
316 }
317
318 static void
servo_disconnect(struct usb_interface * interface)319 servo_disconnect(struct usb_interface *interface)
320 {
321 struct phidget_servo *dev;
322 int servo_count, i;
323
324 dev = usb_get_intfdata(interface);
325 usb_set_intfdata(interface, NULL);
326
327 if (!dev)
328 return;
329
330 servo_count = dev->type & SERVO_COUNT_QUAD ? 4 : 1;
331
332 for (i=0; i<servo_count; i++)
333 device_remove_file(dev->dev, &dev_attrs[i]);
334
335 device_unregister(dev->dev);
336 usb_put_dev(dev->udev);
337
338 dev_info(&interface->dev, "USB %d-Motor PhidgetServo v%d.0 detached\n",
339 servo_count, dev->type & SERVO_VERSION_30 ? 3 : 2);
340
341 clear_bit(dev->dev_no, &device_no);
342 kfree(dev);
343 }
344
345 static struct usb_driver servo_driver = {
346 .name = "phidgetservo",
347 .probe = servo_probe,
348 .disconnect = servo_disconnect,
349 .id_table = id_table
350 };
351
352 static int __init
phidget_servo_init(void)353 phidget_servo_init(void)
354 {
355 int retval;
356
357 retval = usb_register(&servo_driver);
358 if (retval)
359 err("usb_register failed. Error number %d", retval);
360
361 return retval;
362 }
363
364 static void __exit
phidget_servo_exit(void)365 phidget_servo_exit(void)
366 {
367 usb_deregister(&servo_driver);
368 }
369
370 module_init(phidget_servo_init);
371 module_exit(phidget_servo_exit);
372
373 MODULE_AUTHOR(DRIVER_AUTHOR);
374 MODULE_DESCRIPTION(DRIVER_DESC);
375 MODULE_LICENSE("GPL");
376