Commit | Line | Data |
---|---|---|
396138f0 LB |
1 | /* |
2 | * net/dsa/tag_trailer.c - Trailer tag format handling | |
e84665c9 | 3 | * Copyright (c) 2008-2009 Marvell Semiconductor |
396138f0 LB |
4 | * |
5 | * This program is free software; you can redistribute it and/or modify | |
6 | * it under the terms of the GNU General Public License as published by | |
7 | * the Free Software Foundation; either version 2 of the License, or | |
8 | * (at your option) any later version. | |
9 | */ | |
10 | ||
11 | #include <linux/etherdevice.h> | |
12 | #include <linux/list.h> | |
5a0e3ad6 | 13 | #include <linux/slab.h> |
396138f0 LB |
14 | #include "dsa_priv.h" |
15 | ||
4ed70ce9 | 16 | static struct sk_buff *trailer_xmit(struct sk_buff *skb, struct net_device *dev) |
396138f0 LB |
17 | { |
18 | struct dsa_slave_priv *p = netdev_priv(dev); | |
19 | struct sk_buff *nskb; | |
20 | int padlen; | |
21 | u8 *trailer; | |
22 | ||
396138f0 LB |
23 | /* |
24 | * We have to make sure that the trailer ends up as the very | |
25 | * last 4 bytes of the packet. This means that we have to pad | |
26 | * the packet to the minimum ethernet frame size, if necessary, | |
27 | * before adding the trailer. | |
28 | */ | |
29 | padlen = 0; | |
30 | if (skb->len < 60) | |
31 | padlen = 60 - skb->len; | |
32 | ||
33 | nskb = alloc_skb(NET_IP_ALIGN + skb->len + padlen + 4, GFP_ATOMIC); | |
34 | if (nskb == NULL) { | |
35 | kfree_skb(skb); | |
4ed70ce9 | 36 | return NULL; |
396138f0 LB |
37 | } |
38 | skb_reserve(nskb, NET_IP_ALIGN); | |
39 | ||
40 | skb_reset_mac_header(nskb); | |
41 | skb_set_network_header(nskb, skb_network_header(skb) - skb->head); | |
42 | skb_set_transport_header(nskb, skb_transport_header(skb) - skb->head); | |
43 | skb_copy_and_csum_dev(skb, skb_put(nskb, skb->len)); | |
44 | kfree_skb(skb); | |
45 | ||
46 | if (padlen) { | |
47 | u8 *pad = skb_put(nskb, padlen); | |
48 | memset(pad, 0, padlen); | |
49 | } | |
50 | ||
51 | trailer = skb_put(nskb, 4); | |
52 | trailer[0] = 0x80; | |
53 | trailer[1] = 1 << p->port; | |
54 | trailer[2] = 0x10; | |
55 | trailer[3] = 0x00; | |
56 | ||
4ed70ce9 | 57 | return nskb; |
396138f0 LB |
58 | } |
59 | ||
60 | static int trailer_rcv(struct sk_buff *skb, struct net_device *dev, | |
61 | struct packet_type *pt, struct net_device *orig_dev) | |
62 | { | |
e84665c9 LB |
63 | struct dsa_switch_tree *dst = dev->dsa_ptr; |
64 | struct dsa_switch *ds; | |
396138f0 LB |
65 | u8 *trailer; |
66 | int source_port; | |
67 | ||
e84665c9 | 68 | if (unlikely(dst == NULL)) |
396138f0 | 69 | goto out_drop; |
e84665c9 | 70 | ds = dst->ds[0]; |
396138f0 LB |
71 | |
72 | skb = skb_unshare(skb, GFP_ATOMIC); | |
73 | if (skb == NULL) | |
74 | goto out; | |
75 | ||
76 | if (skb_linearize(skb)) | |
77 | goto out_drop; | |
78 | ||
79 | trailer = skb_tail_pointer(skb) - 4; | |
80 | if (trailer[0] != 0x80 || (trailer[1] & 0xf8) != 0x00 || | |
fbd03513 | 81 | (trailer[2] & 0xef) != 0x00 || trailer[3] != 0x00) |
396138f0 LB |
82 | goto out_drop; |
83 | ||
84 | source_port = trailer[1] & 7; | |
85 | if (source_port >= DSA_MAX_PORTS || ds->ports[source_port] == NULL) | |
86 | goto out_drop; | |
87 | ||
88 | pskb_trim_rcsum(skb, skb->len - 4); | |
89 | ||
90 | skb->dev = ds->ports[source_port]; | |
91 | skb_push(skb, ETH_HLEN); | |
14ee6742 | 92 | skb->pkt_type = PACKET_HOST; |
396138f0 LB |
93 | skb->protocol = eth_type_trans(skb, skb->dev); |
94 | ||
396138f0 LB |
95 | skb->dev->stats.rx_packets++; |
96 | skb->dev->stats.rx_bytes += skb->len; | |
97 | ||
98 | netif_receive_skb(skb); | |
99 | ||
100 | return 0; | |
101 | ||
102 | out_drop: | |
103 | kfree_skb(skb); | |
104 | out: | |
105 | return 0; | |
106 | } | |
107 | ||
3e8a72d1 FF |
108 | const struct dsa_device_ops trailer_netdev_ops = { |
109 | .xmit = trailer_xmit, | |
110 | .rcv = trailer_rcv, | |
396138f0 | 111 | }; |