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                 skb_tx_timestamp(skb);
  76
  77                 /* save this skb for tx interrupt echo handling */
  78                 priv->echo_skb[idx] = skb;
  79         } else {
  80                 /* locking problem with netif_stop_queue() ?? */
  81                 netdev_err(dev, "%s: BUG! echo_skb %d is occupied!\n", __func__, idx);
  82                 kfree_skb(skb);
  83                 return -EBUSY;
  84         }
  85
  86         return 0;
  87 }
  88 EXPORT_SYMBOL_GPL(can_put_echo_skb);
  89
  90 struct sk_buff *
  91 __can_get_echo_skb(struct net_device *dev, unsigned int idx, u8 *len_ptr,
  92                    unsigned int *frame_len_ptr)
  93 {
  94         struct can_priv *priv = netdev_priv(dev);
  95
  96         if (idx >= priv->echo_skb_max) {
  97                 netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",
  98                            __func__, idx, priv->echo_skb_max);
  99                 return NULL;
 100         }
 101
 102         if (priv->echo_skb[idx]) {
 103                 /* Using "struct canfd_frame::len" for the frame
 104                  * length is supported on both CAN and CANFD frames.
 105                  */
 106                 struct sk_buff *skb = priv->echo_skb[idx];
 107                 struct can_skb_priv *can_skb_priv = can_skb_prv(skb);
 108                 struct canfd_frame *cf = (struct canfd_frame *)skb->data;
 109
 110                 /* get the real payload length for netdev statistics */
 111                 if (cf->can_id & CAN_RTR_FLAG)
 112                         *len_ptr = 0;
 113                 else
 114                         *len_ptr = cf->len;
 115
 116                 if (frame_len_ptr)
 117                         *frame_len_ptr = can_skb_priv->frame_len;
 118
 119                 priv->echo_skb[idx] = NULL;
 120
 121                 if (skb->pkt_type == PACKET_LOOPBACK) {
 122                         skb->pkt_type = PACKET_BROADCAST;
 123                 } else {
 124                         dev_consume_skb_any(skb);
 125                         return NULL;
 126                 }
 127
 128                 return skb;
 129         }
 130
 131         return NULL;
 132 }
 133
 134 /* Get the skb from the stack and loop it back locally
 135  *
 136  * The function is typically called when the TX done interrupt
 137  * is handled in the device driver. The driver must protect
 138  * access to priv->echo_skb, if necessary.
 139  */
 140 unsigned int can_get_echo_skb(struct net_device *dev, unsigned int idx,
 141                               unsigned int *frame_len_ptr)
 142 {
 143         struct sk_buff *skb;
 144         u8 len;
 145
 146         skb = __can_get_echo_skb(dev, idx, &len, frame_len_ptr);
 147         if (!skb)
 148                 return 0;
 149
 150         skb_get(skb);
 151         if (netif_rx(skb) == NET_RX_SUCCESS)
 152                 dev_consume_skb_any(skb);
 153         else
 154                 dev_kfree_skb_any(skb);
 155
 156         return len;
 157 }
 158 EXPORT_SYMBOL_GPL(can_get_echo_skb);
 159
 160 /* Remove the skb from the stack and free it.
 161  *
 162  * The function is typically called when TX failed.
 163  */
 164 void can_free_echo_skb(struct net_device *dev, unsigned int idx,
 165                        unsigned int *frame_len_ptr)
 166 {
 167         struct can_priv *priv = netdev_priv(dev);
 168
 169         if (idx >= priv->echo_skb_max) {
 170                 netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",
 171                            __func__, idx, priv->echo_skb_max);
 172                 return;
 173         }
 174
 175         if (priv->echo_skb[idx]) {
 176                 struct sk_buff *skb = priv->echo_skb[idx];
 177                 struct can_skb_priv *can_skb_priv = can_skb_prv(skb);
 178
 179                 if (frame_len_ptr)
 180                         *frame_len_ptr = can_skb_priv->frame_len;
 181
 182                 dev_kfree_skb_any(skb);
 183                 priv->echo_skb[idx] = NULL;
 184         }
 185 }
 186 EXPORT_SYMBOL_GPL(can_free_echo_skb);
 187
 188 struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf)
 189 {
 190         struct sk_buff *skb;
 191
 192         skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
 193                                sizeof(struct can_frame));
 194         if (unlikely(!skb)) {
 195                 *cf = NULL;
 196
 197                 return NULL;
 198         }
 199
 200         skb->protocol = htons(ETH_P_CAN);
 201         skb->pkt_type = PACKET_BROADCAST;
 202         skb->ip_summed = CHECKSUM_UNNECESSARY;
 203
 204         skb_reset_mac_header(skb);
 205         skb_reset_network_header(skb);
 206         skb_reset_transport_header(skb);
 207
 208         can_skb_reserve(skb);
 209         can_skb_prv(skb)->ifindex = dev->ifindex;
 210         can_skb_prv(skb)->skbcnt = 0;
 211
 212         *cf = skb_put_zero(skb, sizeof(struct can_frame));
 213
 214         return skb;
 215 }
 216 EXPORT_SYMBOL_GPL(alloc_can_skb);
 217
 218 struct sk_buff *alloc_canfd_skb(struct net_device *dev,
 219                                 struct canfd_frame **cfd)
 220 {
 221         struct sk_buff *skb;
 222
 223         skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
 224                                sizeof(struct canfd_frame));
 225         if (unlikely(!skb)) {
 226                 *cfd = NULL;
 227
 228                 return NULL;
 229         }
 230
 231         skb->protocol = htons(ETH_P_CANFD);
 232         skb->pkt_type = PACKET_BROADCAST;
 233         skb->ip_summed = CHECKSUM_UNNECESSARY;
 234
 235         skb_reset_mac_header(skb);
 236         skb_reset_network_header(skb);
 237         skb_reset_transport_header(skb);
 238
 239         can_skb_reserve(skb);
 240         can_skb_prv(skb)->ifindex = dev->ifindex;
 241         can_skb_prv(skb)->skbcnt = 0;
 242
 243         *cfd = skb_put_zero(skb, sizeof(struct canfd_frame));
 244
 245         return skb;
 246 }
 247 EXPORT_SYMBOL_GPL(alloc_canfd_skb);
 248
 249 struct sk_buff *alloc_can_err_skb(struct net_device *dev, struct can_frame **cf)
 250 {
 251         struct sk_buff *skb;
 252
 253         skb = alloc_can_skb(dev, cf);
 254         if (unlikely(!skb))
 255                 return NULL;
 256
 257         (*cf)->can_id = CAN_ERR_FLAG;
 258         (*cf)->len = CAN_ERR_DLC;
 259
 260         return skb;
 261 }
 262 EXPORT_SYMBOL_GPL(alloc_can_err_skb);
 263
 264 /* Check for outgoing skbs that have not been created by the CAN subsystem */
 265 static bool can_skb_headroom_valid(struct net_device *dev, struct sk_buff *skb)
 266 {
 267         /* af_packet creates a headroom of HH_DATA_MOD bytes which is fine */
 268         if (WARN_ON_ONCE(skb_headroom(skb) < sizeof(struct can_skb_priv)))
 269                 return false;
 270
 271         /* af_packet does not apply CAN skb specific settings */
 272         if (skb->ip_summed == CHECKSUM_NONE) {
 273                 /* init headroom */
 274                 can_skb_prv(skb)->ifindex = dev->ifindex;
 275                 can_skb_prv(skb)->skbcnt = 0;
 276
 277                 skb->ip_summed = CHECKSUM_UNNECESSARY;
 278
 279                 /* perform proper loopback on capable devices */
 280                 if (dev->flags & IFF_ECHO)
 281                         skb->pkt_type = PACKET_LOOPBACK;
 282                 else
 283                         skb->pkt_type = PACKET_HOST;
 284
 285                 skb_reset_mac_header(skb);
 286                 skb_reset_network_header(skb);
 287                 skb_reset_transport_header(skb);
 288         }
 289
 290         return true;
 291 }
 292
 293 /* Drop a given socketbuffer if it does not contain a valid CAN frame. */
 294 bool can_dropped_invalid_skb(struct net_device *dev, struct sk_buff *skb)
 295 {
 296         const struct canfd_frame *cfd = (struct canfd_frame *)skb->data;
 297         struct can_priv *priv = netdev_priv(dev);
 298
 299         if (skb->protocol == htons(ETH_P_CAN)) {
 300                 if (unlikely(skb->len != CAN_MTU ||
 301                              cfd->len > CAN_MAX_DLEN))
 302                         goto inval_skb;
 303         } else if (skb->protocol == htons(ETH_P_CANFD)) {
 304                 if (unlikely(skb->len != CANFD_MTU ||
 305                              cfd->len > CANFD_MAX_DLEN))
 306                         goto inval_skb;
 307         } else {
 308                 goto inval_skb;
 309         }
 310
 311         if (!can_skb_headroom_valid(dev, skb)) {
 312                 goto inval_skb;
 313         } else if (priv->ctrlmode & CAN_CTRLMODE_LISTENONLY) {
 314                 netdev_info_once(dev,
 315                                  "interface in listen only mode, dropping skb\n");
 316                 goto inval_skb;
 317         }
 318
 319         return false;
 320
 321 inval_skb:
 322         kfree_skb(skb);
 323         dev->stats.tx_dropped++;
 324         return true;
 325 }
 326 EXPORT_SYMBOL_GPL(can_dropped_invalid_skb);