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