1 files changed, 38 insertions, 26 deletions

diff --git a/drivers/net/can/sja1000/sja1000.c b/drivers/net/can/sja1000/sja1000.c
index 16d2ecd2a3b7..145b1a731a53 100644
--- a/drivers/net/can/sja1000/sja1000.c
+++ b/drivers/net/can/sja1000/sja1000.c
@@ -130,8 +130,12 @@ static void set_normal_mode(struct net_device *dev)
                 /* check reset bit */
                 if ((status & MOD_RM) == 0) {
                         priv->can.state = CAN_STATE_ERROR_ACTIVE;
-                        /* enable all interrupts */
-                        priv->write_reg(priv, REG_IER, IRQ_ALL);
+                        /* enable interrupts */
+                        if (priv->can.ctrlmode & CAN_CTRLMODE_BERR_REPORTING)
+                                priv->write_reg(priv, REG_IER, IRQ_ALL);
+                        else
+                                priv->write_reg(priv, REG_IER,
+                                                IRQ_ALL & ~IRQ_BEI);
                         return;
                 }
 
@@ -203,6 +207,17 @@ static int sja1000_set_bittiming(struct net_device *dev)
         return 0;
 }
 
+static int sja1000_get_berr_counter(const struct net_device *dev,
+                                    struct can_berr_counter *bec)
+{
+        struct sja1000_priv *priv = netdev_priv(dev);
+
+        bec->txerr = priv->read_reg(priv, REG_TXERR);
+        bec->rxerr = priv->read_reg(priv, REG_RXERR);
+
+        return 0;
+}
+
 /*
  * initialize SJA1000 chip:
  *   - reset chip
@@ -249,6 +264,9 @@ static netdev_tx_t sja1000_start_xmit(struct sk_buff *skb,
         uint8_t dreg;
         int i;
 
+        if (can_dropped_invalid_skb(dev, skb))
+                return NETDEV_TX_OK;
+
         netif_stop_queue(dev);
 
         fi = dlc = cf->can_dlc;
@@ -293,17 +311,14 @@ static void sja1000_rx(struct net_device *dev)
         uint8_t fi;
         uint8_t dreg;
         canid_t id;
-        uint8_t dlc;
         int i;
 
-        skb = dev_alloc_skb(sizeof(struct can_frame));
+        /* create zero'ed CAN frame buffer */
+        skb = alloc_can_skb(dev, &cf);
         if (skb == NULL)
                 return;
-        skb->dev = dev;
-        skb->protocol = htons(ETH_P_CAN);
 
         fi = priv->read_reg(priv, REG_FI);
-        dlc = fi & 0x0F;
 
         if (fi & FI_FF) {
                 /* extended frame format (EFF) */
@@ -320,18 +335,15 @@ static void sja1000_rx(struct net_device *dev)
                     | (priv->read_reg(priv, REG_ID2) >> 5);
         }
 
-        if (fi & FI_RTR)
+        if (fi & FI_RTR) {
                 id |= CAN_RTR_FLAG;
+        } else {
+                cf->can_dlc = get_can_dlc(fi & 0x0F);
+                for (i = 0; i < cf->can_dlc; i++)
+                        cf->data[i] = priv->read_reg(priv, dreg++);
+        }
 
-        cf = (struct can_frame *)skb_put(skb, sizeof(struct can_frame));
-        memset(cf, 0, sizeof(struct can_frame));
         cf->can_id = id;
-        cf->can_dlc = dlc;
-        for (i = 0; i < dlc; i++)
-                cf->data[i] = priv->read_reg(priv, dreg++);
-
-        while (i < 8)
-                cf->data[i++] = 0;
 
         /* release receive buffer */
         priv->write_reg(priv, REG_CMR, CMD_RRB);
@@ -339,7 +351,7 @@ static void sja1000_rx(struct net_device *dev)
         netif_rx(skb);
 
         stats->rx_packets++;
-        stats->rx_bytes += dlc;
+        stats->rx_bytes += cf->can_dlc;
 }
 
 static int sja1000_err(struct net_device *dev, uint8_t isrc, uint8_t status)
@@ -351,15 +363,9 @@ static int sja1000_err(struct net_device *dev, uint8_t isrc, uint8_t status)
         enum can_state state = priv->can.state;
         uint8_t ecc, alc;
 
-        skb = dev_alloc_skb(sizeof(struct can_frame));
+        skb = alloc_can_err_skb(dev, &cf);
         if (skb == NULL)
                 return -ENOMEM;
-        skb->dev = dev;
-        skb->protocol = htons(ETH_P_CAN);
-        cf = (struct can_frame *)skb_put(skb, sizeof(struct can_frame));
-        memset(cf, 0, sizeof(struct can_frame));
-        cf->can_id = CAN_ERR_FLAG;
-        cf->can_dlc = CAN_ERR_DLC;
 
         if (isrc & IRQ_DOI) {
                 /* data overrun interrupt */
@@ -446,6 +452,8 @@ static int sja1000_err(struct net_device *dev, uint8_t isrc, uint8_t status)
                                 CAN_ERR_CRTL_TX_PASSIVE :
                                 CAN_ERR_CRTL_RX_PASSIVE;
                 }
+                cf->data[6] = txerr;
+                cf->data[7] = rxerr;
         }
 
         priv->can.state = state;
@@ -526,7 +534,7 @@ static int sja1000_open(struct net_device *dev)
 
         /* register interrupt handler, if not done by the device driver */
         if (!(priv->flags & SJA1000_CUSTOM_IRQ_HANDLER)) {
-                err = request_irq(dev->irq, &sja1000_interrupt, priv->irq_flags,
+                err = request_irq(dev->irq, sja1000_interrupt, priv->irq_flags,
                                   dev->name, (void *)dev);
                 if (err) {
                         close_candev(dev);
@@ -565,7 +573,8 @@ struct net_device *alloc_sja1000dev(int sizeof_priv)
         struct net_device *dev;
         struct sja1000_priv *priv;
 
-        dev = alloc_candev(sizeof(struct sja1000_priv) + sizeof_priv);
+        dev = alloc_candev(sizeof(struct sja1000_priv) + sizeof_priv,
+                SJA1000_ECHO_SKB_MAX);
         if (!dev)
                 return NULL;
 
@@ -575,6 +584,9 @@ struct net_device *alloc_sja1000dev(int sizeof_priv)
         priv->can.bittiming_const = &sja1000_bittiming_const;
         priv->can.do_set_bittiming = sja1000_set_bittiming;
         priv->can.do_set_mode = sja1000_set_mode;
+        priv->can.do_get_berr_counter = sja1000_get_berr_counter;
+        priv->can.ctrlmode_supported = CAN_CTRLMODE_3_SAMPLES |
+                CAN_CTRLMODE_BERR_REPORTING;
 
         if (sizeof_priv)
                 priv->priv = (void *)priv + sizeof(struct sja1000_priv);