net: dsa: microchip: ptp: add packet transmission timestamping

[linux-2.6-microblaze.git] / net / dsa / tag_ksz.c

diff --git a/net/dsa/tag_ksz.c b/net/dsa/tag_ksz.c
index 6603eaa..e14ee26 100644 (file)
--- a/net/dsa/tag_ksz.c
+++ b/net/dsa/tag_ksz.c
@@ -26,6 +26,7 @@
 struct ksz_tagger_private {
        struct ksz_tagger_data data; /* Must be first */
        unsigned long state;
+       struct kthread_worker *xmit_worker;
 };
 
 static struct ksz_tagger_private *
@@ -48,6 +49,7 @@ static void ksz_disconnect(struct dsa_switch *ds)
 {
        struct ksz_tagger_private *priv = ds->tagger_data;
 
+       kthread_destroy_worker(priv->xmit_worker);
        kfree(priv);
        ds->tagger_data = NULL;
 }
@@ -55,12 +57,23 @@ static void ksz_disconnect(struct dsa_switch *ds)
 static int ksz_connect(struct dsa_switch *ds)
 {
        struct ksz_tagger_data *tagger_data;
+       struct kthread_worker *xmit_worker;
        struct ksz_tagger_private *priv;
+       int ret;
 
        priv = kzalloc(sizeof(*priv), GFP_KERNEL);
        if (!priv)
                return -ENOMEM;
 
+       xmit_worker = kthread_create_worker(0, "dsa%d:%d_xmit",
+                                           ds->dst->index, ds->index);
+       if (IS_ERR(xmit_worker)) {
+               ret = PTR_ERR(xmit_worker);
+               kfree(priv);
+               return ret;
+       }
+
+       priv->xmit_worker = xmit_worker;
        /* Export functions for switch driver use */
        tagger_data = &priv->data;
        tagger_data->hwtstamp_set_state = ksz_hwtstamp_set_state;
@@ -191,6 +204,41 @@ static void ksz_xmit_timestamp(struct dsa_port *dp, struct sk_buff *skb)
        put_unaligned_be32(0, skb_put(skb, KSZ_PTP_TAG_LEN));
 }
 
+/* Defer transmit if waiting for egress time stamp is required.  */
+static struct sk_buff *ksz_defer_xmit(struct dsa_port *dp, struct sk_buff *skb)
+{
+       struct ksz_tagger_data *tagger_data = ksz_tagger_data(dp->ds);
+       struct ksz_tagger_private *priv = ksz_tagger_private(dp->ds);
+       void (*xmit_work_fn)(struct kthread_work *work);
+       struct sk_buff *clone = KSZ_SKB_CB(skb)->clone;
+       struct ksz_deferred_xmit_work *xmit_work;
+       struct kthread_worker *xmit_worker;
+
+       if (!clone)
+               return skb;  /* no deferred xmit for this packet */
+
+       xmit_work_fn = tagger_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;
+
+       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(xmit_worker, &xmit_work->work);
+
+       return NULL;
+}
+
 static struct sk_buff *ksz9477_xmit(struct sk_buff *skb,
                                    struct net_device *dev)
 {
@@ -215,7 +263,7 @@ static struct sk_buff *ksz9477_xmit(struct sk_buff *skb,
 
        *tag = cpu_to_be16(val);
 
-       return skb;
+       return ksz_defer_xmit(dp, skb);
 }
 
 static struct sk_buff *ksz9477_rcv(struct sk_buff *skb, struct net_device *dev)
@@ -271,7 +319,7 @@ static struct sk_buff *ksz9893_xmit(struct sk_buff *skb,
        if (is_link_local_ether_addr(addr))
                *tag |= KSZ9893_TAIL_TAG_OVERRIDE;
 
-       return skb;
+       return ksz_defer_xmit(dp, skb);
 }
 
 static const struct dsa_device_ops ksz9893_netdev_ops = {
@@ -336,7 +384,7 @@ static struct sk_buff *lan937x_xmit(struct sk_buff *skb,
 
        put_unaligned_be16(val, tag);
 
-       return skb;
+       return ksz_defer_xmit(dp, skb);
 }
 
 static const struct dsa_device_ops lan937x_netdev_ops = {