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