API tools faq
paste
Advertisement
Guest User

[Updated] Linux Kernel Module to Access GPS Device

a guest
Jul 30th, 2014
468
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 13.75 KB | None | 0 0
raw download clone embed print report
  1. #include <linux/device.h>
  2. #include <linux/errno.h>
  3. #include <linux/kernel.h>
  4. #include <linux/module.h>
  5. #include <linux/moduleparam.h>
  6. #include <linux/mutex.h>
  7. #include <linux/slab.h>
  8. #include <linux/stat.h>
  9. #include <linux/uaccess.h>
  10. #include <linux/usb.h>
  11. #include <linux/workqueue.h>
  12.  
  13. #define USB_VENDOR_ID 0x1546
  14. #define USB_PRODUCT_ID 0x01a7
  15. #define USB_DRIVER_NAME "ublox"
  16. #define UBLOX_MINOR_BASE 192
  17. #define WORK_QUEUE_NAME "ublox_work_queue"
  18.  
  19. #define PRINT_FIELD(structure, field)                                    \
  20.     do {                                                             \
  21.         printk(KERN_DEBUG "%s:%d: %s = %u\n", __func__, __LINE__, #field, structure->field); \
  22.     } while (0)
  23. #define get_parent_ublox_device(kref_variable) container_of(kref_variable, struct ublox_device, kref)
  24.  
  25. static int usb_probe(struct usb_interface *interface, const struct usb_device_id *usb_device_id);
  26. static void usb_disconnect(struct usb_interface *interface);
  27. static void usb_print_interface_description(struct usb_interface_descriptor *interface_descriptor);
  28. static int ublox_open(struct inode *inode, struct file *file);
  29. static int ublox_release(struct inode *inode, struct file *file);
  30. static ssize_t ublox_read(struct file *file, char *buffer, size_t count, loff_t *pos);
  31. static void ublox_read_bulk_callback(struct urb *urb);
  32. static void ublox_destroy_device(struct kref *kref);
  33. static void work_queue_task_callback(struct work_struct *work);
  34.  
  35. static struct usb_device_id supported_usb_ids [] = {
  36.     { USB_DEVICE(USB_VENDOR_ID, USB_PRODUCT_ID) },
  37.     { } /* Terminating entry */
  38. };
  39. MODULE_DEVICE_TABLE(usb, supported_usb_ids);
  40.  
  41. static struct usb_driver usb_driver = {
  42.     .name = USB_DRIVER_NAME,
  43.     .id_table = supported_usb_ids,
  44.     .probe = usb_probe,
  45.     .disconnect = usb_disconnect
  46. };
  47.  
  48. static const struct file_operations ublox_fops = {
  49.     .owner = THIS_MODULE,
  50.     .open = ublox_open,
  51.     .release = ublox_release,
  52.     .read = ublox_read
  53. };
  54.  
  55. static struct usb_class_driver ublox_class = {
  56.     .name = USB_DRIVER_NAME,
  57.     .fops = &ublox_fops,
  58.     .minor_base = UBLOX_MINOR_BASE
  59. };
  60.  
  61. struct ublox_device {
  62.     struct usb_device   *usb_device;            /* The usb device to which the ublox device belongs */
  63.     struct usb_interface    *interface;         /* The interface for the ublox device */
  64.     __u8            bulk_in_bEndpointAddress;   /* Address for reading data from ublox device */
  65.     size_t          bulk_in_buffer_size;        /* Size of the input buffer */
  66.     struct urb      *bulk_in_urb;           /* URB for reading data */
  67.     unsigned char       *bulk_in_buffer;        /* Buffer to receive data */
  68.     struct mutex        bulk_in_mutex;          /* Allow only one bulk read operation at a time */
  69.     wait_queue_head_t   bulk_in_wait_queue;     /* Allow only one reader and send all others to sleep */
  70.     bool            read_in_progress;       /* Signal a pending read operation */
  71.     unsigned int        transferred_bytes;      /* The transferred bytes during the last read operation */
  72.     int         status_last_transmission;   /* Status of last transmission */
  73.     __u8            bulk_out_bEndpointAddress;  /* Address for writing data to ublox device */
  74.     size_t          bulk_out_buffer_size;       /* Size of the output buffer */
  75.     struct kref     kref;               /* Reference counter */
  76. };
  77.  
  78. static struct workqueue_struct *work_queue;
  79. static struct delayed_work work_queue_task;
  80. static int keep_task_in_work_queue = 1;
  81.  
  82. static unsigned long usb_polling_interval = HZ; /* HZ = [Interrupts] / [s] => poll each second per default */
  83. module_param(usb_polling_interval, ulong, S_IRUGO | S_IWUSR);
  84. MODULE_PARM_DESC(usb_polling_interval, "Define how often the GPS device is polled via USB. The rate is defined in interrupts. Setting to \"1\" causes a poll at each interrupt. Setting to \"HZ\" causes one poll per second.");
  85.  
  86. int init_module(void)
  87. {
  88.     int return_value;
  89.  
  90.     printk(KERN_DEBUG "%s:%d: initializing module ublox...\n", __func__, __LINE__);
  91.  
  92.     return_value = usb_register(&usb_driver);
  93.     if (return_value) {
  94.         printk(KERN_ERR "%s:%d: usb_register(...) failed (%d)!\n", __func__, __LINE__, return_value);
  95.         goto exit;
  96.     }
  97.  
  98.     printk(KERN_DEBUG "%s:%d: creating work queue...\n", __func__, __LINE__);
  99.  
  100.     work_queue = create_singlethread_workqueue(WORK_QUEUE_NAME);
  101.     if (work_queue == NULL) {
  102.         printk(KERN_ERR "%s:%d: create_singlethread_workqueue(...) failed!\n", __func__, __LINE__);
  103.         return_value = -ENOMEM;
  104.         goto exit;
  105.     }
  106.  
  107.     INIT_DELAYED_WORK(&work_queue_task, work_queue_task_callback);
  108.     queue_delayed_work(work_queue, &work_queue_task, usb_polling_interval);
  109.  
  110.     printk(KERN_DEBUG "%s:%d: interrupts_per_second = %d\n", __func__, __LINE__, HZ);
  111.     printk(KERN_DEBUG "%s:%d: usb_polling_interval = %lu\n", __func__, __LINE__, usb_polling_interval);
  112.  
  113.     printk(KERN_DEBUG "%s:%d: created work queue.\n", __func__, __LINE__);
  114.  
  115.     printk(KERN_DEBUG "%s:%d: initialized module ublox.\n", __func__, __LINE__);
  116.  
  117. exit:
  118.     return return_value;
  119. }
  120.  
  121. void cleanup_module(void)
  122. {
  123.     keep_task_in_work_queue = 0;
  124.     cancel_delayed_work(&work_queue_task);
  125.     flush_workqueue(work_queue);
  126.     destroy_workqueue(work_queue);
  127.  
  128.     usb_deregister(&usb_driver);
  129.  
  130.     printk(KERN_DEBUG "%s:%d: cleaned up module ublox.\n", __func__, __LINE__);
  131. }
  132.  
  133. static int usb_probe(struct usb_interface *interface, const struct usb_device_id *usb_device_id)
  134. {
  135.     struct usb_host_interface *current_interface = interface->cur_altsetting;
  136.     struct ublox_device *ublox_device = NULL;
  137.     int in_endpoint_found = 0;
  138.     int out_endpoint_found = 0;
  139.     int return_value = -EINVAL;
  140.     int i;
  141.  
  142.     printk(KERN_DEBUG "%s:%d: probing USB interface...\n", __func__, __LINE__);
  143.  
  144.     /* Accept only interfaces with two endpoints (one for reading, one for writing) */
  145.     if (current_interface->desc.bNumEndpoints != 2) {
  146.         dev_warn(&interface->dev, "%s:%d: ignore USB interface!\n", __func__, __LINE__);
  147.         usb_print_interface_description(&current_interface->desc);
  148.     } else {
  149.         ublox_device = kzalloc(sizeof(*ublox_device), GFP_KERNEL);
  150.         if (ublox_device == NULL) {
  151.             return_value = -ENOMEM;
  152.             goto error;
  153.         }
  154.  
  155.         ublox_device->usb_device = usb_get_dev(interface_to_usbdev(interface));
  156.         ublox_device->interface = interface;
  157.         ublox_device->read_in_progress = 0;
  158.         ublox_device->transferred_bytes = 0;
  159.  
  160.         mutex_init(&ublox_device->bulk_in_mutex);
  161.         init_waitqueue_head(&ublox_device->bulk_in_wait_queue);
  162.         kref_init(&ublox_device->kref);
  163.  
  164.         for (i = 0; i < current_interface->desc.bNumEndpoints; i++) {
  165.             struct usb_endpoint_descriptor endpoint = current_interface->endpoint[i].desc;
  166.  
  167.             if (usb_endpoint_is_bulk_in(&endpoint)) {
  168.                 ublox_device->bulk_in_bEndpointAddress = endpoint.bEndpointAddress;
  169.                 ublox_device->bulk_in_buffer_size = endpoint.wMaxPacketSize;
  170.  
  171.                 ublox_device->bulk_in_buffer = kzalloc(endpoint.wMaxPacketSize, GFP_KERNEL);
  172.                 if (ublox_device->bulk_in_buffer == NULL) {
  173.                     return_value = -ENOMEM;
  174.                     goto error;
  175.                 }
  176.  
  177.                 in_endpoint_found = 1;
  178.             }
  179.  
  180.             if (usb_endpoint_is_bulk_out(&endpoint)) {
  181.                 ublox_device->bulk_out_bEndpointAddress = endpoint.bEndpointAddress;
  182.                 ublox_device->bulk_out_buffer_size = endpoint.wMaxPacketSize;
  183.  
  184.                 out_endpoint_found = 1;
  185.             }
  186.         }
  187.  
  188.         if (in_endpoint_found && out_endpoint_found) {
  189.             dev_info(&interface->dev, "%s:%d: handle USB interface.\n", __func__, __LINE__);
  190.             usb_print_interface_description(&current_interface->desc);
  191.  
  192.             return_value = usb_register_dev(interface, &ublox_class);
  193.             if (return_value) {
  194.                 dev_err(&interface->dev, "%s:%d: usb_register_dev(..) failed (%d)!\n", __func__, __LINE__, return_value);
  195.                 goto error;
  196.             }
  197.  
  198.             usb_set_intfdata(interface, ublox_device);
  199.         }
  200.         else
  201.             goto error;
  202.     }
  203.  
  204.     printk(KERN_DEBUG "%s:%d: probed USB interface.\n", __func__, __LINE__);
  205.  
  206.     return return_value;
  207.  
  208. error:
  209.     if (ublox_device)
  210.         kref_put(&ublox_device->kref, ublox_destroy_device);
  211.  
  212.     return return_value;
  213. }
  214.  
  215. static void usb_disconnect(struct usb_interface *interface)
  216. {
  217.     struct ublox_device *ublox_device = usb_get_intfdata(interface);
  218.     int minor = interface->minor;
  219.  
  220.     /* Wait for pending I/O operations and then prevent more I/O operations from starting */
  221.     mutex_lock(&ublox_device->bulk_in_mutex);
  222.  
  223.     usb_set_intfdata(interface, NULL);
  224.     usb_deregister_dev(interface, &ublox_class);
  225.     kref_put(&ublox_device->kref, ublox_destroy_device);
  226.  
  227.     mutex_unlock(&ublox_device->bulk_in_mutex);
  228.  
  229.     dev_info(&interface->dev, "%s:%d: disconnected USB interface with minor number %d.\n", __func__, __LINE__, minor);
  230. }
  231.  
  232. static void usb_print_interface_description(struct usb_interface_descriptor *interface_descriptor)
  233. {
  234.     PRINT_FIELD(interface_descriptor, bInterfaceNumber);
  235.     PRINT_FIELD(interface_descriptor, bNumEndpoints);
  236.     PRINT_FIELD(interface_descriptor, bInterfaceClass);
  237.     PRINT_FIELD(interface_descriptor, bInterfaceSubClass);
  238. }
  239.  
  240. static int ublox_open(struct inode *inode, struct file *file)
  241. {
  242.     struct ublox_device *ublox_device;
  243.     struct usb_interface *interface;
  244.     int minor;
  245.     int return_value = 0;
  246.  
  247.     printk(KERN_DEBUG "%s:%d: opening ublox device...\n", __func__, __LINE__);
  248.  
  249.     minor = iminor(inode);
  250.  
  251.     interface = usb_find_interface(&usb_driver, minor);
  252.     if (!interface) {
  253.         printk(KERN_ERR "%s:%d: usb_find_interface(...) failed for minor %d!\n", __func__, __LINE__, minor);
  254.         return_value = -ENODEV;
  255.         goto exit;
  256.     }
  257.  
  258.     ublox_device = usb_get_intfdata(interface);
  259.     if (!ublox_device) {
  260.         printk(KERN_ERR "%s:%d: usb_get_intfdata(...) failed!\n", __func__, __LINE__);
  261.         return_value = -ENODEV;
  262.         goto exit;
  263.     }
  264.  
  265.     kref_get(&ublox_device->kref);
  266.     file->private_data = ublox_device;
  267.  
  268.     printk(KERN_DEBUG "%s:%d: opened ublox device.\n", __func__, __LINE__);
  269. exit:
  270.     return return_value;
  271. }
  272.  
  273. static int ublox_release(struct inode *inode, struct file *file)
  274. {
  275.     struct ublox_device *ublox_device = file->private_data;
  276.  
  277.     printk(KERN_DEBUG "%s:%d: releasing ublox device...\n", __func__, __LINE__);
  278.  
  279.     if (ublox_device == NULL)
  280.         return -ENODEV;
  281.  
  282.     kref_put(&ublox_device->kref, ublox_destroy_device);
  283.  
  284.     printk(KERN_DEBUG "%s:%d: released ublox device.\n", __func__, __LINE__);
  285.  
  286.     return 0;
  287. }
  288.  
  289. static ssize_t ublox_read(struct file *file, char *buffer, size_t count, loff_t *pos)
  290. {
  291.     struct ublox_device *ublox_device = file->private_data;
  292.     int bytes_to_read = 0;
  293.     int return_value = 0;
  294.  
  295.     if (ublox_device == NULL)
  296.         return -ENODEV;
  297.  
  298.     return_value = mutex_lock_interruptible(&ublox_device->bulk_in_mutex);
  299.     if (return_value < 0)
  300.         return -EINTR;
  301.  
  302.     if (ublox_device->interface == NULL) { /* disconnect(...) was called in the meantime */
  303.         return_value = -ENODEV;
  304.         goto exit;
  305.     }
  306.  
  307.     printk(KERN_DEBUG "%s:%d: reading ublox device...\n", __func__, __LINE__);
  308.     printk(KERN_DEBUG "%s:%d: count = %d\n", __func__, __LINE__, count);
  309.     printk(KERN_DEBUG "%s:%d: pos = %lu\n", __func__, __LINE__, (unsigned long)*pos);
  310.     printk(KERN_DEBUG "%s:%d: pid = %d\n", __func__, __LINE__, current->pid);
  311.  
  312.     bytes_to_read = min(count, ublox_device->bulk_in_buffer_size);
  313. retry:
  314.     ublox_device->bulk_in_urb = usb_alloc_urb(0, GFP_KERNEL);
  315.     if (ublox_device->bulk_in_urb == NULL) {
  316.         return_value = -ENOMEM;
  317.         goto exit;
  318.     }
  319.  
  320.     usb_fill_bulk_urb(ublox_device->bulk_in_urb,
  321.             ublox_device->usb_device,
  322.             usb_rcvbulkpipe(ublox_device->usb_device, ublox_device->bulk_in_bEndpointAddress),
  323.             ublox_device->bulk_in_buffer,
  324.             bytes_to_read,
  325.             ublox_read_bulk_callback,
  326.             ublox_device);
  327.  
  328.     ublox_device->read_in_progress = 1;
  329.  
  330.     return_value = usb_submit_urb(ublox_device->bulk_in_urb, GFP_KERNEL);
  331.     if (return_value) {
  332.         printk(KERN_ERR "%s:%d: usb_submit_urb(...) failed (%d)!\n", __func__, __LINE__, return_value);
  333.         ublox_device->read_in_progress = 0;
  334.         goto exit;
  335.     }
  336.  
  337.     /* Go to sleep until read operation has finished */
  338.     return_value = wait_event_interruptible(ublox_device->bulk_in_wait_queue, (!ublox_device->read_in_progress));
  339.     if (return_value < 0)
  340.         goto exit;
  341.  
  342.     return_value = ublox_device->status_last_transmission;
  343.     if (return_value)
  344.         goto exit;
  345.  
  346.     if (ublox_device->transferred_bytes > bytes_to_read) {
  347.         return_value = -EIO;
  348.         goto exit;
  349.     } else if (ublox_device->transferred_bytes == 0) {
  350.         goto retry;
  351.     }
  352.  
  353.     if (copy_to_user(buffer, ublox_device->bulk_in_buffer, ublox_device->transferred_bytes)) {
  354.         return_value = -EFAULT;
  355.         goto exit;
  356.     } else {
  357.         return_value = ublox_device->transferred_bytes;
  358.     }
  359.  
  360.  
  361. exit:
  362.     printk(KERN_DEBUG "%s:%d: status = %d\n", __func__, __LINE__, ublox_device->status_last_transmission);
  363.     printk(KERN_DEBUG "%s:%d: bytes_to_read = %d\n", __func__, __LINE__, bytes_to_read);
  364.     printk(KERN_DEBUG "%s:%d: transferred_bytes = %d\n", __func__, __LINE__, ublox_device->transferred_bytes);
  365.  
  366.     printk(KERN_DEBUG "%s:%d: read ublox device.\n", __func__, __LINE__);
  367.  
  368.     mutex_unlock(&ublox_device->bulk_in_mutex);
  369.     return return_value;
  370. }
  371.  
  372. static void ublox_read_bulk_callback(struct urb *urb)
  373. {
  374.     struct ublox_device *ublox_device = urb->context;
  375.     int status = urb->status;
  376.  
  377.     ublox_device->status_last_transmission = urb->status;
  378.  
  379.     if (status) {
  380.         if (status == -ENOENT || status == -ECONNRESET || status == -ESHUTDOWN)
  381.             printk(KERN_WARNING "%s:%d: sync/async/unlink I/O error (%d)!\n", __func__, __LINE__, status);
  382.         else
  383.             printk(KERN_ERR "%s:%d: critical I/O error (%d)\n", __func__, __LINE__, status);
  384.     }
  385.  
  386.     ublox_device->transferred_bytes = urb->actual_length;
  387.     ublox_device->read_in_progress = 0;
  388.  
  389.     usb_free_urb(ublox_device->bulk_in_urb);
  390.     wake_up_interruptible(&ublox_device->bulk_in_wait_queue);
  391. }
  392.  
  393. static void ublox_destroy_device(struct kref *kref)
  394. {
  395.     struct ublox_device *ublox_device = get_parent_ublox_device(kref);
  396.  
  397.     kfree(ublox_device->bulk_in_buffer);
  398.     kfree(ublox_device);
  399. }
  400.  
  401. static void work_queue_task_callback(struct work_struct *work)
  402. {
  403.     printk(KERN_DEBUG "%s:%d: in work_queue_task_callback(...).\n", __func__, __LINE__);
  404.  
  405.     if (keep_task_in_work_queue)
  406.         queue_delayed_work(work_queue, &work_queue_task, usb_polling_interval);
  407. }
  408.  
  409. MODULE_DESCRIPTION("Poll u-blox NEO-7 GPS device and acquire PPS information periodically.");
  410. MODULE_LICENSE("GPL");
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!