1 files changed, 35 insertions, 13 deletions

diff --git a/drivers/net/can/dev.c b/drivers/net/can/dev.c
index 49163570a63a..3b3f88ffab53 100644
--- a/drivers/net/can/dev.c
+++ b/drivers/net/can/dev.c
@@ -477,6 +477,34 @@ void can_put_echo_skb(struct sk_buff *skb, struct net_device *dev,
 }
 EXPORT_SYMBOL_GPL(can_put_echo_skb);
 
+struct sk_buff *__can_get_echo_skb(struct net_device *dev, unsigned int idx, u8 *len_ptr)
+{
+        struct can_priv *priv = netdev_priv(dev);
+        struct sk_buff *skb = priv->echo_skb[idx];
+        struct canfd_frame *cf;
+
+        if (idx >= priv->echo_skb_max) {
+                netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",
+                           __func__, idx, priv->echo_skb_max);
+                return NULL;
+        }
+
+        if (!skb) {
+                netdev_err(dev, "%s: BUG! Trying to echo non existing skb: can_priv::echo_skb[%u]\n",
+                           __func__, idx);
+                return NULL;
+        }
+
+        /* Using "struct canfd_frame::len" for the frame
+         * length is supported on both CAN and CANFD frames.
+         */
+        cf = (struct canfd_frame *)skb->data;
+        *len_ptr = cf->len;
+        priv->echo_skb[idx] = NULL;
+
+        return skb;
+}
+
 /*
  * Get the skb from the stack and loop it back locally
  *
@@ -486,22 +514,16 @@ EXPORT_SYMBOL_GPL(can_put_echo_skb);
  */
 unsigned int can_get_echo_skb(struct net_device *dev, unsigned int idx)
 {
-        struct can_priv *priv = netdev_priv(dev);
-
-        BUG_ON(idx >= priv->echo_skb_max);
-
-        if (priv->echo_skb[idx]) {
-                struct sk_buff *skb = priv->echo_skb[idx];
-                struct can_frame *cf = (struct can_frame *)skb->data;
-                u8 dlc = cf->can_dlc;
+        struct sk_buff *skb;
+        u8 len;
 
-                netif_rx(priv->echo_skb[idx]);
-                priv->echo_skb[idx] = NULL;
+        skb = __can_get_echo_skb(dev, idx, &len);
+        if (!skb)
+                return 0;
 
-                return dlc;
-        }
+        netif_rx(skb);
 
-        return 0;
+        return len;
 }
 EXPORT_SYMBOL_GPL(can_get_echo_skb);