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 9 /* Local echo of CAN messages 10 * 11 * CAN network devices *should* support a local echo functionality 12 * (see Documentation/networking/can.rst). To test the handling of CAN 13 * interfaces that do not support the local echo both driver types are 14 * implemented. In the case that the driver does not support the echo 15 * the IFF_ECHO remains clear in dev->flags. This causes the PF_CAN core 16 * to perform the echo as a fallback solution. 17 */ 18 void can_flush_echo_skb(struct net_device *dev) 19 { 20 struct can_priv *priv = netdev_priv(dev); 21 struct net_device_stats *stats = &dev->stats; 22 int i; 23 24 for (i = 0; i < priv->echo_skb_max; i++) { 25 if (priv->echo_skb[i]) { 26 kfree_skb(priv->echo_skb[i]); 27 priv->echo_skb[i] = NULL; 28 stats->tx_dropped++; 29 stats->tx_aborted_errors++; 30 } 31 } 32 } 33 34 /* Put the skb on the stack to be looped backed locally lateron 35 * 36 * The function is typically called in the start_xmit function 37 * of the device driver. The driver must protect access to 38 * priv->echo_skb, if necessary. 39 */ 40 int can_put_echo_skb(struct sk_buff *skb, struct net_device *dev, 41 unsigned int idx, unsigned int frame_len) 42 { 43 struct can_priv *priv = netdev_priv(dev); 44 45 BUG_ON(idx >= priv->echo_skb_max); 46 47 /* check flag whether this packet has to be looped back */ 48 if (!(dev->flags & IFF_ECHO) || skb->pkt_type != PACKET_LOOPBACK || 49 (skb->protocol != htons(ETH_P_CAN) && 50 skb->protocol != htons(ETH_P_CANFD))) { 51 kfree_skb(skb); 52 return 0; 53 } 54 55 if (!priv->echo_skb[idx]) { 56 skb = can_create_echo_skb(skb); 57 if (!skb) 58 return -ENOMEM; 59 60 /* make settings for echo to reduce code in irq context */ 61 skb->pkt_type = PACKET_BROADCAST; 62 skb->ip_summed = CHECKSUM_UNNECESSARY; 63 skb->dev = dev; 64 65 /* save frame_len to reuse it when transmission is completed */ 66 can_skb_prv(skb)->frame_len = frame_len; 67 68 skb_tx_timestamp(skb); 69 70 /* save this skb for tx interrupt echo handling */ 71 priv->echo_skb[idx] = skb; 72 } else { 73 /* locking problem with netif_stop_queue() ?? */ 74 netdev_err(dev, "%s: BUG! echo_skb %d is occupied!\n", __func__, idx); 75 kfree_skb(skb); 76 return -EBUSY; 77 } 78 79 return 0; 80 } 81 EXPORT_SYMBOL_GPL(can_put_echo_skb); 82 83 struct sk_buff * 84 __can_get_echo_skb(struct net_device *dev, unsigned int idx, u8 *len_ptr, 85 unsigned int *frame_len_ptr) 86 { 87 struct can_priv *priv = netdev_priv(dev); 88 89 if (idx >= priv->echo_skb_max) { 90 netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n", 91 __func__, idx, priv->echo_skb_max); 92 return NULL; 93 } 94 95 if (priv->echo_skb[idx]) { 96 /* Using "struct canfd_frame::len" for the frame 97 * length is supported on both CAN and CANFD frames. 98 */ 99 struct sk_buff *skb = priv->echo_skb[idx]; 100 struct can_skb_priv *can_skb_priv = can_skb_prv(skb); 101 struct canfd_frame *cf = (struct canfd_frame *)skb->data; 102 103 /* get the real payload length for netdev statistics */ 104 if (cf->can_id & CAN_RTR_FLAG) 105 *len_ptr = 0; 106 else 107 *len_ptr = cf->len; 108 109 if (frame_len_ptr) 110 *frame_len_ptr = can_skb_priv->frame_len; 111 112 priv->echo_skb[idx] = NULL; 113 114 return skb; 115 } 116 117 return NULL; 118 } 119 120 /* Get the skb from the stack and loop it back locally 121 * 122 * The function is typically called when the TX done interrupt 123 * is handled in the device driver. The driver must protect 124 * access to priv->echo_skb, if necessary. 125 */ 126 unsigned int can_get_echo_skb(struct net_device *dev, unsigned int idx, 127 unsigned int *frame_len_ptr) 128 { 129 struct sk_buff *skb; 130 u8 len; 131 132 skb = __can_get_echo_skb(dev, idx, &len, frame_len_ptr); 133 if (!skb) 134 return 0; 135 136 skb_get(skb); 137 if (netif_rx(skb) == NET_RX_SUCCESS) 138 dev_consume_skb_any(skb); 139 else 140 dev_kfree_skb_any(skb); 141 142 return len; 143 } 144 EXPORT_SYMBOL_GPL(can_get_echo_skb); 145 146 /* Remove the skb from the stack and free it. 147 * 148 * The function is typically called when TX failed. 149 */ 150 void can_free_echo_skb(struct net_device *dev, unsigned int idx) 151 { 152 struct can_priv *priv = netdev_priv(dev); 153 154 BUG_ON(idx >= priv->echo_skb_max); 155 156 if (priv->echo_skb[idx]) { 157 dev_kfree_skb_any(priv->echo_skb[idx]); 158 priv->echo_skb[idx] = NULL; 159 } 160 } 161 EXPORT_SYMBOL_GPL(can_free_echo_skb); 162 163 struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf) 164 { 165 struct sk_buff *skb; 166 167 skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) + 168 sizeof(struct can_frame)); 169 if (unlikely(!skb)) 170 return NULL; 171 172 skb->protocol = htons(ETH_P_CAN); 173 skb->pkt_type = PACKET_BROADCAST; 174 skb->ip_summed = CHECKSUM_UNNECESSARY; 175 176 skb_reset_mac_header(skb); 177 skb_reset_network_header(skb); 178 skb_reset_transport_header(skb); 179 180 can_skb_reserve(skb); 181 can_skb_prv(skb)->ifindex = dev->ifindex; 182 can_skb_prv(skb)->skbcnt = 0; 183 184 *cf = skb_put_zero(skb, sizeof(struct can_frame)); 185 186 return skb; 187 } 188 EXPORT_SYMBOL_GPL(alloc_can_skb); 189 190 struct sk_buff *alloc_canfd_skb(struct net_device *dev, 191 struct canfd_frame **cfd) 192 { 193 struct sk_buff *skb; 194 195 skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) + 196 sizeof(struct canfd_frame)); 197 if (unlikely(!skb)) 198 return NULL; 199 200 skb->protocol = htons(ETH_P_CANFD); 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 *cfd = skb_put_zero(skb, sizeof(struct canfd_frame)); 213 214 return skb; 215 } 216 EXPORT_SYMBOL_GPL(alloc_canfd_skb); 217 218 struct sk_buff *alloc_can_err_skb(struct net_device *dev, struct can_frame **cf) 219 { 220 struct sk_buff *skb; 221 222 skb = alloc_can_skb(dev, cf); 223 if (unlikely(!skb)) 224 return NULL; 225 226 (*cf)->can_id = CAN_ERR_FLAG; 227 (*cf)->len = CAN_ERR_DLC; 228 229 return skb; 230 } 231 EXPORT_SYMBOL_GPL(alloc_can_err_skb); 232