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, &priv->state); 45 else 46 clear_bit(KSZ_HWTS_EN, &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(priv->xmit_worker); 54 kfree(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(sizeof(*priv), GFP_KERNEL); 66 if (!priv) 67 return -ENOMEM; 68 69 xmit_worker = kthread_run_worker(0, "dsa%d:%d_xmit", 70 ds->dst->index, ds->index); 71 if (IS_ERR(xmit_worker)) { 72 ret = PTR_ERR(xmit_worker); 73 kfree(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, 0, port); 91 if (!skb->dev) 92 return NULL; 93 94 if (pskb_trim_rcsum(skb, 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, 0x0=port1, 0x2=port3, 0x3=port4) 115 */ 116 117 #define KSZ8795_TAIL_TAG_EG_PORT_M GENMASK(1, 0) 118 #define KSZ8795_TAIL_TAG_OVERRIDE BIT(6) 119 #define KSZ8795_TAIL_TAG_LOOKUP BIT(7) 120 121 static struct sk_buff *ksz8795_xmit(struct sk_buff *skb, struct net_device *dev) 122 { 123 struct dsa_port *dp = dsa_user_to_port(dev); 124 struct ethhdr *hdr; 125 u8 *tag; 126 127 if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb)) 128 return NULL; 129 130 /* Tag encoding */ 131 tag = skb_put(skb, KSZ_INGRESS_TAG_LEN); 132 hdr = skb_eth_hdr(skb); 133 134 *tag = 1 << dp->index; 135 if (is_link_local_ether_addr(hdr->h_dest)) 136 *tag |= KSZ8795_TAIL_TAG_OVERRIDE; 137 138 return skb; 139 } 140 141 static struct sk_buff *ksz8795_rcv(struct sk_buff *skb, struct net_device *dev) 142 { 143 u8 *tag; 144 145 if (skb_linearize(skb)) 146 return NULL; 147 148 tag = skb_tail_pointer(skb) - KSZ_EGRESS_TAG_LEN; 149 150 return ksz_common_rcv(skb, dev, tag[0] & KSZ8795_TAIL_TAG_EG_PORT_M, 151 KSZ_EGRESS_TAG_LEN); 152 } 153 154 static const struct dsa_device_ops ksz8795_netdev_ops = { 155 .name = KSZ8795_NAME, 156 .proto = DSA_TAG_PROTO_KSZ8795, 157 .xmit = ksz8795_xmit, 158 .rcv = ksz8795_rcv, 159 .needed_tailroom = KSZ_INGRESS_TAG_LEN, 160 }; 161 162 DSA_TAG_DRIVER(ksz8795_netdev_ops); 163 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ8795, KSZ8795_NAME); 164 165 /* 166 * For Ingress (Host -> KSZ9477), 2/6 bytes are added before FCS. 167 * --------------------------------------------------------------------------- 168 * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|tag1(1byte)| 169 * FCS(4bytes) 170 * --------------------------------------------------------------------------- 171 * ts : time stamp (Present only if PTP is enabled in the Hardware) 172 * tag0 : Prioritization (not used now) 173 * tag1 : each bit represents port (eg, 0x01=port1, 0x02=port2, 0x10=port5) 174 * 175 * For Egress (KSZ9477 -> Host), 1/5 bytes is added before FCS. 176 * --------------------------------------------------------------------------- 177 * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|FCS(4bytes) 178 * --------------------------------------------------------------------------- 179 * ts : time stamp (Present only if bit 7 of tag0 is set) 180 * tag0 : zero-based value represents port 181 * (eg, 0x00=port1, 0x02=port3, 0x06=port7) 182 */ 183 184 #define KSZ9477_INGRESS_TAG_LEN 2 185 #define KSZ9477_PTP_TAG_LEN 4 186 #define KSZ9477_PTP_TAG_INDICATION BIT(7) 187 188 #define KSZ9477_TAIL_TAG_EG_PORT_M GENMASK(2, 0) 189 #define KSZ9477_TAIL_TAG_PRIO GENMASK(8, 7) 190 #define KSZ9477_TAIL_TAG_OVERRIDE BIT(9) 191 #define KSZ9477_TAIL_TAG_LOOKUP BIT(10) 192 193 static void ksz_rcv_timestamp(struct sk_buff *skb, u8 *tag) 194 { 195 u8 *tstamp_raw = tag - KSZ_PTP_TAG_LEN; 196 ktime_t tstamp; 197 198 tstamp = ksz_decode_tstamp(get_unaligned_be32(tstamp_raw)); 199 KSZ_SKB_CB(skb)->tstamp = tstamp; 200 } 201 202 /* Time stamp tag *needs* to be inserted if PTP is enabled in hardware. 203 * Regardless of Whether it is a PTP frame or not. 204 */ 205 static void ksz_xmit_timestamp(struct dsa_port *dp, struct sk_buff *skb) 206 { 207 struct ksz_tagger_private *priv; 208 struct ptp_header *ptp_hdr; 209 unsigned int ptp_type; 210 u32 tstamp_raw = 0; 211 s64 correction; 212 213 priv = ksz_tagger_private(dp->ds); 214 215 if (!test_bit(KSZ_HWTS_EN, &priv->state)) 216 return; 217 218 if (!KSZ_SKB_CB(skb)->update_correction) 219 goto output_tag; 220 221 ptp_type = KSZ_SKB_CB(skb)->ptp_type; 222 223 ptp_hdr = ptp_parse_header(skb, ptp_type); 224 if (!ptp_hdr) 225 goto output_tag; 226 227 correction = (s64)get_unaligned_be64(&ptp_hdr->correction); 228 229 if (correction < 0) { 230 struct timespec64 ts; 231 232 ts = ns_to_timespec64(-correction >> 16); 233 tstamp_raw = ((ts.tv_sec & 3) << 30) | ts.tv_nsec; 234 235 /* Set correction field to 0 and update UDP checksum */ 236 ptp_header_update_correction(skb, ptp_type, ptp_hdr, 0); 237 } 238 239 output_tag: 240 put_unaligned_be32(tstamp_raw, skb_put(skb, KSZ_PTP_TAG_LEN)); 241 } 242 243 /* Defer transmit if waiting for egress time stamp is required. */ 244 static struct sk_buff *ksz_defer_xmit(struct dsa_port *dp, struct sk_buff *skb) 245 { 246 struct ksz_tagger_data *tagger_data = ksz_tagger_data(dp->ds); 247 struct ksz_tagger_private *priv = ksz_tagger_private(dp->ds); 248 void (*xmit_work_fn)(struct kthread_work *work); 249 struct sk_buff *clone = KSZ_SKB_CB(skb)->clone; 250 struct ksz_deferred_xmit_work *xmit_work; 251 struct kthread_worker *xmit_worker; 252 253 if (!clone) 254 return skb; /* no deferred xmit for this packet */ 255 256 xmit_work_fn = tagger_data->xmit_work_fn; 257 xmit_worker = priv->xmit_worker; 258 259 if (!xmit_work_fn || !xmit_worker) 260 return NULL; 261 262 xmit_work = kzalloc(sizeof(*xmit_work), GFP_ATOMIC); 263 if (!xmit_work) 264 return NULL; 265 266 kthread_init_work(&xmit_work->work, xmit_work_fn); 267 /* Increase refcount so the kfree_skb in dsa_user_xmit 268 * won't really free the packet. 269 */ 270 xmit_work->dp = dp; 271 xmit_work->skb = skb_get(skb); 272 273 kthread_queue_work(xmit_worker, &xmit_work->work); 274 275 return NULL; 276 } 277 278 static struct sk_buff *ksz9477_xmit(struct sk_buff *skb, 279 struct net_device *dev) 280 { 281 u16 queue_mapping = skb_get_queue_mapping(skb); 282 u8 prio = netdev_txq_to_tc(dev, queue_mapping); 283 struct dsa_port *dp = dsa_user_to_port(dev); 284 struct ethhdr *hdr; 285 __be16 *tag; 286 u16 val; 287 288 if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb)) 289 return NULL; 290 291 /* Tag encoding */ 292 ksz_xmit_timestamp(dp, skb); 293 294 tag = skb_put(skb, KSZ9477_INGRESS_TAG_LEN); 295 hdr = skb_eth_hdr(skb); 296 297 val = BIT(dp->index); 298 299 val |= FIELD_PREP(KSZ9477_TAIL_TAG_PRIO, prio); 300 301 if (is_link_local_ether_addr(hdr->h_dest)) 302 val |= KSZ9477_TAIL_TAG_OVERRIDE; 303 304 if (dev->features & NETIF_F_HW_HSR_DUP) { 305 struct net_device *hsr_dev = dp->hsr_dev; 306 struct dsa_port *other_dp; 307 308 dsa_hsr_foreach_port(other_dp, dp->ds, hsr_dev) 309 val |= BIT(other_dp->index); 310 } 311 312 *tag = cpu_to_be16(val); 313 314 return ksz_defer_xmit(dp, skb); 315 } 316 317 static struct sk_buff *ksz9477_rcv(struct sk_buff *skb, struct net_device *dev) 318 { 319 unsigned int len = KSZ_EGRESS_TAG_LEN; 320 unsigned int port; 321 u8 *tag; 322 323 if (skb_linearize(skb)) 324 return NULL; 325 326 /* Tag decoding */ 327 tag = skb_tail_pointer(skb) - KSZ_EGRESS_TAG_LEN; 328 port = tag[0] & KSZ9477_TAIL_TAG_EG_PORT_M; 329 330 /* Extra 4-bytes PTP timestamp */ 331 if (tag[0] & KSZ9477_PTP_TAG_INDICATION) { 332 ksz_rcv_timestamp(skb, tag); 333 len += KSZ_PTP_TAG_LEN; 334 } 335 336 return ksz_common_rcv(skb, dev, port, len); 337 } 338 339 static const struct dsa_device_ops ksz9477_netdev_ops = { 340 .name = KSZ9477_NAME, 341 .proto = DSA_TAG_PROTO_KSZ9477, 342 .xmit = ksz9477_xmit, 343 .rcv = ksz9477_rcv, 344 .connect = ksz_connect, 345 .disconnect = ksz_disconnect, 346 .needed_tailroom = KSZ9477_INGRESS_TAG_LEN + KSZ_PTP_TAG_LEN, 347 }; 348 349 DSA_TAG_DRIVER(ksz9477_netdev_ops); 350 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ9477, KSZ9477_NAME); 351 352 #define KSZ9893_TAIL_TAG_PRIO GENMASK(4, 3) 353 #define KSZ9893_TAIL_TAG_OVERRIDE BIT(5) 354 #define KSZ9893_TAIL_TAG_LOOKUP BIT(6) 355 356 static struct sk_buff *ksz9893_xmit(struct sk_buff *skb, 357 struct net_device *dev) 358 { 359 u16 queue_mapping = skb_get_queue_mapping(skb); 360 u8 prio = netdev_txq_to_tc(dev, queue_mapping); 361 struct dsa_port *dp = dsa_user_to_port(dev); 362 struct ethhdr *hdr; 363 u8 *tag; 364 365 if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb)) 366 return NULL; 367 368 /* Tag encoding */ 369 ksz_xmit_timestamp(dp, skb); 370 371 tag = skb_put(skb, KSZ_INGRESS_TAG_LEN); 372 hdr = skb_eth_hdr(skb); 373 374 *tag = BIT(dp->index); 375 376 *tag |= FIELD_PREP(KSZ9893_TAIL_TAG_PRIO, prio); 377 378 if (is_link_local_ether_addr(hdr->h_dest)) 379 *tag |= KSZ9893_TAIL_TAG_OVERRIDE; 380 381 return ksz_defer_xmit(dp, skb); 382 } 383 384 static const struct dsa_device_ops ksz9893_netdev_ops = { 385 .name = KSZ9893_NAME, 386 .proto = DSA_TAG_PROTO_KSZ9893, 387 .xmit = ksz9893_xmit, 388 .rcv = ksz9477_rcv, 389 .connect = ksz_connect, 390 .disconnect = ksz_disconnect, 391 .needed_tailroom = KSZ_INGRESS_TAG_LEN + KSZ_PTP_TAG_LEN, 392 }; 393 394 DSA_TAG_DRIVER(ksz9893_netdev_ops); 395 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ9893, KSZ9893_NAME); 396 397 /* For xmit, 2/6 bytes are added before FCS. 398 * --------------------------------------------------------------------------- 399 * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|tag1(1byte)| 400 * FCS(4bytes) 401 * --------------------------------------------------------------------------- 402 * ts : time stamp (Present only if PTP is enabled in the Hardware) 403 * tag0 : represents tag override, lookup and valid 404 * tag1 : each bit represents port (eg, 0x01=port1, 0x02=port2, 0x80=port8) 405 * 406 * For rcv, 1/5 bytes is added before FCS. 407 * --------------------------------------------------------------------------- 408 * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|FCS(4bytes) 409 * --------------------------------------------------------------------------- 410 * ts : time stamp (Present only if bit 7 of tag0 is set) 411 * tag0 : zero-based value represents port 412 * (eg, 0x00=port1, 0x02=port3, 0x07=port8) 413 */ 414 #define LAN937X_EGRESS_TAG_LEN 2 415 416 #define LAN937X_TAIL_TAG_BLOCKING_OVERRIDE BIT(11) 417 #define LAN937X_TAIL_TAG_LOOKUP BIT(12) 418 #define LAN937X_TAIL_TAG_VALID BIT(13) 419 #define LAN937X_TAIL_TAG_PRIO GENMASK(10, 8) 420 #define LAN937X_TAIL_TAG_PORT_MASK 7 421 422 static struct sk_buff *lan937x_xmit(struct sk_buff *skb, 423 struct net_device *dev) 424 { 425 u16 queue_mapping = skb_get_queue_mapping(skb); 426 u8 prio = netdev_txq_to_tc(dev, queue_mapping); 427 struct dsa_port *dp = dsa_user_to_port(dev); 428 const struct ethhdr *hdr = eth_hdr(skb); 429 __be16 *tag; 430 u16 val; 431 432 if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb)) 433 return NULL; 434 435 ksz_xmit_timestamp(dp, skb); 436 437 tag = skb_put(skb, LAN937X_EGRESS_TAG_LEN); 438 439 val = BIT(dp->index); 440 441 val |= FIELD_PREP(LAN937X_TAIL_TAG_PRIO, prio); 442 443 if (is_link_local_ether_addr(hdr->h_dest)) 444 val |= LAN937X_TAIL_TAG_BLOCKING_OVERRIDE; 445 446 /* Tail tag valid bit - This bit should always be set by the CPU */ 447 val |= LAN937X_TAIL_TAG_VALID; 448 449 put_unaligned_be16(val, tag); 450 451 return ksz_defer_xmit(dp, skb); 452 } 453 454 static const struct dsa_device_ops lan937x_netdev_ops = { 455 .name = LAN937X_NAME, 456 .proto = DSA_TAG_PROTO_LAN937X, 457 .xmit = lan937x_xmit, 458 .rcv = ksz9477_rcv, 459 .connect = ksz_connect, 460 .disconnect = ksz_disconnect, 461 .needed_tailroom = LAN937X_EGRESS_TAG_LEN + KSZ_PTP_TAG_LEN, 462 }; 463 464 DSA_TAG_DRIVER(lan937x_netdev_ops); 465 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_LAN937X, LAN937X_NAME); 466 467 static struct dsa_tag_driver *dsa_tag_driver_array[] = { 468 &DSA_TAG_DRIVER_NAME(ksz8795_netdev_ops), 469 &DSA_TAG_DRIVER_NAME(ksz9477_netdev_ops), 470 &DSA_TAG_DRIVER_NAME(ksz9893_netdev_ops), 471 &DSA_TAG_DRIVER_NAME(lan937x_netdev_ops), 472 }; 473 474 module_dsa_tag_drivers(dsa_tag_driver_array); 475 476 MODULE_DESCRIPTION("DSA tag driver for Microchip 8795/937x/9477/9893 families of switches"); 477 MODULE_LICENSE("GPL"); 478