375 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/* Copyright (C) 2005 Marc Kleine-Budde, Pengutronix3 * Copyright (C) 2006 Andrey Volkov, Varma Electronics4 * Copyright (C) 2008-2009 Wolfgang Grandegger <wg@grandegger.com>5 */6 7#include <linux/can/dev.h>8#include <linux/module.h>9 10#define MOD_DESC "CAN device driver interface"11 12MODULE_DESCRIPTION(MOD_DESC);13MODULE_LICENSE("GPL v2");14MODULE_AUTHOR("Wolfgang Grandegger <wg@grandegger.com>");15 16/* Local echo of CAN messages17 *18 * CAN network devices *should* support a local echo functionality19 * (see Documentation/networking/can.rst). To test the handling of CAN20 * interfaces that do not support the local echo both driver types are21 * implemented. In the case that the driver does not support the echo22 * the IFF_ECHO remains clear in dev->flags. This causes the PF_CAN core23 * to perform the echo as a fallback solution.24 */25void can_flush_echo_skb(struct net_device *dev)26{27 struct can_priv *priv = netdev_priv(dev);28 struct net_device_stats *stats = &dev->stats;29 int i;30 31 for (i = 0; i < priv->echo_skb_max; i++) {32 if (priv->echo_skb[i]) {33 kfree_skb(priv->echo_skb[i]);34 priv->echo_skb[i] = NULL;35 stats->tx_dropped++;36 stats->tx_aborted_errors++;37 }38 }39}40 41/* Put the skb on the stack to be looped backed locally lateron42 *43 * The function is typically called in the start_xmit function44 * of the device driver. The driver must protect access to45 * priv->echo_skb, if necessary.46 */47int can_put_echo_skb(struct sk_buff *skb, struct net_device *dev,48 unsigned int idx, unsigned int frame_len)49{50 struct can_priv *priv = netdev_priv(dev);51 52 if (idx >= priv->echo_skb_max) {53 netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",54 __func__, idx, priv->echo_skb_max);55 return -EINVAL;56 }57 58 /* check flag whether this packet has to be looped back */59 if (!(dev->flags & IFF_ECHO) ||60 (skb->protocol != htons(ETH_P_CAN) &&61 skb->protocol != htons(ETH_P_CANFD) &&62 skb->protocol != htons(ETH_P_CANXL))) {63 kfree_skb(skb);64 return 0;65 }66 67 if (!priv->echo_skb[idx]) {68 skb = can_create_echo_skb(skb);69 if (!skb)70 return -ENOMEM;71 72 /* make settings for echo to reduce code in irq context */73 skb->ip_summed = CHECKSUM_UNNECESSARY;74 skb->dev = dev;75 76 /* save frame_len to reuse it when transmission is completed */77 can_skb_prv(skb)->frame_len = frame_len;78 79 if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)80 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;81 82 skb_tx_timestamp(skb);83 84 /* save this skb for tx interrupt echo handling */85 priv->echo_skb[idx] = skb;86 } else {87 /* locking problem with netif_stop_queue() ?? */88 netdev_err(dev, "%s: BUG! echo_skb %d is occupied!\n", __func__, idx);89 kfree_skb(skb);90 return -EBUSY;91 }92 93 return 0;94}95EXPORT_SYMBOL_GPL(can_put_echo_skb);96 97struct sk_buff *98__can_get_echo_skb(struct net_device *dev, unsigned int idx,99 unsigned int *len_ptr, unsigned int *frame_len_ptr)100{101 struct can_priv *priv = netdev_priv(dev);102 103 if (idx >= priv->echo_skb_max) {104 netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",105 __func__, idx, priv->echo_skb_max);106 return NULL;107 }108 109 if (priv->echo_skb[idx]) {110 /* Using "struct canfd_frame::len" for the frame111 * length is supported on both CAN and CANFD frames.112 */113 struct sk_buff *skb = priv->echo_skb[idx];114 struct can_skb_priv *can_skb_priv = can_skb_prv(skb);115 116 if (skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)117 skb_tstamp_tx(skb, skb_hwtstamps(skb));118 119 /* get the real payload length for netdev statistics */120 *len_ptr = can_skb_get_data_len(skb);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 locally141 *142 * The function is typically called when the TX done interrupt143 * is handled in the device driver. The driver must protect144 * access to priv->echo_skb, if necessary.145 */146unsigned 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 unsigned int 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 else160 dev_kfree_skb_any(skb);161 162 return len;163}164EXPORT_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 */170void 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}192EXPORT_SYMBOL_GPL(can_free_echo_skb);193 194/* fill common values for CAN sk_buffs */195static void init_can_skb_reserve(struct sk_buff *skb)196{197 skb->pkt_type = PACKET_BROADCAST;198 skb->ip_summed = CHECKSUM_UNNECESSARY;199 200 skb_reset_mac_header(skb);201 skb_reset_network_header(skb);202 skb_reset_transport_header(skb);203 204 can_skb_reserve(skb);205 can_skb_prv(skb)->skbcnt = 0;206}207 208struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf)209{210 struct sk_buff *skb;211 212 skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +213 sizeof(struct can_frame));214 if (unlikely(!skb)) {215 *cf = NULL;216 217 return NULL;218 }219 220 skb->protocol = htons(ETH_P_CAN);221 init_can_skb_reserve(skb);222 can_skb_prv(skb)->ifindex = dev->ifindex;223 224 *cf = skb_put_zero(skb, sizeof(struct can_frame));225 226 return skb;227}228EXPORT_SYMBOL_GPL(alloc_can_skb);229 230struct sk_buff *alloc_canfd_skb(struct net_device *dev,231 struct canfd_frame **cfd)232{233 struct sk_buff *skb;234 235 skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +236 sizeof(struct canfd_frame));237 if (unlikely(!skb)) {238 *cfd = NULL;239 240 return NULL;241 }242 243 skb->protocol = htons(ETH_P_CANFD);244 init_can_skb_reserve(skb);245 can_skb_prv(skb)->ifindex = dev->ifindex;246 247 *cfd = skb_put_zero(skb, sizeof(struct canfd_frame));248 249 /* set CAN FD flag by default */250 (*cfd)->flags = CANFD_FDF;251 252 return skb;253}254EXPORT_SYMBOL_GPL(alloc_canfd_skb);255 256struct sk_buff *alloc_canxl_skb(struct net_device *dev,257 struct canxl_frame **cxl,258 unsigned int data_len)259{260 struct sk_buff *skb;261 262 if (data_len < CANXL_MIN_DLEN || data_len > CANXL_MAX_DLEN)263 goto out_error;264 265 skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +266 CANXL_HDR_SIZE + data_len);267 if (unlikely(!skb))268 goto out_error;269 270 skb->protocol = htons(ETH_P_CANXL);271 init_can_skb_reserve(skb);272 can_skb_prv(skb)->ifindex = dev->ifindex;273 274 *cxl = skb_put_zero(skb, CANXL_HDR_SIZE + data_len);275 276 /* set CAN XL flag and length information by default */277 (*cxl)->flags = CANXL_XLF;278 (*cxl)->len = data_len;279 280 return skb;281 282out_error:283 *cxl = NULL;284 285 return NULL;286}287EXPORT_SYMBOL_GPL(alloc_canxl_skb);288 289struct sk_buff *alloc_can_err_skb(struct net_device *dev, struct can_frame **cf)290{291 struct sk_buff *skb;292 293 skb = alloc_can_skb(dev, cf);294 if (unlikely(!skb))295 return NULL;296 297 (*cf)->can_id = CAN_ERR_FLAG;298 (*cf)->len = CAN_ERR_DLC;299 300 return skb;301}302EXPORT_SYMBOL_GPL(alloc_can_err_skb);303 304/* Check for outgoing skbs that have not been created by the CAN subsystem */305static bool can_skb_headroom_valid(struct net_device *dev, struct sk_buff *skb)306{307 /* af_packet creates a headroom of HH_DATA_MOD bytes which is fine */308 if (WARN_ON_ONCE(skb_headroom(skb) < sizeof(struct can_skb_priv)))309 return false;310 311 /* af_packet does not apply CAN skb specific settings */312 if (skb->ip_summed == CHECKSUM_NONE) {313 /* init headroom */314 can_skb_prv(skb)->ifindex = dev->ifindex;315 can_skb_prv(skb)->skbcnt = 0;316 317 skb->ip_summed = CHECKSUM_UNNECESSARY;318 319 /* perform proper loopback on capable devices */320 if (dev->flags & IFF_ECHO)321 skb->pkt_type = PACKET_LOOPBACK;322 else323 skb->pkt_type = PACKET_HOST;324 325 skb_reset_mac_header(skb);326 skb_reset_network_header(skb);327 skb_reset_transport_header(skb);328 329 /* set CANFD_FDF flag for CAN FD frames */330 if (can_is_canfd_skb(skb)) {331 struct canfd_frame *cfd;332 333 cfd = (struct canfd_frame *)skb->data;334 cfd->flags |= CANFD_FDF;335 }336 }337 338 return true;339}340 341/* Drop a given socketbuffer if it does not contain a valid CAN frame. */342bool can_dropped_invalid_skb(struct net_device *dev, struct sk_buff *skb)343{344 switch (ntohs(skb->protocol)) {345 case ETH_P_CAN:346 if (!can_is_can_skb(skb))347 goto inval_skb;348 break;349 350 case ETH_P_CANFD:351 if (!can_is_canfd_skb(skb))352 goto inval_skb;353 break;354 355 case ETH_P_CANXL:356 if (!can_is_canxl_skb(skb))357 goto inval_skb;358 break;359 360 default:361 goto inval_skb;362 }363 364 if (!can_skb_headroom_valid(dev, skb))365 goto inval_skb;366 367 return false;368 369inval_skb:370 kfree_skb(skb);371 dev->stats.tx_dropped++;372 return true;373}374EXPORT_SYMBOL_GPL(can_dropped_invalid_skb);375