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.
		
		
		
		
		
			
		
			
				
					
					
						
							616 lines
						
					
					
						
							16 KiB
						
					
					
				
			
		
		
	
	
							616 lines
						
					
					
						
							16 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
 | 
						|
 | 
						|
#define SAFETY_ALLOUTPUT 17
 | 
						|
#define SAFETY_SILENT 0
 | 
						|
 | 
						|
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 ? SAFETY_ALLOUTPUT : SAFETY_SILENT, 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");
 | 
						|
 |