phidgetmotorcontrol.c 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466
  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. #include <linux/kernel.h>
  12. #include <linux/errno.h>
  13. #include <linux/init.h>
  14. #include <linux/module.h>
  15. #include <linux/usb.h>
  16. #include "phidget.h"
  17. #define DRIVER_AUTHOR "Sean Young <sean@mess.org>"
  18. #define DRIVER_DESC "USB PhidgetMotorControl Driver"
  19. #define USB_VENDOR_ID_GLAB 0x06c2
  20. #define USB_DEVICE_ID_MOTORCONTROL 0x0058
  21. #define URB_INT_SIZE 8
  22. static unsigned long device_no;
  23. struct motorcontrol {
  24. struct usb_device *udev;
  25. struct usb_interface *intf;
  26. struct device *dev;
  27. int dev_no;
  28. u8 inputs[4];
  29. s8 desired_speed[2];
  30. s8 speed[2];
  31. s16 _current[2];
  32. s8 acceleration[2];
  33. struct urb *irq;
  34. unsigned char *data;
  35. dma_addr_t data_dma;
  36. struct work_struct do_notify;
  37. unsigned long input_events;
  38. unsigned long speed_events;
  39. unsigned long exceed_events;
  40. };
  41. static struct usb_device_id id_table[] = {
  42. { USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_MOTORCONTROL) },
  43. {}
  44. };
  45. MODULE_DEVICE_TABLE(usb, id_table);
  46. static int set_motor(struct motorcontrol *mc, int motor)
  47. {
  48. u8 *buffer;
  49. int speed, speed2, acceleration;
  50. int retval;
  51. buffer = kzalloc(8, GFP_KERNEL);
  52. if (!buffer) {
  53. dev_err(&mc->intf->dev, "%s - out of memory\n", __FUNCTION__);
  54. return -ENOMEM;
  55. }
  56. acceleration = mc->acceleration[motor] * 10;
  57. /* -127 <= speed <= 127 */
  58. speed = (mc->desired_speed[motor] * 127) / 100;
  59. /* -0x7300 <= speed2 <= 0x7300 */
  60. speed2 = (mc->desired_speed[motor] * 230 * 128) / 100;
  61. buffer[0] = motor;
  62. buffer[1] = speed;
  63. buffer[2] = acceleration >> 8;
  64. buffer[3] = acceleration;
  65. buffer[4] = speed2 >> 8;
  66. buffer[5] = speed2;
  67. retval = usb_control_msg(mc->udev,
  68. usb_sndctrlpipe(mc->udev, 0),
  69. 0x09, 0x21, 0x0200, 0x0000, buffer, 8, 2000);
  70. if (retval != 8)
  71. dev_err(&mc->intf->dev, "usb_control_msg returned %d\n",
  72. retval);
  73. kfree(buffer);
  74. return retval < 0 ? retval : 0;
  75. }
  76. static void motorcontrol_irq(struct urb *urb, struct pt_regs *regs)
  77. {
  78. struct motorcontrol *mc = urb->context;
  79. unsigned char *buffer = mc->data;
  80. int i, level;
  81. int status;
  82. switch (urb->status) {
  83. case 0: /* success */
  84. break;
  85. case -ECONNRESET: /* unlink */
  86. case -ENOENT:
  87. case -ESHUTDOWN:
  88. return;
  89. /* -EPIPE: should clear the halt */
  90. default: /* error */
  91. goto resubmit;
  92. }
  93. /* digital inputs */
  94. for (i=0; i<4; i++) {
  95. level = (buffer[0] >> i) & 1;
  96. if (mc->inputs[i] != level) {
  97. mc->inputs[i] = level;
  98. set_bit(i, &mc->input_events);
  99. }
  100. }
  101. /* motor speed */
  102. if (buffer[2] == 0) {
  103. for (i=0; i<2; i++) {
  104. level = ((s8)buffer[4+i]) * 100 / 127;
  105. if (mc->speed[i] != level) {
  106. mc->speed[i] = level;
  107. set_bit(i, &mc->speed_events);
  108. }
  109. }
  110. } else {
  111. int index = buffer[3] & 1;
  112. level = ((s8)buffer[4] << 8) | buffer[5];
  113. level = level * 100 / 29440;
  114. if (mc->speed[index] != level) {
  115. mc->speed[index] = level;
  116. set_bit(index, &mc->speed_events);
  117. }
  118. level = ((s8)buffer[6] << 8) | buffer[7];
  119. mc->_current[index] = level * 100 / 1572;
  120. }
  121. if (buffer[1] & 1)
  122. set_bit(0, &mc->exceed_events);
  123. if (buffer[1] & 2)
  124. set_bit(1, &mc->exceed_events);
  125. if (mc->input_events || mc->exceed_events || mc->speed_events)
  126. schedule_work(&mc->do_notify);
  127. resubmit:
  128. status = usb_submit_urb(urb, SLAB_ATOMIC);
  129. if (status)
  130. dev_err(&mc->intf->dev,
  131. "can't resubmit intr, %s-%s/motorcontrol0, status %d",
  132. mc->udev->bus->bus_name,
  133. mc->udev->devpath, status);
  134. }
  135. static void do_notify(void *data)
  136. {
  137. struct motorcontrol *mc = data;
  138. int i;
  139. char sysfs_file[8];
  140. for (i=0; i<4; i++) {
  141. if (test_and_clear_bit(i, &mc->input_events)) {
  142. sprintf(sysfs_file, "input%d", i);
  143. sysfs_notify(&mc->dev->kobj, NULL, sysfs_file);
  144. }
  145. }
  146. for (i=0; i<2; i++) {
  147. if (test_and_clear_bit(i, &mc->speed_events)) {
  148. sprintf(sysfs_file, "speed%d", i);
  149. sysfs_notify(&mc->dev->kobj, NULL, sysfs_file);
  150. }
  151. }
  152. for (i=0; i<2; i++) {
  153. if (test_and_clear_bit(i, &mc->exceed_events))
  154. dev_warn(&mc->intf->dev,
  155. "motor #%d exceeds 1.5 Amp current limit\n", i);
  156. }
  157. }
  158. #define show_set_speed(value) \
  159. static ssize_t set_speed##value(struct device *dev, \
  160. struct device_attribute *attr, \
  161. const char *buf, size_t count) \
  162. { \
  163. struct motorcontrol *mc = dev_get_drvdata(dev); \
  164. int speed; \
  165. int retval; \
  166. \
  167. if (sscanf(buf, "%d", &speed) < 1) \
  168. return -EINVAL; \
  169. \
  170. if (speed < -100 || speed > 100) \
  171. return -EINVAL; \
  172. \
  173. mc->desired_speed[value] = speed; \
  174. \
  175. retval = set_motor(mc, value); \
  176. \
  177. return retval ? retval : count; \
  178. } \
  179. \
  180. static ssize_t show_speed##value(struct device *dev, \
  181. struct device_attribute *attr, \
  182. char *buf) \
  183. { \
  184. struct motorcontrol *mc = dev_get_drvdata(dev); \
  185. \
  186. return sprintf(buf, "%d\n", mc->speed[value]); \
  187. }
  188. #define speed_attr(value) \
  189. __ATTR(speed##value, S_IWUGO | S_IRUGO, \
  190. show_speed##value, set_speed##value)
  191. show_set_speed(0);
  192. show_set_speed(1);
  193. #define show_set_acceleration(value) \
  194. static ssize_t set_acceleration##value(struct device *dev, \
  195. struct device_attribute *attr, \
  196. const char *buf, size_t count) \
  197. { \
  198. struct motorcontrol *mc = dev_get_drvdata(dev); \
  199. int acceleration; \
  200. int retval; \
  201. \
  202. if (sscanf(buf, "%d", &acceleration) < 1) \
  203. return -EINVAL; \
  204. \
  205. if (acceleration < 0 || acceleration > 100) \
  206. return -EINVAL; \
  207. \
  208. mc->acceleration[value] = acceleration; \
  209. \
  210. retval = set_motor(mc, value); \
  211. \
  212. return retval ? retval : count; \
  213. } \
  214. \
  215. static ssize_t show_acceleration##value(struct device *dev, \
  216. struct device_attribute *attr, \
  217. char *buf) \
  218. { \
  219. struct motorcontrol *mc = dev_get_drvdata(dev); \
  220. \
  221. return sprintf(buf, "%d\n", mc->acceleration[value]); \
  222. }
  223. #define acceleration_attr(value) \
  224. __ATTR(acceleration##value, S_IWUGO | S_IRUGO, \
  225. show_acceleration##value, set_acceleration##value)
  226. show_set_acceleration(0);
  227. show_set_acceleration(1);
  228. #define show_current(value) \
  229. static ssize_t show_current##value(struct device *dev, \
  230. struct device_attribute *attr, \
  231. char *buf) \
  232. { \
  233. struct motorcontrol *mc = dev_get_drvdata(dev); \
  234. \
  235. return sprintf(buf, "%dmA\n", (int)mc->_current[value]); \
  236. }
  237. #define current_attr(value) \
  238. __ATTR(current##value, S_IRUGO, show_current##value, NULL)
  239. show_current(0);
  240. show_current(1);
  241. #define show_input(value) \
  242. static ssize_t show_input##value(struct device *dev, \
  243. struct device_attribute *attr, \
  244. char *buf) \
  245. { \
  246. struct motorcontrol *mc = dev_get_drvdata(dev); \
  247. \
  248. return sprintf(buf, "%d\n", (int)mc->inputs[value]); \
  249. }
  250. #define input_attr(value) \
  251. __ATTR(input##value, S_IRUGO, show_input##value, NULL)
  252. show_input(0);
  253. show_input(1);
  254. show_input(2);
  255. show_input(3);
  256. static struct device_attribute dev_attrs[] = {
  257. input_attr(0),
  258. input_attr(1),
  259. input_attr(2),
  260. input_attr(3),
  261. speed_attr(0),
  262. speed_attr(1),
  263. acceleration_attr(0),
  264. acceleration_attr(1),
  265. current_attr(0),
  266. current_attr(1)
  267. };
  268. static int motorcontrol_probe(struct usb_interface *intf, const struct usb_device_id *id)
  269. {
  270. struct usb_device *dev = interface_to_usbdev(intf);
  271. struct usb_host_interface *interface;
  272. struct usb_endpoint_descriptor *endpoint;
  273. struct motorcontrol *mc;
  274. int pipe, maxp, rc = -ENOMEM;
  275. int bit, value, i;
  276. interface = intf->cur_altsetting;
  277. if (interface->desc.bNumEndpoints != 1)
  278. return -ENODEV;
  279. endpoint = &interface->endpoint[0].desc;
  280. if (!(endpoint->bEndpointAddress & 0x80))
  281. return -ENODEV;
  282. /*
  283. * bmAttributes
  284. */
  285. pipe = usb_rcvintpipe(dev, endpoint->bEndpointAddress);
  286. maxp = usb_maxpacket(dev, pipe, usb_pipeout(pipe));
  287. mc = kzalloc(sizeof(*mc), GFP_KERNEL);
  288. if (!mc)
  289. goto out;
  290. mc->dev_no = -1;
  291. mc->data = usb_buffer_alloc(dev, URB_INT_SIZE, SLAB_ATOMIC, &mc->data_dma);
  292. if (!mc->data)
  293. goto out;
  294. mc->irq = usb_alloc_urb(0, GFP_KERNEL);
  295. if (!mc->irq)
  296. goto out;
  297. mc->udev = usb_get_dev(dev);
  298. mc->intf = intf;
  299. mc->acceleration[0] = mc->acceleration[1] = 10;
  300. INIT_WORK(&mc->do_notify, do_notify, mc);
  301. usb_fill_int_urb(mc->irq, mc->udev, pipe, mc->data,
  302. maxp > URB_INT_SIZE ? URB_INT_SIZE : maxp,
  303. motorcontrol_irq, mc, endpoint->bInterval);
  304. mc->irq->transfer_dma = mc->data_dma;
  305. mc->irq->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
  306. usb_set_intfdata(intf, mc);
  307. do {
  308. bit = find_first_zero_bit(&device_no, sizeof(device_no));
  309. value = test_and_set_bit(bit, &device_no);
  310. } while(value);
  311. mc->dev_no = bit;
  312. mc->dev = device_create(phidget_class, &mc->udev->dev, 0,
  313. "motorcontrol%d", mc->dev_no);
  314. if (IS_ERR(mc->dev)) {
  315. rc = PTR_ERR(mc->dev);
  316. mc->dev = NULL;
  317. goto out;
  318. }
  319. dev_set_drvdata(mc->dev, mc);
  320. if (usb_submit_urb(mc->irq, GFP_KERNEL)) {
  321. rc = -EIO;
  322. goto out;
  323. }
  324. for (i=0; i<ARRAY_SIZE(dev_attrs); i++) {
  325. rc = device_create_file(mc->dev, &dev_attrs[i]);
  326. if (rc)
  327. goto out2;
  328. }
  329. dev_info(&intf->dev, "USB PhidgetMotorControl attached\n");
  330. return 0;
  331. out2:
  332. while (i-- > 0)
  333. device_remove_file(mc->dev, &dev_attrs[i]);
  334. out:
  335. if (mc) {
  336. if (mc->irq)
  337. usb_free_urb(mc->irq);
  338. if (mc->data)
  339. usb_buffer_free(dev, URB_INT_SIZE, mc->data, mc->data_dma);
  340. if (mc->dev)
  341. device_unregister(mc->dev);
  342. if (mc->dev_no >= 0)
  343. clear_bit(mc->dev_no, &device_no);
  344. kfree(mc);
  345. }
  346. return rc;
  347. }
  348. static void motorcontrol_disconnect(struct usb_interface *interface)
  349. {
  350. struct motorcontrol *mc;
  351. int i;
  352. mc = usb_get_intfdata(interface);
  353. usb_set_intfdata(interface, NULL);
  354. if (!mc)
  355. return;
  356. usb_kill_urb(mc->irq);
  357. usb_free_urb(mc->irq);
  358. usb_buffer_free(mc->udev, URB_INT_SIZE, mc->data, mc->data_dma);
  359. cancel_delayed_work(&mc->do_notify);
  360. for (i=0; i<ARRAY_SIZE(dev_attrs); i++)
  361. device_remove_file(mc->dev, &dev_attrs[i]);
  362. device_unregister(mc->dev);
  363. usb_put_dev(mc->udev);
  364. clear_bit(mc->dev_no, &device_no);
  365. kfree(mc);
  366. dev_info(&interface->dev, "USB PhidgetMotorControl detached\n");
  367. }
  368. static struct usb_driver motorcontrol_driver = {
  369. .name = "phidgetmotorcontrol",
  370. .probe = motorcontrol_probe,
  371. .disconnect = motorcontrol_disconnect,
  372. .id_table = id_table
  373. };
  374. static int __init motorcontrol_init(void)
  375. {
  376. int retval = 0;
  377. retval = usb_register(&motorcontrol_driver);
  378. if (retval)
  379. err("usb_register failed. Error number %d", retval);
  380. return retval;
  381. }
  382. static void __exit motorcontrol_exit(void)
  383. {
  384. usb_deregister(&motorcontrol_driver);
  385. }
  386. module_init(motorcontrol_init);
  387. module_exit(motorcontrol_exit);
  388. MODULE_AUTHOR(DRIVER_AUTHOR);
  389. MODULE_DESCRIPTION(DRIVER_DESC);
  390. MODULE_LICENSE("GPL");