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