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) || 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->ip_summed = CHECKSUM_UNNECESSARY; 62 skb->dev = dev; 63 64 /* save frame_len to reuse it when transmission is completed */ 65 can_skb_prv(skb)->frame_len = frame_len; 66 67 skb_tx_timestamp(skb); 68 69 /* save this skb for tx interrupt echo handling */ 70 priv->echo_skb[idx] = skb; 71 } else { 72 /* locking problem with netif_stop_queue() ?? */ 73 netdev_err(dev, "%s: BUG! echo_skb %d is occupied!\n", __func__, idx); 74 kfree_skb(skb); 75 return -EBUSY; 76 } 77 78 return 0; 79 } 80 EXPORT_SYMBOL_GPL(can_put_echo_skb); 81 82 struct sk_buff * 83 __can_get_echo_skb(struct net_device *dev, unsigned int idx, u8 *len_ptr, 84 unsigned int *frame_len_ptr) 85 { 86 struct can_priv *priv = netdev_priv(dev); 87 88 if (idx >= priv->echo_skb_max) { 89 netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n", 90 __func__, idx, priv->echo_skb_max); 91 return NULL; 92 } 93 94 if (priv->echo_skb[idx]) { 95 /* Using "struct canfd_frame::len" for the frame 96 * length is supported on both CAN and CANFD frames. 97 */ 98 struct sk_buff *skb = priv->echo_skb[idx]; 99 struct can_skb_priv *can_skb_priv = can_skb_prv(skb); 100 struct canfd_frame *cf = (struct canfd_frame *)skb->data; 101 102 /* get the real payload length for netdev statistics */ 103 if (cf->can_id & CAN_RTR_FLAG) 104 *len_ptr = 0; 105 else 106 *len_ptr = cf->len; 107 108 if (frame_len_ptr) 109 *frame_len_ptr = can_skb_priv->frame_len; 110 111 priv->echo_skb[idx] = NULL; 112 113 if (skb->pkt_type == PACKET_LOOPBACK) { 114 skb->pkt_type = PACKET_BROADCAST; 115 } else { 116 dev_consume_skb_any(skb); 117 return NULL; 118 } 119 120 return skb; 121 } 122 123 return NULL; 124 } 125 126 /* Get the skb from the stack and loop it back locally 127 * 128 * The function is typically called when the TX done interrupt 129 * is handled in the device driver. The driver must protect 130 * access to priv->echo_skb, if necessary. 131 */ 132 unsigned int can_get_echo_skb(struct net_device *dev, unsigned int idx, 133 unsigned int *frame_len_ptr) 134 { 135 struct sk_buff *skb; 136 u8 len; 137 138 skb = __can_get_echo_skb(dev, idx, &len, frame_len_ptr); 139 if (!skb) 140 return 0; 141 142 skb_get(skb); 143 if (netif_rx(skb) == NET_RX_SUCCESS) 144 dev_consume_skb_any(skb); 145 else 146 dev_kfree_skb_any(skb); 147 148 return len; 149 } 150 EXPORT_SYMBOL_GPL(can_get_echo_skb); 151 152 /* Remove the skb from the stack and free it. 153 * 154 * The function is typically called when TX failed. 155 */ 156 void can_free_echo_skb(struct net_device *dev, unsigned int idx, 157 unsigned int *frame_len_ptr) 158 { 159 struct can_priv *priv = netdev_priv(dev); 160 161 if (idx >= priv->echo_skb_max) { 162 netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n", 163 __func__, idx, priv->echo_skb_max); 164 return; 165 } 166 167 if (priv->echo_skb[idx]) { 168 struct sk_buff *skb = priv->echo_skb[idx]; 169 struct can_skb_priv *can_skb_priv = can_skb_prv(skb); 170 171 if (frame_len_ptr) 172 *frame_len_ptr = can_skb_priv->frame_len; 173 174 dev_kfree_skb_any(skb); 175 priv->echo_skb[idx] = NULL; 176 } 177 } 178 EXPORT_SYMBOL_GPL(can_free_echo_skb); 179 180 struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf) 181 { 182 struct sk_buff *skb; 183 184 skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) + 185 sizeof(struct can_frame)); 186 if (unlikely(!skb)) { 187 *cf = NULL; 188 189 return NULL; 190 } 191 192 skb->protocol = htons(ETH_P_CAN); 193 skb->pkt_type = PACKET_BROADCAST; 194 skb->ip_summed = CHECKSUM_UNNECESSARY; 195 196 skb_reset_mac_header(skb); 197 skb_reset_network_header(skb); 198 skb_reset_transport_header(skb); 199 200 can_skb_reserve(skb); 201 can_skb_prv(skb)->ifindex = dev->ifindex; 202 can_skb_prv(skb)->skbcnt = 0; 203 204 *cf = skb_put_zero(skb, sizeof(struct can_frame)); 205 206 return skb; 207 } 208 EXPORT_SYMBOL_GPL(alloc_can_skb); 209 210 struct sk_buff *alloc_canfd_skb(struct net_device *dev, 211 struct canfd_frame **cfd) 212 { 213 struct sk_buff *skb; 214 215 skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) + 216 sizeof(struct canfd_frame)); 217 if (unlikely(!skb)) { 218 *cfd = NULL; 219 220 return NULL; 221 } 222 223 skb->protocol = htons(ETH_P_CANFD); 224 skb->pkt_type = PACKET_BROADCAST; 225 skb->ip_summed = CHECKSUM_UNNECESSARY; 226 227 skb_reset_mac_header(skb); 228 skb_reset_network_header(skb); 229 skb_reset_transport_header(skb); 230 231 can_skb_reserve(skb); 232 can_skb_prv(skb)->ifindex = dev->ifindex; 233 can_skb_prv(skb)->skbcnt = 0; 234 235 *cfd = skb_put_zero(skb, sizeof(struct canfd_frame)); 236 237 return skb; 238 } 239 EXPORT_SYMBOL_GPL(alloc_canfd_skb); 240 241 struct sk_buff *alloc_can_err_skb(struct net_device *dev, struct can_frame **cf) 242 { 243 struct sk_buff *skb; 244 245 skb = alloc_can_skb(dev, cf); 246 if (unlikely(!skb)) 247 return NULL; 248 249 (*cf)->can_id = CAN_ERR_FLAG; 250 (*cf)->len = CAN_ERR_DLC; 251 252 return skb; 253 } 254 EXPORT_SYMBOL_GPL(alloc_can_err_skb); 255