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 43static 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 65MODULE_DEVICE_TABLE(usb, id_table); 66 67static int unsigned long device_no; 68 69struct 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 79static int 80change_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 __FUNCTION__); 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 152static int 153change_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 __FUNCTION__); 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) \ 204static 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 \ 230static 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) 243show_set(0); 244show_set(1); 245show_set(2); 246show_set(3); 247 248static struct device_attribute dev_attrs[] = { 249 servo_attr(0), servo_attr(1), servo_attr(2), servo_attr(3) 250}; 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 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", __FUNCTION__); 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, 0, 279 "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 dev_set_drvdata(dev->dev, dev); 286 287 servo_count = dev->type & SERVO_COUNT_QUAD ? 4 : 1; 288 289 for (i=0; i<servo_count; i++) { 290 rc = device_create_file(dev->dev, &dev_attrs[i]); 291 if (rc) 292 goto out2; 293 } 294 295 dev_info(&interface->dev, "USB %d-Motor PhidgetServo v%d.0 attached\n", 296 servo_count, dev->type & SERVO_VERSION_30 ? 3 : 2); 297 298 if (!(dev->type & SERVO_VERSION_30)) 299 dev_info(&interface->dev, 300 "WARNING: v2.0 not tested! Please report if it works.\n"); 301 302 return 0; 303out2: 304 while (i-- > 0) 305 device_remove_file(dev->dev, &dev_attrs[i]); 306out: 307 if (dev) { 308 if (dev->dev) 309 device_unregister(dev->dev); 310 if (dev->dev_no >= 0) 311 clear_bit(dev->dev_no, &device_no); 312 313 kfree(dev); 314 } 315 316 return rc; 317} 318 319static void 320servo_disconnect(struct usb_interface *interface) 321{ 322 struct phidget_servo *dev; 323 int servo_count, i; 324 325 dev = usb_get_intfdata(interface); 326 usb_set_intfdata(interface, NULL); 327 328 if (!dev) 329 return; 330 331 servo_count = dev->type & SERVO_COUNT_QUAD ? 4 : 1; 332 333 for (i=0; i<servo_count; i++) 334 device_remove_file(dev->dev, &dev_attrs[i]); 335 336 device_unregister(dev->dev); 337 usb_put_dev(dev->udev); 338 339 dev_info(&interface->dev, "USB %d-Motor PhidgetServo v%d.0 detached\n", 340 servo_count, dev->type & SERVO_VERSION_30 ? 3 : 2); 341 342 clear_bit(dev->dev_no, &device_no); 343 kfree(dev); 344} 345 346static struct usb_driver servo_driver = { 347 .name = "phidgetservo", 348 .probe = servo_probe, 349 .disconnect = servo_disconnect, 350 .id_table = id_table 351}; 352 353static int __init 354phidget_servo_init(void) 355{ 356 int retval; 357 358 retval = usb_register(&servo_driver); 359 if (retval) 360 err("usb_register failed. Error number %d", retval); 361 362 return retval; 363} 364 365static void __exit 366phidget_servo_exit(void) 367{ 368 usb_deregister(&servo_driver); 369} 370 371module_init(phidget_servo_init); 372module_exit(phidget_servo_exit); 373 374MODULE_AUTHOR(DRIVER_AUTHOR); 375MODULE_DESCRIPTION(DRIVER_DESC); 376MODULE_LICENSE("GPL"); 377