[PATCH net-next 0/6] Egress VLAN modification using VCAP ES0 on Ocelot switches

@@ -142,17 +142,77 @@ ocelot_find_vcap_filter_that_points_at(struct ocelot *ocelot, int chain)
 	return NULL;
 }
 
+static int
+ocelot_flower_parse_ingress_vlan_modify(struct ocelot *ocelot, int port,
+					struct ocelot_vcap_filter *filter,
+					const struct flow_action_entry *a,
+					struct netlink_ext_ack *extack)
+{
+	struct ocelot_port *ocelot_port = ocelot->ports[port];
+
+	if (filter->goto_target != -1) {
+		NL_SET_ERR_MSG_MOD(extack,
+				   "Last action must be GOTO");
+		return -EOPNOTSUPP;
+	}
+
+	if (!ocelot_port->vlan_aware) {
+		NL_SET_ERR_MSG_MOD(extack,
+				   "Can only modify VLAN under VLAN aware bridge");
+		return -EOPNOTSUPP;
+	}
+
+	filter->action.vid_replace_ena = true;
+	filter->action.pcp_dei_ena = true;
+	filter->action.vid = a->vlan.vid;
+	filter->action.pcp = a->vlan.prio;
+	filter->type = OCELOT_VCAP_FILTER_OFFLOAD;
+
+	return 0;
+}
+
+static int
+ocelot_flower_parse_egress_vlan_modify(struct ocelot_vcap_filter *filter,
+				       const struct flow_action_entry *a,
+				       struct netlink_ext_ack *extack)
+{
+	enum ocelot_tag_tpid_sel tpid;
+
+	switch (ntohs(a->vlan.proto)) {
+	case ETH_P_8021Q:
+		tpid = OCELOT_TAG_TPID_SEL_8021Q;
+		break;
+	case ETH_P_8021AD:
+		tpid = OCELOT_TAG_TPID_SEL_8021AD;
+		break;
+	default:
+		NL_SET_ERR_MSG_MOD(extack,
+				   "Cannot modify custom TPID");
+		return -EOPNOTSUPP;
+	}
+
+	filter->action.tag_a_tpid_sel = tpid;
+	filter->action.push_outer_tag = OCELOT_ES0_TAG;
+	filter->action.tag_a_vid_sel = OCELOT_ES0_VID_PLUS_CLASSIFIED_VID;
+	filter->action.vid_a_val = a->vlan.vid;
+	filter->action.pcp_a_val = a->vlan.prio;
+	filter->action.tag_a_pcp_sel = OCELOT_ES0_PCP;
+	filter->type = OCELOT_VCAP_FILTER_OFFLOAD;
+
+	return 0;
+}
+
 static int ocelot_flower_parse_action(struct ocelot *ocelot, int port,
 				      bool ingress, struct flow_cls_offload *f,
 				      struct ocelot_vcap_filter *filter)
 {
-	struct ocelot_port *ocelot_port = ocelot->ports[port];
 	struct netlink_ext_ack *extack = f->common.extack;
 	bool allow_missing_goto_target = false;
 	const struct flow_action_entry *a;
 	enum ocelot_tag_tpid_sel tpid;
 	int i, chain, egress_port;
 	u64 rate;
+	int err;
 
 	if (!flow_action_basic_hw_stats_check(&f->rule->action,
 					      f->common.extack))

@@ -273,26 +333,20 @@ static int ocelot_flower_parse_action(struct ocelot *ocelot, int port,
 			filter->type = OCELOT_VCAP_FILTER_OFFLOAD;
 			break;
 		case FLOW_ACTION_VLAN_MANGLE:
-			if (filter->block_id != VCAP_IS1) {
-				NL_SET_ERR_MSG_MOD(extack,
-						   "VLAN modify action can only be offloaded to VCAP IS1");
-				return -EOPNOTSUPP;
-			}
-			if (filter->goto_target != -1) {
+			if (filter->block_id == VCAP_IS1) {
+				err = ocelot_flower_parse_ingress_vlan_modify(ocelot, port,
+									      filter, a,
+									      extack);
+			} else if (filter->block_id == VCAP_ES0) {
+				err = ocelot_flower_parse_egress_vlan_modify(filter, a,
+									     extack);
+			} else {
 				NL_SET_ERR_MSG_MOD(extack,
-						   "Last action must be GOTO");
-				return -EOPNOTSUPP;
+						   "VLAN modify action can only be offloaded to VCAP IS1 or ES0");
+				err = -EOPNOTSUPP;
 			}
-			if (!ocelot_port->vlan_aware) {
-				NL_SET_ERR_MSG_MOD(extack,
-						   "Can only modify VLAN under VLAN aware bridge");
-				return -EOPNOTSUPP;
-			}
-			filter->action.vid_replace_ena = true;
-			filter->action.pcp_dei_ena = true;
-			filter->action.vid = a->vlan.vid;
-			filter->action.pcp = a->vlan.prio;
-			filter->type = OCELOT_VCAP_FILTER_OFFLOAD;
+			if (err)
+				return err;
 			break;
 		case FLOW_ACTION_PRIORITY:
 			if (filter->block_id != VCAP_IS1) {

@@ -340,7 +394,7 @@ static int ocelot_flower_parse_action(struct ocelot *ocelot, int port,
 			}
 			filter->action.tag_a_tpid_sel = tpid;
 			filter->action.push_outer_tag = OCELOT_ES0_TAG;
-			filter->action.tag_a_vid_sel = 1;
+			filter->action.tag_a_vid_sel = OCELOT_ES0_VID;
 			filter->action.vid_a_val = a->vlan.vid;
 			filter->action.pcp_a_val = a->vlan.prio;
 			filter->type = OCELOT_VCAP_FILTER_OFFLOAD;

@@ -678,6 +732,31 @@ static int ocelot_vcap_dummy_filter_del(struct ocelot *ocelot,
 	return 0;
 }
 
+/* If we have an egress VLAN modification rule, we need to actually write the
+ * delta between the input VLAN (from the key) and the output VLAN (from the
+ * action), but the action was parsed first. So we need to patch the delta into
+ * the action here.
+ */
+static int
+ocelot_flower_patch_es0_vlan_modify(struct ocelot_vcap_filter *filter,
+				    struct netlink_ext_ack *extack)
+{
+	if (filter->block_id != VCAP_ES0 ||
+	    filter->action.tag_a_vid_sel != OCELOT_ES0_VID_PLUS_CLASSIFIED_VID)
+		return 0;
+
+	if (filter->vlan.vid.mask != VLAN_VID_MASK) {
+		NL_SET_ERR_MSG_MOD(extack,
+				   "VCAP ES0 VLAN rewriting needs a full VLAN in the key");
+		return -EOPNOTSUPP;
+	}
+
+	filter->action.vid_a_val -= filter->vlan.vid.value;
+	filter->action.vid_a_val &= VLAN_VID_MASK;
+
+	return 0;
+}
+
 int ocelot_cls_flower_replace(struct ocelot *ocelot, int port,
 			      struct flow_cls_offload *f, bool ingress)
 {

@@ -701,6 +780,12 @@ int ocelot_cls_flower_replace(struct ocelot *ocelot, int port,
 		return ret;
 	}
 
+	ret = ocelot_flower_patch_es0_vlan_modify(filter, extack);
+	if (ret) {
+		kfree(filter);
+		return ret;
+	}
+
 	/* The non-optional GOTOs for the TCAM skeleton don't need
 	 * to be actually offloaded.
 	 */

@@ -576,6 +576,16 @@ enum ocelot_mask_mode {
 	OCELOT_MASK_MODE_REDIRECT,
 };
 
+enum ocelot_es0_vid_sel {
+	OCELOT_ES0_VID_PLUS_CLASSIFIED_VID = 0,
+	OCELOT_ES0_VID = 1,
+};
+
+enum ocelot_es0_pcp_sel {
+	OCELOT_CLASSIFIED_PCP = 0,
+	OCELOT_ES0_PCP = 1,
+};
+
 enum ocelot_es0_tag {
 	OCELOT_NO_ES0_TAG,
 	OCELOT_ES0_TAG,

@@ -916,7 +916,7 @@ void ocelot_port_inject_frame(struct ocelot *ocelot, int port, int grp,
 	ocelot_ifh_set_bypass(ifh, 1);
 	ocelot_ifh_set_dest(ifh, BIT_ULL(port));
 	ocelot_ifh_set_tag_type(ifh, IFH_TAG_TYPE_C);
-	ocelot_ifh_set_vid(ifh, skb_vlan_tag_get(skb));
+	ocelot_ifh_set_vlan_tci(ifh, skb_vlan_tag_get(skb));
 	ocelot_ifh_set_rew_op(ifh, rew_op);
 
 	for (i = 0; i < OCELOT_TAG_LEN / 4; i++)

@@ -210,9 +210,9 @@ static inline void ocelot_ifh_set_tag_type(void *injection, u64 tag_type)
 	packing(injection, &tag_type, 16, 16, OCELOT_TAG_LEN, PACK, 0);
 }
 
-static inline void ocelot_ifh_set_vid(void *injection, u64 vid)
+static inline void ocelot_ifh_set_vlan_tci(void *injection, u64 vlan_tci)
 {
-	packing(injection, &vid, 11, 0, OCELOT_TAG_LEN, PACK, 0);
+	packing(injection, &vlan_tci, 15, 0, OCELOT_TAG_LEN, PACK, 0);
 }
 
 #endif

@@ -5,15 +5,52 @@
 #include <soc/mscc/ocelot.h>
 #include "dsa_priv.h"
 
+/* If the port is under a VLAN-aware bridge, remove the VLAN header from the
+ * payload and move it into the DSA tag, which will make the switch classify
+ * the packet to the bridge VLAN. Otherwise, leave the classified VLAN at zero,
+ * which is the pvid of standalone and VLAN-unaware bridge ports.
+ */
+static void ocelot_xmit_get_vlan_info(struct sk_buff *skb, struct dsa_port *dp,
+				      u64 *vlan_tci, u64 *tag_type)
+{
+	struct net_device *br = READ_ONCE(dp->bridge_dev);
+	struct vlan_ethhdr *hdr;
+	u16 proto, tci;
+
+	if (!br || !br_vlan_enabled(br)) {
+		*vlan_tci = 0;
+		*tag_type = IFH_TAG_TYPE_C;
+		return;
+	}
+
+	hdr = (struct vlan_ethhdr *)skb_mac_header(skb);
+	br_vlan_get_proto(br, &proto);
+
+	if (ntohs(hdr->h_vlan_proto) == proto) {
+		__skb_vlan_pop(skb, &tci);
+		*vlan_tci = tci;
+	} else {
+		rcu_read_lock();
+		br_vlan_get_pvid_rcu(br, &tci);
+		rcu_read_unlock();
+		*vlan_tci = tci;
+	}
+
+	*tag_type = (proto != ETH_P_8021Q) ? IFH_TAG_TYPE_S : IFH_TAG_TYPE_C;
+}
+
 static void ocelot_xmit_common(struct sk_buff *skb, struct net_device *netdev,
 			       __be32 ifh_prefix, void **ifh)
 {
 	struct dsa_port *dp = dsa_slave_to_port(netdev);
 	struct dsa_switch *ds = dp->ds;
+	u64 vlan_tci, tag_type;
 	void *injection;
 	__be32 *prefix;
 	u32 rew_op = 0;
 
+	ocelot_xmit_get_vlan_info(skb, dp, &vlan_tci, &tag_type);
+
 	injection = skb_push(skb, OCELOT_TAG_LEN);
 	prefix = skb_push(skb, OCELOT_SHORT_PREFIX_LEN);
 

@@ -22,6 +59,8 @@ static void ocelot_xmit_common(struct sk_buff *skb, struct net_device *netdev,
 	ocelot_ifh_set_bypass(injection, 1);
 	ocelot_ifh_set_src(injection, ds->num_ports);
 	ocelot_ifh_set_qos_class(injection, skb->priority);
+	ocelot_ifh_set_vlan_tci(injection, vlan_tci);
+	ocelot_ifh_set_tag_type(injection, tag_type);
 
 	rew_op = ocelot_ptp_rew_op(skb);
 	if (rew_op)

@@ -156,6 +156,11 @@ create_tcam_skeleton()
 
 setup_prepare()
 {
+	ip link set $eth0 up
+	ip link set $eth1 up
+	ip link set $eth2 up
+	ip link set $eth3 up
+
 	create_tcam_skeleton $eth0
 
 	ip link add br0 type bridge

@@ -247,9 +247,9 @@ test_vlan_push()
 	tcpdump_cleanup
 }
 
-test_vlan_modify()
+test_vlan_ingress_modify()
 {
-	printf "Testing VLAN modification..		"
+	printf "Testing ingress VLAN modification..		"
 
 	ip link set br0 type bridge vlan_filtering 1
 	bridge vlan add dev $eth0 vid 200

@@ -309,7 +309,7 @@ trap cleanup EXIT
 ALL_TESTS="
 	test_vlan_pop
 	test_vlan_push
-	test_vlan_modify
+	test_vlan_ingress_modify
 	test_skbedit_priority
 "
 

@@ -285,6 +285,44 @@ test_vlan_ingress_modify()
 	ip link set br0 type bridge vlan_filtering 0
 }
 
+test_vlan_egress_modify()
+{
+	printf "Testing egress VLAN modification..		"
+
+	tc qdisc add dev $eth1 clsact
+
+	ip link set br0 type bridge vlan_filtering 1
+	bridge vlan add dev $eth0 vid 200
+	bridge vlan add dev $eth1 vid 200
+
+	tc filter add dev $eth1 egress chain $(ES0) pref 3 \
+		protocol 802.1Q flower skip_sw vlan_id 200 vlan_prio 0 \
+		action vlan modify id 300 priority 7
+
+	tcpdump_start $eth2
+
+	$MZ $eth3.200 -q -c 1 -p 64 -a $eth3_mac -b $eth2_mac -t ip
+
+	sleep 1
+
+	tcpdump_stop
+
+	if tcpdump_show | grep -q "$eth3_mac > $eth2_mac, .* vlan 300"; then
+		echo "OK"
+	else
+		echo "FAIL"
+	fi
+
+	tcpdump_cleanup
+
+	tc filter del dev $eth1 egress chain $(ES0) pref 3
+	tc qdisc del dev $eth1 clsact
+
+	bridge vlan del dev $eth0 vid 200
+	bridge vlan del dev $eth1 vid 200
+	ip link set br0 type bridge vlan_filtering 0
+}
+
 test_skbedit_priority()
 {
 	local num_pkts=100

@@ -310,6 +348,7 @@ ALL_TESTS="
 	test_vlan_pop
 	test_vlan_push
 	test_vlan_ingress_modify
+	test_vlan_egress_modify
 	test_skbedit_priority
 "