| /* |
| * USB Phidget MotorControl driver |
| * |
| * Copyright (C) 2006 Sean Young <sean@mess.org> |
| * |
| * 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 |
| * the Free Software Foundation; either version 2 of the License, or |
| * (at your option) any later version. |
| */ |
| |
| #include <linux/kernel.h> |
| #include <linux/errno.h> |
| #include <linux/init.h> |
| #include <linux/module.h> |
| #include <linux/usb.h> |
| |
| #include "phidget.h" |
| |
| #define DRIVER_AUTHOR "Sean Young <sean@mess.org>" |
| #define DRIVER_DESC "USB PhidgetMotorControl Driver" |
| |
| #define USB_VENDOR_ID_GLAB 0x06c2 |
| #define USB_DEVICE_ID_MOTORCONTROL 0x0058 |
| |
| #define URB_INT_SIZE 8 |
| |
| static unsigned long device_no; |
| |
| struct motorcontrol { |
| struct usb_device *udev; |
| struct usb_interface *intf; |
| struct device *dev; |
| int dev_no; |
| u8 inputs[4]; |
| s8 desired_speed[2]; |
| s8 speed[2]; |
| s16 _current[2]; |
| s8 acceleration[2]; |
| struct urb *irq; |
| unsigned char *data; |
| dma_addr_t data_dma; |
| |
| struct work_struct do_notify; |
| unsigned long input_events; |
| unsigned long speed_events; |
| unsigned long exceed_events; |
| }; |
| |
| static struct usb_device_id id_table[] = { |
| { USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_MOTORCONTROL) }, |
| {} |
| }; |
| MODULE_DEVICE_TABLE(usb, id_table); |
| |
| static int set_motor(struct motorcontrol *mc, int motor) |
| { |
| u8 *buffer; |
| int speed, speed2, acceleration; |
| int retval; |
| |
| buffer = kzalloc(8, GFP_KERNEL); |
| if (!buffer) { |
| dev_err(&mc->intf->dev, "%s - out of memory\n", __FUNCTION__); |
| return -ENOMEM; |
| } |
| |
| acceleration = mc->acceleration[motor] * 10; |
| /* -127 <= speed <= 127 */ |
| speed = (mc->desired_speed[motor] * 127) / 100; |
| /* -0x7300 <= speed2 <= 0x7300 */ |
| speed2 = (mc->desired_speed[motor] * 230 * 128) / 100; |
| |
| buffer[0] = motor; |
| buffer[1] = speed; |
| buffer[2] = acceleration >> 8; |
| buffer[3] = acceleration; |
| buffer[4] = speed2 >> 8; |
| buffer[5] = speed2; |
| |
| retval = usb_control_msg(mc->udev, |
| usb_sndctrlpipe(mc->udev, 0), |
| 0x09, 0x21, 0x0200, 0x0000, buffer, 8, 2000); |
| |
| if (retval != 8) |
| dev_err(&mc->intf->dev, "usb_control_msg returned %d\n", |
| retval); |
| kfree(buffer); |
| |
| return retval < 0 ? retval : 0; |
| } |
| |
| static void motorcontrol_irq(struct urb *urb, struct pt_regs *regs) |
| { |
| struct motorcontrol *mc = urb->context; |
| unsigned char *buffer = mc->data; |
| int i, level; |
| int status; |
| |
| switch (urb->status) { |
| case 0: /* success */ |
| break; |
| case -ECONNRESET: /* unlink */ |
| case -ENOENT: |
| case -ESHUTDOWN: |
| return; |
| /* -EPIPE: should clear the halt */ |
| default: /* error */ |
| goto resubmit; |
| } |
| |
| /* digital inputs */ |
| for (i=0; i<4; i++) { |
| level = (buffer[0] >> i) & 1; |
| if (mc->inputs[i] != level) { |
| mc->inputs[i] = level; |
| set_bit(i, &mc->input_events); |
| } |
| } |
| |
| /* motor speed */ |
| if (buffer[2] == 0) { |
| for (i=0; i<2; i++) { |
| level = ((s8)buffer[4+i]) * 100 / 127; |
| if (mc->speed[i] != level) { |
| mc->speed[i] = level; |
| set_bit(i, &mc->speed_events); |
| } |
| } |
| } else { |
| int index = buffer[3] & 1; |
| |
| level = ((s8)buffer[4] << 8) | buffer[5]; |
| level = level * 100 / 29440; |
| if (mc->speed[index] != level) { |
| mc->speed[index] = level; |
| set_bit(index, &mc->speed_events); |
| } |
| |
| level = ((s8)buffer[6] << 8) | buffer[7]; |
| mc->_current[index] = level * 100 / 1572; |
| } |
| |
| if (buffer[1] & 1) |
| set_bit(0, &mc->exceed_events); |
| |
| if (buffer[1] & 2) |
| set_bit(1, &mc->exceed_events); |
| |
| if (mc->input_events || mc->exceed_events || mc->speed_events) |
| schedule_work(&mc->do_notify); |
| |
| resubmit: |
| status = usb_submit_urb(urb, SLAB_ATOMIC); |
| if (status) |
| dev_err(&mc->intf->dev, |
| "can't resubmit intr, %s-%s/motorcontrol0, status %d", |
| mc->udev->bus->bus_name, |
| mc->udev->devpath, status); |
| } |
| |
| static void do_notify(void *data) |
| { |
| struct motorcontrol *mc = data; |
| int i; |
| char sysfs_file[8]; |
| |
| for (i=0; i<4; i++) { |
| if (test_and_clear_bit(i, &mc->input_events)) { |
| sprintf(sysfs_file, "input%d", i); |
| sysfs_notify(&mc->dev->kobj, NULL, sysfs_file); |
| } |
| } |
| |
| for (i=0; i<2; i++) { |
| if (test_and_clear_bit(i, &mc->speed_events)) { |
| sprintf(sysfs_file, "speed%d", i); |
| sysfs_notify(&mc->dev->kobj, NULL, sysfs_file); |
| } |
| } |
| |
| for (i=0; i<2; i++) { |
| if (test_and_clear_bit(i, &mc->exceed_events)) |
| dev_warn(&mc->intf->dev, |
| "motor #%d exceeds 1.5 Amp current limit\n", i); |
| } |
| } |
| |
| #define show_set_speed(value) \ |
| static ssize_t set_speed##value(struct device *dev, \ |
| struct device_attribute *attr, \ |
| const char *buf, size_t count) \ |
| { \ |
| struct motorcontrol *mc = dev_get_drvdata(dev); \ |
| int speed; \ |
| int retval; \ |
| \ |
| if (sscanf(buf, "%d", &speed) < 1) \ |
| return -EINVAL; \ |
| \ |
| if (speed < -100 || speed > 100) \ |
| return -EINVAL; \ |
| \ |
| mc->desired_speed[value] = speed; \ |
| \ |
| retval = set_motor(mc, value); \ |
| \ |
| return retval ? retval : count; \ |
| } \ |
| \ |
| static ssize_t show_speed##value(struct device *dev, \ |
| struct device_attribute *attr, \ |
| char *buf) \ |
| { \ |
| struct motorcontrol *mc = dev_get_drvdata(dev); \ |
| \ |
| return sprintf(buf, "%d\n", mc->speed[value]); \ |
| } |
| |
| #define speed_attr(value) \ |
| __ATTR(speed##value, S_IWUGO | S_IRUGO, \ |
| show_speed##value, set_speed##value) |
| |
| show_set_speed(0); |
| show_set_speed(1); |
| |
| #define show_set_acceleration(value) \ |
| static ssize_t set_acceleration##value(struct device *dev, \ |
| struct device_attribute *attr, \ |
| const char *buf, size_t count) \ |
| { \ |
| struct motorcontrol *mc = dev_get_drvdata(dev); \ |
| int acceleration; \ |
| int retval; \ |
| \ |
| if (sscanf(buf, "%d", &acceleration) < 1) \ |
| return -EINVAL; \ |
| \ |
| if (acceleration < 0 || acceleration > 100) \ |
| return -EINVAL; \ |
| \ |
| mc->acceleration[value] = acceleration; \ |
| \ |
| retval = set_motor(mc, value); \ |
| \ |
| return retval ? retval : count; \ |
| } \ |
| \ |
| static ssize_t show_acceleration##value(struct device *dev, \ |
| struct device_attribute *attr, \ |
| char *buf) \ |
| { \ |
| struct motorcontrol *mc = dev_get_drvdata(dev); \ |
| \ |
| return sprintf(buf, "%d\n", mc->acceleration[value]); \ |
| } |
| |
| #define acceleration_attr(value) \ |
| __ATTR(acceleration##value, S_IWUGO | S_IRUGO, \ |
| show_acceleration##value, set_acceleration##value) |
| |
| show_set_acceleration(0); |
| show_set_acceleration(1); |
| |
| #define show_current(value) \ |
| static ssize_t show_current##value(struct device *dev, \ |
| struct device_attribute *attr, \ |
| char *buf) \ |
| { \ |
| struct motorcontrol *mc = dev_get_drvdata(dev); \ |
| \ |
| return sprintf(buf, "%dmA\n", (int)mc->_current[value]); \ |
| } |
| |
| #define current_attr(value) \ |
| __ATTR(current##value, S_IRUGO, show_current##value, NULL) |
| |
| show_current(0); |
| show_current(1); |
| |
| #define show_input(value) \ |
| static ssize_t show_input##value(struct device *dev, \ |
| struct device_attribute *attr, \ |
| char *buf) \ |
| { \ |
| struct motorcontrol *mc = dev_get_drvdata(dev); \ |
| \ |
| return sprintf(buf, "%d\n", (int)mc->inputs[value]); \ |
| } |
| |
| #define input_attr(value) \ |
| __ATTR(input##value, S_IRUGO, show_input##value, NULL) |
| |
| show_input(0); |
| show_input(1); |
| show_input(2); |
| show_input(3); |
| |
| static struct device_attribute dev_attrs[] = { |
| input_attr(0), |
| input_attr(1), |
| input_attr(2), |
| input_attr(3), |
| speed_attr(0), |
| speed_attr(1), |
| acceleration_attr(0), |
| acceleration_attr(1), |
| current_attr(0), |
| current_attr(1) |
| }; |
| |
| static int motorcontrol_probe(struct usb_interface *intf, const struct usb_device_id *id) |
| { |
| struct usb_device *dev = interface_to_usbdev(intf); |
| struct usb_host_interface *interface; |
| struct usb_endpoint_descriptor *endpoint; |
| struct motorcontrol *mc; |
| int pipe, maxp, rc = -ENOMEM; |
| int bit, value, i; |
| |
| interface = intf->cur_altsetting; |
| if (interface->desc.bNumEndpoints != 1) |
| return -ENODEV; |
| |
| endpoint = &interface->endpoint[0].desc; |
| if (!(endpoint->bEndpointAddress & 0x80)) |
| return -ENODEV; |
| |
| /* |
| * bmAttributes |
| */ |
| pipe = usb_rcvintpipe(dev, endpoint->bEndpointAddress); |
| maxp = usb_maxpacket(dev, pipe, usb_pipeout(pipe)); |
| |
| mc = kzalloc(sizeof(*mc), GFP_KERNEL); |
| if (!mc) |
| goto out; |
| |
| mc->dev_no = -1; |
| mc->data = usb_buffer_alloc(dev, URB_INT_SIZE, SLAB_ATOMIC, &mc->data_dma); |
| if (!mc->data) |
| goto out; |
| |
| mc->irq = usb_alloc_urb(0, GFP_KERNEL); |
| if (!mc->irq) |
| goto out; |
| |
| mc->udev = usb_get_dev(dev); |
| mc->intf = intf; |
| mc->acceleration[0] = mc->acceleration[1] = 10; |
| INIT_WORK(&mc->do_notify, do_notify, mc); |
| usb_fill_int_urb(mc->irq, mc->udev, pipe, mc->data, |
| maxp > URB_INT_SIZE ? URB_INT_SIZE : maxp, |
| motorcontrol_irq, mc, endpoint->bInterval); |
| mc->irq->transfer_dma = mc->data_dma; |
| mc->irq->transfer_flags |= URB_NO_TRANSFER_DMA_MAP; |
| |
| usb_set_intfdata(intf, mc); |
| |
| do { |
| bit = find_first_zero_bit(&device_no, sizeof(device_no)); |
| value = test_and_set_bit(bit, &device_no); |
| } while(value); |
| mc->dev_no = bit; |
| |
| mc->dev = device_create(phidget_class, &mc->udev->dev, 0, |
| "motorcontrol%d", mc->dev_no); |
| if (IS_ERR(mc->dev)) { |
| rc = PTR_ERR(mc->dev); |
| mc->dev = NULL; |
| goto out; |
| } |
| |
| dev_set_drvdata(mc->dev, mc); |
| |
| if (usb_submit_urb(mc->irq, GFP_KERNEL)) { |
| rc = -EIO; |
| goto out; |
| } |
| |
| for (i=0; i<ARRAY_SIZE(dev_attrs); i++) { |
| rc = device_create_file(mc->dev, &dev_attrs[i]); |
| if (rc) |
| goto out2; |
| } |
| |
| dev_info(&intf->dev, "USB PhidgetMotorControl attached\n"); |
| |
| return 0; |
| out2: |
| while (i-- > 0) |
| device_remove_file(mc->dev, &dev_attrs[i]); |
| out: |
| if (mc) { |
| if (mc->irq) |
| usb_free_urb(mc->irq); |
| if (mc->data) |
| usb_buffer_free(dev, URB_INT_SIZE, mc->data, mc->data_dma); |
| if (mc->dev) |
| device_unregister(mc->dev); |
| if (mc->dev_no >= 0) |
| clear_bit(mc->dev_no, &device_no); |
| |
| kfree(mc); |
| } |
| |
| return rc; |
| } |
| |
| static void motorcontrol_disconnect(struct usb_interface *interface) |
| { |
| struct motorcontrol *mc; |
| int i; |
| |
| mc = usb_get_intfdata(interface); |
| usb_set_intfdata(interface, NULL); |
| if (!mc) |
| return; |
| |
| usb_kill_urb(mc->irq); |
| usb_free_urb(mc->irq); |
| usb_buffer_free(mc->udev, URB_INT_SIZE, mc->data, mc->data_dma); |
| |
| cancel_delayed_work(&mc->do_notify); |
| |
| for (i=0; i<ARRAY_SIZE(dev_attrs); i++) |
| device_remove_file(mc->dev, &dev_attrs[i]); |
| |
| device_unregister(mc->dev); |
| |
| usb_put_dev(mc->udev); |
| clear_bit(mc->dev_no, &device_no); |
| kfree(mc); |
| |
| dev_info(&interface->dev, "USB PhidgetMotorControl detached\n"); |
| } |
| |
| static struct usb_driver motorcontrol_driver = { |
| .name = "phidgetmotorcontrol", |
| .probe = motorcontrol_probe, |
| .disconnect = motorcontrol_disconnect, |
| .id_table = id_table |
| }; |
| |
| static int __init motorcontrol_init(void) |
| { |
| int retval = 0; |
| |
| retval = usb_register(&motorcontrol_driver); |
| if (retval) |
| err("usb_register failed. Error number %d", retval); |
| |
| return retval; |
| } |
| |
| static void __exit motorcontrol_exit(void) |
| { |
| usb_deregister(&motorcontrol_driver); |
| } |
| |
| module_init(motorcontrol_init); |
| module_exit(motorcontrol_exit); |
| |
| MODULE_AUTHOR(DRIVER_AUTHOR); |
| MODULE_DESCRIPTION(DRIVER_DESC); |
| MODULE_LICENSE("GPL"); |