drivers/net/can/dev/skb.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /* Copyright (C) 2005 Marc Kleine-Budde, Pengutronix
   3  * Copyright (C) 2006 Andrey Volkov, Varma Electronics
   4  * Copyright (C) 2008-2009 Wolfgang Grandegger <[email protected]>
   5  */
   6
   7 #include <linux/can/dev.h>
   8
   9 /* Local echo of CAN messages
  10  *
  11  * CAN network devices *should* support a local echo functionality
  12  * (see Documentation/networking/can.rst). To test the handling of CAN
  13  * interfaces that do not support the local echo both driver types are
  14  * implemented. In the case that the driver does not support the echo
  15  * the IFF_ECHO remains clear in dev->flags. This causes the PF_CAN core
  16  * to perform the echo as a fallback solution.
  17  */
  18 void can_flush_echo_skb(struct net_device *dev)
  19 {
  20         struct can_priv *priv = netdev_priv(dev);
  21         struct net_device_stats *stats = &dev->stats;
  22         int i;
  23
  24         for (i = 0; i < priv->echo_skb_max; i++) {
  25                 if (priv->echo_skb[i]) {
  26                         kfree_skb(priv->echo_skb[i]);
  27                         priv->echo_skb[i] = NULL;
  28                         stats->tx_dropped++;
  29                         stats->tx_aborted_errors++;
  30                 }
  31         }
  32 }
  33
  34 /* Put the skb on the stack to be looped backed locally lateron
  35  *
  36  * The function is typically called in the start_xmit function
  37  * of the device driver. The driver must protect access to
  38  * priv->echo_skb, if necessary.
  39  */
  40 int can_put_echo_skb(struct sk_buff *skb, struct net_device *dev,
  41                      unsigned int idx, unsigned int frame_len)
  42 {
  43         struct can_priv *priv = netdev_priv(dev);
  44
  45         BUG_ON(idx >= priv->echo_skb_max);
  46
  47         /* check flag whether this packet has to be looped back */
  48         if (!(dev->flags & IFF_ECHO) ||
  49             (skb->protocol != htons(ETH_P_CAN) &&
  50              skb->protocol != htons(ETH_P_CANFD))) {
  51                 kfree_skb(skb);
  52                 return 0;
  53         }
  54
  55         if (!priv->echo_skb[idx]) {
  56                 skb = can_create_echo_skb(skb);
  57                 if (!skb)
  58                         return -ENOMEM;
  59
  60                 /* make settings for echo to reduce code in irq context */
  61                 skb->ip_summed = CHECKSUM_UNNECESSARY;
  62                 skb->dev = dev;
  63
  64                 /* save frame_len to reuse it when transmission is completed */
  65                 can_skb_prv(skb)->frame_len = frame_len;
  66
  67                 skb_tx_timestamp(skb);
  68
  69                 /* save this skb for tx interrupt echo handling */
  70                 priv->echo_skb[idx] = skb;
  71         } else {
  72                 /* locking problem with netif_stop_queue() ?? */
  73                 netdev_err(dev, "%s: BUG! echo_skb %d is occupied!\n", __func__, idx);
  74                 kfree_skb(skb);
  75                 return -EBUSY;
  76         }
  77
  78         return 0;
  79 }
  80 EXPORT_SYMBOL_GPL(can_put_echo_skb);
  81
  82 struct sk_buff *
  83 __can_get_echo_skb(struct net_device *dev, unsigned int idx, u8 *len_ptr,
  84                    unsigned int *frame_len_ptr)
  85 {
  86         struct can_priv *priv = netdev_priv(dev);
  87
  88         if (idx >= priv->echo_skb_max) {
  89                 netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",
  90                            __func__, idx, priv->echo_skb_max);
  91                 return NULL;
  92         }
  93
  94         if (priv->echo_skb[idx]) {
  95                 /* Using "struct canfd_frame::len" for the frame
  96                  * length is supported on both CAN and CANFD frames.
  97                  */
  98                 struct sk_buff *skb = priv->echo_skb[idx];
  99                 struct can_skb_priv *can_skb_priv = can_skb_prv(skb);
 100                 struct canfd_frame *cf = (struct canfd_frame *)skb->data;
 101
 102                 /* get the real payload length for netdev statistics */
 103                 if (cf->can_id & CAN_RTR_FLAG)
 104                         *len_ptr = 0;
 105                 else
 106                         *len_ptr = cf->len;
 107
 108                 if (frame_len_ptr)
 109                         *frame_len_ptr = can_skb_priv->frame_len;
 110
 111                 priv->echo_skb[idx] = NULL;
 112
 113                 if (skb->pkt_type == PACKET_LOOPBACK) {
 114                         skb->pkt_type = PACKET_BROADCAST;
 115                 } else {
 116                         dev_consume_skb_any(skb);
 117                         return NULL;
 118                 }
 119
 120                 return skb;
 121         }
 122
 123         return NULL;
 124 }
 125
 126 /* Get the skb from the stack and loop it back locally
 127  *
 128  * The function is typically called when the TX done interrupt
 129  * is handled in the device driver. The driver must protect
 130  * access to priv->echo_skb, if necessary.
 131  */
 132 unsigned int can_get_echo_skb(struct net_device *dev, unsigned int idx,
 133                               unsigned int *frame_len_ptr)
 134 {
 135         struct sk_buff *skb;
 136         u8 len;
 137
 138         skb = __can_get_echo_skb(dev, idx, &len, frame_len_ptr);
 139         if (!skb)
 140                 return 0;
 141
 142         skb_get(skb);
 143         if (netif_rx(skb) == NET_RX_SUCCESS)
 144                 dev_consume_skb_any(skb);
 145         else
 146                 dev_kfree_skb_any(skb);
 147
 148         return len;
 149 }
 150 EXPORT_SYMBOL_GPL(can_get_echo_skb);
 151
 152 /* Remove the skb from the stack and free it.
 153  *
 154  * The function is typically called when TX failed.
 155  */
 156 void can_free_echo_skb(struct net_device *dev, unsigned int idx,
 157                        unsigned int *frame_len_ptr)
 158 {
 159         struct can_priv *priv = netdev_priv(dev);
 160
 161         if (idx >= priv->echo_skb_max) {
 162                 netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",
 163                            __func__, idx, priv->echo_skb_max);
 164                 return;
 165         }
 166
 167         if (priv->echo_skb[idx]) {
 168                 struct sk_buff *skb = priv->echo_skb[idx];
 169                 struct can_skb_priv *can_skb_priv = can_skb_prv(skb);
 170
 171                 if (frame_len_ptr)
 172                         *frame_len_ptr = can_skb_priv->frame_len;
 173
 174                 dev_kfree_skb_any(skb);
 175                 priv->echo_skb[idx] = NULL;
 176         }
 177 }
 178 EXPORT_SYMBOL_GPL(can_free_echo_skb);
 179
 180 struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf)
 181 {
 182         struct sk_buff *skb;
 183
 184         skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
 185                                sizeof(struct can_frame));
 186         if (unlikely(!skb)) {
 187                 *cf = NULL;
 188
 189                 return NULL;
 190         }
 191
 192         skb->protocol = htons(ETH_P_CAN);
 193         skb->pkt_type = PACKET_BROADCAST;
 194         skb->ip_summed = CHECKSUM_UNNECESSARY;
 195
 196         skb_reset_mac_header(skb);
 197         skb_reset_network_header(skb);
 198         skb_reset_transport_header(skb);
 199
 200         can_skb_reserve(skb);
 201         can_skb_prv(skb)->ifindex = dev->ifindex;
 202         can_skb_prv(skb)->skbcnt = 0;
 203
 204         *cf = skb_put_zero(skb, sizeof(struct can_frame));
 205
 206         return skb;
 207 }
 208 EXPORT_SYMBOL_GPL(alloc_can_skb);
 209
 210 struct sk_buff *alloc_canfd_skb(struct net_device *dev,
 211                                 struct canfd_frame **cfd)
 212 {
 213         struct sk_buff *skb;
 214
 215         skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
 216                                sizeof(struct canfd_frame));
 217         if (unlikely(!skb)) {
 218                 *cfd = NULL;
 219
 220                 return NULL;
 221         }
 222
 223         skb->protocol = htons(ETH_P_CANFD);
 224         skb->pkt_type = PACKET_BROADCAST;
 225         skb->ip_summed = CHECKSUM_UNNECESSARY;
 226
 227         skb_reset_mac_header(skb);
 228         skb_reset_network_header(skb);
 229         skb_reset_transport_header(skb);
 230
 231         can_skb_reserve(skb);
 232         can_skb_prv(skb)->ifindex = dev->ifindex;
 233         can_skb_prv(skb)->skbcnt = 0;
 234
 235         *cfd = skb_put_zero(skb, sizeof(struct canfd_frame));
 236
 237         return skb;
 238 }
 239 EXPORT_SYMBOL_GPL(alloc_canfd_skb);
 240
 241 struct sk_buff *alloc_can_err_skb(struct net_device *dev, struct can_frame **cf)
 242 {
 243         struct sk_buff *skb;
 244
 245         skb = alloc_can_skb(dev, cf);
 246         if (unlikely(!skb))
 247                 return NULL;
 248
 249         (*cf)->can_id = CAN_ERR_FLAG;
 250         (*cf)->len = CAN_ERR_DLC;
 251
 252         return skb;
 253 }
 254 EXPORT_SYMBOL_GPL(alloc_can_err_skb);