You can not select more than 25 topics
			Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
		
		
		
		
		
			
		
			
				
					
					
						
							613 lines
						
					
					
						
							15 KiB
						
					
					
				
			
		
		
	
	
							613 lines
						
					
					
						
							15 KiB
						
					
					
				| /**
 | |
|  * @file    panda.c
 | |
|  * @author  Jessy Diamond Exum
 | |
|  * @date    16 June 2017
 | |
|  * @version 0.1
 | |
|  * @brief   Driver for the Comma.ai Panda CAN adapter to allow it to be controlled via
 | |
|  * the Linux SocketCAN interface.
 | |
|  * @see https://github.com/commaai/panda for the full project.
 | |
|  * @see Inspired by net/can/usb/mcba_usb.c from Linux Kernel 4.12-rc4.
 | |
|  */
 | |
| 
 | |
| #include <linux/can.h>
 | |
| #include <linux/can/dev.h>
 | |
| #include <linux/can/error.h>
 | |
| #include <linux/init.h>             // Macros used to mark up functions e.g., __init __exit
 | |
| #include <linux/kernel.h>           // Contains types, macros, functions for the kernel
 | |
| #include <linux/module.h>           // Core header for loading LKMs into the kernel
 | |
| #include <linux/netdevice.h>
 | |
| #include <linux/usb.h>
 | |
| 
 | |
| /* vendor and product id */
 | |
| #define PANDA_MODULE_NAME "panda"
 | |
| #define PANDA_VENDOR_ID 0XBBAA
 | |
| #define PANDA_PRODUCT_ID 0XDDCC
 | |
| 
 | |
| #define PANDA_MAX_TX_URBS 20
 | |
| #define PANDA_CTX_FREE PANDA_MAX_TX_URBS
 | |
| 
 | |
| #define PANDA_USB_RX_BUFF_SIZE 0x40
 | |
| #define PANDA_USB_TX_BUFF_SIZE (sizeof(struct panda_usb_can_msg))
 | |
| 
 | |
| #define PANDA_NUM_CAN_INTERFACES 3
 | |
| 
 | |
| #define PANDA_CAN_TRANSMIT 1
 | |
| #define PANDA_CAN_EXTENDED 4
 | |
| 
 | |
| #define PANDA_BITRATE 500000
 | |
| 
 | |
| #define PANDA_DLC_MASK  0x0F
 | |
| 
 | |
| struct panda_usb_ctx {
 | |
|   struct panda_inf_priv *priv;
 | |
|   u32 ndx;
 | |
|   u8 dlc;
 | |
| };
 | |
| 
 | |
| struct panda_dev_priv;
 | |
| 
 | |
| struct panda_inf_priv {
 | |
|   struct can_priv can;
 | |
|   struct panda_usb_ctx tx_context[PANDA_MAX_TX_URBS];
 | |
|   struct net_device *netdev;
 | |
|   struct usb_anchor tx_submitted;
 | |
|   atomic_t free_ctx_cnt;
 | |
|   u8 interface_num;
 | |
|   u8 mcu_can_ifnum;
 | |
|   struct panda_dev_priv *priv_dev;
 | |
| };
 | |
| 
 | |
| struct panda_dev_priv {
 | |
|   struct usb_device *udev;
 | |
|   struct device *dev;
 | |
|   struct usb_anchor rx_submitted;
 | |
|   struct panda_inf_priv *interfaces[PANDA_NUM_CAN_INTERFACES];
 | |
| };
 | |
| 
 | |
| struct __packed panda_usb_can_msg {
 | |
|   u32 rir;
 | |
|   u32 bus_dat_len;
 | |
|   u8 data[8];
 | |
| };
 | |
| 
 | |
| static const struct usb_device_id panda_usb_table[] = {
 | |
|   { USB_DEVICE(PANDA_VENDOR_ID, PANDA_PRODUCT_ID) },
 | |
|   {} /* Terminating entry */
 | |
| };
 | |
| 
 | |
| MODULE_DEVICE_TABLE(usb, panda_usb_table);
 | |
| 
 | |
| 
 | |
| // panda:       CAN1 = 0   CAN2 = 1   CAN3 = 4
 | |
| const int can_numbering[] = {0,1,4};
 | |
| 
 | |
| struct panda_inf_priv *
 | |
| panda_get_inf_from_bus_id(struct panda_dev_priv *priv_dev, int bus_id){
 | |
|   int inf_num;
 | |
|   for(inf_num = 0; inf_num < PANDA_NUM_CAN_INTERFACES; inf_num++)
 | |
|     if(can_numbering[inf_num] == bus_id)
 | |
|       return priv_dev->interfaces[inf_num];
 | |
|   return NULL;
 | |
| }
 | |
| 
 | |
| // CTX handling shamlessly ripped from mcba_usb.c linux driver
 | |
| static inline void panda_init_ctx(struct panda_inf_priv *priv)
 | |
| {
 | |
|   int i = 0;
 | |
| 
 | |
|   for (i = 0; i < PANDA_MAX_TX_URBS; i++) {
 | |
|     priv->tx_context[i].ndx = PANDA_CTX_FREE;
 | |
|     priv->tx_context[i].priv = priv;
 | |
|   }
 | |
| 
 | |
|   atomic_set(&priv->free_ctx_cnt, ARRAY_SIZE(priv->tx_context));
 | |
| }
 | |
| 
 | |
| static inline struct panda_usb_ctx *panda_usb_get_free_ctx(struct panda_inf_priv *priv,
 | |
| 							 struct can_frame *cf)
 | |
| {
 | |
|   int i = 0;
 | |
|   struct panda_usb_ctx *ctx = NULL;
 | |
| 
 | |
|   for (i = 0; i < PANDA_MAX_TX_URBS; i++) {
 | |
|     if (priv->tx_context[i].ndx == PANDA_CTX_FREE) {
 | |
|       ctx = &priv->tx_context[i];
 | |
|       ctx->ndx = i;
 | |
|       ctx->dlc = cf->can_dlc;
 | |
| 
 | |
|       atomic_dec(&priv->free_ctx_cnt);
 | |
|       break;
 | |
|     }
 | |
|   }
 | |
| 
 | |
|   printk("CTX num %d\n", atomic_read(&priv->free_ctx_cnt));
 | |
|   if (!atomic_read(&priv->free_ctx_cnt)){
 | |
|     /* That was the last free ctx. Slow down tx path */
 | |
|     printk("SENDING TOO FAST\n");
 | |
|     netif_stop_queue(priv->netdev);
 | |
|   }
 | |
| 
 | |
|   return ctx;
 | |
| }
 | |
| 
 | |
| /* panda_usb_free_ctx and panda_usb_get_free_ctx are executed by different
 | |
|  * threads. The order of execution in below function is important.
 | |
|  */
 | |
| static inline void panda_usb_free_ctx(struct panda_usb_ctx *ctx)
 | |
| {
 | |
|   /* Increase number of free ctxs before freeing ctx */
 | |
|   atomic_inc(&ctx->priv->free_ctx_cnt);
 | |
| 
 | |
|   ctx->ndx = PANDA_CTX_FREE;
 | |
| 
 | |
|   /* Wake up the queue once ctx is marked free */
 | |
|   netif_wake_queue(ctx->priv->netdev);
 | |
| }
 | |
| 
 | |
| 
 | |
| 
 | |
| static void panda_urb_unlink(struct panda_inf_priv *priv)
 | |
| {
 | |
|   usb_kill_anchored_urbs(&priv->priv_dev->rx_submitted);
 | |
|   usb_kill_anchored_urbs(&priv->tx_submitted);
 | |
| }
 | |
| 
 | |
| static int panda_set_output_enable(struct panda_inf_priv* priv, bool enable){
 | |
|   return usb_control_msg(priv->priv_dev->udev,
 | |
| 			 usb_sndctrlpipe(priv->priv_dev->udev, 0),
 | |
| 			 0xDC, USB_TYPE_VENDOR | USB_RECIP_DEVICE,
 | |
| 			 enable ? 0x1337 : 0, 0, NULL, 0, USB_CTRL_SET_TIMEOUT);
 | |
| }
 | |
| 
 | |
| static void panda_usb_write_bulk_callback(struct urb *urb)
 | |
| {
 | |
|   struct panda_usb_ctx *ctx = urb->context;
 | |
|   struct net_device *netdev;
 | |
| 
 | |
|   WARN_ON(!ctx);
 | |
| 
 | |
|   netdev = ctx->priv->netdev;
 | |
| 
 | |
|   /* free up our allocated buffer */
 | |
|   usb_free_coherent(urb->dev, urb->transfer_buffer_length,
 | |
| 		    urb->transfer_buffer, urb->transfer_dma);
 | |
| 
 | |
|   if (!netif_device_present(netdev))
 | |
|     return;
 | |
| 
 | |
|   netdev->stats.tx_packets++;
 | |
|   netdev->stats.tx_bytes += ctx->dlc;
 | |
| 
 | |
|   can_get_echo_skb(netdev, ctx->ndx);
 | |
| 
 | |
|   if (urb->status)
 | |
|     netdev_info(netdev, "Tx URB aborted (%d)\n", urb->status);
 | |
| 
 | |
|   /* Release the context */
 | |
|   panda_usb_free_ctx(ctx);
 | |
| }
 | |
| 
 | |
| 
 | |
| static netdev_tx_t panda_usb_xmit(struct panda_inf_priv *priv,
 | |
| 				  struct panda_usb_can_msg *usb_msg,
 | |
| 				  struct panda_usb_ctx *ctx)
 | |
| {
 | |
|   struct urb *urb;
 | |
|   u8 *buf;
 | |
|   int err;
 | |
| 
 | |
|   /* create a URB, and a buffer for it, and copy the data to the URB */
 | |
|   urb = usb_alloc_urb(0, GFP_ATOMIC);
 | |
|   if (!urb)
 | |
|     return -ENOMEM;
 | |
| 
 | |
|   buf = usb_alloc_coherent(priv->priv_dev->udev,
 | |
| 			   PANDA_USB_TX_BUFF_SIZE, GFP_ATOMIC,
 | |
| 			   &urb->transfer_dma);
 | |
|   if (!buf) {
 | |
|     err = -ENOMEM;
 | |
|     goto nomembuf;
 | |
|   }
 | |
| 
 | |
|   memcpy(buf, usb_msg, PANDA_USB_TX_BUFF_SIZE);
 | |
| 
 | |
|   usb_fill_bulk_urb(urb, priv->priv_dev->udev,
 | |
| 		    usb_sndbulkpipe(priv->priv_dev->udev, 3), buf,
 | |
| 		    PANDA_USB_TX_BUFF_SIZE, panda_usb_write_bulk_callback,
 | |
| 		    ctx);
 | |
| 
 | |
|   urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 | |
|   usb_anchor_urb(urb, &priv->tx_submitted);
 | |
| 
 | |
|   err = usb_submit_urb(urb, GFP_ATOMIC);
 | |
|   if (unlikely(err))
 | |
|     goto failed;
 | |
| 
 | |
|   /* Release our reference to this URB, the USB core will eventually free it entirely. */
 | |
|   usb_free_urb(urb);
 | |
| 
 | |
|   return 0;
 | |
| 
 | |
|  failed:
 | |
|   usb_unanchor_urb(urb);
 | |
|   usb_free_coherent(priv->priv_dev->udev, PANDA_USB_TX_BUFF_SIZE, buf, urb->transfer_dma);
 | |
| 
 | |
|   if (err == -ENODEV)
 | |
|     netif_device_detach(priv->netdev);
 | |
|   else
 | |
|     netdev_warn(priv->netdev, "failed tx_urb %d\n", err);
 | |
| 
 | |
|  nomembuf:
 | |
|   usb_free_urb(urb);
 | |
| 
 | |
|   return err;
 | |
| }
 | |
| 
 | |
| static void panda_usb_process_can_rx(struct panda_dev_priv *priv_dev,
 | |
| 				     struct panda_usb_can_msg *msg)
 | |
| {
 | |
|   struct can_frame *cf;
 | |
|   struct sk_buff *skb;
 | |
|   int bus_num;
 | |
|   struct panda_inf_priv *priv_inf;
 | |
|   struct net_device_stats *stats;
 | |
| 
 | |
|   bus_num = (msg->bus_dat_len >> 4) & 0xf;
 | |
|   priv_inf = panda_get_inf_from_bus_id(priv_dev, bus_num);
 | |
|   if(!priv_inf){
 | |
|     printk("Got something on an unused interface %d\n", bus_num);
 | |
|     return;
 | |
|   }
 | |
|   printk("Recv bus %d\n", bus_num);
 | |
| 
 | |
|   stats = &priv_inf->netdev->stats;
 | |
|   //u16 sid;
 | |
| 
 | |
|   if (!netif_device_present(priv_inf->netdev))
 | |
|     return;
 | |
| 
 | |
|   skb = alloc_can_skb(priv_inf->netdev, &cf);
 | |
|   if (!skb)
 | |
|     return;
 | |
| 
 | |
|   if(msg->rir & PANDA_CAN_EXTENDED){
 | |
|     cf->can_id = (msg->rir >> 3) | CAN_EFF_FLAG;
 | |
|   }else{
 | |
|     cf->can_id = (msg->rir >> 21);
 | |
|   }
 | |
| 
 | |
|   // TODO: Handle Remote Frames
 | |
|   //if (msg->dlc & MCBA_DLC_RTR_MASK)
 | |
|   //  cf->can_id |= CAN_RTR_FLAG;
 | |
| 
 | |
|   cf->can_dlc = get_can_dlc(msg->bus_dat_len & PANDA_DLC_MASK);
 | |
| 
 | |
|   memcpy(cf->data, msg->data, cf->can_dlc);
 | |
| 
 | |
|   stats->rx_packets++;
 | |
|   stats->rx_bytes += cf->can_dlc;
 | |
| 
 | |
|   netif_rx(skb);
 | |
| }
 | |
| 
 | |
| static void panda_usb_read_int_callback(struct urb *urb)
 | |
| {
 | |
|   struct panda_dev_priv *priv_dev = urb->context;
 | |
|   int retval;
 | |
|   int pos = 0;
 | |
|   int inf_num;
 | |
| 
 | |
|   switch (urb->status) {
 | |
|   case 0: /* success */
 | |
|     break;
 | |
|   case -ENOENT:
 | |
|   case -ESHUTDOWN:
 | |
|     return;
 | |
|   default:
 | |
|     dev_info(priv_dev->dev, "Rx URB aborted (%d)\n", urb->status);
 | |
|     goto resubmit_urb;
 | |
|   }
 | |
| 
 | |
|   while (pos < urb->actual_length) {
 | |
|     struct panda_usb_can_msg *msg;
 | |
| 
 | |
|     if (pos + sizeof(struct panda_usb_can_msg) > urb->actual_length) {
 | |
|       dev_err(priv_dev->dev, "format error\n");
 | |
|       break;
 | |
|     }
 | |
| 
 | |
|     msg = (struct panda_usb_can_msg *)(urb->transfer_buffer + pos);
 | |
| 
 | |
|     panda_usb_process_can_rx(priv_dev, msg);
 | |
| 
 | |
|     pos += sizeof(struct panda_usb_can_msg);
 | |
|   }
 | |
| 
 | |
|  resubmit_urb:
 | |
|   usb_fill_int_urb(urb, priv_dev->udev,
 | |
| 		    usb_rcvintpipe(priv_dev->udev, 1),
 | |
| 		    urb->transfer_buffer, PANDA_USB_RX_BUFF_SIZE,
 | |
| 		    panda_usb_read_int_callback, priv_dev, 5);
 | |
| 
 | |
|   retval = usb_submit_urb(urb, GFP_ATOMIC);
 | |
| 
 | |
|   if (retval == -ENODEV){
 | |
|     for(inf_num = 0; inf_num < PANDA_NUM_CAN_INTERFACES; inf_num++)
 | |
|       if(priv_dev->interfaces[inf_num])
 | |
| 	netif_device_detach(priv_dev->interfaces[inf_num]->netdev);
 | |
|   }else if (retval)
 | |
|     dev_err(priv_dev->dev, "failed resubmitting read bulk urb: %d\n", retval);
 | |
| }
 | |
| 
 | |
| 
 | |
| static int panda_usb_start(struct panda_dev_priv *priv_dev)
 | |
| {
 | |
|   int err;
 | |
|   struct urb *urb = NULL;
 | |
|   u8 *buf;
 | |
|   int inf_num;
 | |
| 
 | |
|   for(inf_num = 0; inf_num < PANDA_NUM_CAN_INTERFACES; inf_num++)
 | |
|     panda_init_ctx(priv_dev->interfaces[inf_num]);
 | |
| 
 | |
|   err = usb_set_interface(priv_dev->udev, 0, 1);
 | |
|   if (err) {
 | |
|     dev_err(priv_dev->dev, "Can not set alternate setting to 1, error: %i", err);
 | |
|     return err;
 | |
|   }
 | |
| 
 | |
|   /* create a URB, and a buffer for it */
 | |
|   urb = usb_alloc_urb(0, GFP_KERNEL);
 | |
|   if (!urb) {
 | |
|     return -ENOMEM;
 | |
|   }
 | |
| 
 | |
|   buf = usb_alloc_coherent(priv_dev->udev, PANDA_USB_RX_BUFF_SIZE,
 | |
| 			   GFP_KERNEL, &urb->transfer_dma);
 | |
|   if (!buf) {
 | |
|     dev_err(priv_dev->dev, "No memory left for USB buffer\n");
 | |
|     usb_free_urb(urb);
 | |
|     return -ENOMEM;
 | |
|   }
 | |
| 
 | |
|   usb_fill_int_urb(urb, priv_dev->udev,
 | |
|                    usb_rcvintpipe(priv_dev->udev, 1),
 | |
|                    buf, PANDA_USB_RX_BUFF_SIZE,
 | |
|                    panda_usb_read_int_callback, priv_dev, 5);
 | |
|   urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 | |
| 
 | |
|   usb_anchor_urb(urb, &priv_dev->rx_submitted);
 | |
| 
 | |
|   err = usb_submit_urb(urb, GFP_KERNEL);
 | |
|   if (err) {
 | |
|   usb_unanchor_urb(urb);
 | |
|     usb_free_coherent(priv_dev->udev, PANDA_USB_RX_BUFF_SIZE,
 | |
| 		      buf, urb->transfer_dma);
 | |
|     usb_free_urb(urb);
 | |
|     dev_err(priv_dev->dev, "Failed in start, while submitting urb.\n");
 | |
|     return err;
 | |
|   }
 | |
| 
 | |
|   /* Drop reference, USB core will take care of freeing it */
 | |
|   usb_free_urb(urb);
 | |
| 
 | |
| 
 | |
|   return 0;
 | |
| }
 | |
| 
 | |
| /* Open USB device */
 | |
| static int panda_usb_open(struct net_device *netdev)
 | |
| {
 | |
|   struct panda_inf_priv *priv = netdev_priv(netdev);
 | |
|   int err;
 | |
| 
 | |
|   /* common open */
 | |
|   err = open_candev(netdev);
 | |
|   if (err)
 | |
|     return err;
 | |
| 
 | |
|   //priv->can_speed_check = true;
 | |
|   priv->can.state = CAN_STATE_ERROR_ACTIVE;
 | |
| 
 | |
|   netif_start_queue(netdev);
 | |
| 
 | |
|   return 0;
 | |
| }
 | |
| 
 | |
| /* Close USB device */
 | |
| static int panda_usb_close(struct net_device *netdev)
 | |
| {
 | |
|   struct panda_inf_priv *priv = netdev_priv(netdev);
 | |
| 
 | |
|   priv->can.state = CAN_STATE_STOPPED;
 | |
| 
 | |
|   netif_stop_queue(netdev);
 | |
| 
 | |
|   /* Stop polling */
 | |
|   panda_urb_unlink(priv);
 | |
| 
 | |
|   close_candev(netdev);
 | |
| 
 | |
|   return 0;
 | |
| }
 | |
| 
 | |
| static netdev_tx_t panda_usb_start_xmit(struct sk_buff *skb,
 | |
| 					struct net_device *netdev)
 | |
| {
 | |
|   struct panda_inf_priv *priv_inf = netdev_priv(netdev);
 | |
|   struct can_frame *cf = (struct can_frame *)skb->data;
 | |
|   struct panda_usb_ctx *ctx = NULL;
 | |
|   struct net_device_stats *stats = &priv_inf->netdev->stats;
 | |
|   int err;
 | |
|   struct panda_usb_can_msg usb_msg = {};
 | |
|   int bus = priv_inf->mcu_can_ifnum;
 | |
| 
 | |
|   if (can_dropped_invalid_skb(netdev, skb)){
 | |
|     printk("Invalid CAN packet");
 | |
|     return NETDEV_TX_OK;
 | |
|   }
 | |
| 
 | |
|   ctx = panda_usb_get_free_ctx(priv_inf, cf);
 | |
| 
 | |
|   //Warning: cargo cult. Can't tell what this is for, but it is
 | |
|   //everywhere and encouraged in the documentation.
 | |
|   can_put_echo_skb(skb, priv_inf->netdev, ctx->ndx);
 | |
| 
 | |
|   if(cf->can_id & CAN_EFF_FLAG){
 | |
|     usb_msg.rir = cpu_to_le32(((cf->can_id & 0x1FFFFFFF) << 3) |
 | |
| 			      PANDA_CAN_TRANSMIT | PANDA_CAN_EXTENDED);
 | |
|   }else{
 | |
|     usb_msg.rir = cpu_to_le32(((cf->can_id & 0x7FF) << 21) | PANDA_CAN_TRANSMIT);
 | |
|   }
 | |
|   usb_msg.bus_dat_len = cpu_to_le32((cf->can_dlc & 0x0F) | (bus << 4));
 | |
| 
 | |
|   memcpy(usb_msg.data, cf->data, cf->can_dlc);
 | |
| 
 | |
|   //TODO Handle Remote Frames
 | |
|   //if (cf->can_id & CAN_RTR_FLAG)
 | |
|   //  usb_msg.dlc |= PANDA_DLC_RTR_MASK;
 | |
| 
 | |
|   netdev_err(netdev, "Received data from socket. canid: %x; len: %d\n", cf->can_id, cf->can_dlc);
 | |
| 
 | |
|   err = panda_usb_xmit(priv_inf, &usb_msg, ctx);
 | |
|   if (err)
 | |
|     goto xmit_failed;
 | |
| 
 | |
|   return NETDEV_TX_OK;
 | |
| 
 | |
|  xmit_failed:
 | |
|   can_free_echo_skb(priv_inf->netdev, ctx->ndx);
 | |
|   panda_usb_free_ctx(ctx);
 | |
|   dev_kfree_skb(skb);
 | |
|   stats->tx_dropped++;
 | |
| 
 | |
|   return NETDEV_TX_OK;
 | |
| }
 | |
| 
 | |
| static const struct net_device_ops panda_netdev_ops = {
 | |
|   .ndo_open = panda_usb_open,
 | |
|   .ndo_stop = panda_usb_close,
 | |
|   .ndo_start_xmit = panda_usb_start_xmit,
 | |
| };
 | |
| 
 | |
| static int panda_usb_probe(struct usb_interface *intf,
 | |
| 			   const struct usb_device_id *id)
 | |
| {
 | |
|   struct net_device *netdev;
 | |
|   struct panda_inf_priv *priv_inf;
 | |
|   int err = -ENOMEM;
 | |
|   int inf_num;
 | |
|   struct panda_dev_priv *priv_dev;
 | |
|   struct usb_device *usbdev = interface_to_usbdev(intf);
 | |
| 
 | |
|   priv_dev = kzalloc(sizeof(struct panda_dev_priv), GFP_KERNEL);
 | |
|   if (!priv_dev) {
 | |
|     dev_err(&intf->dev, "Couldn't alloc priv_dev\n");
 | |
|     return -ENOMEM;
 | |
|   }
 | |
|   priv_dev->udev = usbdev;
 | |
|   priv_dev->dev = &intf->dev;
 | |
|   usb_set_intfdata(intf, priv_dev);
 | |
| 
 | |
|   ////// Interface privs
 | |
|   for(inf_num = 0; inf_num < PANDA_NUM_CAN_INTERFACES; inf_num++){
 | |
|     netdev = alloc_candev(sizeof(struct panda_inf_priv), PANDA_MAX_TX_URBS);
 | |
|     if (!netdev) {
 | |
|       dev_err(&intf->dev, "Couldn't alloc candev\n");
 | |
|       goto cleanup_candev;
 | |
|     }
 | |
|     netdev->netdev_ops = &panda_netdev_ops;
 | |
|     netdev->flags |= IFF_ECHO; /* we support local echo */
 | |
| 
 | |
|     priv_inf = netdev_priv(netdev);
 | |
|     priv_inf->netdev = netdev;
 | |
|     priv_inf->priv_dev = priv_dev;
 | |
|     priv_inf->interface_num = inf_num;
 | |
|     priv_inf->mcu_can_ifnum = can_numbering[inf_num];
 | |
| 
 | |
|     init_usb_anchor(&priv_dev->rx_submitted);
 | |
|     init_usb_anchor(&priv_inf->tx_submitted);
 | |
| 
 | |
|     /* Init CAN device */
 | |
|     priv_inf->can.state = CAN_STATE_STOPPED;
 | |
|     priv_inf->can.bittiming.bitrate = PANDA_BITRATE;
 | |
| 
 | |
|     SET_NETDEV_DEV(netdev, &intf->dev);
 | |
| 
 | |
|     err = register_candev(netdev);
 | |
|     if (err) {
 | |
|       netdev_err(netdev, "couldn't register PANDA CAN device: %d\n", err);
 | |
|       free_candev(priv_inf->netdev);
 | |
|       goto cleanup_candev;
 | |
|     }
 | |
| 
 | |
|     priv_dev->interfaces[inf_num] = priv_inf;
 | |
|   }
 | |
| 
 | |
|   err = panda_usb_start(priv_dev);
 | |
|   if (err) {
 | |
|     dev_err(&intf->dev, "Failed to initialize Comma.ai Panda CAN controller\n");
 | |
|     goto cleanup_candev;
 | |
|   }
 | |
| 
 | |
|   err = panda_set_output_enable(priv_inf, true);
 | |
|   if (err) {
 | |
|     dev_info(&intf->dev, "Failed to initialize send enable message to Panda.\n");
 | |
|     goto cleanup_candev;
 | |
|   }
 | |
| 
 | |
|   dev_info(&intf->dev, "Comma.ai Panda CAN controller connected\n");
 | |
| 
 | |
|   return 0;
 | |
| 
 | |
|  cleanup_candev:
 | |
|   for(inf_num = 0; inf_num < PANDA_NUM_CAN_INTERFACES; inf_num++){
 | |
|     priv_inf = priv_dev->interfaces[inf_num];
 | |
|     if(priv_inf){
 | |
|       unregister_candev(priv_inf->netdev);
 | |
|       free_candev(priv_inf->netdev);
 | |
|     }else
 | |
|       break;
 | |
|   }
 | |
| 
 | |
|   kfree(priv_dev);
 | |
| 
 | |
|   return err;
 | |
| }
 | |
| 
 | |
| /* Called by the usb core when driver is unloaded or device is removed */
 | |
| static void panda_usb_disconnect(struct usb_interface *intf)
 | |
| {
 | |
|   struct panda_dev_priv *priv_dev = usb_get_intfdata(intf);
 | |
|   struct panda_inf_priv *priv_inf;
 | |
|   int inf_num;
 | |
| 
 | |
|   usb_set_intfdata(intf, NULL);
 | |
| 
 | |
|   for(inf_num = 0; inf_num < PANDA_NUM_CAN_INTERFACES; inf_num++){
 | |
|     priv_inf = priv_dev->interfaces[inf_num];
 | |
|     if(priv_inf){
 | |
|       netdev_info(priv_inf->netdev, "device disconnected\n");
 | |
|       unregister_candev(priv_inf->netdev);
 | |
|       free_candev(priv_inf->netdev);
 | |
|     }else
 | |
|       break;
 | |
|   }
 | |
| 
 | |
|   panda_urb_unlink(priv_inf);
 | |
|   kfree(priv_dev);
 | |
| }
 | |
| 
 | |
| static struct usb_driver panda_usb_driver = {
 | |
|   .name = PANDA_MODULE_NAME,
 | |
|   .probe = panda_usb_probe,
 | |
|   .disconnect = panda_usb_disconnect,
 | |
|   .id_table = panda_usb_table,
 | |
| };
 | |
| 
 | |
| module_usb_driver(panda_usb_driver);
 | |
| 
 | |
| MODULE_LICENSE("GPL");
 | |
| MODULE_AUTHOR("Jessy Diamond Exum <jessy.diamondman@gmail.com>");
 | |
| MODULE_DESCRIPTION("SocketCAN driver for Comma.ai's Panda Adapter.");
 | |
| MODULE_VERSION("0.1");
 | |
| 
 |