1 // SPDX-License-Identifier: GPL-2.0 2 /* Copyright 2020-2021 NXP Semiconductors 3 * 4 * An implementation of the software-defined tag_8021q.c tagger format, which 5 * also preserves full functionality under a vlan_filtering bridge. It does 6 * this by using the TCAM engines for: 7 * - pushing the RX VLAN as a second, outer tag, on egress towards the CPU port 8 * - redirecting towards the correct front port based on TX VLAN and popping 9 * that on egress 10 */ 11 #include <linux/dsa/8021q.h> 12 #include <soc/mscc/ocelot.h> 13 #include <soc/mscc/ocelot_ptp.h> 14 #include "dsa_priv.h" 15 16 static struct sk_buff *ocelot_xmit_ptp(struct dsa_port *dp, 17 struct sk_buff *skb, 18 struct sk_buff *clone) 19 { 20 struct ocelot *ocelot = dp->ds->priv; 21 struct ocelot_port *ocelot_port; 22 int port = dp->index; 23 u32 rew_op; 24 25 if (!ocelot_can_inject(ocelot, 0)) 26 return NULL; 27 28 ocelot_port = ocelot->ports[port]; 29 rew_op = ocelot_port->ptp_cmd; 30 31 /* Retrieve timestamp ID populated inside skb->cb[0] of the 32 * clone by ocelot_port_add_txtstamp_skb 33 */ 34 if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) 35 rew_op |= clone->cb[0] << 3; 36 37 ocelot_port_inject_frame(ocelot, port, 0, rew_op, skb); 38 39 return NULL; 40 } 41 42 static struct sk_buff *ocelot_xmit(struct sk_buff *skb, 43 struct net_device *netdev) 44 { 45 struct dsa_port *dp = dsa_slave_to_port(netdev); 46 u16 tx_vid = dsa_8021q_tx_vid(dp->ds, dp->index); 47 u16 queue_mapping = skb_get_queue_mapping(skb); 48 u8 pcp = netdev_txq_to_tc(netdev, queue_mapping); 49 struct sk_buff *clone = DSA_SKB_CB(skb)->clone; 50 51 /* TX timestamping was requested, so inject through MMIO */ 52 if (clone) 53 return ocelot_xmit_ptp(dp, skb, clone); 54 55 return dsa_8021q_xmit(skb, netdev, ETH_P_8021Q, 56 ((pcp << VLAN_PRIO_SHIFT) | tx_vid)); 57 } 58 59 static struct sk_buff *ocelot_rcv(struct sk_buff *skb, 60 struct net_device *netdev, 61 struct packet_type *pt) 62 { 63 int src_port, switch_id, qos_class; 64 u16 vid, tci; 65 66 skb_push_rcsum(skb, ETH_HLEN); 67 if (skb_vlan_tag_present(skb)) { 68 tci = skb_vlan_tag_get(skb); 69 __vlan_hwaccel_clear_tag(skb); 70 } else { 71 __skb_vlan_pop(skb, &tci); 72 } 73 skb_pull_rcsum(skb, ETH_HLEN); 74 75 vid = tci & VLAN_VID_MASK; 76 src_port = dsa_8021q_rx_source_port(vid); 77 switch_id = dsa_8021q_rx_switch_id(vid); 78 qos_class = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT; 79 80 skb->dev = dsa_master_find_slave(netdev, switch_id, src_port); 81 if (!skb->dev) 82 return NULL; 83 84 skb->offload_fwd_mark = 1; 85 skb->priority = qos_class; 86 87 return skb; 88 } 89 90 static const struct dsa_device_ops ocelot_8021q_netdev_ops = { 91 .name = "ocelot-8021q", 92 .proto = DSA_TAG_PROTO_OCELOT_8021Q, 93 .xmit = ocelot_xmit, 94 .rcv = ocelot_rcv, 95 .overhead = VLAN_HLEN, 96 .promisc_on_master = true, 97 }; 98 99 MODULE_LICENSE("GPL v2"); 100 MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_OCELOT_8021Q); 101 102 module_dsa_tag_driver(ocelot_8021q_netdev_ops); 103