1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
|
// SPDX-License-Identifier: GPL-2.0
/* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com>
*/
#include <linux/if_vlan.h>
#include <linux/dsa/sja1105.h>
#include <linux/dsa/8021q.h>
#include <linux/packing.h>
#include "dsa_priv.h"
/* Similar to is_link_local_ether_addr(hdr->h_dest) but also covers PTP */
static inline bool sja1105_is_link_local(const struct sk_buff *skb)
{
const struct ethhdr *hdr = eth_hdr(skb);
u64 dmac = ether_addr_to_u64(hdr->h_dest);
if (ntohs(hdr->h_proto) == ETH_P_SJA1105_META)
return false;
if ((dmac & SJA1105_LINKLOCAL_FILTER_A_MASK) ==
SJA1105_LINKLOCAL_FILTER_A)
return true;
if ((dmac & SJA1105_LINKLOCAL_FILTER_B_MASK) ==
SJA1105_LINKLOCAL_FILTER_B)
return true;
return false;
}
struct sja1105_meta {
u64 tstamp;
u64 dmac_byte_4;
u64 dmac_byte_3;
u64 source_port;
u64 switch_id;
};
static void sja1105_meta_unpack(const struct sk_buff *skb,
struct sja1105_meta *meta)
{
u8 *buf = skb_mac_header(skb) + ETH_HLEN;
/* UM10944.pdf section 4.2.17 AVB Parameters:
* Structure of the meta-data follow-up frame.
* It is in network byte order, so there are no quirks
* while unpacking the meta frame.
*
* Also SJA1105 E/T only populates bits 23:0 of the timestamp
* whereas P/Q/R/S does 32 bits. Since the structure is the
* same and the E/T puts zeroes in the high-order byte, use
* a unified unpacking command for both device series.
*/
packing(buf, &meta->tstamp, 31, 0, 4, UNPACK, 0);
packing(buf + 4, &meta->dmac_byte_4, 7, 0, 1, UNPACK, 0);
packing(buf + 5, &meta->dmac_byte_3, 7, 0, 1, UNPACK, 0);
packing(buf + 6, &meta->source_port, 7, 0, 1, UNPACK, 0);
packing(buf + 7, &meta->switch_id, 7, 0, 1, UNPACK, 0);
}
static inline bool sja1105_is_meta_frame(const struct sk_buff *skb)
{
const struct ethhdr *hdr = eth_hdr(skb);
u64 smac = ether_addr_to_u64(hdr->h_source);
u64 dmac = ether_addr_to_u64(hdr->h_dest);
if (smac != SJA1105_META_SMAC)
return false;
if (dmac != SJA1105_META_DMAC)
return false;
if (ntohs(hdr->h_proto) != ETH_P_SJA1105_META)
return false;
return true;
}
/* This is the first time the tagger sees the frame on RX.
* Figure out if we can decode it.
*/
static bool sja1105_filter(const struct sk_buff *skb, struct net_device *dev)
{
if (!dsa_port_is_vlan_filtering(dev->dsa_ptr))
return true;
if (sja1105_is_link_local(skb))
return true;
if (sja1105_is_meta_frame(skb))
return true;
return false;
}
static struct sk_buff *sja1105_xmit(struct sk_buff *skb,
struct net_device *netdev)
{
struct dsa_port *dp = dsa_slave_to_port(netdev);
struct dsa_switch *ds = dp->ds;
u16 tx_vid = dsa_8021q_tx_vid(ds, dp->index);
u8 pcp = skb->priority;
/* Transmitting management traffic does not rely upon switch tagging,
* but instead SPI-installed management routes. Part 2 of this
* is the .port_deferred_xmit driver callback.
*/
if (unlikely(sja1105_is_link_local(skb)))
return dsa_defer_xmit(skb, netdev);
/* If we are under a vlan_filtering bridge, IP termination on
* switch ports based on 802.1Q tags is simply too brittle to
* be passable. So just defer to the dsa_slave_notag_xmit
* implementation.
*/
if (dsa_port_is_vlan_filtering(dp))
return skb;
return dsa_8021q_xmit(skb, netdev, ETH_P_SJA1105,
((pcp << VLAN_PRIO_SHIFT) | tx_vid));
}
static void sja1105_transfer_meta(struct sk_buff *skb,
const struct sja1105_meta *meta)
{
struct ethhdr *hdr = eth_hdr(skb);
hdr->h_dest[3] = meta->dmac_byte_3;
hdr->h_dest[4] = meta->dmac_byte_4;
SJA1105_SKB_CB(skb)->meta_tstamp = meta->tstamp;
}
/* This is a simple state machine which follows the hardware mechanism of
* generating RX timestamps:
*
* After each timestampable skb (all traffic for which send_meta1 and
* send_meta0 is true, aka all MAC-filtered link-local traffic) a meta frame
* containing a partial timestamp is immediately generated by the switch and
* sent as a follow-up to the link-local frame on the CPU port.
*
* The meta frames have no unique identifier (such as sequence number) by which
* one may pair them to the correct timestampable frame.
* Instead, the switch has internal logic that ensures no frames are sent on
* the CPU port between a link-local timestampable frame and its corresponding
* meta follow-up. It also ensures strict ordering between ports (lower ports
* have higher priority towards the CPU port). For this reason, a per-port
* data structure is not needed/desirable.
*
* This function pairs the link-local frame with its partial timestamp from the
* meta follow-up frame. The full timestamp will be reconstructed later in a
* work queue.
*/
static struct sk_buff
*sja1105_rcv_meta_state_machine(struct sk_buff *skb,
struct sja1105_meta *meta,
bool is_link_local,
bool is_meta)
{
struct sja1105_port *sp;
struct dsa_port *dp;
dp = dsa_slave_to_port(skb->dev);
sp = dp->priv;
/* Step 1: A timestampable frame was received.
* Buffer it until we get its meta frame.
*/
if (is_link_local && sp->data->hwts_rx_en) {
spin_lock(&sp->data->meta_lock);
/* Was this a link-local frame instead of the meta
* that we were expecting?
*/
if (sp->data->stampable_skb) {
dev_err_ratelimited(dp->ds->dev,
"Expected meta frame, is %12llx "
"in the DSA master multicast filter?\n",
SJA1105_META_DMAC);
}
/* Hold a reference to avoid dsa_switch_rcv
* from freeing the skb.
*/
sp->data->stampable_skb = skb_get(skb);
spin_unlock(&sp->data->meta_lock);
/* Tell DSA we got nothing */
return NULL;
/* Step 2: The meta frame arrived.
* Time to take the stampable skb out of the closet, annotate it
* with the partial timestamp, and pretend that we received it
* just now (basically masquerade the buffered frame as the meta
* frame, which serves no further purpose).
*/
} else if (is_meta) {
struct sk_buff *stampable_skb;
spin_lock(&sp->data->meta_lock);
stampable_skb = sp->data->stampable_skb;
sp->data->stampable_skb = NULL;
/* Was this a meta frame instead of the link-local
* that we were expecting?
*/
if (!stampable_skb) {
dev_err_ratelimited(dp->ds->dev,
"Unexpected meta frame\n");
spin_unlock(&sp->data->meta_lock);
return NULL;
}
if (stampable_skb->dev != skb->dev) {
dev_err_ratelimited(dp->ds->dev,
"Meta frame on wrong port\n");
spin_unlock(&sp->data->meta_lock);
return NULL;
}
/* Free the meta frame and give DSA the buffered stampable_skb
* for further processing up the network stack.
*/
kfree_skb(skb);
skb = stampable_skb;
sja1105_transfer_meta(skb, meta);
spin_unlock(&sp->data->meta_lock);
}
return skb;
}
static struct sk_buff *sja1105_rcv(struct sk_buff *skb,
struct net_device *netdev,
struct packet_type *pt)
{
struct sja1105_meta meta = {0};
int source_port, switch_id;
struct vlan_ethhdr *hdr;
u16 tpid, vid, tci;
bool is_link_local;
bool is_tagged;
bool is_meta;
hdr = vlan_eth_hdr(skb);
tpid = ntohs(hdr->h_vlan_proto);
is_tagged = (tpid == ETH_P_SJA1105);
is_link_local = sja1105_is_link_local(skb);
is_meta = sja1105_is_meta_frame(skb);
skb->offload_fwd_mark = 1;
if (is_tagged) {
/* Normal traffic path. */
tci = ntohs(hdr->h_vlan_TCI);
vid = tci & VLAN_VID_MASK;
source_port = dsa_8021q_rx_source_port(vid);
switch_id = dsa_8021q_rx_switch_id(vid);
skb->priority = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
} else if (is_link_local) {
/* Management traffic path. Switch embeds the switch ID and
* port ID into bytes of the destination MAC, courtesy of
* the incl_srcpt options.
*/
source_port = hdr->h_dest[3];
switch_id = hdr->h_dest[4];
/* Clear the DMAC bytes that were mangled by the switch */
hdr->h_dest[3] = 0;
hdr->h_dest[4] = 0;
} else if (is_meta) {
sja1105_meta_unpack(skb, &meta);
source_port = meta.source_port;
switch_id = meta.switch_id;
} else {
return NULL;
}
skb->dev = dsa_master_find_slave(netdev, switch_id, source_port);
if (!skb->dev) {
netdev_warn(netdev, "Couldn't decode source port\n");
return NULL;
}
/* Delete/overwrite fake VLAN header, DSA expects to not find
* it there, see dsa_switch_rcv: skb_push(skb, ETH_HLEN).
*/
if (is_tagged)
skb = dsa_8021q_remove_header(skb);
return sja1105_rcv_meta_state_machine(skb, &meta, is_link_local,
is_meta);
}
static struct dsa_device_ops sja1105_netdev_ops = {
.name = "sja1105",
.proto = DSA_TAG_PROTO_SJA1105,
.xmit = sja1105_xmit,
.rcv = sja1105_rcv,
.filter = sja1105_filter,
.overhead = VLAN_HLEN,
};
MODULE_LICENSE("GPL v
|