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 28static unsigned long device_no; 29 30struct 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 delayed_work do_notify; 45 unsigned long input_events; 46 unsigned long speed_events; 47 unsigned long exceed_events; 48}; 49 50static struct usb_device_id id_table[] = { 51 { USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_MOTORCONTROL) }, 52 {} 53}; 54MODULE_DEVICE_TABLE(usb, id_table); 55 56static 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 93static void motorcontrol_irq(struct urb *urb) 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_delayed_work(&mc->do_notify, 0); 152 153resubmit: 154 status = usb_submit_urb(urb, GFP_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 162static void do_notify(struct work_struct *work) 163{ 164 struct motorcontrol *mc = 165 container_of(work, struct motorcontrol, do_notify.work); 166 int i; 167 char sysfs_file[8]; 168 169 for (i=0; i<4; i++) { 170 if (test_and_clear_bit(i, &mc->input_events)) { 171 sprintf(sysfs_file, "input%d", i); 172 sysfs_notify(&mc->dev->kobj, NULL, sysfs_file); 173 } 174 } 175 176 for (i=0; i<2; i++) { 177 if (test_and_clear_bit(i, &mc->speed_events)) { 178 sprintf(sysfs_file, "speed%d", i); 179 sysfs_notify(&mc->dev->kobj, NULL, sysfs_file); 180 } 181 } 182 183 for (i=0; i<2; i++) { 184 if (test_and_clear_bit(i, &mc->exceed_events)) 185 dev_warn(&mc->intf->dev, 186 "motor #%d exceeds 1.5 Amp current limit\n", i); 187 } 188} 189 190#define show_set_speed(value) \ 191static ssize_t set_speed##value(struct device *dev, \ 192 struct device_attribute *attr, \ 193 const char *buf, size_t count) \ 194{ \ 195 struct motorcontrol *mc = dev_get_drvdata(dev); \ 196 int speed; \ 197 int retval; \ 198 \ 199 if (sscanf(buf, "%d", &speed) < 1) \ 200 return -EINVAL; \ 201 \ 202 if (speed < -100 || speed > 100) \ 203 return -EINVAL; \ 204 \ 205 mc->desired_speed[value] = speed; \ 206 \ 207 retval = set_motor(mc, value); \ 208 \ 209 return retval ? retval : count; \ 210} \ 211 \ 212static ssize_t show_speed##value(struct device *dev, \ 213 struct device_attribute *attr, \ 214 char *buf) \ 215{ \ 216 struct motorcontrol *mc = dev_get_drvdata(dev); \ 217 \ 218 return sprintf(buf, "%d\n", mc->speed[value]); \ 219} 220 221#define speed_attr(value) \ 222 __ATTR(speed##value, S_IWUGO | S_IRUGO, \ 223 show_speed##value, set_speed##value) 224 225show_set_speed(0); 226show_set_speed(1); 227 228#define show_set_acceleration(value) \ 229static ssize_t set_acceleration##value(struct device *dev, \ 230 struct device_attribute *attr, \ 231 const char *buf, size_t count) \ 232{ \ 233 struct motorcontrol *mc = dev_get_drvdata(dev); \ 234 int acceleration; \ 235 int retval; \ 236 \ 237 if (sscanf(buf, "%d", &acceleration) < 1) \ 238 return -EINVAL; \ 239 \ 240 if (acceleration < 0 || acceleration > 100) \ 241 return -EINVAL; \ 242 \ 243 mc->acceleration[value] = acceleration; \ 244 \ 245 retval = set_motor(mc, value); \ 246 \ 247 return retval ? retval : count; \ 248} \ 249 \ 250static ssize_t show_acceleration##value(struct device *dev, \ 251 struct device_attribute *attr, \ 252 char *buf) \ 253{ \ 254 struct motorcontrol *mc = dev_get_drvdata(dev); \ 255 \ 256 return sprintf(buf, "%d\n", mc->acceleration[value]); \ 257} 258 259#define acceleration_attr(value) \ 260 __ATTR(acceleration##value, S_IWUGO | S_IRUGO, \ 261 show_acceleration##value, set_acceleration##value) 262 263show_set_acceleration(0); 264show_set_acceleration(1); 265 266#define show_current(value) \ 267static ssize_t show_current##value(struct device *dev, \ 268 struct device_attribute *attr, \ 269 char *buf) \ 270{ \ 271 struct motorcontrol *mc = dev_get_drvdata(dev); \ 272 \ 273 return sprintf(buf, "%dmA\n", (int)mc->_current[value]); \ 274} 275 276#define current_attr(value) \ 277 __ATTR(current##value, S_IRUGO, show_current##value, NULL) 278 279show_current(0); 280show_current(1); 281 282#define show_input(value) \ 283static ssize_t show_input##value(struct device *dev, \ 284 struct device_attribute *attr, \ 285 char *buf) \ 286{ \ 287 struct motorcontrol *mc = dev_get_drvdata(dev); \ 288 \ 289 return sprintf(buf, "%d\n", (int)mc->inputs[value]); \ 290} 291 292#define input_attr(value) \ 293 __ATTR(input##value, S_IRUGO, show_input##value, NULL) 294 295show_input(0); 296show_input(1); 297show_input(2); 298show_input(3); 299 300static struct device_attribute dev_attrs[] = { 301 input_attr(0), 302 input_attr(1), 303 input_attr(2), 304 input_attr(3), 305 speed_attr(0), 306 speed_attr(1), 307 acceleration_attr(0), 308 acceleration_attr(1), 309 current_attr(0), 310 current_attr(1) 311}; 312 313static int motorcontrol_probe(struct usb_interface *intf, const struct usb_device_id *id) 314{ 315 struct usb_device *dev = interface_to_usbdev(intf); 316 struct usb_host_interface *interface; 317 struct usb_endpoint_descriptor *endpoint; 318 struct motorcontrol *mc; 319 int pipe, maxp, rc = -ENOMEM; 320 int bit, value, i; 321 322 interface = intf->cur_altsetting; 323 if (interface->desc.bNumEndpoints != 1) 324 return -ENODEV; 325 326 endpoint = &interface->endpoint[0].desc; 327 if (!usb_endpoint_dir_in(endpoint)) 328 return -ENODEV; 329 330 /* 331 * bmAttributes 332 */ 333 pipe = usb_rcvintpipe(dev, endpoint->bEndpointAddress); 334 maxp = usb_maxpacket(dev, pipe, usb_pipeout(pipe)); 335 336 mc = kzalloc(sizeof(*mc), GFP_KERNEL); 337 if (!mc) 338 goto out; 339 340 mc->dev_no = -1; 341 mc->data = usb_buffer_alloc(dev, URB_INT_SIZE, GFP_ATOMIC, &mc->data_dma); 342 if (!mc->data) 343 goto out; 344 345 mc->irq = usb_alloc_urb(0, GFP_KERNEL); 346 if (!mc->irq) 347 goto out; 348 349 mc->udev = usb_get_dev(dev); 350 mc->intf = intf; 351 mc->acceleration[0] = mc->acceleration[1] = 10; 352 INIT_DELAYED_WORK(&mc->do_notify, do_notify); 353 usb_fill_int_urb(mc->irq, mc->udev, pipe, mc->data, 354 maxp > URB_INT_SIZE ? URB_INT_SIZE : maxp, 355 motorcontrol_irq, mc, endpoint->bInterval); 356 mc->irq->transfer_dma = mc->data_dma; 357 mc->irq->transfer_flags |= URB_NO_TRANSFER_DMA_MAP; 358 359 usb_set_intfdata(intf, mc); 360 361 do { 362 bit = find_first_zero_bit(&device_no, sizeof(device_no)); 363 value = test_and_set_bit(bit, &device_no); 364 } while(value); 365 mc->dev_no = bit; 366 367 mc->dev = device_create(phidget_class, &mc->udev->dev, 0, 368 "motorcontrol%d", mc->dev_no); 369 if (IS_ERR(mc->dev)) { 370 rc = PTR_ERR(mc->dev); 371 mc->dev = NULL; 372 goto out; 373 } 374 375 dev_set_drvdata(mc->dev, mc); 376 377 if (usb_submit_urb(mc->irq, GFP_KERNEL)) { 378 rc = -EIO; 379 goto out; 380 } 381 382 for (i=0; i<ARRAY_SIZE(dev_attrs); i++) { 383 rc = device_create_file(mc->dev, &dev_attrs[i]); 384 if (rc) 385 goto out2; 386 } 387 388 dev_info(&intf->dev, "USB PhidgetMotorControl attached\n"); 389 390 return 0; 391out2: 392 while (i-- > 0) 393 device_remove_file(mc->dev, &dev_attrs[i]); 394out: 395 if (mc) { 396 usb_free_urb(mc->irq); 397 if (mc->data) 398 usb_buffer_free(dev, URB_INT_SIZE, mc->data, mc->data_dma); 399 if (mc->dev) 400 device_unregister(mc->dev); 401 if (mc->dev_no >= 0) 402 clear_bit(mc->dev_no, &device_no); 403 404 kfree(mc); 405 } 406 407 return rc; 408} 409 410static void motorcontrol_disconnect(struct usb_interface *interface) 411{ 412 struct motorcontrol *mc; 413 int i; 414 415 mc = usb_get_intfdata(interface); 416 usb_set_intfdata(interface, NULL); 417 if (!mc) 418 return; 419 420 usb_kill_urb(mc->irq); 421 usb_free_urb(mc->irq); 422 usb_buffer_free(mc->udev, URB_INT_SIZE, mc->data, mc->data_dma); 423 424 cancel_delayed_work(&mc->do_notify); 425 426 for (i=0; i<ARRAY_SIZE(dev_attrs); i++) 427 device_remove_file(mc->dev, &dev_attrs[i]); 428 429 device_unregister(mc->dev); 430 431 usb_put_dev(mc->udev); 432 clear_bit(mc->dev_no, &device_no); 433 kfree(mc); 434 435 dev_info(&interface->dev, "USB PhidgetMotorControl detached\n"); 436} 437 438static struct usb_driver motorcontrol_driver = { 439 .name = "phidgetmotorcontrol", 440 .probe = motorcontrol_probe, 441 .disconnect = motorcontrol_disconnect, 442 .id_table = id_table 443}; 444 445static int __init motorcontrol_init(void) 446{ 447 int retval = 0; 448 449 retval = usb_register(&motorcontrol_driver); 450 if (retval) 451 err("usb_register failed. Error number %d", retval); 452 453 return retval; 454} 455 456static void __exit motorcontrol_exit(void) 457{ 458 usb_deregister(&motorcontrol_driver); 459} 460 461module_init(motorcontrol_init); 462module_exit(motorcontrol_exit); 463 464MODULE_AUTHOR(DRIVER_AUTHOR); 465MODULE_DESCRIPTION(DRIVER_DESC); 466MODULE_LICENSE("GPL"); 467