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