Commit | Line | Data |
---|---|---|
227d07a0 VO |
1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com> | |
3 | */ | |
4 | #include <linux/if_vlan.h> | |
5 | #include <linux/dsa/sja1105.h> | |
6 | #include <linux/dsa/8021q.h> | |
7 | #include <linux/packing.h> | |
8 | #include "dsa_priv.h" | |
9 | ||
4913b8eb VO |
10 | /* Is this a TX or an RX header? */ |
11 | #define SJA1110_HEADER_HOST_TO_SWITCH BIT(15) | |
12 | ||
13 | /* RX header */ | |
14 | #define SJA1110_RX_HEADER_IS_METADATA BIT(14) | |
15 | #define SJA1110_RX_HEADER_HOST_ONLY BIT(13) | |
16 | #define SJA1110_RX_HEADER_HAS_TRAILER BIT(12) | |
17 | ||
18 | /* Trap-to-host format (no trailer present) */ | |
19 | #define SJA1110_RX_HEADER_SRC_PORT(x) (((x) & GENMASK(7, 4)) >> 4) | |
20 | #define SJA1110_RX_HEADER_SWITCH_ID(x) ((x) & GENMASK(3, 0)) | |
21 | ||
22 | /* Timestamp format (trailer present) */ | |
23 | #define SJA1110_RX_HEADER_TRAILER_POS(x) ((x) & GENMASK(11, 0)) | |
24 | ||
25 | #define SJA1110_RX_TRAILER_SWITCH_ID(x) (((x) & GENMASK(7, 4)) >> 4) | |
26 | #define SJA1110_RX_TRAILER_SRC_PORT(x) ((x) & GENMASK(3, 0)) | |
27 | ||
566b18c8 VO |
28 | /* Meta frame format (for 2-step TX timestamps) */ |
29 | #define SJA1110_RX_HEADER_N_TS(x) (((x) & GENMASK(8, 4)) >> 4) | |
30 | ||
4913b8eb VO |
31 | /* TX header */ |
32 | #define SJA1110_TX_HEADER_UPDATE_TC BIT(14) | |
33 | #define SJA1110_TX_HEADER_TAKE_TS BIT(13) | |
34 | #define SJA1110_TX_HEADER_TAKE_TS_CASC BIT(12) | |
35 | #define SJA1110_TX_HEADER_HAS_TRAILER BIT(11) | |
36 | ||
37 | /* Only valid if SJA1110_TX_HEADER_HAS_TRAILER is false */ | |
38 | #define SJA1110_TX_HEADER_PRIO(x) (((x) << 7) & GENMASK(10, 7)) | |
39 | #define SJA1110_TX_HEADER_TSTAMP_ID(x) ((x) & GENMASK(7, 0)) | |
40 | ||
41 | /* Only valid if SJA1110_TX_HEADER_HAS_TRAILER is true */ | |
42 | #define SJA1110_TX_HEADER_TRAILER_POS(x) ((x) & GENMASK(10, 0)) | |
43 | ||
44 | #define SJA1110_TX_TRAILER_TSTAMP_ID(x) (((x) << 24) & GENMASK(31, 24)) | |
45 | #define SJA1110_TX_TRAILER_PRIO(x) (((x) << 21) & GENMASK(23, 21)) | |
46 | #define SJA1110_TX_TRAILER_SWITCHID(x) (((x) << 12) & GENMASK(15, 12)) | |
47 | #define SJA1110_TX_TRAILER_DESTPORTS(x) (((x) << 1) & GENMASK(11, 1)) | |
48 | ||
566b18c8 VO |
49 | #define SJA1110_META_TSTAMP_SIZE 10 |
50 | ||
4913b8eb VO |
51 | #define SJA1110_HEADER_LEN 4 |
52 | #define SJA1110_RX_TRAILER_LEN 13 | |
53 | #define SJA1110_TX_TRAILER_LEN 4 | |
54 | #define SJA1110_MAX_PADDING_LEN 15 | |
55 | ||
227d07a0 VO |
56 | /* Similar to is_link_local_ether_addr(hdr->h_dest) but also covers PTP */ |
57 | static inline bool sja1105_is_link_local(const struct sk_buff *skb) | |
58 | { | |
59 | const struct ethhdr *hdr = eth_hdr(skb); | |
60 | u64 dmac = ether_addr_to_u64(hdr->h_dest); | |
61 | ||
79fa7061 VO |
62 | if (ntohs(hdr->h_proto) == ETH_P_SJA1105_META) |
63 | return false; | |
227d07a0 VO |
64 | if ((dmac & SJA1105_LINKLOCAL_FILTER_A_MASK) == |
65 | SJA1105_LINKLOCAL_FILTER_A) | |
66 | return true; | |
67 | if ((dmac & SJA1105_LINKLOCAL_FILTER_B_MASK) == | |
68 | SJA1105_LINKLOCAL_FILTER_B) | |
69 | return true; | |
70 | return false; | |
71 | } | |
72 | ||
e53e18a6 VO |
73 | struct sja1105_meta { |
74 | u64 tstamp; | |
75 | u64 dmac_byte_4; | |
76 | u64 dmac_byte_3; | |
77 | u64 source_port; | |
78 | u64 switch_id; | |
79 | }; | |
80 | ||
81 | static void sja1105_meta_unpack(const struct sk_buff *skb, | |
82 | struct sja1105_meta *meta) | |
83 | { | |
84 | u8 *buf = skb_mac_header(skb) + ETH_HLEN; | |
85 | ||
86 | /* UM10944.pdf section 4.2.17 AVB Parameters: | |
87 | * Structure of the meta-data follow-up frame. | |
88 | * It is in network byte order, so there are no quirks | |
89 | * while unpacking the meta frame. | |
90 | * | |
91 | * Also SJA1105 E/T only populates bits 23:0 of the timestamp | |
92 | * whereas P/Q/R/S does 32 bits. Since the structure is the | |
93 | * same and the E/T puts zeroes in the high-order byte, use | |
94 | * a unified unpacking command for both device series. | |
95 | */ | |
96 | packing(buf, &meta->tstamp, 31, 0, 4, UNPACK, 0); | |
97 | packing(buf + 4, &meta->dmac_byte_4, 7, 0, 1, UNPACK, 0); | |
98 | packing(buf + 5, &meta->dmac_byte_3, 7, 0, 1, UNPACK, 0); | |
99 | packing(buf + 6, &meta->source_port, 7, 0, 1, UNPACK, 0); | |
100 | packing(buf + 7, &meta->switch_id, 7, 0, 1, UNPACK, 0); | |
101 | } | |
102 | ||
d3f9b90b VO |
103 | static inline bool sja1105_is_meta_frame(const struct sk_buff *skb) |
104 | { | |
105 | const struct ethhdr *hdr = eth_hdr(skb); | |
106 | u64 smac = ether_addr_to_u64(hdr->h_source); | |
107 | u64 dmac = ether_addr_to_u64(hdr->h_dest); | |
108 | ||
109 | if (smac != SJA1105_META_SMAC) | |
110 | return false; | |
111 | if (dmac != SJA1105_META_DMAC) | |
112 | return false; | |
113 | if (ntohs(hdr->h_proto) != ETH_P_SJA1105_META) | |
114 | return false; | |
115 | return true; | |
116 | } | |
117 | ||
a68578c2 VO |
118 | /* Calls sja1105_port_deferred_xmit in sja1105_main.c */ |
119 | static struct sk_buff *sja1105_defer_xmit(struct sja1105_port *sp, | |
120 | struct sk_buff *skb) | |
121 | { | |
122 | /* Increase refcount so the kfree_skb in dsa_slave_xmit | |
123 | * won't really free the packet. | |
124 | */ | |
125 | skb_queue_tail(&sp->xmit_queue, skb_get(skb)); | |
126 | kthread_queue_work(sp->xmit_worker, &sp->xmit_work); | |
127 | ||
128 | return NULL; | |
129 | } | |
130 | ||
38b5beea VO |
131 | static u16 sja1105_xmit_tpid(struct sja1105_port *sp) |
132 | { | |
133 | return sp->xmit_tpid; | |
134 | } | |
135 | ||
b6ad86e6 VO |
136 | static struct sk_buff *sja1105_imprecise_xmit(struct sk_buff *skb, |
137 | struct net_device *netdev) | |
138 | { | |
139 | struct dsa_port *dp = dsa_slave_to_port(netdev); | |
140 | struct net_device *br = dp->bridge_dev; | |
141 | u16 tx_vid; | |
142 | ||
143 | /* If the port is under a VLAN-aware bridge, just slide the | |
144 | * VLAN-tagged packet into the FDB and hope for the best. | |
145 | * This works because we support a single VLAN-aware bridge | |
146 | * across the entire dst, and its VLANs cannot be shared with | |
147 | * any standalone port. | |
148 | */ | |
149 | if (br_vlan_enabled(br)) | |
150 | return skb; | |
151 | ||
152 | /* If the port is under a VLAN-unaware bridge, use an imprecise | |
153 | * TX VLAN that targets the bridge's entire broadcast domain, | |
154 | * instead of just the specific port. | |
155 | */ | |
156 | tx_vid = dsa_8021q_bridge_tx_fwd_offload_vid(dp->bridge_num); | |
157 | ||
158 | return dsa_8021q_xmit(skb, netdev, sja1105_xmit_tpid(dp->priv), tx_vid); | |
159 | } | |
160 | ||
227d07a0 VO |
161 | static struct sk_buff *sja1105_xmit(struct sk_buff *skb, |
162 | struct net_device *netdev) | |
163 | { | |
164 | struct dsa_port *dp = dsa_slave_to_port(netdev); | |
2821d50f | 165 | u16 tx_vid = dsa_8021q_tx_vid(dp->ds, dp->index); |
5f06c63b VO |
166 | u16 queue_mapping = skb_get_queue_mapping(skb); |
167 | u8 pcp = netdev_txq_to_tc(netdev, queue_mapping); | |
227d07a0 | 168 | |
b6ad86e6 VO |
169 | if (skb->offload_fwd_mark) |
170 | return sja1105_imprecise_xmit(skb, netdev); | |
171 | ||
227d07a0 VO |
172 | /* Transmitting management traffic does not rely upon switch tagging, |
173 | * but instead SPI-installed management routes. Part 2 of this | |
174 | * is the .port_deferred_xmit driver callback. | |
175 | */ | |
176 | if (unlikely(sja1105_is_link_local(skb))) | |
a68578c2 | 177 | return sja1105_defer_xmit(dp->priv, skb); |
227d07a0 | 178 | |
38b5beea | 179 | return dsa_8021q_xmit(skb, netdev, sja1105_xmit_tpid(dp->priv), |
227d07a0 VO |
180 | ((pcp << VLAN_PRIO_SHIFT) | tx_vid)); |
181 | } | |
182 | ||
4913b8eb VO |
183 | static struct sk_buff *sja1110_xmit(struct sk_buff *skb, |
184 | struct net_device *netdev) | |
185 | { | |
566b18c8 | 186 | struct sk_buff *clone = SJA1105_SKB_CB(skb)->clone; |
4913b8eb VO |
187 | struct dsa_port *dp = dsa_slave_to_port(netdev); |
188 | u16 tx_vid = dsa_8021q_tx_vid(dp->ds, dp->index); | |
189 | u16 queue_mapping = skb_get_queue_mapping(skb); | |
190 | u8 pcp = netdev_txq_to_tc(netdev, queue_mapping); | |
191 | struct ethhdr *eth_hdr; | |
192 | __be32 *tx_trailer; | |
193 | __be16 *tx_header; | |
194 | int trailer_pos; | |
195 | ||
b6ad86e6 VO |
196 | if (skb->offload_fwd_mark) |
197 | return sja1105_imprecise_xmit(skb, netdev); | |
198 | ||
4913b8eb VO |
199 | /* Transmitting control packets is done using in-band control |
200 | * extensions, while data packets are transmitted using | |
201 | * tag_8021q TX VLANs. | |
202 | */ | |
203 | if (likely(!sja1105_is_link_local(skb))) | |
204 | return dsa_8021q_xmit(skb, netdev, sja1105_xmit_tpid(dp->priv), | |
205 | ((pcp << VLAN_PRIO_SHIFT) | tx_vid)); | |
206 | ||
207 | skb_push(skb, SJA1110_HEADER_LEN); | |
208 | ||
209 | /* Move Ethernet header to the left, making space for DSA tag */ | |
210 | memmove(skb->data, skb->data + SJA1110_HEADER_LEN, 2 * ETH_ALEN); | |
211 | ||
212 | trailer_pos = skb->len; | |
213 | ||
214 | /* On TX, skb->data points to skb_mac_header(skb) */ | |
215 | eth_hdr = (struct ethhdr *)skb->data; | |
216 | tx_header = (__be16 *)(eth_hdr + 1); | |
217 | tx_trailer = skb_put(skb, SJA1110_TX_TRAILER_LEN); | |
218 | ||
219 | eth_hdr->h_proto = htons(ETH_P_SJA1110); | |
220 | ||
221 | *tx_header = htons(SJA1110_HEADER_HOST_TO_SWITCH | | |
222 | SJA1110_TX_HEADER_HAS_TRAILER | | |
223 | SJA1110_TX_HEADER_TRAILER_POS(trailer_pos)); | |
224 | *tx_trailer = cpu_to_be32(SJA1110_TX_TRAILER_PRIO(pcp) | | |
225 | SJA1110_TX_TRAILER_SWITCHID(dp->ds->index) | | |
226 | SJA1110_TX_TRAILER_DESTPORTS(BIT(dp->index))); | |
566b18c8 VO |
227 | if (clone) { |
228 | u8 ts_id = SJA1105_SKB_CB(clone)->ts_id; | |
229 | ||
230 | *tx_header |= htons(SJA1110_TX_HEADER_TAKE_TS); | |
231 | *tx_trailer |= cpu_to_be32(SJA1110_TX_TRAILER_TSTAMP_ID(ts_id)); | |
232 | } | |
4913b8eb VO |
233 | |
234 | return skb; | |
235 | } | |
236 | ||
f3097be2 VO |
237 | static void sja1105_transfer_meta(struct sk_buff *skb, |
238 | const struct sja1105_meta *meta) | |
239 | { | |
240 | struct ethhdr *hdr = eth_hdr(skb); | |
241 | ||
242 | hdr->h_dest[3] = meta->dmac_byte_3; | |
243 | hdr->h_dest[4] = meta->dmac_byte_4; | |
617ef8d9 | 244 | SJA1105_SKB_CB(skb)->tstamp = meta->tstamp; |
f3097be2 VO |
245 | } |
246 | ||
247 | /* This is a simple state machine which follows the hardware mechanism of | |
248 | * generating RX timestamps: | |
249 | * | |
250 | * After each timestampable skb (all traffic for which send_meta1 and | |
251 | * send_meta0 is true, aka all MAC-filtered link-local traffic) a meta frame | |
252 | * containing a partial timestamp is immediately generated by the switch and | |
253 | * sent as a follow-up to the link-local frame on the CPU port. | |
254 | * | |
255 | * The meta frames have no unique identifier (such as sequence number) by which | |
256 | * one may pair them to the correct timestampable frame. | |
257 | * Instead, the switch has internal logic that ensures no frames are sent on | |
258 | * the CPU port between a link-local timestampable frame and its corresponding | |
259 | * meta follow-up. It also ensures strict ordering between ports (lower ports | |
260 | * have higher priority towards the CPU port). For this reason, a per-port | |
261 | * data structure is not needed/desirable. | |
262 | * | |
263 | * This function pairs the link-local frame with its partial timestamp from the | |
264 | * meta follow-up frame. The full timestamp will be reconstructed later in a | |
265 | * work queue. | |
266 | */ | |
267 | static struct sk_buff | |
268 | *sja1105_rcv_meta_state_machine(struct sk_buff *skb, | |
269 | struct sja1105_meta *meta, | |
270 | bool is_link_local, | |
271 | bool is_meta) | |
272 | { | |
273 | struct sja1105_port *sp; | |
274 | struct dsa_port *dp; | |
275 | ||
276 | dp = dsa_slave_to_port(skb->dev); | |
277 | sp = dp->priv; | |
278 | ||
279 | /* Step 1: A timestampable frame was received. | |
280 | * Buffer it until we get its meta frame. | |
281 | */ | |
3e8db7e5 VO |
282 | if (is_link_local) { |
283 | if (!test_bit(SJA1105_HWTS_RX_EN, &sp->data->state)) | |
284 | /* Do normal processing. */ | |
285 | return skb; | |
286 | ||
f3097be2 VO |
287 | spin_lock(&sp->data->meta_lock); |
288 | /* Was this a link-local frame instead of the meta | |
289 | * that we were expecting? | |
290 | */ | |
291 | if (sp->data->stampable_skb) { | |
292 | dev_err_ratelimited(dp->ds->dev, | |
293 | "Expected meta frame, is %12llx " | |
294 | "in the DSA master multicast filter?\n", | |
295 | SJA1105_META_DMAC); | |
93fa8587 | 296 | kfree_skb(sp->data->stampable_skb); |
f3097be2 VO |
297 | } |
298 | ||
299 | /* Hold a reference to avoid dsa_switch_rcv | |
300 | * from freeing the skb. | |
301 | */ | |
302 | sp->data->stampable_skb = skb_get(skb); | |
303 | spin_unlock(&sp->data->meta_lock); | |
304 | ||
305 | /* Tell DSA we got nothing */ | |
306 | return NULL; | |
307 | ||
308 | /* Step 2: The meta frame arrived. | |
309 | * Time to take the stampable skb out of the closet, annotate it | |
310 | * with the partial timestamp, and pretend that we received it | |
311 | * just now (basically masquerade the buffered frame as the meta | |
312 | * frame, which serves no further purpose). | |
313 | */ | |
314 | } else if (is_meta) { | |
315 | struct sk_buff *stampable_skb; | |
316 | ||
3e8db7e5 VO |
317 | /* Drop the meta frame if we're not in the right state |
318 | * to process it. | |
319 | */ | |
320 | if (!test_bit(SJA1105_HWTS_RX_EN, &sp->data->state)) | |
321 | return NULL; | |
322 | ||
f3097be2 VO |
323 | spin_lock(&sp->data->meta_lock); |
324 | ||
325 | stampable_skb = sp->data->stampable_skb; | |
326 | sp->data->stampable_skb = NULL; | |
327 | ||
328 | /* Was this a meta frame instead of the link-local | |
329 | * that we were expecting? | |
330 | */ | |
331 | if (!stampable_skb) { | |
332 | dev_err_ratelimited(dp->ds->dev, | |
333 | "Unexpected meta frame\n"); | |
334 | spin_unlock(&sp->data->meta_lock); | |
335 | return NULL; | |
336 | } | |
337 | ||
338 | if (stampable_skb->dev != skb->dev) { | |
339 | dev_err_ratelimited(dp->ds->dev, | |
340 | "Meta frame on wrong port\n"); | |
341 | spin_unlock(&sp->data->meta_lock); | |
342 | return NULL; | |
343 | } | |
344 | ||
345 | /* Free the meta frame and give DSA the buffered stampable_skb | |
346 | * for further processing up the network stack. | |
347 | */ | |
348 | kfree_skb(skb); | |
f163fed2 | 349 | skb = stampable_skb; |
f3097be2 | 350 | sja1105_transfer_meta(skb, meta); |
f3097be2 VO |
351 | |
352 | spin_unlock(&sp->data->meta_lock); | |
353 | } | |
354 | ||
355 | return skb; | |
356 | } | |
357 | ||
233697b3 VO |
358 | static bool sja1105_skb_has_tag_8021q(const struct sk_buff *skb) |
359 | { | |
360 | u16 tpid = ntohs(eth_hdr(skb)->h_proto); | |
361 | ||
362 | return tpid == ETH_P_SJA1105 || tpid == ETH_P_8021Q || | |
363 | skb_vlan_tag_present(skb); | |
364 | } | |
365 | ||
4913b8eb VO |
366 | static bool sja1110_skb_has_inband_control_extension(const struct sk_buff *skb) |
367 | { | |
368 | return ntohs(eth_hdr(skb)->h_proto) == ETH_P_SJA1110; | |
369 | } | |
370 | ||
884be12f VO |
371 | /* Returns true for imprecise RX and sets the @vid. |
372 | * Returns false for precise RX and sets @source_port and @switch_id. | |
373 | */ | |
374 | static bool sja1105_vlan_rcv(struct sk_buff *skb, int *source_port, | |
375 | int *switch_id, u16 *vid) | |
376 | { | |
377 | struct vlan_ethhdr *hdr = (struct vlan_ethhdr *)skb_mac_header(skb); | |
378 | u16 vlan_tci; | |
379 | ||
380 | if (skb_vlan_tag_present(skb)) | |
381 | vlan_tci = skb_vlan_tag_get(skb); | |
382 | else | |
383 | vlan_tci = ntohs(hdr->h_vlan_TCI); | |
384 | ||
385 | if (vid_is_dsa_8021q_rxvlan(vlan_tci & VLAN_VID_MASK)) { | |
386 | dsa_8021q_rcv(skb, source_port, switch_id); | |
387 | return false; | |
388 | } | |
389 | ||
390 | /* Try our best with imprecise RX */ | |
391 | *vid = vlan_tci & VLAN_VID_MASK; | |
392 | ||
393 | return true; | |
394 | } | |
395 | ||
227d07a0 VO |
396 | static struct sk_buff *sja1105_rcv(struct sk_buff *skb, |
397 | struct net_device *netdev, | |
398 | struct packet_type *pt) | |
399 | { | |
884be12f | 400 | int source_port = -1, switch_id = -1; |
e53e18a6 | 401 | struct sja1105_meta meta = {0}; |
884be12f | 402 | bool imprecise_rx = false; |
e80f40cb | 403 | struct ethhdr *hdr; |
42824463 | 404 | bool is_link_local; |
e53e18a6 | 405 | bool is_meta; |
884be12f | 406 | u16 vid; |
227d07a0 | 407 | |
e80f40cb | 408 | hdr = eth_hdr(skb); |
42824463 | 409 | is_link_local = sja1105_is_link_local(skb); |
e53e18a6 | 410 | is_meta = sja1105_is_meta_frame(skb); |
227d07a0 VO |
411 | |
412 | skb->offload_fwd_mark = 1; | |
413 | ||
233697b3 | 414 | if (sja1105_skb_has_tag_8021q(skb)) { |
42824463 | 415 | /* Normal traffic path. */ |
884be12f VO |
416 | imprecise_rx = sja1105_vlan_rcv(skb, &source_port, &switch_id, |
417 | &vid); | |
42824463 | 418 | } else if (is_link_local) { |
227d07a0 VO |
419 | /* Management traffic path. Switch embeds the switch ID and |
420 | * port ID into bytes of the destination MAC, courtesy of | |
421 | * the incl_srcpt options. | |
422 | */ | |
423 | source_port = hdr->h_dest[3]; | |
424 | switch_id = hdr->h_dest[4]; | |
425 | /* Clear the DMAC bytes that were mangled by the switch */ | |
426 | hdr->h_dest[3] = 0; | |
427 | hdr->h_dest[4] = 0; | |
e53e18a6 VO |
428 | } else if (is_meta) { |
429 | sja1105_meta_unpack(skb, &meta); | |
430 | source_port = meta.source_port; | |
431 | switch_id = meta.switch_id; | |
227d07a0 | 432 | } else { |
42824463 | 433 | return NULL; |
227d07a0 VO |
434 | } |
435 | ||
884be12f VO |
436 | if (imprecise_rx) |
437 | skb->dev = dsa_find_designated_bridge_port_by_vid(netdev, vid); | |
438 | else | |
439 | skb->dev = dsa_master_find_slave(netdev, switch_id, source_port); | |
227d07a0 VO |
440 | if (!skb->dev) { |
441 | netdev_warn(netdev, "Couldn't decode source port\n"); | |
442 | return NULL; | |
443 | } | |
444 | ||
f3097be2 VO |
445 | return sja1105_rcv_meta_state_machine(skb, &meta, is_link_local, |
446 | is_meta); | |
227d07a0 VO |
447 | } |
448 | ||
566b18c8 VO |
449 | static struct sk_buff *sja1110_rcv_meta(struct sk_buff *skb, u16 rx_header) |
450 | { | |
451 | int switch_id = SJA1110_RX_HEADER_SWITCH_ID(rx_header); | |
452 | int n_ts = SJA1110_RX_HEADER_N_TS(rx_header); | |
453 | struct net_device *master = skb->dev; | |
454 | struct dsa_port *cpu_dp; | |
455 | u8 *buf = skb->data + 2; | |
456 | struct dsa_switch *ds; | |
457 | int i; | |
458 | ||
459 | cpu_dp = master->dsa_ptr; | |
460 | ds = dsa_switch_find(cpu_dp->dst->index, switch_id); | |
461 | if (!ds) { | |
462 | net_err_ratelimited("%s: cannot find switch id %d\n", | |
463 | master->name, switch_id); | |
464 | return NULL; | |
465 | } | |
466 | ||
467 | for (i = 0; i <= n_ts; i++) { | |
468 | u8 ts_id, source_port, dir; | |
469 | u64 tstamp; | |
470 | ||
471 | ts_id = buf[0]; | |
472 | source_port = (buf[1] & GENMASK(7, 4)) >> 4; | |
473 | dir = (buf[1] & BIT(3)) >> 3; | |
474 | tstamp = be64_to_cpu(*(__be64 *)(buf + 2)); | |
475 | ||
476 | sja1110_process_meta_tstamp(ds, source_port, ts_id, dir, | |
477 | tstamp); | |
478 | ||
479 | buf += SJA1110_META_TSTAMP_SIZE; | |
480 | } | |
481 | ||
482 | /* Discard the meta frame, we've consumed the timestamps it contained */ | |
483 | return NULL; | |
484 | } | |
485 | ||
4913b8eb VO |
486 | static struct sk_buff *sja1110_rcv_inband_control_extension(struct sk_buff *skb, |
487 | int *source_port, | |
488 | int *switch_id) | |
489 | { | |
490 | u16 rx_header; | |
491 | ||
492 | if (unlikely(!pskb_may_pull(skb, SJA1110_HEADER_LEN))) | |
493 | return NULL; | |
494 | ||
495 | /* skb->data points to skb_mac_header(skb) + ETH_HLEN, which is exactly | |
496 | * what we need because the caller has checked the EtherType (which is | |
497 | * located 2 bytes back) and we just need a pointer to the header that | |
498 | * comes afterwards. | |
499 | */ | |
500 | rx_header = ntohs(*(__be16 *)skb->data); | |
566b18c8 VO |
501 | |
502 | if (rx_header & SJA1110_RX_HEADER_IS_METADATA) | |
503 | return sja1110_rcv_meta(skb, rx_header); | |
4913b8eb VO |
504 | |
505 | /* Timestamp frame, we have a trailer */ | |
506 | if (rx_header & SJA1110_RX_HEADER_HAS_TRAILER) { | |
507 | int start_of_padding = SJA1110_RX_HEADER_TRAILER_POS(rx_header); | |
508 | u8 *rx_trailer = skb_tail_pointer(skb) - SJA1110_RX_TRAILER_LEN; | |
509 | u64 *tstamp = &SJA1105_SKB_CB(skb)->tstamp; | |
510 | u8 last_byte = rx_trailer[12]; | |
511 | ||
512 | /* The timestamp is unaligned, so we need to use packing() | |
513 | * to get it | |
514 | */ | |
515 | packing(rx_trailer, tstamp, 63, 0, 8, UNPACK, 0); | |
516 | ||
517 | *source_port = SJA1110_RX_TRAILER_SRC_PORT(last_byte); | |
518 | *switch_id = SJA1110_RX_TRAILER_SWITCH_ID(last_byte); | |
519 | ||
520 | /* skb->len counts from skb->data, while start_of_padding | |
521 | * counts from the destination MAC address. Right now skb->data | |
522 | * is still as set by the DSA master, so to trim away the | |
523 | * padding and trailer we need to account for the fact that | |
524 | * skb->data points to skb_mac_header(skb) + ETH_HLEN. | |
525 | */ | |
526 | pskb_trim_rcsum(skb, start_of_padding - ETH_HLEN); | |
527 | /* Trap-to-host frame, no timestamp trailer */ | |
528 | } else { | |
529 | *source_port = SJA1110_RX_HEADER_SRC_PORT(rx_header); | |
530 | *switch_id = SJA1110_RX_HEADER_SWITCH_ID(rx_header); | |
531 | } | |
532 | ||
533 | /* Advance skb->data past the DSA header */ | |
534 | skb_pull_rcsum(skb, SJA1110_HEADER_LEN); | |
535 | ||
536 | /* Remove the DSA header */ | |
537 | memmove(skb->data - ETH_HLEN, skb->data - ETH_HLEN - SJA1110_HEADER_LEN, | |
538 | 2 * ETH_ALEN); | |
539 | ||
540 | /* With skb->data in its final place, update the MAC header | |
541 | * so that eth_hdr() continues to works properly. | |
542 | */ | |
543 | skb_set_mac_header(skb, -ETH_HLEN); | |
544 | ||
545 | return skb; | |
546 | } | |
547 | ||
548 | static struct sk_buff *sja1110_rcv(struct sk_buff *skb, | |
549 | struct net_device *netdev, | |
550 | struct packet_type *pt) | |
551 | { | |
0fac6aa0 | 552 | int source_port = -1, switch_id = -1; |
884be12f VO |
553 | bool imprecise_rx = false; |
554 | u16 vid; | |
4913b8eb VO |
555 | |
556 | skb->offload_fwd_mark = 1; | |
557 | ||
558 | if (sja1110_skb_has_inband_control_extension(skb)) { | |
559 | skb = sja1110_rcv_inband_control_extension(skb, &source_port, | |
560 | &switch_id); | |
561 | if (!skb) | |
562 | return NULL; | |
563 | } | |
564 | ||
565 | /* Packets with in-band control extensions might still have RX VLANs */ | |
566 | if (likely(sja1105_skb_has_tag_8021q(skb))) | |
884be12f VO |
567 | imprecise_rx = sja1105_vlan_rcv(skb, &source_port, &switch_id, |
568 | &vid); | |
4913b8eb | 569 | |
884be12f VO |
570 | if (imprecise_rx) |
571 | skb->dev = dsa_find_designated_bridge_port_by_vid(netdev, vid); | |
572 | else | |
573 | skb->dev = dsa_master_find_slave(netdev, switch_id, source_port); | |
4913b8eb | 574 | if (!skb->dev) { |
884be12f | 575 | netdev_warn(netdev, "Couldn't decode source port\n"); |
4913b8eb VO |
576 | return NULL; |
577 | } | |
578 | ||
4913b8eb VO |
579 | return skb; |
580 | } | |
581 | ||
e6652979 VO |
582 | static void sja1105_flow_dissect(const struct sk_buff *skb, __be16 *proto, |
583 | int *offset) | |
584 | { | |
585 | /* No tag added for management frames, all ok */ | |
586 | if (unlikely(sja1105_is_link_local(skb))) | |
587 | return; | |
588 | ||
589 | dsa_tag_generic_flow_dissect(skb, proto, offset); | |
590 | } | |
591 | ||
4913b8eb VO |
592 | static void sja1110_flow_dissect(const struct sk_buff *skb, __be16 *proto, |
593 | int *offset) | |
594 | { | |
595 | /* Management frames have 2 DSA tags on RX, so the needed_headroom we | |
596 | * declared is fine for the generic dissector adjustment procedure. | |
597 | */ | |
598 | if (unlikely(sja1105_is_link_local(skb))) | |
599 | return dsa_tag_generic_flow_dissect(skb, proto, offset); | |
600 | ||
601 | /* For the rest, there is a single DSA tag, the tag_8021q one */ | |
602 | *offset = VLAN_HLEN; | |
603 | *proto = ((__be16 *)skb->data)[(VLAN_HLEN / 2) - 1]; | |
604 | } | |
605 | ||
097f0244 | 606 | static const struct dsa_device_ops sja1105_netdev_ops = { |
227d07a0 VO |
607 | .name = "sja1105", |
608 | .proto = DSA_TAG_PROTO_SJA1105, | |
609 | .xmit = sja1105_xmit, | |
610 | .rcv = sja1105_rcv, | |
4e500251 | 611 | .needed_headroom = VLAN_HLEN, |
e6652979 | 612 | .flow_dissect = sja1105_flow_dissect, |
707091eb | 613 | .promisc_on_master = true, |
227d07a0 VO |
614 | }; |
615 | ||
4913b8eb | 616 | DSA_TAG_DRIVER(sja1105_netdev_ops); |
227d07a0 VO |
617 | MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_SJA1105); |
618 | ||
4913b8eb VO |
619 | static const struct dsa_device_ops sja1110_netdev_ops = { |
620 | .name = "sja1110", | |
621 | .proto = DSA_TAG_PROTO_SJA1110, | |
622 | .xmit = sja1110_xmit, | |
623 | .rcv = sja1110_rcv, | |
4913b8eb VO |
624 | .flow_dissect = sja1110_flow_dissect, |
625 | .needed_headroom = SJA1110_HEADER_LEN + VLAN_HLEN, | |
626 | .needed_tailroom = SJA1110_RX_TRAILER_LEN + SJA1110_MAX_PADDING_LEN, | |
627 | }; | |
628 | ||
629 | DSA_TAG_DRIVER(sja1110_netdev_ops); | |
630 | MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_SJA1110); | |
631 | ||
632 | static struct dsa_tag_driver *sja1105_tag_driver_array[] = { | |
633 | &DSA_TAG_DRIVER_NAME(sja1105_netdev_ops), | |
634 | &DSA_TAG_DRIVER_NAME(sja1110_netdev_ops), | |
635 | }; | |
636 | ||
637 | module_dsa_tag_drivers(sja1105_tag_driver_array); | |
638 | ||
639 | MODULE_LICENSE("GPL v2"); |