1 files changed, 148 insertions, 35 deletions

diff --git a/drivers/net/can/dev.c b/drivers/net/can/dev.c
index fc59bc6f040b..c7a260478749 100644
--- a/drivers/net/can/dev.c
+++ b/drivers/net/can/dev.c
@@ -99,10 +99,10 @@ static int can_update_spt(const struct can_bittiming_const *btc,
         return 1000 * (tseg + 1 - *tseg2) / (tseg + 1);
 }
 
-static int can_calc_bittiming(struct net_device *dev, struct can_bittiming *bt)
+static int can_calc_bittiming(struct net_device *dev, struct can_bittiming *bt,
+                              const struct can_bittiming_const *btc)
 {
         struct can_priv *priv = netdev_priv(dev);
-        const struct can_bittiming_const *btc = priv->bittiming_const;
         long rate, best_rate = 0;
         long best_error = 1000000000, error = 0;
         int best_tseg = 0, best_brp = 0, brp = 0;
@@ -110,9 +110,6 @@ static int can_calc_bittiming(struct net_device *dev, struct can_bittiming *bt)
         int spt_error = 1000, spt = 0, sampl_pt;
         u64 v64;
 
-        if (!priv->bittiming_const)
-                return -ENOTSUPP;
-
         /* Use CIA recommended sample points */
         if (bt->sample_point) {
                 sampl_pt = bt->sample_point;
@@ -204,7 +201,8 @@ static int can_calc_bittiming(struct net_device *dev, struct can_bittiming *bt)
         return 0;
 }
 #else /* !CONFIG_CAN_CALC_BITTIMING */
-static int can_calc_bittiming(struct net_device *dev, struct can_bittiming *bt)
+static int can_calc_bittiming(struct net_device *dev, struct can_bittiming *bt,
+                              const struct can_bittiming_const *btc)
 {
         netdev_err(dev, "bit-timing calculation not available\n");
         return -EINVAL;
@@ -217,16 +215,13 @@ static int can_calc_bittiming(struct net_device *dev, struct can_bittiming *bt)
  * prescaler value brp. You can find more information in the header
  * file linux/can/netlink.h.
  */
-static int can_fixup_bittiming(struct net_device *dev, struct can_bittiming *bt)
+static int can_fixup_bittiming(struct net_device *dev, struct can_bittiming *bt,
+                               const struct can_bittiming_const *btc)
 {
         struct can_priv *priv = netdev_priv(dev);
-        const struct can_bittiming_const *btc = priv->bittiming_const;
         int tseg1, alltseg;
         u64 brp64;
 
-        if (!priv->bittiming_const)
-                return -ENOTSUPP;
-
         tseg1 = bt->prop_seg + bt->phase_seg1;
         if (!bt->sjw)
                 bt->sjw = 1;
@@ -254,26 +249,29 @@ static int can_fixup_bittiming(struct net_device *dev, struct can_bittiming *bt)
         return 0;
 }
 
-static int can_get_bittiming(struct net_device *dev, struct can_bittiming *bt)
+static int can_get_bittiming(struct net_device *dev, struct can_bittiming *bt,
+                             const struct can_bittiming_const *btc)
 {
-        struct can_priv *priv = netdev_priv(dev);
         int err;
 
         /* Check if the CAN device has bit-timing parameters */
-        if (priv->bittiming_const) {
+        if (!btc)
+                return -ENOTSUPP;
 
-                /* Non-expert mode? Check if the bitrate has been pre-defined */
-                if (!bt->tq)
-                        /* Determine bit-timing parameters */
-                        err = can_calc_bittiming(dev, bt);
-                else
-                        /* Check bit-timing params and calculate proper brp */
-                        err = can_fixup_bittiming(dev, bt);
-                if (err)
-                        return err;
-        }
+        /*
+         * Depending on the given can_bittiming parameter structure the CAN
+         * timing parameters are calculated based on the provided bitrate OR
+         * alternatively the CAN timing parameters (tq, prop_seg, etc.) are
+         * provided directly which are then checked and fixed up.
+         */
+        if (!bt->tq && bt->bitrate)
+                err = can_calc_bittiming(dev, bt, btc);
+        else if (bt->tq && !bt->bitrate)
+                err = can_fixup_bittiming(dev, bt, btc);
+        else
+                err = -EINVAL;
 
-        return 0;
+        return err;
 }
 
 /*
@@ -317,7 +315,9 @@ void can_put_echo_skb(struct sk_buff *skb, struct net_device *dev,
         BUG_ON(idx >= priv->echo_skb_max);
 
         /* check flag whether this packet has to be looped back */
-        if (!(dev->flags & IFF_ECHO) || skb->pkt_type != PACKET_LOOPBACK) {
+        if (!(dev->flags & IFF_ECHO) || skb->pkt_type != PACKET_LOOPBACK ||
+            (skb->protocol != htons(ETH_P_CAN) &&
+             skb->protocol != htons(ETH_P_CANFD))) {
                 kfree_skb(skb);
                 return;
         }
@@ -329,7 +329,6 @@ void can_put_echo_skb(struct sk_buff *skb, struct net_device *dev,
                         return;
 
                 /* make settings for echo to reduce code in irq context */
-                skb->protocol = htons(ETH_P_CAN);
                 skb->pkt_type = PACKET_BROADCAST;
                 skb->ip_summed = CHECKSUM_UNNECESSARY;
                 skb->dev = dev;
@@ -512,6 +511,30 @@ struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf)
 }
 EXPORT_SYMBOL_GPL(alloc_can_skb);
 
+struct sk_buff *alloc_canfd_skb(struct net_device *dev,
+                                struct canfd_frame **cfd)
+{
+        struct sk_buff *skb;
+
+        skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
+                               sizeof(struct canfd_frame));
+        if (unlikely(!skb))
+                return NULL;
+
+        skb->protocol = htons(ETH_P_CANFD);
+        skb->pkt_type = PACKET_BROADCAST;
+        skb->ip_summed = CHECKSUM_UNNECESSARY;
+
+        can_skb_reserve(skb);
+        can_skb_prv(skb)->ifindex = dev->ifindex;
+
+        *cfd = (struct canfd_frame *)skb_put(skb, sizeof(struct canfd_frame));
+        memset(*cfd, 0, sizeof(struct canfd_frame));
+
+        return skb;
+}
+EXPORT_SYMBOL_GPL(alloc_canfd_skb);
+
 struct sk_buff *alloc_can_err_skb(struct net_device *dev, struct can_frame **cf)
 {
         struct sk_buff *skb;
@@ -572,6 +595,39 @@ void free_candev(struct net_device *dev)
 EXPORT_SYMBOL_GPL(free_candev);
 
 /*
+ * changing MTU and control mode for CAN/CANFD devices
+ */
+int can_change_mtu(struct net_device *dev, int new_mtu)
+{
+        struct can_priv *priv = netdev_priv(dev);
+
+        /* Do not allow changing the MTU while running */
+        if (dev->flags & IFF_UP)
+                return -EBUSY;
+
+        /* allow change of MTU according to the CANFD ability of the device */
+        switch (new_mtu) {
+        case CAN_MTU:
+                priv->ctrlmode &= ~CAN_CTRLMODE_FD;
+                break;
+
+        case CANFD_MTU:
+                if (!(priv->ctrlmode_supported & CAN_CTRLMODE_FD))
+                        return -EINVAL;
+
+                priv->ctrlmode |= CAN_CTRLMODE_FD;
+                break;
+
+        default:
+                return -EINVAL;
+        }
+
+        dev->mtu = new_mtu;
+        return 0;
+}
+EXPORT_SYMBOL_GPL(can_change_mtu);
+
+/*
  * Common open function when the device gets opened.
  *
  * This function should be called in the open function of the device
@@ -581,11 +637,19 @@ int open_candev(struct net_device *dev)
 {
         struct can_priv *priv = netdev_priv(dev);
 
-        if (!priv->bittiming.tq && !priv->bittiming.bitrate) {
+        if (!priv->bittiming.bitrate) {
                 netdev_err(dev, "bit-timing not yet defined\n");
                 return -EINVAL;
         }
 
+        /* For CAN FD the data bitrate has to be >= the arbitration bitrate */
+        if ((priv->ctrlmode & CAN_CTRLMODE_FD) &&
+            (!priv->data_bittiming.bitrate ||
+             (priv->data_bittiming.bitrate < priv->bittiming.bitrate))) {
+                netdev_err(dev, "incorrect/missing data bit-timing\n");
+                return -EINVAL;
+        }
+
         /* Switch carrier on if device was stopped while in bus-off state */
         if (!netif_carrier_ok(dev))
                 netif_carrier_on(dev);
@@ -624,6 +688,10 @@ static const struct nla_policy can_policy[IFLA_CAN_MAX + 1] = {
                                 = { .len = sizeof(struct can_bittiming_const) },
         [IFLA_CAN_CLOCK]        = { .len = sizeof(struct can_clock) },
         [IFLA_CAN_BERR_COUNTER] = { .len = sizeof(struct can_berr_counter) },
+        [IFLA_CAN_DATA_BITTIMING]
+                                = { .len = sizeof(struct can_bittiming) },
+        [IFLA_CAN_DATA_BITTIMING_CONST]
+                                = { .len = sizeof(struct can_bittiming_const) },
 };
 
 static int can_changelink(struct net_device *dev,
@@ -642,9 +710,7 @@ static int can_changelink(struct net_device *dev,
                 if (dev->flags & IFF_UP)
                         return -EBUSY;
                 memcpy(&bt, nla_data(data[IFLA_CAN_BITTIMING]), sizeof(bt));
-                if ((!bt.bitrate && !bt.tq) || (bt.bitrate && bt.tq))
-                        return -EINVAL;
-                err = can_get_bittiming(dev, &bt);
+                err = can_get_bittiming(dev, &bt, priv->bittiming_const);
                 if (err)
                         return err;
                 memcpy(&priv->bittiming, &bt, sizeof(bt));
@@ -668,6 +734,12 @@ static int can_changelink(struct net_device *dev,
                         return -EOPNOTSUPP;
                 priv->ctrlmode &= ~cm->mask;
                 priv->ctrlmode |= cm->flags;
+
+                /* CAN_CTRLMODE_FD can only be set when driver supports FD */
+                if (priv->ctrlmode & CAN_CTRLMODE_FD)
+                        dev->mtu = CANFD_MTU;
+                else
+                        dev->mtu = CAN_MTU;
         }
 
         if (data[IFLA_CAN_RESTART_MS]) {
@@ -686,6 +758,27 @@ static int can_changelink(struct net_device *dev,
                         return err;
         }
 
+        if (data[IFLA_CAN_DATA_BITTIMING]) {
+                struct can_bittiming dbt;
+
+                /* Do not allow changing bittiming while running */
+                if (dev->flags & IFF_UP)
+                        return -EBUSY;
+                memcpy(&dbt, nla_data(data[IFLA_CAN_DATA_BITTIMING]),
+                       sizeof(dbt));
+                err = can_get_bittiming(dev, &dbt, priv->data_bittiming_const);
+                if (err)
+                        return err;
+                memcpy(&priv->data_bittiming, &dbt, sizeof(dbt));
+
+                if (priv->do_set_data_bittiming) {
+                        /* Finally, set the bit-timing registers */
+                        err = priv->do_set_data_bittiming(dev);
+                        if (err)
+                                return err;
+                }
+        }
+
         return 0;
 }
 
@@ -694,7 +787,8 @@ static size_t can_get_size(const struct net_device *dev)
         struct can_priv *priv = netdev_priv(dev);
         size_t size = 0;
 
-        size += nla_total_size(sizeof(struct can_bittiming));   /* IFLA_CAN_BITTIMING */
+        if (priv->bittiming.bitrate)                            /* IFLA_CAN_BITTIMING */
+                size += nla_total_size(sizeof(struct can_bittiming));
         if (priv->bittiming_const)                              /* IFLA_CAN_BITTIMING_CONST */
                 size += nla_total_size(sizeof(struct can_bittiming_const));
         size += nla_total_size(sizeof(struct can_clock));       /* IFLA_CAN_CLOCK */
@@ -703,6 +797,10 @@ static size_t can_get_size(const struct net_device *dev)
         size += nla_total_size(sizeof(u32));                    /* IFLA_CAN_RESTART_MS */
         if (priv->do_get_berr_counter)                          /* IFLA_CAN_BERR_COUNTER */
                 size += nla_total_size(sizeof(struct can_berr_counter));
+        if (priv->data_bittiming.bitrate)                       /* IFLA_CAN_DATA_BITTIMING */
+                size += nla_total_size(sizeof(struct can_bittiming));
+        if (priv->data_bittiming_const)                         /* IFLA_CAN_DATA_BITTIMING_CONST */
+                size += nla_total_size(sizeof(struct can_bittiming_const));
 
         return size;
 }
@@ -716,19 +814,34 @@ static int can_fill_info(struct sk_buff *skb, const struct net_device *dev)
 
         if (priv->do_get_state)
                 priv->do_get_state(dev, &state);
-        if (nla_put(skb, IFLA_CAN_BITTIMING,
-                    sizeof(priv->bittiming), &priv->bittiming) ||
+
+        if ((priv->bittiming.bitrate &&
+             nla_put(skb, IFLA_CAN_BITTIMING,
+                     sizeof(priv->bittiming), &priv->bittiming)) ||
+
             (priv->bittiming_const &&
              nla_put(skb, IFLA_CAN_BITTIMING_CONST,
                      sizeof(*priv->bittiming_const), priv->bittiming_const)) ||
+
             nla_put(skb, IFLA_CAN_CLOCK, sizeof(cm), &priv->clock) ||
             nla_put_u32(skb, IFLA_CAN_STATE, state) ||
             nla_put(skb, IFLA_CAN_CTRLMODE, sizeof(cm), &cm) ||
             nla_put_u32(skb, IFLA_CAN_RESTART_MS, priv->restart_ms) ||
+
             (priv->do_get_berr_counter &&
              !priv->do_get_berr_counter(dev, &bec) &&
-             nla_put(skb, IFLA_CAN_BERR_COUNTER, sizeof(bec), &bec)))
+             nla_put(skb, IFLA_CAN_BERR_COUNTER, sizeof(bec), &bec)) ||
+
+            (priv->data_bittiming.bitrate &&
+             nla_put(skb, IFLA_CAN_DATA_BITTIMING,
+                     sizeof(priv->data_bittiming), &priv->data_bittiming)) ||
+
+            (priv->data_bittiming_const &&
+             nla_put(skb, IFLA_CAN_DATA_BITTIMING_CONST,
+                     sizeof(*priv->data_bittiming_const),
+                     priv->data_bittiming_const)))
                 return -EMSGSIZE;
+
         return 0;
 }