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_create_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_master_find_slave(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, 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_slave_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(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, 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(get_unaligned_be32(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(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, ptp_type); 216 if (!ptp_hdr) 217 goto output_tag; 218 219 correction = (s64)get_unaligned_be64(&ptp_hdr->correction); 220 221 if (correction < 0) { 222 struct timespec64 ts; 223 224 ts = ns_to_timespec64(-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, ptp_type, ptp_hdr, 0); 229 } 230 231 output_tag: 232 put_unaligned_be32(tstamp_raw, 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(dp->ds); 239 struct ksz_tagger_private *priv = ksz_tagger_private(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(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_slave_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(xmit_worker, &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, queue_mapping); 275 struct dsa_port *dp = dsa_slave_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(hdr->h_dest)) 294 val |= KSZ9477_TAIL_TAG_OVERRIDE; 295 296 *tag = cpu_to_be16(val); 297 298 return ksz_defer_xmit(dp, skb); 299 } 300 301 static struct sk_buff *ksz9477_rcv(struct sk_buff *skb, struct net_device *dev) 302 { 303 /* Tag decoding */ 304 u8 *tag = skb_tail_pointer(skb) - KSZ_EGRESS_TAG_LEN; 305 unsigned int port = tag[0] & 7; 306 unsigned int len = KSZ_EGRESS_TAG_LEN; 307 308 /* Extra 4-bytes PTP timestamp */ 309 if (tag[0] & KSZ9477_PTP_TAG_INDICATION) { 310 ksz_rcv_timestamp(skb, tag); 311 len += KSZ_PTP_TAG_LEN; 312 } 313 314 return ksz_common_rcv(skb, dev, port, len); 315 } 316 317 static const struct dsa_device_ops ksz9477_netdev_ops = { 318 .name = KSZ9477_NAME, 319 .proto = DSA_TAG_PROTO_KSZ9477, 320 .xmit = ksz9477_xmit, 321 .rcv = ksz9477_rcv, 322 .connect = ksz_connect, 323 .disconnect = ksz_disconnect, 324 .needed_tailroom = KSZ9477_INGRESS_TAG_LEN + KSZ_PTP_TAG_LEN, 325 }; 326 327 DSA_TAG_DRIVER(ksz9477_netdev_ops); 328 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ9477, KSZ9477_NAME); 329 330 #define KSZ9893_TAIL_TAG_PRIO GENMASK(4, 3) 331 #define KSZ9893_TAIL_TAG_OVERRIDE BIT(5) 332 #define KSZ9893_TAIL_TAG_LOOKUP BIT(6) 333 334 static struct sk_buff *ksz9893_xmit(struct sk_buff *skb, 335 struct net_device *dev) 336 { 337 u16 queue_mapping = skb_get_queue_mapping(skb); 338 u8 prio = netdev_txq_to_tc(dev, queue_mapping); 339 struct dsa_port *dp = dsa_slave_to_port(dev); 340 struct ethhdr *hdr; 341 u8 *tag; 342 343 if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb)) 344 return NULL; 345 346 /* Tag encoding */ 347 ksz_xmit_timestamp(dp, skb); 348 349 tag = skb_put(skb, KSZ_INGRESS_TAG_LEN); 350 hdr = skb_eth_hdr(skb); 351 352 *tag = BIT(dp->index); 353 354 *tag |= FIELD_PREP(KSZ9893_TAIL_TAG_PRIO, prio); 355 356 if (is_link_local_ether_addr(hdr->h_dest)) 357 *tag |= KSZ9893_TAIL_TAG_OVERRIDE; 358 359 return ksz_defer_xmit(dp, skb); 360 } 361 362 static const struct dsa_device_ops ksz9893_netdev_ops = { 363 .name = KSZ9893_NAME, 364 .proto = DSA_TAG_PROTO_KSZ9893, 365 .xmit = ksz9893_xmit, 366 .rcv = ksz9477_rcv, 367 .connect = ksz_connect, 368 .disconnect = ksz_disconnect, 369 .needed_tailroom = KSZ_INGRESS_TAG_LEN + KSZ_PTP_TAG_LEN, 370 }; 371 372 DSA_TAG_DRIVER(ksz9893_netdev_ops); 373 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ9893, KSZ9893_NAME); 374 375 /* For xmit, 2/6 bytes are added before FCS. 376 * --------------------------------------------------------------------------- 377 * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|tag1(1byte)| 378 * FCS(4bytes) 379 * --------------------------------------------------------------------------- 380 * ts : time stamp (Present only if PTP is enabled in the Hardware) 381 * tag0 : represents tag override, lookup and valid 382 * tag1 : each bit represents port (eg, 0x01=port1, 0x02=port2, 0x80=port8) 383 * 384 * For rcv, 1/5 bytes is added before FCS. 385 * --------------------------------------------------------------------------- 386 * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|FCS(4bytes) 387 * --------------------------------------------------------------------------- 388 * ts : time stamp (Present only if bit 7 of tag0 is set) 389 * tag0 : zero-based value represents port 390 * (eg, 0x00=port1, 0x02=port3, 0x07=port8) 391 */ 392 #define LAN937X_EGRESS_TAG_LEN 2 393 394 #define LAN937X_TAIL_TAG_BLOCKING_OVERRIDE BIT(11) 395 #define LAN937X_TAIL_TAG_LOOKUP BIT(12) 396 #define LAN937X_TAIL_TAG_VALID BIT(13) 397 #define LAN937X_TAIL_TAG_PRIO GENMASK(10, 8) 398 #define LAN937X_TAIL_TAG_PORT_MASK 7 399 400 static struct sk_buff *lan937x_xmit(struct sk_buff *skb, 401 struct net_device *dev) 402 { 403 u16 queue_mapping = skb_get_queue_mapping(skb); 404 u8 prio = netdev_txq_to_tc(dev, queue_mapping); 405 struct dsa_port *dp = dsa_slave_to_port(dev); 406 const struct ethhdr *hdr = eth_hdr(skb); 407 __be16 *tag; 408 u16 val; 409 410 if (skb->ip_summed == CHECKSUM_PARTIAL && skb_checksum_help(skb)) 411 return NULL; 412 413 ksz_xmit_timestamp(dp, skb); 414 415 tag = skb_put(skb, LAN937X_EGRESS_TAG_LEN); 416 417 val = BIT(dp->index); 418 419 val |= FIELD_PREP(LAN937X_TAIL_TAG_PRIO, prio); 420 421 if (is_link_local_ether_addr(hdr->h_dest)) 422 val |= LAN937X_TAIL_TAG_BLOCKING_OVERRIDE; 423 424 /* Tail tag valid bit - This bit should always be set by the CPU */ 425 val |= LAN937X_TAIL_TAG_VALID; 426 427 put_unaligned_be16(val, tag); 428 429 return ksz_defer_xmit(dp, skb); 430 } 431 432 static const struct dsa_device_ops lan937x_netdev_ops = { 433 .name = LAN937X_NAME, 434 .proto = DSA_TAG_PROTO_LAN937X, 435 .xmit = lan937x_xmit, 436 .rcv = ksz9477_rcv, 437 .connect = ksz_connect, 438 .disconnect = ksz_disconnect, 439 .needed_tailroom = LAN937X_EGRESS_TAG_LEN + KSZ_PTP_TAG_LEN, 440 }; 441 442 DSA_TAG_DRIVER(lan937x_netdev_ops); 443 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_LAN937X, LAN937X_NAME); 444 445 static struct dsa_tag_driver *dsa_tag_driver_array[] = { 446 &DSA_TAG_DRIVER_NAME(ksz8795_netdev_ops), 447 &DSA_TAG_DRIVER_NAME(ksz9477_netdev_ops), 448 &DSA_TAG_DRIVER_NAME(ksz9893_netdev_ops), 449 &DSA_TAG_DRIVER_NAME(lan937x_netdev_ops), 450 }; 451 452 module_dsa_tag_drivers(dsa_tag_driver_array); 453 454 MODULE_LICENSE("GPL"); 455