diff options
Diffstat (limited to '')
| -rw-r--r-- | drivers/net/can/dev/skb.c | 231 |
1 files changed, 231 insertions, 0 deletions
diff --git a/drivers/net/can/dev/skb.c b/drivers/net/can/dev/skb.c new file mode 100644 index 000000000000..6a64fe410987 --- /dev/null +++ b/drivers/net/can/dev/skb.c @@ -0,0 +1,231 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* Copyright (C) 2005 Marc Kleine-Budde, Pengutronix + * Copyright (C) 2006 Andrey Volkov, Varma Electronics + * Copyright (C) 2008-2009 Wolfgang Grandegger <wg@grandegger.com> + */ + +#include <linux/can/dev.h> + +/* Local echo of CAN messages + * + * CAN network devices *should* support a local echo functionality + * (see Documentation/networking/can.rst). To test the handling of CAN + * interfaces that do not support the local echo both driver types are + * implemented. In the case that the driver does not support the echo + * the IFF_ECHO remains clear in dev->flags. This causes the PF_CAN core + * to perform the echo as a fallback solution. + */ +void can_flush_echo_skb(struct net_device *dev) +{ + struct can_priv *priv = netdev_priv(dev); + struct net_device_stats *stats = &dev->stats; + int i; + + for (i = 0; i < priv->echo_skb_max; i++) { + if (priv->echo_skb[i]) { + kfree_skb(priv->echo_skb[i]); + priv->echo_skb[i] = NULL; + stats->tx_dropped++; + stats->tx_aborted_errors++; + } + } +} + +/* Put the skb on the stack to be looped backed locally lateron + * + * The function is typically called in the start_xmit function + * of the device driver. The driver must protect access to + * priv->echo_skb, if necessary. + */ +int can_put_echo_skb(struct sk_buff *skb, struct net_device *dev, + unsigned int idx, unsigned int frame_len) +{ + struct can_priv *priv = netdev_priv(dev); + + BUG_ON(idx >= priv->echo_skb_max); + + /* check flag whether this packet has to be looped back */ + if (!(dev->flags & IFF_ECHO) || skb->pkt_type != PACKET_LOOPBACK || + (skb->protocol != htons(ETH_P_CAN) && + skb->protocol != htons(ETH_P_CANFD))) { + kfree_skb(skb); + return 0; + } + + if (!priv->echo_skb[idx]) { + skb = can_create_echo_skb(skb); + if (!skb) + return -ENOMEM; + + /* make settings for echo to reduce code in irq context */ + skb->pkt_type = PACKET_BROADCAST; + skb->ip_summed = CHECKSUM_UNNECESSARY; + skb->dev = dev; + + /* save frame_len to reuse it when transmission is completed */ + can_skb_prv(skb)->frame_len = frame_len; + + skb_tx_timestamp(skb); + + /* save this skb for tx interrupt echo handling */ + priv->echo_skb[idx] = skb; + } else { + /* locking problem with netif_stop_queue() ?? */ + netdev_err(dev, "%s: BUG! echo_skb %d is occupied!\n", __func__, idx); + kfree_skb(skb); + return -EBUSY; + } + + return 0; +} +EXPORT_SYMBOL_GPL(can_put_echo_skb); + +struct sk_buff * +__can_get_echo_skb(struct net_device *dev, unsigned int idx, u8 *len_ptr, + unsigned int *frame_len_ptr) +{ + struct can_priv *priv = netdev_priv(dev); + + if (idx >= priv->echo_skb_max) { + netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n", + __func__, idx, priv->echo_skb_max); + return NULL; + } + + if (priv->echo_skb[idx]) { + /* Using "struct canfd_frame::len" for the frame + * length is supported on both CAN and CANFD frames. + */ + struct sk_buff *skb = priv->echo_skb[idx]; + struct can_skb_priv *can_skb_priv = can_skb_prv(skb); + struct canfd_frame *cf = (struct canfd_frame *)skb->data; + + /* get the real payload length for netdev statistics */ + if (cf->can_id & CAN_RTR_FLAG) + *len_ptr = 0; + else + *len_ptr = cf->len; + + if (frame_len_ptr) + *frame_len_ptr = can_skb_priv->frame_len; + + priv->echo_skb[idx] = NULL; + + return skb; + } + + return NULL; +} + +/* Get the skb from the stack and loop it back locally + * + * The function is typically called when the TX done interrupt + * is handled in the device driver. The driver must protect + * access to priv->echo_skb, if necessary. + */ +unsigned int can_get_echo_skb(struct net_device *dev, unsigned int idx, + unsigned int *frame_len_ptr) +{ + struct sk_buff *skb; + u8 len; + + skb = __can_get_echo_skb(dev, idx, &len, frame_len_ptr); + if (!skb) + return 0; + + skb_get(skb); + if (netif_rx(skb) == NET_RX_SUCCESS) + dev_consume_skb_any(skb); + else + dev_kfree_skb_any(skb); + + return len; +} +EXPORT_SYMBOL_GPL(can_get_echo_skb); + +/* Remove the skb from the stack and free it. + * + * The function is typically called when TX failed. + */ +void can_free_echo_skb(struct net_device *dev, unsigned int idx) +{ + struct can_priv *priv = netdev_priv(dev); + + BUG_ON(idx >= priv->echo_skb_max); + + if (priv->echo_skb[idx]) { + dev_kfree_skb_any(priv->echo_skb[idx]); + priv->echo_skb[idx] = NULL; + } +} +EXPORT_SYMBOL_GPL(can_free_echo_skb); + +struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf) +{ + struct sk_buff *skb; + + skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) + + sizeof(struct can_frame)); + if (unlikely(!skb)) + return NULL; + + skb->protocol = htons(ETH_P_CAN); + skb->pkt_type = PACKET_BROADCAST; + skb->ip_summed = CHECKSUM_UNNECESSARY; + + skb_reset_mac_header(skb); + skb_reset_network_header(skb); + skb_reset_transport_header(skb); + + can_skb_reserve(skb); + can_skb_prv(skb)->ifindex = dev->ifindex; + can_skb_prv(skb)->skbcnt = 0; + + *cf = skb_put_zero(skb, sizeof(struct can_frame)); + + return skb; +} +EXPORT_SYMBOL_GPL(alloc_can_skb); + +struct sk_buff *alloc_canfd_skb(struct net_device *dev, + struct canfd_frame **cfd) +{ + struct sk_buff *skb; + + skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) + + sizeof(struct canfd_frame)); + if (unlikely(!skb)) + return NULL; + + skb->protocol = htons(ETH_P_CANFD); + skb->pkt_type = PACKET_BROADCAST; + skb->ip_summed = CHECKSUM_UNNECESSARY; + + skb_reset_mac_header(skb); + skb_reset_network_header(skb); + skb_reset_transport_header(skb); + + can_skb_reserve(skb); + can_skb_prv(skb)->ifindex = dev->ifindex; + can_skb_prv(skb)->skbcnt = 0; + + *cfd = skb_put_zero(skb, sizeof(struct canfd_frame)); + + return skb; +} +EXPORT_SYMBOL_GPL(alloc_canfd_skb); + +struct sk_buff *alloc_can_err_skb(struct net_device *dev, struct can_frame **cf) +{ + struct sk_buff *skb; + + skb = alloc_can_skb(dev, cf); + if (unlikely(!skb)) + return NULL; + + (*cf)->can_id = CAN_ERR_FLAG; + (*cf)->len = CAN_ERR_DLC; + + return skb; +} +EXPORT_SYMBOL_GPL(alloc_can_err_skb); |