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 <wg@grandegger.com>
   5  */
   6
   7 #include <linux/can/dev.h>
   8 #include <linux/module.h>
   9
  10 #define MOD_DESC "CAN device driver interface"
  11
  12 MODULE_DESCRIPTION(MOD_DESC);
  13 MODULE_LICENSE("GPL v2");
  14 MODULE_AUTHOR("Wolfgang Grandegger <wg@grandegger.com>");
  15
  16 /* Local echo of CAN messages
  17  *
  18  * CAN network devices *should* support a local echo functionality
  19  * (see Documentation/networking/can.rst). To test the handling of CAN
  20  * interfaces that do not support the local echo both driver types are
  21  * implemented. In the case that the driver does not support the echo
  22  * the IFF_ECHO remains clear in dev->flags. This causes the PF_CAN core
  23  * to perform the echo as a fallback solution.
  24  */
  25 void can_flush_echo_skb(struct net_device *dev)
  26 {
  27         struct can_priv *priv = netdev_priv(dev);
  28         struct net_device_stats *stats = &dev->stats;
  29         int i;
  30
  31         for (i = 0; i < priv->echo_skb_max; i++) {
  32                 if (priv->echo_skb[i]) {
  33                         kfree_skb(priv->echo_skb[i]);
  34                         priv->echo_skb[i] = NULL;
  35                         stats->tx_dropped++;
  36                         stats->tx_aborted_errors++;
  37                 }
  38         }
  39 }
  40
  41 /* Put the skb on the stack to be looped backed locally lateron
  42  *
  43  * The function is typically called in the start_xmit function
  44  * of the device driver. The driver must protect access to
  45  * priv->echo_skb, if necessary.
  46  */
  47 int can_put_echo_skb(struct sk_buff *skb, struct net_device *dev,
  48                      unsigned int idx, unsigned int frame_len)
  49 {
  50         struct can_priv *priv = netdev_priv(dev);
  51
  52         BUG_ON(idx >= priv->echo_skb_max);
  53
  54         /* check flag whether this packet has to be looped back */
  55         if (!(dev->flags & IFF_ECHO) ||
  56             (skb->protocol != htons(ETH_P_CAN) &&
  57              skb->protocol != htons(ETH_P_CANFD))) {
  58                 kfree_skb(skb);
  59                 return 0;
  60         }
  61
  62         if (!priv->echo_skb[idx]) {
  63                 skb = can_create_echo_skb(skb);
  64                 if (!skb)
  65                         return -ENOMEM;
  66
  67                 /* make settings for echo to reduce code in irq context */
  68                 skb->ip_summed = CHECKSUM_UNNECESSARY;
  69                 skb->dev = dev;
  70
  71                 /* save frame_len to reuse it when transmission is completed */
  72                 can_skb_prv(skb)->frame_len = frame_len;
  73
  74                 if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)
  75                         skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
  76
  77                 skb_tx_timestamp(skb);
  78
  79                 /* save this skb for tx interrupt echo handling */
  80                 priv->echo_skb[idx] = skb;
  81         } else {
  82                 /* locking problem with netif_stop_queue() ?? */
  83                 netdev_err(dev, "%s: BUG! echo_skb %d is occupied!\n", __func__, idx);
  84                 kfree_skb(skb);
  85                 return -EBUSY;
  86         }
  87
  88         return 0;
  89 }
  90 EXPORT_SYMBOL_GPL(can_put_echo_skb);
  91
  92 struct sk_buff *
  93 __can_get_echo_skb(struct net_device *dev, unsigned int idx,
  94                    unsigned int *len_ptr, unsigned int *frame_len_ptr)
  95 {
  96         struct can_priv *priv = netdev_priv(dev);
  97
  98         if (idx >= priv->echo_skb_max) {
  99                 netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",
 100                            __func__, idx, priv->echo_skb_max);
 101                 return NULL;
 102         }
 103
 104         if (priv->echo_skb[idx]) {
 105                 /* Using "struct canfd_frame::len" for the frame
 106                  * length is supported on both CAN and CANFD frames.
 107                  */
 108                 struct sk_buff *skb = priv->echo_skb[idx];
 109                 struct can_skb_priv *can_skb_priv = can_skb_prv(skb);
 110
 111                 if (skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)
 112                         skb_tstamp_tx(skb, skb_hwtstamps(skb));
 113
 114                 /* get the real payload length for netdev statistics */
 115                 *len_ptr = can_skb_get_data_len(skb);
 116
 117                 if (frame_len_ptr)
 118                         *frame_len_ptr = can_skb_priv->frame_len;
 119
 120                 priv->echo_skb[idx] = NULL;
 121
 122                 if (skb->pkt_type == PACKET_LOOPBACK) {
 123                         skb->pkt_type = PACKET_BROADCAST;
 124                 } else {
 125                         dev_consume_skb_any(skb);
 126                         return NULL;
 127                 }
 128
 129                 return skb;
 130         }
 131
 132         return NULL;
 133 }
 134
 135 /* Get the skb from the stack and loop it back locally
 136  *
 137  * The function is typically called when the TX done interrupt
 138  * is handled in the device driver. The driver must protect
 139  * access to priv->echo_skb, if necessary.
 140  */
 141 unsigned int can_get_echo_skb(struct net_device *dev, unsigned int idx,
 142                               unsigned int *frame_len_ptr)
 143 {
 144         struct sk_buff *skb;
 145         unsigned int len;
 146
 147         skb = __can_get_echo_skb(dev, idx, &len, frame_len_ptr);
 148         if (!skb)
 149                 return 0;
 150
 151         skb_get(skb);
 152         if (netif_rx(skb) == NET_RX_SUCCESS)
 153                 dev_consume_skb_any(skb);
 154         else
 155                 dev_kfree_skb_any(skb);
 156
 157         return len;
 158 }
 159 EXPORT_SYMBOL_GPL(can_get_echo_skb);
 160
 161 /* Remove the skb from the stack and free it.
 162  *
 163  * The function is typically called when TX failed.
 164  */
 165 void can_free_echo_skb(struct net_device *dev, unsigned int idx,
 166                        unsigned int *frame_len_ptr)
 167 {
 168         struct can_priv *priv = netdev_priv(dev);
 169
 170         if (idx >= priv->echo_skb_max) {
 171                 netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",
 172                            __func__, idx, priv->echo_skb_max);
 173                 return;
 174         }
 175
 176         if (priv->echo_skb[idx]) {
 177                 struct sk_buff *skb = priv->echo_skb[idx];
 178                 struct can_skb_priv *can_skb_priv = can_skb_prv(skb);
 179
 180                 if (frame_len_ptr)
 181                         *frame_len_ptr = can_skb_priv->frame_len;
 182
 183                 dev_kfree_skb_any(skb);
 184                 priv->echo_skb[idx] = NULL;
 185         }
 186 }
 187 EXPORT_SYMBOL_GPL(can_free_echo_skb);
 188
 189 /* fill common values for CAN sk_buffs */
 190 static void init_can_skb_reserve(struct sk_buff *skb)
 191 {
 192         skb->pkt_type = PACKET_BROADCAST;
 193         skb->ip_summed = CHECKSUM_UNNECESSARY;
 194
 195         skb_reset_mac_header(skb);
 196         skb_reset_network_header(skb);
 197         skb_reset_transport_header(skb);
 198
 199         can_skb_reserve(skb);
 200         can_skb_prv(skb)->skbcnt = 0;
 201 }
 202
 203 struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf)
 204 {
 205         struct sk_buff *skb;
 206
 207         skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
 208                                sizeof(struct can_frame));
 209         if (unlikely(!skb)) {
 210                 *cf = NULL;
 211
 212                 return NULL;
 213         }
 214
 215         skb->protocol = htons(ETH_P_CAN);
 216         init_can_skb_reserve(skb);
 217         can_skb_prv(skb)->ifindex = dev->ifindex;
 218
 219         *cf = skb_put_zero(skb, sizeof(struct can_frame));
 220
 221         return skb;
 222 }
 223 EXPORT_SYMBOL_GPL(alloc_can_skb);
 224
 225 struct sk_buff *alloc_canfd_skb(struct net_device *dev,
 226                                 struct canfd_frame **cfd)
 227 {
 228         struct sk_buff *skb;
 229
 230         skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
 231                                sizeof(struct canfd_frame));
 232         if (unlikely(!skb)) {
 233                 *cfd = NULL;
 234
 235                 return NULL;
 236         }
 237
 238         skb->protocol = htons(ETH_P_CANFD);
 239         init_can_skb_reserve(skb);
 240         can_skb_prv(skb)->ifindex = dev->ifindex;
 241
 242         *cfd = skb_put_zero(skb, sizeof(struct canfd_frame));
 243
 244         /* set CAN FD flag by default */
 245         (*cfd)->flags = CANFD_FDF;
 246
 247         return skb;
 248 }
 249 EXPORT_SYMBOL_GPL(alloc_canfd_skb);
 250
 251 struct sk_buff *alloc_canxl_skb(struct net_device *dev,
 252                                 struct canxl_frame **cxl,
 253                                 unsigned int data_len)
 254 {
 255         struct sk_buff *skb;
 256
 257         if (data_len < CANXL_MIN_DLEN || data_len > CANXL_MAX_DLEN)
 258                 goto out_error;
 259
 260         skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
 261                                CANXL_HDR_SIZE + data_len);
 262         if (unlikely(!skb))
 263                 goto out_error;
 264
 265         skb->protocol = htons(ETH_P_CANXL);
 266         init_can_skb_reserve(skb);
 267         can_skb_prv(skb)->ifindex = dev->ifindex;
 268
 269         *cxl = skb_put_zero(skb, CANXL_HDR_SIZE + data_len);
 270
 271         /* set CAN XL flag and length information by default */
 272         (*cxl)->flags = CANXL_XLF;
 273         (*cxl)->len = data_len;
 274
 275         return skb;
 276
 277 out_error:
 278         *cxl = NULL;
 279
 280         return NULL;
 281 }
 282 EXPORT_SYMBOL_GPL(alloc_canxl_skb);
 283
 284 struct sk_buff *alloc_can_err_skb(struct net_device *dev, struct can_frame **cf)
 285 {
 286         struct sk_buff *skb;
 287
 288         skb = alloc_can_skb(dev, cf);
 289         if (unlikely(!skb))
 290                 return NULL;
 291
 292         (*cf)->can_id = CAN_ERR_FLAG;
 293         (*cf)->len = CAN_ERR_DLC;
 294
 295         return skb;
 296 }
 297 EXPORT_SYMBOL_GPL(alloc_can_err_skb);
 298
 299 /* Check for outgoing skbs that have not been created by the CAN subsystem */
 300 static bool can_skb_headroom_valid(struct net_device *dev, struct sk_buff *skb)
 301 {
 302         /* af_packet creates a headroom of HH_DATA_MOD bytes which is fine */
 303         if (WARN_ON_ONCE(skb_headroom(skb) < sizeof(struct can_skb_priv)))
 304                 return false;
 305
 306         /* af_packet does not apply CAN skb specific settings */
 307         if (skb->ip_summed == CHECKSUM_NONE) {
 308                 /* init headroom */
 309                 can_skb_prv(skb)->ifindex = dev->ifindex;
 310                 can_skb_prv(skb)->skbcnt = 0;
 311
 312                 skb->ip_summed = CHECKSUM_UNNECESSARY;
 313
 314                 /* perform proper loopback on capable devices */
 315                 if (dev->flags & IFF_ECHO)
 316                         skb->pkt_type = PACKET_LOOPBACK;
 317                 else
 318                         skb->pkt_type = PACKET_HOST;
 319
 320                 skb_reset_mac_header(skb);
 321                 skb_reset_network_header(skb);
 322                 skb_reset_transport_header(skb);
 323
 324                 /* set CANFD_FDF flag for CAN FD frames */
 325                 if (can_is_canfd_skb(skb)) {
 326                         struct canfd_frame *cfd;
 327
 328                         cfd = (struct canfd_frame *)skb->data;
 329                         cfd->flags |= CANFD_FDF;
 330                 }
 331         }
 332
 333         return true;
 334 }
 335
 336 /* Drop a given socketbuffer if it does not contain a valid CAN frame. */
 337 bool can_dropped_invalid_skb(struct net_device *dev, struct sk_buff *skb)
 338 {
 339         switch (ntohs(skb->protocol)) {
 340         case ETH_P_CAN:
 341                 if (!can_is_can_skb(skb))
 342                         goto inval_skb;
 343                 break;
 344
 345         case ETH_P_CANFD:
 346                 if (!can_is_canfd_skb(skb))
 347                         goto inval_skb;
 348                 break;
 349
 350         case ETH_P_CANXL:
 351                 if (!can_is_canxl_skb(skb))
 352                         goto inval_skb;
 353                 break;
 354
 355         default:
 356                 goto inval_skb;
 357         }
 358
 359         if (!can_skb_headroom_valid(dev, skb))
 360                 goto inval_skb;
 361
 362         return false;
 363
 364 inval_skb:
 365         kfree_skb(skb);
 366         dev->stats.tx_dropped++;
 367         return true;
 368 }
 369 EXPORT_SYMBOL_GPL(can_dropped_invalid_skb);