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