1 | // SPDX-License-Identifier: GPL-2.0+ |
2 | /* |
3 | * net/dsa/tag_ksz.c - Microchip KSZ Switch tag format handling |
4 | * Copyright (c) 2017 Microchip Technology |
5 | */ |
6 | |
7 | #include <linux/dsa/ksz_common.h> |
8 | #include <linux/etherdevice.h> |
9 | #include <linux/list.h> |
10 | #include <linux/ptp_classify.h> |
11 | #include <net/dsa.h> |
12 | |
13 | #include "tag.h" |
14 | |
15 | #define KSZ8795_NAME "ksz8795" |
16 | #define KSZ9477_NAME "ksz9477" |
17 | #define KSZ9893_NAME "ksz9893" |
18 | #define LAN937X_NAME "lan937x" |
19 | |
20 | /* Typically only one byte is used for tail tag. */ |
21 | #define KSZ_PTP_TAG_LEN 4 |
22 | #define KSZ_EGRESS_TAG_LEN 1 |
23 | #define KSZ_INGRESS_TAG_LEN 1 |
24 | |
25 | #define KSZ_HWTS_EN 0 |
26 | |
27 | struct ksz_tagger_private { |
28 | struct ksz_tagger_data data; /* Must be first */ |
29 | unsigned long state; |
30 | struct kthread_worker *xmit_worker; |
31 | }; |
32 | |
33 | static struct ksz_tagger_private * |
34 | ksz_tagger_private(struct dsa_switch *ds) |
35 | { |
36 | return ds->tagger_data; |
37 | } |
38 | |
39 | static void ksz_hwtstamp_set_state(struct dsa_switch *ds, bool on) |
40 | { |
41 | struct ksz_tagger_private *priv = ksz_tagger_private(ds); |
42 | |
43 | if (on) |
44 | set_bit(KSZ_HWTS_EN, addr: &priv->state); |
45 | else |
46 | clear_bit(KSZ_HWTS_EN, addr: &priv->state); |
47 | } |
48 | |
49 | static void ksz_disconnect(struct dsa_switch *ds) |
50 | { |
51 | struct ksz_tagger_private *priv = ds->tagger_data; |
52 | |
53 | kthread_destroy_worker(worker: priv->xmit_worker); |
54 | kfree(objp: priv); |
55 | ds->tagger_data = NULL; |
56 | } |
57 | |
58 | static int ksz_connect(struct dsa_switch *ds) |
59 | { |
60 | struct ksz_tagger_data *tagger_data; |
61 | struct kthread_worker *xmit_worker; |
62 | struct ksz_tagger_private *priv; |
63 | int ret; |
64 | |
65 | priv = kzalloc(size: sizeof(*priv), GFP_KERNEL); |
66 | if (!priv) |
67 | return -ENOMEM; |
68 | |
69 | xmit_worker = kthread_create_worker(flags: 0, namefmt: "dsa%d:%d_xmit" , |
70 | ds->dst->index, ds->index); |
71 | if (IS_ERR(ptr: xmit_worker)) { |
72 | ret = PTR_ERR(ptr: xmit_worker); |
73 | kfree(objp: priv); |
74 | return ret; |
75 | } |
76 | |
77 | priv->xmit_worker = xmit_worker; |
78 | /* Export functions for switch driver use */ |
79 | tagger_data = &priv->data; |
80 | tagger_data->hwtstamp_set_state = ksz_hwtstamp_set_state; |
81 | ds->tagger_data = priv; |
82 | |
83 | return 0; |
84 | } |
85 | |
86 | static struct sk_buff *ksz_common_rcv(struct sk_buff *skb, |
87 | struct net_device *dev, |
88 | unsigned int port, unsigned int len) |
89 | { |
90 | skb->dev = dsa_conduit_find_user(dev, device: 0, port); |
91 | if (!skb->dev) |
92 | return NULL; |
93 | |
94 | if (pskb_trim_rcsum(skb, len: skb->len - len)) |
95 | return NULL; |
96 | |
97 | dsa_default_offload_fwd_mark(skb); |
98 | |
99 | return skb; |
100 | } |
101 | |
102 | /* |
103 | * For Ingress (Host -> KSZ8795), 1 byte is added before FCS. |
104 | * --------------------------------------------------------------------------- |
105 | * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|tag(1byte)|FCS(4bytes) |
106 | * --------------------------------------------------------------------------- |
107 | * tag : each bit represents port (eg, 0x01=port1, 0x02=port2, 0x10=port5) |
108 | * |
109 | * For Egress (KSZ8795 -> Host), 1 byte is added before FCS. |
110 | * --------------------------------------------------------------------------- |
111 | * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|tag0(1byte)|FCS(4bytes) |
112 | * --------------------------------------------------------------------------- |
113 | * tag0 : zero-based value represents port |
114 | * (eg, 0x00=port1, 0x02=port3, 0x06=port7) |
115 | */ |
116 | |
117 | #define KSZ8795_TAIL_TAG_OVERRIDE BIT(6) |
118 | #define KSZ8795_TAIL_TAG_LOOKUP BIT(7) |
119 | |
120 | static struct sk_buff *ksz8795_xmit(struct sk_buff *skb, struct net_device *dev) |
121 | { |
122 | struct dsa_port *dp = dsa_user_to_port(dev); |
123 | struct ethhdr *hdr; |
124 | u8 *tag; |
125 | |
126 | if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb)) |
127 | return NULL; |
128 | |
129 | /* Tag encoding */ |
130 | tag = skb_put(skb, KSZ_INGRESS_TAG_LEN); |
131 | hdr = skb_eth_hdr(skb); |
132 | |
133 | *tag = 1 << dp->index; |
134 | if (is_link_local_ether_addr(addr: hdr->h_dest)) |
135 | *tag |= KSZ8795_TAIL_TAG_OVERRIDE; |
136 | |
137 | return skb; |
138 | } |
139 | |
140 | static struct sk_buff *ksz8795_rcv(struct sk_buff *skb, struct net_device *dev) |
141 | { |
142 | u8 *tag = skb_tail_pointer(skb) - KSZ_EGRESS_TAG_LEN; |
143 | |
144 | return ksz_common_rcv(skb, dev, port: tag[0] & 7, KSZ_EGRESS_TAG_LEN); |
145 | } |
146 | |
147 | static const struct dsa_device_ops ksz8795_netdev_ops = { |
148 | .name = KSZ8795_NAME, |
149 | .proto = DSA_TAG_PROTO_KSZ8795, |
150 | .xmit = ksz8795_xmit, |
151 | .rcv = ksz8795_rcv, |
152 | .needed_tailroom = KSZ_INGRESS_TAG_LEN, |
153 | }; |
154 | |
155 | DSA_TAG_DRIVER(ksz8795_netdev_ops); |
156 | MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ8795, KSZ8795_NAME); |
157 | |
158 | /* |
159 | * For Ingress (Host -> KSZ9477), 2/6 bytes are added before FCS. |
160 | * --------------------------------------------------------------------------- |
161 | * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|tag1(1byte)| |
162 | * FCS(4bytes) |
163 | * --------------------------------------------------------------------------- |
164 | * ts : time stamp (Present only if PTP is enabled in the Hardware) |
165 | * tag0 : Prioritization (not used now) |
166 | * tag1 : each bit represents port (eg, 0x01=port1, 0x02=port2, 0x10=port5) |
167 | * |
168 | * For Egress (KSZ9477 -> Host), 1/5 bytes is added before FCS. |
169 | * --------------------------------------------------------------------------- |
170 | * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|FCS(4bytes) |
171 | * --------------------------------------------------------------------------- |
172 | * ts : time stamp (Present only if bit 7 of tag0 is set) |
173 | * tag0 : zero-based value represents port |
174 | * (eg, 0x00=port1, 0x02=port3, 0x06=port7) |
175 | */ |
176 | |
177 | #define KSZ9477_INGRESS_TAG_LEN 2 |
178 | #define KSZ9477_PTP_TAG_LEN 4 |
179 | #define KSZ9477_PTP_TAG_INDICATION 0x80 |
180 | |
181 | #define KSZ9477_TAIL_TAG_PRIO GENMASK(8, 7) |
182 | #define KSZ9477_TAIL_TAG_OVERRIDE BIT(9) |
183 | #define KSZ9477_TAIL_TAG_LOOKUP BIT(10) |
184 | |
185 | static void ksz_rcv_timestamp(struct sk_buff *skb, u8 *tag) |
186 | { |
187 | u8 *tstamp_raw = tag - KSZ_PTP_TAG_LEN; |
188 | ktime_t tstamp; |
189 | |
190 | tstamp = ksz_decode_tstamp(tstamp: get_unaligned_be32(p: tstamp_raw)); |
191 | KSZ_SKB_CB(skb)->tstamp = tstamp; |
192 | } |
193 | |
194 | /* Time stamp tag *needs* to be inserted if PTP is enabled in hardware. |
195 | * Regardless of Whether it is a PTP frame or not. |
196 | */ |
197 | static void ksz_xmit_timestamp(struct dsa_port *dp, struct sk_buff *skb) |
198 | { |
199 | struct ksz_tagger_private *priv; |
200 | struct ptp_header *ptp_hdr; |
201 | unsigned int ptp_type; |
202 | u32 tstamp_raw = 0; |
203 | s64 correction; |
204 | |
205 | priv = ksz_tagger_private(ds: dp->ds); |
206 | |
207 | if (!test_bit(KSZ_HWTS_EN, &priv->state)) |
208 | return; |
209 | |
210 | if (!KSZ_SKB_CB(skb)->update_correction) |
211 | goto output_tag; |
212 | |
213 | ptp_type = KSZ_SKB_CB(skb)->ptp_type; |
214 | |
215 | ptp_hdr = ptp_parse_header(skb, type: ptp_type); |
216 | if (!ptp_hdr) |
217 | goto output_tag; |
218 | |
219 | correction = (s64)get_unaligned_be64(p: &ptp_hdr->correction); |
220 | |
221 | if (correction < 0) { |
222 | struct timespec64 ts; |
223 | |
224 | ts = ns_to_timespec64(nsec: -correction >> 16); |
225 | tstamp_raw = ((ts.tv_sec & 3) << 30) | ts.tv_nsec; |
226 | |
227 | /* Set correction field to 0 and update UDP checksum */ |
228 | ptp_header_update_correction(skb, type: ptp_type, hdr: ptp_hdr, correction: 0); |
229 | } |
230 | |
231 | output_tag: |
232 | put_unaligned_be32(val: tstamp_raw, p: skb_put(skb, KSZ_PTP_TAG_LEN)); |
233 | } |
234 | |
235 | /* Defer transmit if waiting for egress time stamp is required. */ |
236 | static struct sk_buff *ksz_defer_xmit(struct dsa_port *dp, struct sk_buff *skb) |
237 | { |
238 | struct ksz_tagger_data *tagger_data = ksz_tagger_data(ds: dp->ds); |
239 | struct ksz_tagger_private *priv = ksz_tagger_private(ds: dp->ds); |
240 | void (*xmit_work_fn)(struct kthread_work *work); |
241 | struct sk_buff *clone = KSZ_SKB_CB(skb)->clone; |
242 | struct ksz_deferred_xmit_work *xmit_work; |
243 | struct kthread_worker *xmit_worker; |
244 | |
245 | if (!clone) |
246 | return skb; /* no deferred xmit for this packet */ |
247 | |
248 | xmit_work_fn = tagger_data->xmit_work_fn; |
249 | xmit_worker = priv->xmit_worker; |
250 | |
251 | if (!xmit_work_fn || !xmit_worker) |
252 | return NULL; |
253 | |
254 | xmit_work = kzalloc(size: sizeof(*xmit_work), GFP_ATOMIC); |
255 | if (!xmit_work) |
256 | return NULL; |
257 | |
258 | kthread_init_work(&xmit_work->work, xmit_work_fn); |
259 | /* Increase refcount so the kfree_skb in dsa_user_xmit |
260 | * won't really free the packet. |
261 | */ |
262 | xmit_work->dp = dp; |
263 | xmit_work->skb = skb_get(skb); |
264 | |
265 | kthread_queue_work(worker: xmit_worker, work: &xmit_work->work); |
266 | |
267 | return NULL; |
268 | } |
269 | |
270 | static struct sk_buff *ksz9477_xmit(struct sk_buff *skb, |
271 | struct net_device *dev) |
272 | { |
273 | u16 queue_mapping = skb_get_queue_mapping(skb); |
274 | u8 prio = netdev_txq_to_tc(dev, txq: queue_mapping); |
275 | struct dsa_port *dp = dsa_user_to_port(dev); |
276 | struct ethhdr *hdr; |
277 | __be16 *tag; |
278 | u16 val; |
279 | |
280 | if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb)) |
281 | return NULL; |
282 | |
283 | /* Tag encoding */ |
284 | ksz_xmit_timestamp(dp, skb); |
285 | |
286 | tag = skb_put(skb, KSZ9477_INGRESS_TAG_LEN); |
287 | hdr = skb_eth_hdr(skb); |
288 | |
289 | val = BIT(dp->index); |
290 | |
291 | val |= FIELD_PREP(KSZ9477_TAIL_TAG_PRIO, prio); |
292 | |
293 | if (is_link_local_ether_addr(addr: hdr->h_dest)) |
294 | val |= KSZ9477_TAIL_TAG_OVERRIDE; |
295 | |
296 | if (dev->features & NETIF_F_HW_HSR_DUP) { |
297 | struct net_device *hsr_dev = dp->hsr_dev; |
298 | struct dsa_port *other_dp; |
299 | |
300 | dsa_hsr_foreach_port(other_dp, dp->ds, hsr_dev) |
301 | val |= BIT(other_dp->index); |
302 | } |
303 | |
304 | *tag = cpu_to_be16(val); |
305 | |
306 | return ksz_defer_xmit(dp, skb); |
307 | } |
308 | |
309 | static struct sk_buff *ksz9477_rcv(struct sk_buff *skb, struct net_device *dev) |
310 | { |
311 | /* Tag decoding */ |
312 | u8 *tag = skb_tail_pointer(skb) - KSZ_EGRESS_TAG_LEN; |
313 | unsigned int port = tag[0] & 7; |
314 | unsigned int len = KSZ_EGRESS_TAG_LEN; |
315 | |
316 | /* Extra 4-bytes PTP timestamp */ |
317 | if (tag[0] & KSZ9477_PTP_TAG_INDICATION) { |
318 | ksz_rcv_timestamp(skb, tag); |
319 | len += KSZ_PTP_TAG_LEN; |
320 | } |
321 | |
322 | return ksz_common_rcv(skb, dev, port, len); |
323 | } |
324 | |
325 | static const struct dsa_device_ops ksz9477_netdev_ops = { |
326 | .name = KSZ9477_NAME, |
327 | .proto = DSA_TAG_PROTO_KSZ9477, |
328 | .xmit = ksz9477_xmit, |
329 | .rcv = ksz9477_rcv, |
330 | .connect = ksz_connect, |
331 | .disconnect = ksz_disconnect, |
332 | .needed_tailroom = KSZ9477_INGRESS_TAG_LEN + KSZ_PTP_TAG_LEN, |
333 | }; |
334 | |
335 | DSA_TAG_DRIVER(ksz9477_netdev_ops); |
336 | MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ9477, KSZ9477_NAME); |
337 | |
338 | #define KSZ9893_TAIL_TAG_PRIO GENMASK(4, 3) |
339 | #define KSZ9893_TAIL_TAG_OVERRIDE BIT(5) |
340 | #define KSZ9893_TAIL_TAG_LOOKUP BIT(6) |
341 | |
342 | static struct sk_buff *ksz9893_xmit(struct sk_buff *skb, |
343 | struct net_device *dev) |
344 | { |
345 | u16 queue_mapping = skb_get_queue_mapping(skb); |
346 | u8 prio = netdev_txq_to_tc(dev, txq: queue_mapping); |
347 | struct dsa_port *dp = dsa_user_to_port(dev); |
348 | struct ethhdr *hdr; |
349 | u8 *tag; |
350 | |
351 | if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb)) |
352 | return NULL; |
353 | |
354 | /* Tag encoding */ |
355 | ksz_xmit_timestamp(dp, skb); |
356 | |
357 | tag = skb_put(skb, KSZ_INGRESS_TAG_LEN); |
358 | hdr = skb_eth_hdr(skb); |
359 | |
360 | *tag = BIT(dp->index); |
361 | |
362 | *tag |= FIELD_PREP(KSZ9893_TAIL_TAG_PRIO, prio); |
363 | |
364 | if (is_link_local_ether_addr(addr: hdr->h_dest)) |
365 | *tag |= KSZ9893_TAIL_TAG_OVERRIDE; |
366 | |
367 | return ksz_defer_xmit(dp, skb); |
368 | } |
369 | |
370 | static const struct dsa_device_ops ksz9893_netdev_ops = { |
371 | .name = KSZ9893_NAME, |
372 | .proto = DSA_TAG_PROTO_KSZ9893, |
373 | .xmit = ksz9893_xmit, |
374 | .rcv = ksz9477_rcv, |
375 | .connect = ksz_connect, |
376 | .disconnect = ksz_disconnect, |
377 | .needed_tailroom = KSZ_INGRESS_TAG_LEN + KSZ_PTP_TAG_LEN, |
378 | }; |
379 | |
380 | DSA_TAG_DRIVER(ksz9893_netdev_ops); |
381 | MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ9893, KSZ9893_NAME); |
382 | |
383 | /* For xmit, 2/6 bytes are added before FCS. |
384 | * --------------------------------------------------------------------------- |
385 | * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|tag1(1byte)| |
386 | * FCS(4bytes) |
387 | * --------------------------------------------------------------------------- |
388 | * ts : time stamp (Present only if PTP is enabled in the Hardware) |
389 | * tag0 : represents tag override, lookup and valid |
390 | * tag1 : each bit represents port (eg, 0x01=port1, 0x02=port2, 0x80=port8) |
391 | * |
392 | * For rcv, 1/5 bytes is added before FCS. |
393 | * --------------------------------------------------------------------------- |
394 | * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|FCS(4bytes) |
395 | * --------------------------------------------------------------------------- |
396 | * ts : time stamp (Present only if bit 7 of tag0 is set) |
397 | * tag0 : zero-based value represents port |
398 | * (eg, 0x00=port1, 0x02=port3, 0x07=port8) |
399 | */ |
400 | #define LAN937X_EGRESS_TAG_LEN 2 |
401 | |
402 | #define LAN937X_TAIL_TAG_BLOCKING_OVERRIDE BIT(11) |
403 | #define LAN937X_TAIL_TAG_LOOKUP BIT(12) |
404 | #define LAN937X_TAIL_TAG_VALID BIT(13) |
405 | #define LAN937X_TAIL_TAG_PRIO GENMASK(10, 8) |
406 | #define LAN937X_TAIL_TAG_PORT_MASK 7 |
407 | |
408 | static struct sk_buff *lan937x_xmit(struct sk_buff *skb, |
409 | struct net_device *dev) |
410 | { |
411 | u16 queue_mapping = skb_get_queue_mapping(skb); |
412 | u8 prio = netdev_txq_to_tc(dev, txq: queue_mapping); |
413 | struct dsa_port *dp = dsa_user_to_port(dev); |
414 | const struct ethhdr *hdr = eth_hdr(skb); |
415 | __be16 *tag; |
416 | u16 val; |
417 | |
418 | if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb)) |
419 | return NULL; |
420 | |
421 | ksz_xmit_timestamp(dp, skb); |
422 | |
423 | tag = skb_put(skb, LAN937X_EGRESS_TAG_LEN); |
424 | |
425 | val = BIT(dp->index); |
426 | |
427 | val |= FIELD_PREP(LAN937X_TAIL_TAG_PRIO, prio); |
428 | |
429 | if (is_link_local_ether_addr(addr: hdr->h_dest)) |
430 | val |= LAN937X_TAIL_TAG_BLOCKING_OVERRIDE; |
431 | |
432 | /* Tail tag valid bit - This bit should always be set by the CPU */ |
433 | val |= LAN937X_TAIL_TAG_VALID; |
434 | |
435 | put_unaligned_be16(val, p: tag); |
436 | |
437 | return ksz_defer_xmit(dp, skb); |
438 | } |
439 | |
440 | static const struct dsa_device_ops lan937x_netdev_ops = { |
441 | .name = LAN937X_NAME, |
442 | .proto = DSA_TAG_PROTO_LAN937X, |
443 | .xmit = lan937x_xmit, |
444 | .rcv = ksz9477_rcv, |
445 | .connect = ksz_connect, |
446 | .disconnect = ksz_disconnect, |
447 | .needed_tailroom = LAN937X_EGRESS_TAG_LEN + KSZ_PTP_TAG_LEN, |
448 | }; |
449 | |
450 | DSA_TAG_DRIVER(lan937x_netdev_ops); |
451 | MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_LAN937X, LAN937X_NAME); |
452 | |
453 | static struct dsa_tag_driver *dsa_tag_driver_array[] = { |
454 | &DSA_TAG_DRIVER_NAME(ksz8795_netdev_ops), |
455 | &DSA_TAG_DRIVER_NAME(ksz9477_netdev_ops), |
456 | &DSA_TAG_DRIVER_NAME(ksz9893_netdev_ops), |
457 | &DSA_TAG_DRIVER_NAME(lan937x_netdev_ops), |
458 | }; |
459 | |
460 | module_dsa_tag_drivers(dsa_tag_driver_array); |
461 | |
462 | MODULE_LICENSE("GPL" ); |
463 | |