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, u8 *len_ptr,
  95                    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                 struct canfd_frame *cf = (struct canfd_frame *)skb->data;
 112
 113                 if (skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)
 114                         skb_tstamp_tx(skb, skb_hwtstamps(skb));
 115
 116                 /* get the real payload length for netdev statistics */
 117                 if (cf->can_id & CAN_RTR_FLAG)
 118                         *len_ptr = 0;
 119                 else
 120                         *len_ptr = cf->len;
 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         u8 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 struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf)
 195 {
 196         struct sk_buff *skb;
 197
 198         skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
 199                                sizeof(struct can_frame));
 200         if (unlikely(!skb)) {
 201                 *cf = NULL;
 202
 203                 return NULL;
 204         }
 205
 206         skb->protocol = htons(ETH_P_CAN);
 207         skb->pkt_type = PACKET_BROADCAST;
 208         skb->ip_summed = CHECKSUM_UNNECESSARY;
 209
 210         skb_reset_mac_header(skb);
 211         skb_reset_network_header(skb);
 212         skb_reset_transport_header(skb);
 213
 214         can_skb_reserve(skb);
 215         can_skb_prv(skb)->ifindex = dev->ifindex;
 216         can_skb_prv(skb)->skbcnt = 0;
 217
 218         *cf = skb_put_zero(skb, sizeof(struct can_frame));
 219
 220         return skb;
 221 }
 222 EXPORT_SYMBOL_GPL(alloc_can_skb);
 223
 224 struct sk_buff *alloc_canfd_skb(struct net_device *dev,
 225                                 struct canfd_frame **cfd)
 226 {
 227         struct sk_buff *skb;
 228
 229         skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
 230                                sizeof(struct canfd_frame));
 231         if (unlikely(!skb)) {
 232                 *cfd = NULL;
 233
 234                 return NULL;
 235         }
 236
 237         skb->protocol = htons(ETH_P_CANFD);
 238         skb->pkt_type = PACKET_BROADCAST;
 239         skb->ip_summed = CHECKSUM_UNNECESSARY;
 240
 241         skb_reset_mac_header(skb);
 242         skb_reset_network_header(skb);
 243         skb_reset_transport_header(skb);
 244
 245         can_skb_reserve(skb);
 246         can_skb_prv(skb)->ifindex = dev->ifindex;
 247         can_skb_prv(skb)->skbcnt = 0;
 248
 249         *cfd = skb_put_zero(skb, sizeof(struct canfd_frame));
 250
 251         return skb;
 252 }
 253 EXPORT_SYMBOL_GPL(alloc_canfd_skb);
 254
 255 struct sk_buff *alloc_can_err_skb(struct net_device *dev, struct can_frame **cf)
 256 {
 257         struct sk_buff *skb;
 258
 259         skb = alloc_can_skb(dev, cf);
 260         if (unlikely(!skb))
 261                 return NULL;
 262
 263         (*cf)->can_id = CAN_ERR_FLAG;
 264         (*cf)->len = CAN_ERR_DLC;
 265
 266         return skb;
 267 }
 268 EXPORT_SYMBOL_GPL(alloc_can_err_skb);
 269
 270 /* Check for outgoing skbs that have not been created by the CAN subsystem */
 271 static bool can_skb_headroom_valid(struct net_device *dev, struct sk_buff *skb)
 272 {
 273         /* af_packet creates a headroom of HH_DATA_MOD bytes which is fine */
 274         if (WARN_ON_ONCE(skb_headroom(skb) < sizeof(struct can_skb_priv)))
 275                 return false;
 276
 277         /* af_packet does not apply CAN skb specific settings */
 278         if (skb->ip_summed == CHECKSUM_NONE) {
 279                 /* init headroom */
 280                 can_skb_prv(skb)->ifindex = dev->ifindex;
 281                 can_skb_prv(skb)->skbcnt = 0;
 282
 283                 skb->ip_summed = CHECKSUM_UNNECESSARY;
 284
 285                 /* perform proper loopback on capable devices */
 286                 if (dev->flags & IFF_ECHO)
 287                         skb->pkt_type = PACKET_LOOPBACK;
 288                 else
 289                         skb->pkt_type = PACKET_HOST;
 290
 291                 skb_reset_mac_header(skb);
 292                 skb_reset_network_header(skb);
 293                 skb_reset_transport_header(skb);
 294         }
 295
 296         return true;
 297 }
 298
 299 /* Drop a given socketbuffer if it does not contain a valid CAN frame. */
 300 bool can_dropped_invalid_skb(struct net_device *dev, struct sk_buff *skb)
 301 {
 302         struct can_priv *priv = netdev_priv(dev);
 303
 304         switch (ntohs(skb->protocol)) {
 305         case ETH_P_CAN:
 306                 if (!can_is_can_skb(skb))
 307                         goto inval_skb;
 308                 break;
 309
 310         case ETH_P_CANFD:
 311                 if (!can_is_canfd_skb(skb))
 312                         goto inval_skb;
 313                 break;
 314
 315         default:
 316                 goto inval_skb;
 317         }
 318
 319         if (!can_skb_headroom_valid(dev, skb)) {
 320                 goto inval_skb;
 321         } else if (priv->ctrlmode & CAN_CTRLMODE_LISTENONLY) {
 322                 netdev_info_once(dev,
 323                                  "interface in listen only mode, dropping skb\n");
 324                 goto inval_skb;
 325         }
 326
 327         return false;
 328
 329 inval_skb:
 330         kfree_skb(skb);
 331         dev->stats.tx_dropped++;
 332         return true;
 333 }
 334 EXPORT_SYMBOL_GPL(can_dropped_invalid_skb);