USB: Add driver for PhidgetMotorControl (d5176b41) · Commits · 戴 / test

drivers/usb/input/hid-core.c

+10 −2

Original line number	Diff line number	Diff line
		@@ -1535,13 +1535,17 @@ void hid_init_reports(struct hid_device *hid)
		#define USB_VENDOR_ID_GLAB 0x06c2
		#define USB_DEVICE_ID_4_PHIDGETSERVO_30 0x0038
		#define USB_DEVICE_ID_1_PHIDGETSERVO_30 0x0039
		#define USB_DEVICE_ID_8_8_8_IF_KIT 0x0045
		#define USB_DEVICE_ID_0_0_4_IF_KIT 0x0040
		#define USB_DEVICE_ID_0_16_16_IF_KIT 0x0044
		#define USB_DEVICE_ID_8_8_8_IF_KIT 0x0045
		#define USB_DEVICE_ID_0_8_7_IF_KIT 0x0051
		#define USB_DEVICE_ID_0_8_8_IF_KIT 0x0053
		#define USB_DEVICE_ID_PHIDGET_MOTORCONTROL 0x0058

		#define USB_VENDOR_ID_WISEGROUP 0x0925
		#define USB_DEVICE_ID_1_PHIDGETSERVO_20 0x8101
		#define USB_DEVICE_ID_4_PHIDGETSERVO_20 0x8104
		#define USB_DEVICE_ID_8_8_4_IF_KIT 0x8201
		#define USB_DEVICE_ID_DUAL_USB_JOYPAD 0x8866

		#define USB_VENDOR_ID_WISEGROUP_LTD 0x6677
		@@ -1620,9 +1624,12 @@ static const struct hid_blacklist {
		{ USB_VENDOR_ID_ESSENTIAL_REALITY, USB_DEVICE_ID_ESSENTIAL_REALITY_P5, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_GLAB, USB_DEVICE_ID_4_PHIDGETSERVO_30, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_GLAB, USB_DEVICE_ID_1_PHIDGETSERVO_30, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_GLAB, USB_DEVICE_ID_8_8_8_IF_KIT, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_GLAB, USB_DEVICE_ID_0_0_4_IF_KIT, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_GLAB, USB_DEVICE_ID_0_16_16_IF_KIT, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_GLAB, USB_DEVICE_ID_8_8_8_IF_KIT, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_GLAB, USB_DEVICE_ID_0_8_7_IF_KIT, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_GLAB, USB_DEVICE_ID_0_8_8_IF_KIT, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_GLAB, USB_DEVICE_ID_PHIDGET_MOTORCONTROL, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_GRIFFIN, USB_DEVICE_ID_POWERMATE, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_GRIFFIN, USB_DEVICE_ID_SOUNDKNOB, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_GTCO, USB_DEVICE_ID_GTCO_90, HID_QUIRK_IGNORE },
		@@ -1701,6 +1708,7 @@ static const struct hid_blacklist {
		{ USB_VENDOR_ID_VERNIER, USB_DEVICE_ID_VERNIER_CYCLOPS, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_4_PHIDGETSERVO_20, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_1_PHIDGETSERVO_20, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_8_8_4_IF_KIT, HID_QUIRK_IGNORE },
		{ USB_VENDOR_ID_YEALINK, USB_DEVICE_ID_YEALINK_P1K_P4K_B2K, HID_QUIRK_IGNORE },

		{ USB_VENDOR_ID_ACECAD, USB_DEVICE_ID_ACECAD_FLAIR, HID_QUIRK_IGNORE },

drivers/usb/misc/Kconfig

+13 −3

Original line number	Diff line number	Diff line
		@@ -116,15 +116,25 @@ config USB_CYTHERM
		module will be called cytherm.

		config USB_PHIDGETKIT
		tristate "USB PhidgetKit support"
		tristate "USB PhidgetInterfaceKit support"
		depends on USB
		help
		Say Y here if you want to connect a PhidgetKit USB device from
		Phidgets Inc.
		Say Y here if you want to connect a PhidgetInterfaceKit USB device
		from Phidgets Inc.

		To compile this driver as a module, choose M here: the
		module will be called phidgetkit.

		config USB_PHIDGETMOTORCONTROL
		tristate "USB PhidgetMotorControl support"
		depends on USB
		help
		Say Y here if you want to connect a PhidgetMotorControl USB device
		from Phidgets Inc.

		To compile this driver as a module, choose M here: the
		module will be called phidgetmotorcontrol.

		config USB_PHIDGETSERVO
		tristate "USB PhidgetServo support"
		depends on USB

drivers/usb/misc/Makefile

+1 −0

Original line number	Diff line number	Diff line
		@@ -14,6 +14,7 @@ obj-$(CONFIG_USB_LD) += ldusb.o
		obj-$(CONFIG_USB_LED) += usbled.o
		obj-$(CONFIG_USB_LEGOTOWER) += legousbtower.o
		obj-$(CONFIG_USB_PHIDGETKIT) += phidgetkit.o
		obj-$(CONFIG_USB_PHIDGETMOTORCONTROL) += phidgetmotorcontrol.o
		obj-$(CONFIG_USB_PHIDGETSERVO) += phidgetservo.o
		obj-$(CONFIG_USB_RIO500) += rio500.o
		obj-$(CONFIG_USB_TEST) += usbtest.o

drivers/usb/misc/phidgetmotorcontrol.c

0 → 100644

+427 −0

Original line number	Diff line number	Diff line
		/*
		* 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>

		#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

		struct motorcontrol {
		struct usb_device *udev;
		struct usb_interface *intf;
		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->intf->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->intf->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 usb_interface *intf = to_usb_interface(dev); \
		struct motorcontrol *mc = usb_get_intfdata(intf); \
		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 usb_interface *intf = to_usb_interface(dev); \
		struct motorcontrol *mc = usb_get_intfdata(intf); \
		\
		return sprintf(buf, "%d\n", mc->speed[value]); \
		} \
		static DEVICE_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 usb_interface *intf = to_usb_interface(dev); \
		struct motorcontrol *mc = usb_get_intfdata(intf); \
		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 usb_interface *intf = to_usb_interface(dev); \
		struct motorcontrol *mc = usb_get_intfdata(intf); \
		\
		return sprintf(buf, "%d\n", mc->acceleration[value]); \
		} \
		static DEVICE_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 usb_interface *intf = to_usb_interface(dev); \
		struct motorcontrol *mc = usb_get_intfdata(intf); \
		\
		return sprintf(buf, "%dmA\n", (int)mc->_current[value]); \
		} \
		static DEVICE_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 usb_interface *intf = to_usb_interface(dev); \
		struct motorcontrol *mc = usb_get_intfdata(intf); \
		\
		return sprintf(buf, "%d\n", (int)mc->inputs[value]); \
		} \
		static DEVICE_ATTR(input##value, S_IRUGO, show_input##value, NULL);

		show_input(0);
		show_input(1);
		show_input(2);
		show_input(3);

		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;

		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->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);

		if (usb_submit_urb(mc->irq, GFP_KERNEL)) {
		rc = -EIO;
		goto out;
		}

		device_create_file(&intf->dev, &dev_attr_input0);
		device_create_file(&intf->dev, &dev_attr_input1);
		device_create_file(&intf->dev, &dev_attr_input2);
		device_create_file(&intf->dev, &dev_attr_input3);

		device_create_file(&intf->dev, &dev_attr_speed0);
		device_create_file(&intf->dev, &dev_attr_speed1);

		device_create_file(&intf->dev, &dev_attr_acceleration0);
		device_create_file(&intf->dev, &dev_attr_acceleration1);

		device_create_file(&intf->dev, &dev_attr_current0);
		device_create_file(&intf->dev, &dev_attr_current1);

		dev_info(&intf->dev, "USB Phidget MotorControl attached\n");

		return 0;

		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);
		kfree(mc);
		}

		return rc;
		}

		static void motorcontrol_disconnect(struct usb_interface *interface)
		{
		struct motorcontrol *mc;

		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);

		device_remove_file(&interface->dev, &dev_attr_input0);
		device_remove_file(&interface->dev, &dev_attr_input1);
		device_remove_file(&interface->dev, &dev_attr_input2);
		device_remove_file(&interface->dev, &dev_attr_input3);

		device_remove_file(&interface->dev, &dev_attr_speed0);
		device_remove_file(&interface->dev, &dev_attr_speed1);

		device_remove_file(&interface->dev, &dev_attr_acceleration0);
		device_remove_file(&interface->dev, &dev_attr_acceleration1);

		device_remove_file(&interface->dev, &dev_attr_current0);
		device_remove_file(&interface->dev, &dev_attr_current1);

		dev_info(&interface->dev, "USB Phidget MotorControl disconnected\n");

		usb_put_dev(mc->udev);
		kfree(mc);
		}

		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");

Admin message