1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* Copyright 2011-2014 Autronica Fire and Security AS |
3 | * |
4 | * Author(s): |
5 | * 2011-2014 Arvid Brodin, arvid.brodin@alten.se |
6 | * |
7 | * Frame router for HSR and PRP. |
8 | */ |
9 | |
10 | #include "hsr_forward.h" |
11 | #include <linux/types.h> |
12 | #include <linux/skbuff.h> |
13 | #include <linux/etherdevice.h> |
14 | #include <linux/if_vlan.h> |
15 | #include "hsr_main.h" |
16 | #include "hsr_framereg.h" |
17 | |
18 | struct hsr_node; |
19 | |
20 | /* The uses I can see for these HSR supervision frames are: |
21 | * 1) Use the frames that are sent after node initialization ("HSR_TLV.Type = |
22 | * 22") to reset any sequence_nr counters belonging to that node. Useful if |
23 | * the other node's counter has been reset for some reason. |
24 | * -- |
25 | * Or not - resetting the counter and bridging the frame would create a |
26 | * loop, unfortunately. |
27 | * |
28 | * 2) Use the LifeCheck frames to detect ring breaks. I.e. if no LifeCheck |
29 | * frame is received from a particular node, we know something is wrong. |
30 | * We just register these (as with normal frames) and throw them away. |
31 | * |
32 | * 3) Allow different MAC addresses for the two slave interfaces, using the |
33 | * MacAddressA field. |
34 | */ |
35 | static bool is_supervision_frame(struct hsr_priv *hsr, struct sk_buff *skb) |
36 | { |
37 | struct ethhdr *eth_hdr; |
38 | struct hsr_sup_tag *hsr_sup_tag; |
39 | struct hsrv1_ethhdr_sp *hsr_V1_hdr; |
40 | struct hsr_sup_tlv *hsr_sup_tlv; |
41 | u16 total_length = 0; |
42 | |
43 | WARN_ON_ONCE(!skb_mac_header_was_set(skb)); |
44 | eth_hdr = (struct ethhdr *)skb_mac_header(skb); |
45 | |
46 | /* Correct addr? */ |
47 | if (!ether_addr_equal(addr1: eth_hdr->h_dest, |
48 | addr2: hsr->sup_multicast_addr)) |
49 | return false; |
50 | |
51 | /* Correct ether type?. */ |
52 | if (!(eth_hdr->h_proto == htons(ETH_P_PRP) || |
53 | eth_hdr->h_proto == htons(ETH_P_HSR))) |
54 | return false; |
55 | |
56 | /* Get the supervision header from correct location. */ |
57 | if (eth_hdr->h_proto == htons(ETH_P_HSR)) { /* Okay HSRv1. */ |
58 | total_length = sizeof(struct hsrv1_ethhdr_sp); |
59 | if (!pskb_may_pull(skb, len: total_length)) |
60 | return false; |
61 | |
62 | hsr_V1_hdr = (struct hsrv1_ethhdr_sp *)skb_mac_header(skb); |
63 | if (hsr_V1_hdr->hsr.encap_proto != htons(ETH_P_PRP)) |
64 | return false; |
65 | |
66 | hsr_sup_tag = &hsr_V1_hdr->hsr_sup; |
67 | } else { |
68 | total_length = sizeof(struct hsrv0_ethhdr_sp); |
69 | if (!pskb_may_pull(skb, len: total_length)) |
70 | return false; |
71 | |
72 | hsr_sup_tag = |
73 | &((struct hsrv0_ethhdr_sp *)skb_mac_header(skb))->hsr_sup; |
74 | } |
75 | |
76 | if (hsr_sup_tag->tlv.HSR_TLV_type != HSR_TLV_ANNOUNCE && |
77 | hsr_sup_tag->tlv.HSR_TLV_type != HSR_TLV_LIFE_CHECK && |
78 | hsr_sup_tag->tlv.HSR_TLV_type != PRP_TLV_LIFE_CHECK_DD && |
79 | hsr_sup_tag->tlv.HSR_TLV_type != PRP_TLV_LIFE_CHECK_DA) |
80 | return false; |
81 | if (hsr_sup_tag->tlv.HSR_TLV_length != 12 && |
82 | hsr_sup_tag->tlv.HSR_TLV_length != sizeof(struct hsr_sup_payload)) |
83 | return false; |
84 | |
85 | /* Get next tlv */ |
86 | total_length += sizeof(struct hsr_sup_tlv) + hsr_sup_tag->tlv.HSR_TLV_length; |
87 | if (!pskb_may_pull(skb, len: total_length)) |
88 | return false; |
89 | skb_pull(skb, len: total_length); |
90 | hsr_sup_tlv = (struct hsr_sup_tlv *)skb->data; |
91 | skb_push(skb, len: total_length); |
92 | |
93 | /* if this is a redbox supervision frame we need to verify |
94 | * that more data is available |
95 | */ |
96 | if (hsr_sup_tlv->HSR_TLV_type == PRP_TLV_REDBOX_MAC) { |
97 | /* tlv length must be a length of a mac address */ |
98 | if (hsr_sup_tlv->HSR_TLV_length != sizeof(struct hsr_sup_payload)) |
99 | return false; |
100 | |
101 | /* make sure another tlv follows */ |
102 | total_length += sizeof(struct hsr_sup_tlv) + hsr_sup_tlv->HSR_TLV_length; |
103 | if (!pskb_may_pull(skb, len: total_length)) |
104 | return false; |
105 | |
106 | /* get next tlv */ |
107 | skb_pull(skb, len: total_length); |
108 | hsr_sup_tlv = (struct hsr_sup_tlv *)skb->data; |
109 | skb_push(skb, len: total_length); |
110 | } |
111 | |
112 | /* end of tlvs must follow at the end */ |
113 | if (hsr_sup_tlv->HSR_TLV_type == HSR_TLV_EOT && |
114 | hsr_sup_tlv->HSR_TLV_length != 0) |
115 | return false; |
116 | |
117 | return true; |
118 | } |
119 | |
120 | static struct sk_buff *create_stripped_skb_hsr(struct sk_buff *skb_in, |
121 | struct hsr_frame_info *frame) |
122 | { |
123 | struct sk_buff *skb; |
124 | int copylen; |
125 | unsigned char *dst, *src; |
126 | |
127 | skb_pull(skb: skb_in, HSR_HLEN); |
128 | skb = __pskb_copy(skb: skb_in, headroom: skb_headroom(skb: skb_in) - HSR_HLEN, GFP_ATOMIC); |
129 | skb_push(skb: skb_in, HSR_HLEN); |
130 | if (!skb) |
131 | return NULL; |
132 | |
133 | skb_reset_mac_header(skb); |
134 | |
135 | if (skb->ip_summed == CHECKSUM_PARTIAL) |
136 | skb->csum_start -= HSR_HLEN; |
137 | |
138 | copylen = 2 * ETH_ALEN; |
139 | if (frame->is_vlan) |
140 | copylen += VLAN_HLEN; |
141 | src = skb_mac_header(skb: skb_in); |
142 | dst = skb_mac_header(skb); |
143 | memcpy(dst, src, copylen); |
144 | |
145 | skb->protocol = eth_hdr(skb)->h_proto; |
146 | return skb; |
147 | } |
148 | |
149 | struct sk_buff *hsr_get_untagged_frame(struct hsr_frame_info *frame, |
150 | struct hsr_port *port) |
151 | { |
152 | if (!frame->skb_std) { |
153 | if (frame->skb_hsr) |
154 | frame->skb_std = |
155 | create_stripped_skb_hsr(skb_in: frame->skb_hsr, frame); |
156 | else |
157 | netdev_warn_once(port->dev, |
158 | "Unexpected frame received in hsr_get_untagged_frame()\n" ); |
159 | |
160 | if (!frame->skb_std) |
161 | return NULL; |
162 | } |
163 | |
164 | return skb_clone(skb: frame->skb_std, GFP_ATOMIC); |
165 | } |
166 | |
167 | struct sk_buff *prp_get_untagged_frame(struct hsr_frame_info *frame, |
168 | struct hsr_port *port) |
169 | { |
170 | if (!frame->skb_std) { |
171 | if (frame->skb_prp) { |
172 | /* trim the skb by len - HSR_HLEN to exclude RCT */ |
173 | skb_trim(skb: frame->skb_prp, |
174 | len: frame->skb_prp->len - HSR_HLEN); |
175 | frame->skb_std = |
176 | __pskb_copy(skb: frame->skb_prp, |
177 | headroom: skb_headroom(skb: frame->skb_prp), |
178 | GFP_ATOMIC); |
179 | } else { |
180 | /* Unexpected */ |
181 | WARN_ONCE(1, "%s:%d: Unexpected frame received (port_src %s)\n" , |
182 | __FILE__, __LINE__, port->dev->name); |
183 | return NULL; |
184 | } |
185 | } |
186 | |
187 | return skb_clone(skb: frame->skb_std, GFP_ATOMIC); |
188 | } |
189 | |
190 | static void prp_set_lan_id(struct prp_rct *trailer, |
191 | struct hsr_port *port) |
192 | { |
193 | int lane_id; |
194 | |
195 | if (port->type == HSR_PT_SLAVE_A) |
196 | lane_id = 0; |
197 | else |
198 | lane_id = 1; |
199 | |
200 | /* Add net_id in the upper 3 bits of lane_id */ |
201 | lane_id |= port->hsr->net_id; |
202 | set_prp_lan_id(rct: trailer, lan_id: lane_id); |
203 | } |
204 | |
205 | /* Tailroom for PRP rct should have been created before calling this */ |
206 | static struct sk_buff *prp_fill_rct(struct sk_buff *skb, |
207 | struct hsr_frame_info *frame, |
208 | struct hsr_port *port) |
209 | { |
210 | struct prp_rct *trailer; |
211 | int min_size = ETH_ZLEN; |
212 | int lsdu_size; |
213 | |
214 | if (!skb) |
215 | return skb; |
216 | |
217 | if (frame->is_vlan) |
218 | min_size = VLAN_ETH_ZLEN; |
219 | |
220 | if (skb_put_padto(skb, len: min_size)) |
221 | return NULL; |
222 | |
223 | trailer = (struct prp_rct *)skb_put(skb, HSR_HLEN); |
224 | lsdu_size = skb->len - 14; |
225 | if (frame->is_vlan) |
226 | lsdu_size -= 4; |
227 | prp_set_lan_id(trailer, port); |
228 | set_prp_LSDU_size(rct: trailer, LSDU_size: lsdu_size); |
229 | trailer->sequence_nr = htons(frame->sequence_nr); |
230 | trailer->PRP_suffix = htons(ETH_P_PRP); |
231 | skb->protocol = eth_hdr(skb)->h_proto; |
232 | |
233 | return skb; |
234 | } |
235 | |
236 | static void hsr_set_path_id(struct hsr_ethhdr *hsr_ethhdr, |
237 | struct hsr_port *port) |
238 | { |
239 | int path_id; |
240 | |
241 | if (port->type == HSR_PT_SLAVE_A) |
242 | path_id = 0; |
243 | else |
244 | path_id = 1; |
245 | |
246 | set_hsr_tag_path(ht: &hsr_ethhdr->hsr_tag, path: path_id); |
247 | } |
248 | |
249 | static struct sk_buff *hsr_fill_tag(struct sk_buff *skb, |
250 | struct hsr_frame_info *frame, |
251 | struct hsr_port *port, u8 proto_version) |
252 | { |
253 | struct hsr_ethhdr *hsr_ethhdr; |
254 | int lsdu_size; |
255 | |
256 | /* pad to minimum packet size which is 60 + 6 (HSR tag) */ |
257 | if (skb_put_padto(skb, ETH_ZLEN + HSR_HLEN)) |
258 | return NULL; |
259 | |
260 | lsdu_size = skb->len - 14; |
261 | if (frame->is_vlan) |
262 | lsdu_size -= 4; |
263 | |
264 | hsr_ethhdr = (struct hsr_ethhdr *)skb_mac_header(skb); |
265 | |
266 | hsr_set_path_id(hsr_ethhdr, port); |
267 | set_hsr_tag_LSDU_size(ht: &hsr_ethhdr->hsr_tag, LSDU_size: lsdu_size); |
268 | hsr_ethhdr->hsr_tag.sequence_nr = htons(frame->sequence_nr); |
269 | hsr_ethhdr->hsr_tag.encap_proto = hsr_ethhdr->ethhdr.h_proto; |
270 | hsr_ethhdr->ethhdr.h_proto = htons(proto_version ? |
271 | ETH_P_HSR : ETH_P_PRP); |
272 | skb->protocol = hsr_ethhdr->ethhdr.h_proto; |
273 | |
274 | return skb; |
275 | } |
276 | |
277 | /* If the original frame was an HSR tagged frame, just clone it to be sent |
278 | * unchanged. Otherwise, create a private frame especially tagged for 'port'. |
279 | */ |
280 | struct sk_buff *hsr_create_tagged_frame(struct hsr_frame_info *frame, |
281 | struct hsr_port *port) |
282 | { |
283 | unsigned char *dst, *src; |
284 | struct sk_buff *skb; |
285 | int movelen; |
286 | |
287 | if (frame->skb_hsr) { |
288 | struct hsr_ethhdr *hsr_ethhdr = |
289 | (struct hsr_ethhdr *)skb_mac_header(skb: frame->skb_hsr); |
290 | |
291 | /* set the lane id properly */ |
292 | hsr_set_path_id(hsr_ethhdr, port); |
293 | return skb_clone(skb: frame->skb_hsr, GFP_ATOMIC); |
294 | } else if (port->dev->features & NETIF_F_HW_HSR_TAG_INS) { |
295 | return skb_clone(skb: frame->skb_std, GFP_ATOMIC); |
296 | } |
297 | |
298 | /* Create the new skb with enough headroom to fit the HSR tag */ |
299 | skb = __pskb_copy(skb: frame->skb_std, |
300 | headroom: skb_headroom(skb: frame->skb_std) + HSR_HLEN, GFP_ATOMIC); |
301 | if (!skb) |
302 | return NULL; |
303 | skb_reset_mac_header(skb); |
304 | |
305 | if (skb->ip_summed == CHECKSUM_PARTIAL) |
306 | skb->csum_start += HSR_HLEN; |
307 | |
308 | movelen = ETH_HLEN; |
309 | if (frame->is_vlan) |
310 | movelen += VLAN_HLEN; |
311 | |
312 | src = skb_mac_header(skb); |
313 | dst = skb_push(skb, HSR_HLEN); |
314 | memmove(dst, src, movelen); |
315 | skb_reset_mac_header(skb); |
316 | |
317 | /* skb_put_padto free skb on error and hsr_fill_tag returns NULL in |
318 | * that case |
319 | */ |
320 | return hsr_fill_tag(skb, frame, port, proto_version: port->hsr->prot_version); |
321 | } |
322 | |
323 | struct sk_buff *prp_create_tagged_frame(struct hsr_frame_info *frame, |
324 | struct hsr_port *port) |
325 | { |
326 | struct sk_buff *skb; |
327 | |
328 | if (frame->skb_prp) { |
329 | struct prp_rct *trailer = skb_get_PRP_rct(skb: frame->skb_prp); |
330 | |
331 | if (trailer) { |
332 | prp_set_lan_id(trailer, port); |
333 | } else { |
334 | WARN_ONCE(!trailer, "errored PRP skb" ); |
335 | return NULL; |
336 | } |
337 | return skb_clone(skb: frame->skb_prp, GFP_ATOMIC); |
338 | } else if (port->dev->features & NETIF_F_HW_HSR_TAG_INS) { |
339 | return skb_clone(skb: frame->skb_std, GFP_ATOMIC); |
340 | } |
341 | |
342 | skb = skb_copy_expand(skb: frame->skb_std, newheadroom: 0, |
343 | newtailroom: skb_tailroom(skb: frame->skb_std) + HSR_HLEN, |
344 | GFP_ATOMIC); |
345 | prp_fill_rct(skb, frame, port); |
346 | |
347 | return skb; |
348 | } |
349 | |
350 | static void hsr_deliver_master(struct sk_buff *skb, struct net_device *dev, |
351 | struct hsr_node *node_src) |
352 | { |
353 | bool was_multicast_frame; |
354 | int res, recv_len; |
355 | |
356 | was_multicast_frame = (skb->pkt_type == PACKET_MULTICAST); |
357 | hsr_addr_subst_source(node: node_src, skb); |
358 | skb_pull(skb, ETH_HLEN); |
359 | recv_len = skb->len; |
360 | res = netif_rx(skb); |
361 | if (res == NET_RX_DROP) { |
362 | dev->stats.rx_dropped++; |
363 | } else { |
364 | dev->stats.rx_packets++; |
365 | dev->stats.rx_bytes += recv_len; |
366 | if (was_multicast_frame) |
367 | dev->stats.multicast++; |
368 | } |
369 | } |
370 | |
371 | static int hsr_xmit(struct sk_buff *skb, struct hsr_port *port, |
372 | struct hsr_frame_info *frame) |
373 | { |
374 | if (frame->port_rcv->type == HSR_PT_MASTER) { |
375 | hsr_addr_subst_dest(node_src: frame->node_src, skb, port); |
376 | |
377 | /* Address substitution (IEC62439-3 pp 26, 50): replace mac |
378 | * address of outgoing frame with that of the outgoing slave's. |
379 | */ |
380 | ether_addr_copy(dst: eth_hdr(skb)->h_source, src: port->dev->dev_addr); |
381 | } |
382 | return dev_queue_xmit(skb); |
383 | } |
384 | |
385 | bool prp_drop_frame(struct hsr_frame_info *frame, struct hsr_port *port) |
386 | { |
387 | return ((frame->port_rcv->type == HSR_PT_SLAVE_A && |
388 | port->type == HSR_PT_SLAVE_B) || |
389 | (frame->port_rcv->type == HSR_PT_SLAVE_B && |
390 | port->type == HSR_PT_SLAVE_A)); |
391 | } |
392 | |
393 | bool hsr_drop_frame(struct hsr_frame_info *frame, struct hsr_port *port) |
394 | { |
395 | if (port->dev->features & NETIF_F_HW_HSR_FWD) |
396 | return prp_drop_frame(frame, port); |
397 | |
398 | return false; |
399 | } |
400 | |
401 | /* Forward the frame through all devices except: |
402 | * - Back through the receiving device |
403 | * - If it's a HSR frame: through a device where it has passed before |
404 | * - if it's a PRP frame: through another PRP slave device (no bridge) |
405 | * - To the local HSR master only if the frame is directly addressed to it, or |
406 | * a non-supervision multicast or broadcast frame. |
407 | * |
408 | * HSR slave devices should insert a HSR tag into the frame, or forward the |
409 | * frame unchanged if it's already tagged. Interlink devices should strip HSR |
410 | * tags if they're of the non-HSR type (but only after duplicate discard). The |
411 | * master device always strips HSR tags. |
412 | */ |
413 | static void hsr_forward_do(struct hsr_frame_info *frame) |
414 | { |
415 | struct hsr_port *port; |
416 | struct sk_buff *skb; |
417 | bool sent = false; |
418 | |
419 | hsr_for_each_port(frame->port_rcv->hsr, port) { |
420 | struct hsr_priv *hsr = port->hsr; |
421 | /* Don't send frame back the way it came */ |
422 | if (port == frame->port_rcv) |
423 | continue; |
424 | |
425 | /* Don't deliver locally unless we should */ |
426 | if (port->type == HSR_PT_MASTER && !frame->is_local_dest) |
427 | continue; |
428 | |
429 | /* Deliver frames directly addressed to us to master only */ |
430 | if (port->type != HSR_PT_MASTER && frame->is_local_exclusive) |
431 | continue; |
432 | |
433 | /* If hardware duplicate generation is enabled, only send out |
434 | * one port. |
435 | */ |
436 | if ((port->dev->features & NETIF_F_HW_HSR_DUP) && sent) |
437 | continue; |
438 | |
439 | /* Don't send frame over port where it has been sent before. |
440 | * Also fro SAN, this shouldn't be done. |
441 | */ |
442 | if (!frame->is_from_san && |
443 | hsr_register_frame_out(port, node: frame->node_src, |
444 | sequence_nr: frame->sequence_nr)) |
445 | continue; |
446 | |
447 | if (frame->is_supervision && port->type == HSR_PT_MASTER) { |
448 | hsr_handle_sup_frame(frame); |
449 | continue; |
450 | } |
451 | |
452 | /* Check if frame is to be dropped. Eg. for PRP no forward |
453 | * between ports. |
454 | */ |
455 | if (hsr->proto_ops->drop_frame && |
456 | hsr->proto_ops->drop_frame(frame, port)) |
457 | continue; |
458 | |
459 | if (port->type != HSR_PT_MASTER) |
460 | skb = hsr->proto_ops->create_tagged_frame(frame, port); |
461 | else |
462 | skb = hsr->proto_ops->get_untagged_frame(frame, port); |
463 | |
464 | if (!skb) { |
465 | frame->port_rcv->dev->stats.rx_dropped++; |
466 | continue; |
467 | } |
468 | |
469 | skb->dev = port->dev; |
470 | if (port->type == HSR_PT_MASTER) { |
471 | hsr_deliver_master(skb, dev: port->dev, node_src: frame->node_src); |
472 | } else { |
473 | if (!hsr_xmit(skb, port, frame)) |
474 | sent = true; |
475 | } |
476 | } |
477 | } |
478 | |
479 | static void check_local_dest(struct hsr_priv *hsr, struct sk_buff *skb, |
480 | struct hsr_frame_info *frame) |
481 | { |
482 | if (hsr_addr_is_self(hsr, addr: eth_hdr(skb)->h_dest)) { |
483 | frame->is_local_exclusive = true; |
484 | skb->pkt_type = PACKET_HOST; |
485 | } else { |
486 | frame->is_local_exclusive = false; |
487 | } |
488 | |
489 | if (skb->pkt_type == PACKET_HOST || |
490 | skb->pkt_type == PACKET_MULTICAST || |
491 | skb->pkt_type == PACKET_BROADCAST) { |
492 | frame->is_local_dest = true; |
493 | } else { |
494 | frame->is_local_dest = false; |
495 | } |
496 | } |
497 | |
498 | static void handle_std_frame(struct sk_buff *skb, |
499 | struct hsr_frame_info *frame) |
500 | { |
501 | struct hsr_port *port = frame->port_rcv; |
502 | struct hsr_priv *hsr = port->hsr; |
503 | |
504 | frame->skb_hsr = NULL; |
505 | frame->skb_prp = NULL; |
506 | frame->skb_std = skb; |
507 | |
508 | if (port->type != HSR_PT_MASTER) { |
509 | frame->is_from_san = true; |
510 | } else { |
511 | /* Sequence nr for the master node */ |
512 | lockdep_assert_held(&hsr->seqnr_lock); |
513 | frame->sequence_nr = hsr->sequence_nr; |
514 | hsr->sequence_nr++; |
515 | } |
516 | } |
517 | |
518 | int hsr_fill_frame_info(__be16 proto, struct sk_buff *skb, |
519 | struct hsr_frame_info *frame) |
520 | { |
521 | struct hsr_port *port = frame->port_rcv; |
522 | struct hsr_priv *hsr = port->hsr; |
523 | |
524 | /* HSRv0 supervisory frames double as a tag so treat them as tagged. */ |
525 | if ((!hsr->prot_version && proto == htons(ETH_P_PRP)) || |
526 | proto == htons(ETH_P_HSR)) { |
527 | /* Check if skb contains hsr_ethhdr */ |
528 | if (skb->mac_len < sizeof(struct hsr_ethhdr)) |
529 | return -EINVAL; |
530 | |
531 | /* HSR tagged frame :- Data or Supervision */ |
532 | frame->skb_std = NULL; |
533 | frame->skb_prp = NULL; |
534 | frame->skb_hsr = skb; |
535 | frame->sequence_nr = hsr_get_skb_sequence_nr(skb); |
536 | return 0; |
537 | } |
538 | |
539 | /* Standard frame or PRP from master port */ |
540 | handle_std_frame(skb, frame); |
541 | |
542 | return 0; |
543 | } |
544 | |
545 | int prp_fill_frame_info(__be16 proto, struct sk_buff *skb, |
546 | struct hsr_frame_info *frame) |
547 | { |
548 | /* Supervision frame */ |
549 | struct prp_rct *rct = skb_get_PRP_rct(skb); |
550 | |
551 | if (rct && |
552 | prp_check_lsdu_size(skb, rct, is_sup: frame->is_supervision)) { |
553 | frame->skb_hsr = NULL; |
554 | frame->skb_std = NULL; |
555 | frame->skb_prp = skb; |
556 | frame->sequence_nr = prp_get_skb_sequence_nr(rct); |
557 | return 0; |
558 | } |
559 | handle_std_frame(skb, frame); |
560 | |
561 | return 0; |
562 | } |
563 | |
564 | static int fill_frame_info(struct hsr_frame_info *frame, |
565 | struct sk_buff *skb, struct hsr_port *port) |
566 | { |
567 | struct hsr_priv *hsr = port->hsr; |
568 | struct hsr_vlan_ethhdr *vlan_hdr; |
569 | struct ethhdr *ethhdr; |
570 | __be16 proto; |
571 | int ret; |
572 | |
573 | /* Check if skb contains ethhdr */ |
574 | if (skb->mac_len < sizeof(struct ethhdr)) |
575 | return -EINVAL; |
576 | |
577 | memset(frame, 0, sizeof(*frame)); |
578 | frame->is_supervision = is_supervision_frame(hsr: port->hsr, skb); |
579 | frame->node_src = hsr_get_node(port, node_db: &hsr->node_db, skb, |
580 | is_sup: frame->is_supervision, |
581 | rx_port: port->type); |
582 | if (!frame->node_src) |
583 | return -1; /* Unknown node and !is_supervision, or no mem */ |
584 | |
585 | ethhdr = (struct ethhdr *)skb_mac_header(skb); |
586 | frame->is_vlan = false; |
587 | proto = ethhdr->h_proto; |
588 | |
589 | if (proto == htons(ETH_P_8021Q)) |
590 | frame->is_vlan = true; |
591 | |
592 | if (frame->is_vlan) { |
593 | vlan_hdr = (struct hsr_vlan_ethhdr *)ethhdr; |
594 | proto = vlan_hdr->vlanhdr.h_vlan_encapsulated_proto; |
595 | /* FIXME: */ |
596 | netdev_warn_once(skb->dev, "VLAN not yet supported" ); |
597 | return -EINVAL; |
598 | } |
599 | |
600 | frame->is_from_san = false; |
601 | frame->port_rcv = port; |
602 | ret = hsr->proto_ops->fill_frame_info(proto, skb, frame); |
603 | if (ret) |
604 | return ret; |
605 | |
606 | check_local_dest(hsr: port->hsr, skb, frame); |
607 | |
608 | return 0; |
609 | } |
610 | |
611 | /* Must be called holding rcu read lock (because of the port parameter) */ |
612 | void hsr_forward_skb(struct sk_buff *skb, struct hsr_port *port) |
613 | { |
614 | struct hsr_frame_info frame; |
615 | |
616 | rcu_read_lock(); |
617 | if (fill_frame_info(frame: &frame, skb, port) < 0) |
618 | goto out_drop; |
619 | |
620 | hsr_register_frame_in(node: frame.node_src, port, sequence_nr: frame.sequence_nr); |
621 | hsr_forward_do(frame: &frame); |
622 | rcu_read_unlock(); |
623 | /* Gets called for ingress frames as well as egress from master port. |
624 | * So check and increment stats for master port only here. |
625 | */ |
626 | if (port->type == HSR_PT_MASTER) { |
627 | port->dev->stats.tx_packets++; |
628 | port->dev->stats.tx_bytes += skb->len; |
629 | } |
630 | |
631 | kfree_skb(skb: frame.skb_hsr); |
632 | kfree_skb(skb: frame.skb_prp); |
633 | kfree_skb(skb: frame.skb_std); |
634 | return; |
635 | |
636 | out_drop: |
637 | rcu_read_unlock(); |
638 | port->dev->stats.tx_dropped++; |
639 | kfree_skb(skb); |
640 | } |
641 | |