1 files changed, 70 insertions, 3 deletions

diff --git a/net/dsa/tag_ocelot_8021q.c b/net/dsa/tag_ocelot_8021q.c
index a1919ea5e828..fe451f4de7ba 100644
--- a/net/dsa/tag_ocelot_8021q.c
+++ b/net/dsa/tag_ocelot_8021q.c
@@ -12,25 +12,39 @@
 #include <linux/dsa/ocelot.h>
 #include "dsa_priv.h"
 
+struct ocelot_8021q_tagger_private {
+	struct ocelot_8021q_tagger_data data; /* Must be first */
+	struct kthread_worker *xmit_worker;
+};
+
 static struct sk_buff *ocelot_defer_xmit(struct dsa_port *dp,
 					 struct sk_buff *skb)
 {
+	struct ocelot_8021q_tagger_private *priv = dp->ds->tagger_data;
+	struct ocelot_8021q_tagger_data *data = &priv->data;
+	void (*xmit_work_fn)(struct kthread_work *work);
 	struct felix_deferred_xmit_work *xmit_work;
-	struct felix_port *felix_port = dp->priv;
+	struct kthread_worker *xmit_worker;
+
+	xmit_work_fn = data->xmit_work_fn;
+	xmit_worker = priv->xmit_worker;
+
+	if (!xmit_work_fn || !xmit_worker)
+		return NULL;
 
 	xmit_work = kzalloc(sizeof(*xmit_work), GFP_ATOMIC);
 	if (!xmit_work)
 		return NULL;
 
 	/* Calls felix_port_deferred_xmit in felix.c */
-	kthread_init_work(&xmit_work->work, felix_port->xmit_work_fn);
+	kthread_init_work(&xmit_work->work, xmit_work_fn);
 	/* Increase refcount so the kfree_skb in dsa_slave_xmit
 	 * won't really free the packet.
 	 */
 	xmit_work->dp = dp;
 	xmit_work->skb = skb_get(skb);
 
-	kthread_queue_work(felix_port->xmit_worker, &xmit_work->work);
+	kthread_queue_work(xmit_worker, &xmit_work->work);
 
 	return NULL;
 }
@@ -67,11 +81,64 @@ static struct sk_buff *ocelot_rcv(struct sk_buff *skb,
 	return skb;
 }
 
+static void ocelot_disconnect(struct dsa_switch_tree *dst)
+{
+	struct ocelot_8021q_tagger_private *priv;
+	struct dsa_port *dp;
+
+	list_for_each_entry(dp, &dst->ports, list) {
+		priv = dp->ds->tagger_data;
+
+		if (!priv)
+			continue;
+
+		if (priv->xmit_worker)
+			kthread_destroy_worker(priv->xmit_worker);
+
+		kfree(priv);
+		dp->ds->tagger_data = NULL;
+	}
+}
+
+static int ocelot_connect(struct dsa_switch_tree *dst)
+{
+	struct ocelot_8021q_tagger_private *priv;
+	struct dsa_port *dp;
+	int err;
+
+	list_for_each_entry(dp, &dst->ports, list) {
+		if (dp->ds->tagger_data)
+			continue;
+
+		priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+		if (!priv) {
+			err = -ENOMEM;
+			goto out;
+		}
+
+		priv->xmit_worker = kthread_create_worker(0, "felix_xmit");
+		if (IS_ERR(priv->xmit_worker)) {
+			err = PTR_ERR(priv->xmit_worker);
+			goto out;
+		}
+
+		dp->ds->tagger_data = priv;
+	}
+
+	return 0;
+
+out:
+	ocelot_disconnect(dst);
+	return err;
+}
+
 static const struct dsa_device_ops ocelot_8021q_netdev_ops = {
 	.name			= "ocelot-8021q",
 	.proto			= DSA_TAG_PROTO_OCELOT_8021Q,
 	.xmit			= ocelot_xmit,
 	.rcv			= ocelot_rcv,
+	.connect		= ocelot_connect,
+	.disconnect		= ocelot_disconnect,
 	.needed_headroom	= VLAN_HLEN,
 	.promisc_on_master	= true,
 };