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> | |
13 | #include <linux/netdevice.h> | |
5a0e3ad6 | 14 | #include <linux/slab.h> |
396138f0 LB |
15 | #include "dsa_priv.h" |
16 | ||
6fef4c0c | 17 | netdev_tx_t trailer_xmit(struct sk_buff *skb, struct net_device *dev) |
396138f0 LB |
18 | { |
19 | struct dsa_slave_priv *p = netdev_priv(dev); | |
20 | struct sk_buff *nskb; | |
21 | int padlen; | |
22 | u8 *trailer; | |
23 | ||
24 | dev->stats.tx_packets++; | |
25 | dev->stats.tx_bytes += skb->len; | |
26 | ||
27 | /* | |
28 | * We have to make sure that the trailer ends up as the very | |
29 | * last 4 bytes of the packet. This means that we have to pad | |
30 | * the packet to the minimum ethernet frame size, if necessary, | |
31 | * before adding the trailer. | |
32 | */ | |
33 | padlen = 0; | |
34 | if (skb->len < 60) | |
35 | padlen = 60 - skb->len; | |
36 | ||
37 | nskb = alloc_skb(NET_IP_ALIGN + skb->len + padlen + 4, GFP_ATOMIC); | |
38 | if (nskb == NULL) { | |
39 | kfree_skb(skb); | |
40 | return NETDEV_TX_OK; | |
41 | } | |
42 | skb_reserve(nskb, NET_IP_ALIGN); | |
43 | ||
44 | skb_reset_mac_header(nskb); | |
45 | skb_set_network_header(nskb, skb_network_header(skb) - skb->head); | |
46 | skb_set_transport_header(nskb, skb_transport_header(skb) - skb->head); | |
47 | skb_copy_and_csum_dev(skb, skb_put(nskb, skb->len)); | |
48 | kfree_skb(skb); | |
49 | ||
50 | if (padlen) { | |
51 | u8 *pad = skb_put(nskb, padlen); | |
52 | memset(pad, 0, padlen); | |
53 | } | |
54 | ||
55 | trailer = skb_put(nskb, 4); | |
56 | trailer[0] = 0x80; | |
57 | trailer[1] = 1 << p->port; | |
58 | trailer[2] = 0x10; | |
59 | trailer[3] = 0x00; | |
60 | ||
61 | nskb->protocol = htons(ETH_P_TRAILER); | |
62 | ||
e84665c9 | 63 | nskb->dev = p->parent->dst->master_netdev; |
396138f0 LB |
64 | dev_queue_xmit(nskb); |
65 | ||
66 | return NETDEV_TX_OK; | |
67 | } | |
68 | ||
69 | static int trailer_rcv(struct sk_buff *skb, struct net_device *dev, | |
70 | struct packet_type *pt, struct net_device *orig_dev) | |
71 | { | |
e84665c9 LB |
72 | struct dsa_switch_tree *dst = dev->dsa_ptr; |
73 | struct dsa_switch *ds; | |
396138f0 LB |
74 | u8 *trailer; |
75 | int source_port; | |
76 | ||
e84665c9 | 77 | if (unlikely(dst == NULL)) |
396138f0 | 78 | goto out_drop; |
e84665c9 | 79 | ds = dst->ds[0]; |
396138f0 LB |
80 | |
81 | skb = skb_unshare(skb, GFP_ATOMIC); | |
82 | if (skb == NULL) | |
83 | goto out; | |
84 | ||
85 | if (skb_linearize(skb)) | |
86 | goto out_drop; | |
87 | ||
88 | trailer = skb_tail_pointer(skb) - 4; | |
89 | if (trailer[0] != 0x80 || (trailer[1] & 0xf8) != 0x00 || | |
90 | (trailer[3] & 0xef) != 0x00 || trailer[3] != 0x00) | |
91 | goto out_drop; | |
92 | ||
93 | source_port = trailer[1] & 7; | |
94 | if (source_port >= DSA_MAX_PORTS || ds->ports[source_port] == NULL) | |
95 | goto out_drop; | |
96 | ||
97 | pskb_trim_rcsum(skb, skb->len - 4); | |
98 | ||
99 | skb->dev = ds->ports[source_port]; | |
100 | skb_push(skb, ETH_HLEN); | |
14ee6742 | 101 | skb->pkt_type = PACKET_HOST; |
396138f0 LB |
102 | skb->protocol = eth_type_trans(skb, skb->dev); |
103 | ||
396138f0 LB |
104 | skb->dev->stats.rx_packets++; |
105 | skb->dev->stats.rx_bytes += skb->len; | |
106 | ||
107 | netif_receive_skb(skb); | |
108 | ||
109 | return 0; | |
110 | ||
111 | out_drop: | |
112 | kfree_skb(skb); | |
113 | out: | |
114 | return 0; | |
115 | } | |
116 | ||
7546dd97 | 117 | static struct packet_type trailer_packet_type __read_mostly = { |
09640e63 | 118 | .type = cpu_to_be16(ETH_P_TRAILER), |
396138f0 LB |
119 | .func = trailer_rcv, |
120 | }; | |
121 | ||
122 | static int __init trailer_init_module(void) | |
123 | { | |
124 | dev_add_pack(&trailer_packet_type); | |
125 | return 0; | |
126 | } | |
127 | module_init(trailer_init_module); | |
128 | ||
129 | static void __exit trailer_cleanup_module(void) | |
130 | { | |
131 | dev_remove_pack(&trailer_packet_type); | |
132 | } | |
133 | module_exit(trailer_cleanup_module); |