X-Git-Url: http://git.onelab.eu/?a=blobdiff_plain;f=drivers%2Fusb%2Fmisc%2Fphidgetservo.c;h=0d9de2f739301b6170635f4461f5dded1d9292dc;hb=refs%2Fheads%2Fvserver;hp=9018774ae413b80973f5b02dba60595c78e26b17;hpb=e812ccbe0c915857ebea6a632bfadc631f7504a9;p=linux-2.6.git diff --git a/drivers/usb/misc/phidgetservo.c b/drivers/usb/misc/phidgetservo.c index 9018774ae..0d9de2f73 100644 --- a/drivers/usb/misc/phidgetservo.c +++ b/drivers/usb/misc/phidgetservo.c @@ -1,7 +1,7 @@ /* * USB PhidgetServo driver 1.0 * - * Copyright (C) 2004 Sean Young + * Copyright (C) 2004, 2006 Sean Young * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -12,17 +12,11 @@ * controllers available at: http://www.phidgets.com/ * * Note that the driver takes input as: degrees.minutes - * -23 < degrees < 203 - * 0 < minutes < 59 * * CAUTION: Generally you should use 0 < degrees < 180 as anything else * is probably beyond the range of your servo and may damage it. */ -#include -#ifdef CONFIG_USB_DEBUG -#define DEBUG 1 -#endif #include #include #include @@ -30,48 +24,73 @@ #include #include +#include "phidget.h" + #define DRIVER_AUTHOR "Sean Young " #define DRIVER_DESC "USB PhidgetServo Driver" -#define VENDOR_ID_GLAB 0x06c2 -#define DEVICE_ID_4MOTOR_SERVO_30 0x0038 -#define DEVICE_ID_1MOTOR_SERVO_30 0x0039 +#define VENDOR_ID_GLAB 0x06c2 +#define DEVICE_ID_GLAB_PHIDGETSERVO_QUAD 0x0038 +#define DEVICE_ID_GLAB_PHIDGETSERVO_UNI 0x0039 + +#define VENDOR_ID_WISEGROUP 0x0925 +#define VENDOR_ID_WISEGROUP_PHIDGETSERVO_QUAD 0x8101 +#define VENDOR_ID_WISEGROUP_PHIDGETSERVO_UNI 0x8104 -#define VENDOR_ID_WISEGROUP 0x0925 -#define DEVICE_ID_1MOTOR_SERVO_20 0x8101 -#define DEVICE_ID_4MOTOR_SERVO_20 0x8104 +#define SERVO_VERSION_30 0x01 +#define SERVO_COUNT_QUAD 0x02 static struct usb_device_id id_table[] = { - {USB_DEVICE(VENDOR_ID_GLAB, DEVICE_ID_4MOTOR_SERVO_30)}, - {USB_DEVICE(VENDOR_ID_GLAB, DEVICE_ID_1MOTOR_SERVO_30)}, - {USB_DEVICE(VENDOR_ID_WISEGROUP, DEVICE_ID_4MOTOR_SERVO_20)}, - {USB_DEVICE(VENDOR_ID_WISEGROUP, DEVICE_ID_1MOTOR_SERVO_20)}, + { + USB_DEVICE(VENDOR_ID_GLAB, DEVICE_ID_GLAB_PHIDGETSERVO_QUAD), + .driver_info = SERVO_VERSION_30 | SERVO_COUNT_QUAD + }, + { + USB_DEVICE(VENDOR_ID_GLAB, DEVICE_ID_GLAB_PHIDGETSERVO_UNI), + .driver_info = SERVO_VERSION_30 + }, + { + USB_DEVICE(VENDOR_ID_WISEGROUP, + VENDOR_ID_WISEGROUP_PHIDGETSERVO_QUAD), + .driver_info = SERVO_COUNT_QUAD + }, + { + USB_DEVICE(VENDOR_ID_WISEGROUP, + VENDOR_ID_WISEGROUP_PHIDGETSERVO_UNI), + .driver_info = 0 + }, {} }; MODULE_DEVICE_TABLE(usb, id_table); +static int unsigned long device_no; + struct phidget_servo { struct usb_device *udev; - int version; - int quad_servo; + struct device *dev; + int dev_no; + ulong type; int pulse[4]; int degrees[4]; int minutes[4]; }; -static void +static int change_position_v30(struct phidget_servo *servo, int servo_no, int degrees, int minutes) { int retval; unsigned char *buffer; + if (degrees < -23 || degrees > 362) + return -EINVAL; + buffer = kmalloc(6, GFP_KERNEL); if (!buffer) { dev_err(&servo->udev->dev, "%s - out of memory\n", __FUNCTION__); - return; + return -ENOMEM; } /* @@ -123,24 +142,28 @@ change_position_v30(struct phidget_servo *servo, int servo_no, int degrees, retval = usb_control_msg(servo->udev, usb_sndctrlpipe(servo->udev, 0), - 0x09, 0x21, 0x0200, 0x0000, buffer, 6, 2 * HZ); - if (retval != 6) - dev_err(&servo->udev->dev, "retval = %d\n", retval); + 0x09, 0x21, 0x0200, 0x0000, buffer, 6, 2000); + kfree(buffer); + + return retval; } -static void +static int change_position_v20(struct phidget_servo *servo, int servo_no, int degrees, int minutes) { int retval; unsigned char *buffer; + if (degrees < -23 || degrees > 278) + return -EINVAL; + buffer = kmalloc(2, GFP_KERNEL); if (!buffer) { dev_err(&servo->udev->dev, "%s - out of memory\n", __FUNCTION__); - return; + return -ENOMEM; } /* @@ -170,131 +193,157 @@ change_position_v20(struct phidget_servo *servo, int servo_no, int degrees, retval = usb_control_msg(servo->udev, usb_sndctrlpipe(servo->udev, 0), - 0x09, 0x21, 0x0200, 0x0000, buffer, 2, 2 * HZ); - if (retval != 2) - dev_err(&servo->udev->dev, "retval = %d\n", retval); + 0x09, 0x21, 0x0200, 0x0000, buffer, 2, 2000); + kfree(buffer); + + return retval; } #define show_set(value) \ -static ssize_t set_servo##value (struct device *dev, \ +static ssize_t set_servo##value (struct device *dev, \ + struct device_attribute *attr, \ const char *buf, size_t count) \ { \ - int degrees, minutes; \ - struct usb_interface *intf = to_usb_interface (dev); \ - struct phidget_servo *servo = usb_get_intfdata (intf); \ + int degrees, minutes, retval; \ + struct phidget_servo *servo = dev_get_drvdata(dev); \ \ minutes = 0; \ /* must at least convert degrees */ \ - if (sscanf (buf, "%d.%d", °rees, &minutes) < 1) { \ + if (sscanf(buf, "%d.%d", °rees, &minutes) < 1) { \ return -EINVAL; \ } \ \ - if (degrees < -23 || degrees > (180 + 23) || \ - minutes < 0 || minutes > 59) { \ + if (minutes < 0 || minutes > 59) \ return -EINVAL; \ - } \ \ - if (servo->version >= 3) \ - change_position_v30 (servo, value, degrees, minutes); \ + if (servo->type & SERVO_VERSION_30) \ + retval = change_position_v30(servo, value, degrees, \ + minutes); \ else \ - change_position_v20 (servo, value, degrees, minutes); \ + retval = change_position_v20(servo, value, degrees, \ + minutes); \ \ - return count; \ + return retval < 0 ? retval : count; \ } \ \ -static ssize_t show_servo##value (struct device *dev, char *buf) \ +static ssize_t show_servo##value (struct device *dev, \ + struct device_attribute *attr, \ + char *buf) \ { \ - struct usb_interface *intf = to_usb_interface (dev); \ - struct phidget_servo *servo = usb_get_intfdata (intf); \ + struct phidget_servo *servo = dev_get_drvdata(dev); \ \ - return sprintf (buf, "%d.%02d\n", servo->degrees[value], \ + return sprintf(buf, "%d.%02d\n", servo->degrees[value], \ servo->minutes[value]); \ -} \ -static DEVICE_ATTR(servo##value, S_IWUGO | S_IRUGO, \ - show_servo##value, set_servo##value); +} +#define servo_attr(value) \ + __ATTR(servo##value, S_IWUGO | S_IRUGO, \ + show_servo##value, set_servo##value) show_set(0); show_set(1); show_set(2); show_set(3); +static struct device_attribute dev_attrs[] = { + servo_attr(0), servo_attr(1), servo_attr(2), servo_attr(3) +}; + static int servo_probe(struct usb_interface *interface, const struct usb_device_id *id) { struct usb_device *udev = interface_to_usbdev(interface); - struct phidget_servo *dev = NULL; + struct phidget_servo *dev; + int bit, value, rc; + int servo_count, i; - dev = kmalloc(sizeof (struct phidget_servo), GFP_KERNEL); + dev = kzalloc(sizeof (struct phidget_servo), GFP_KERNEL); if (dev == NULL) { dev_err(&interface->dev, "%s - out of memory\n", __FUNCTION__); - return -ENOMEM; + rc = -ENOMEM; + goto out; } - memset(dev, 0x00, sizeof (*dev)); dev->udev = usb_get_dev(udev); - switch (udev->descriptor.idVendor) { - case VENDOR_ID_WISEGROUP: - dev->version = 2; - break; - case VENDOR_ID_GLAB: - dev->version = 3; - break; - } - switch (udev->descriptor.idProduct) { - case DEVICE_ID_4MOTOR_SERVO_20: - case DEVICE_ID_4MOTOR_SERVO_30: - dev->quad_servo = 1; - break; - case DEVICE_ID_1MOTOR_SERVO_20: - case DEVICE_ID_1MOTOR_SERVO_30: - dev->quad_servo = 0; - break; + dev->type = id->driver_info; + dev->dev_no = -1; + usb_set_intfdata(interface, dev); + + do { + bit = find_first_zero_bit(&device_no, sizeof(device_no)); + value = test_and_set_bit(bit, &device_no); + } while (value); + dev->dev_no = bit; + + dev->dev = device_create(phidget_class, &dev->udev->dev, 0, + "servo%d", dev->dev_no); + if (IS_ERR(dev->dev)) { + rc = PTR_ERR(dev->dev); + dev->dev = NULL; + goto out; } + dev_set_drvdata(dev->dev, dev); - usb_set_intfdata(interface, dev); + servo_count = dev->type & SERVO_COUNT_QUAD ? 4 : 1; - device_create_file(&interface->dev, &dev_attr_servo0); - if (dev->quad_servo) { - device_create_file(&interface->dev, &dev_attr_servo1); - device_create_file(&interface->dev, &dev_attr_servo2); - device_create_file(&interface->dev, &dev_attr_servo3); + for (i=0; idev, &dev_attrs[i]); + if (rc) + goto out2; } dev_info(&interface->dev, "USB %d-Motor PhidgetServo v%d.0 attached\n", - dev->quad_servo ? 4 : 1, dev->version); - if (dev->version == 2) + servo_count, dev->type & SERVO_VERSION_30 ? 3 : 2); + + if (!(dev->type & SERVO_VERSION_30)) dev_info(&interface->dev, "WARNING: v2.0 not tested! Please report if it works.\n"); return 0; +out2: + while (i-- > 0) + device_remove_file(dev->dev, &dev_attrs[i]); +out: + if (dev) { + if (dev->dev) + device_unregister(dev->dev); + if (dev->dev_no >= 0) + clear_bit(dev->dev_no, &device_no); + + kfree(dev); + } + + return rc; } static void servo_disconnect(struct usb_interface *interface) { struct phidget_servo *dev; + int servo_count, i; dev = usb_get_intfdata(interface); usb_set_intfdata(interface, NULL); - device_remove_file(&interface->dev, &dev_attr_servo0); - if (dev->quad_servo) { - device_remove_file(&interface->dev, &dev_attr_servo1); - device_remove_file(&interface->dev, &dev_attr_servo2); - device_remove_file(&interface->dev, &dev_attr_servo3); - } + if (!dev) + return; - usb_put_dev(dev->udev); + servo_count = dev->type & SERVO_COUNT_QUAD ? 4 : 1; - kfree(dev); + for (i=0; idev, &dev_attrs[i]); + + device_unregister(dev->dev); + usb_put_dev(dev->udev); dev_info(&interface->dev, "USB %d-Motor PhidgetServo v%d.0 detached\n", - dev->quad_servo ? 4 : 1, dev->version); + servo_count, dev->type & SERVO_VERSION_30 ? 3 : 2); + + clear_bit(dev->dev_no, &device_no); + kfree(dev); } static struct usb_driver servo_driver = { - .owner = THIS_MODULE, .name = "phidgetservo", .probe = servo_probe, .disconnect = servo_disconnect, @@ -304,7 +353,7 @@ static struct usb_driver servo_driver = { static int __init phidget_servo_init(void) { - int retval = 0; + int retval; retval = usb_register(&servo_driver); if (retval)