drivers/usb/misc/phidgetmotorcontrol.c

   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
  28 static unsigned long device_no;
  29
  30 struct 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
  50 static struct usb_device_id id_table[] = {
  51         { USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_MOTORCONTROL) },
  52         {}
  53 };
  54 MODULE_DEVICE_TABLE(usb, id_table);
  55
  56 static 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
  93 static void motorcontrol_irq(struct urb *urb, struct pt_regs *regs)
  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
 153 resubmit:
 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
 162 static 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)           \
 190 static 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                                                                         \
 211 static 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 static DEVICE_ATTR(speed##value, S_IWUGO | S_IRUGO,                     \
 220                 show_speed##value, set_speed##value);
 221 show_set_speed(0);
 222 show_set_speed(1);
 223
 224 #define show_set_acceleration(value)            \
 225 static ssize_t set_acceleration##value(struct device *dev,              \
 226                                         struct device_attribute *attr,  \
 227                                         const char *buf, size_t count)  \
 228 {                                                                       \
 229         struct motorcontrol *mc = dev_get_drvdata(dev);                 \
 230         int acceleration;                                               \
 231         int retval;                                                     \
 232                                                                         \
 233         if (sscanf(buf, "%d", &acceleration) < 1)                       \
 234                 return -EINVAL;                                         \
 235                                                                         \
 236         if (acceleration < 0 || acceleration > 100)                     \
 237                 return -EINVAL;                                         \
 238                                                                         \
 239         mc->acceleration[value] = acceleration;                         \
 240                                                                         \
 241         retval = set_motor(mc, value);                                  \
 242                                                                         \
 243         return retval ? retval : count;                                 \
 244 }                                                                       \
 245                                                                         \
 246 static ssize_t show_acceleration##value(struct device *dev,             \
 247                                         struct device_attribute *attr,  \
 248                                                         char *buf)      \
 249 {                                                                       \
 250         struct motorcontrol *mc = dev_get_drvdata(dev);                 \
 251                                                                         \
 252         return sprintf(buf, "%d\n", mc->acceleration[value]);           \
 253 }                                                                       \
 254 static DEVICE_ATTR(acceleration##value, S_IWUGO | S_IRUGO,              \
 255                 show_acceleration##value, set_acceleration##value);
 256 show_set_acceleration(0);
 257 show_set_acceleration(1);
 258
 259 #define show_current(value)     \
 260 static ssize_t show_current##value(struct device *dev,                  \
 261                                         struct device_attribute *attr,  \
 262                                         char *buf)                      \
 263 {                                                                       \
 264         struct motorcontrol *mc = dev_get_drvdata(dev);                 \
 265                                                                         \
 266         return sprintf(buf, "%dmA\n", (int)mc->_current[value]);        \
 267 }                                                                       \
 268 static DEVICE_ATTR(current##value, S_IRUGO, show_current##value, NULL);
 269
 270 show_current(0);
 271 show_current(1);
 272
 273 #define show_input(value)       \
 274 static ssize_t show_input##value(struct device *dev,                    \
 275                                         struct device_attribute *attr,  \
 276                                         char *buf)                      \
 277 {                                                                       \
 278         struct motorcontrol *mc = dev_get_drvdata(dev);                 \
 279                                                                         \
 280         return sprintf(buf, "%d\n", (int)mc->inputs[value]);            \
 281 }                                                                       \
 282 static DEVICE_ATTR(input##value, S_IRUGO, show_input##value, NULL);
 283
 284 show_input(0);
 285 show_input(1);
 286 show_input(2);
 287 show_input(3);
 288
 289 static int motorcontrol_probe(struct usb_interface *intf, const struct usb_device_id *id)
 290 {
 291         struct usb_device *dev = interface_to_usbdev(intf);
 292         struct usb_host_interface *interface;
 293         struct usb_endpoint_descriptor *endpoint;
 294         struct motorcontrol *mc;
 295         int pipe, maxp, rc = -ENOMEM;
 296         int bit, value;
 297
 298         interface = intf->cur_altsetting;
 299         if (interface->desc.bNumEndpoints != 1)
 300                 return -ENODEV;
 301
 302         endpoint = &interface->endpoint[0].desc;
 303         if (!(endpoint->bEndpointAddress & 0x80))
 304                 return -ENODEV;
 305
 306         /*
 307          * bmAttributes
 308          */
 309         pipe = usb_rcvintpipe(dev, endpoint->bEndpointAddress);
 310         maxp = usb_maxpacket(dev, pipe, usb_pipeout(pipe));
 311
 312         mc = kzalloc(sizeof(*mc), GFP_KERNEL);
 313         if (!mc)
 314                 goto out;
 315
 316         mc->dev_no = -1;
 317         mc->data = usb_buffer_alloc(dev, URB_INT_SIZE, SLAB_ATOMIC, &mc->data_dma);
 318         if (!mc->data)
 319                 goto out;
 320
 321         mc->irq = usb_alloc_urb(0, GFP_KERNEL);
 322         if (!mc->irq)
 323                 goto out;
 324
 325         mc->udev = usb_get_dev(dev);
 326         mc->intf = intf;
 327         mc->acceleration[0] = mc->acceleration[1] = 10;
 328         INIT_WORK(&mc->do_notify, do_notify, mc);
 329         usb_fill_int_urb(mc->irq, mc->udev, pipe, mc->data,
 330                         maxp > URB_INT_SIZE ? URB_INT_SIZE : maxp,
 331                         motorcontrol_irq, mc, endpoint->bInterval);
 332         mc->irq->transfer_dma = mc->data_dma;
 333         mc->irq->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 334
 335         usb_set_intfdata(intf, mc);
 336
 337         do {
 338                 bit = find_first_zero_bit(&device_no, sizeof(device_no));
 339                 value = test_and_set_bit(bit, &device_no);
 340         } while(value);
 341         mc->dev_no = bit;
 342
 343         mc->dev = device_create(phidget_class, &mc->udev->dev, 0,
 344                                 "motorcontrol%d", mc->dev_no);
 345         if (IS_ERR(mc->dev)) {
 346                 rc = PTR_ERR(mc->dev);
 347                 mc->dev = NULL;
 348                 goto out;
 349         }
 350
 351         dev_set_drvdata(mc->dev, mc);
 352
 353         if (usb_submit_urb(mc->irq, GFP_KERNEL)) {
 354                 rc = -EIO;
 355                 goto out;
 356         }
 357
 358         device_create_file(mc->dev, &dev_attr_input0);
 359         device_create_file(mc->dev, &dev_attr_input1);
 360         device_create_file(mc->dev, &dev_attr_input2);
 361         device_create_file(mc->dev, &dev_attr_input3);
 362
 363         device_create_file(mc->dev, &dev_attr_speed0);
 364         device_create_file(mc->dev, &dev_attr_speed1);
 365
 366         device_create_file(mc->dev, &dev_attr_acceleration0);
 367         device_create_file(mc->dev, &dev_attr_acceleration1);
 368
 369         device_create_file(mc->dev, &dev_attr_current0);
 370         device_create_file(mc->dev, &dev_attr_current1);
 371
 372         dev_info(&intf->dev, "USB PhidgetMotorControl attached\n");
 373
 374         return 0;
 375
 376 out:
 377         if (mc) {
 378                 if (mc->irq)
 379                         usb_free_urb(mc->irq);
 380                 if (mc->data)
 381                         usb_buffer_free(dev, URB_INT_SIZE, mc->data, mc->data_dma);
 382                 if (mc->dev)
 383                         device_unregister(mc->dev);
 384                 if (mc->dev_no >= 0)
 385                         clear_bit(mc->dev_no, &device_no);
 386
 387                 kfree(mc);
 388         }
 389
 390         return rc;
 391 }
 392
 393 static void motorcontrol_disconnect(struct usb_interface *interface)
 394 {
 395         struct motorcontrol *mc;
 396
 397         mc = usb_get_intfdata(interface);
 398         usb_set_intfdata(interface, NULL);
 399         if (!mc)
 400                 return;
 401
 402         usb_kill_urb(mc->irq);
 403         usb_free_urb(mc->irq);
 404         usb_buffer_free(mc->udev, URB_INT_SIZE, mc->data, mc->data_dma);
 405
 406         cancel_delayed_work(&mc->do_notify);
 407
 408         device_remove_file(mc->dev, &dev_attr_input0);
 409         device_remove_file(mc->dev, &dev_attr_input1);
 410         device_remove_file(mc->dev, &dev_attr_input2);
 411         device_remove_file(mc->dev, &dev_attr_input3);
 412
 413         device_remove_file(mc->dev, &dev_attr_speed0);
 414         device_remove_file(mc->dev, &dev_attr_speed1);
 415
 416         device_remove_file(mc->dev, &dev_attr_acceleration0);
 417         device_remove_file(mc->dev, &dev_attr_acceleration1);
 418
 419         device_remove_file(mc->dev, &dev_attr_current0);
 420         device_remove_file(mc->dev, &dev_attr_current1);
 421
 422         device_unregister(mc->dev);
 423
 424         usb_put_dev(mc->udev);
 425         clear_bit(mc->dev_no, &device_no);
 426         kfree(mc);
 427
 428         dev_info(&interface->dev, "USB PhidgetMotorControl detached\n");
 429 }
 430
 431 static struct usb_driver motorcontrol_driver = {
 432         .name = "phidgetmotorcontrol",
 433         .probe = motorcontrol_probe,
 434         .disconnect = motorcontrol_disconnect,
 435         .id_table = id_table
 436 };
 437
 438 static int __init motorcontrol_init(void)
 439 {
 440         int retval = 0;
 441
 442         retval = usb_register(&motorcontrol_driver);
 443         if (retval)
 444                 err("usb_register failed. Error number %d", retval);
 445
 446         return retval;
 447 }
 448
 449 static void __exit motorcontrol_exit(void)
 450 {
 451         usb_deregister(&motorcontrol_driver);
 452 }
 453
 454 module_init(motorcontrol_init);
 455 module_exit(motorcontrol_exit);
 456
 457 MODULE_AUTHOR(DRIVER_AUTHOR);
 458 MODULE_DESCRIPTION(DRIVER_DESC);
 459 MODULE_LICENSE("GPL");