@@ -2460,6 +2460,7 @@ static int ksz_connect_tag_protocol(struct dsa_switch *ds,
struct ksz_tagger_data *tagger_data;
tagger_data = ksz_tagger_data(ds);
+ tagger_data->xmit_work_fn = ksz_port_deferred_xmit;
tagger_data->meta_tstamp_handler = ksz_tstamp_reconstruct;
return 0;
@@ -2899,6 +2900,7 @@ static const struct dsa_switch_ops ksz_switch_ops = {
.get_ts_info = ksz_get_ts_info,
.port_hwtstamp_get = ksz_hwtstamp_get,
.port_hwtstamp_set = ksz_hwtstamp_set,
+ .port_txtstamp = ksz_port_txtstamp,
};
struct ksz_device *ksz_switch_alloc(struct device *base, void *priv)
@@ -87,6 +87,7 @@ struct ksz_irq {
struct ksz_ptp_irq {
struct ksz_port *port;
u16 ts_reg;
+ bool ts_en;
char name[16];
int irq_num;
};
@@ -115,6 +116,8 @@ struct ksz_port {
u8 hwts_tx_en;
struct ksz_irq ptpirq;
struct ksz_ptp_irq ptpmsg_irq[3];
+ ktime_t tstamp_msg;
+ struct completion tstamp_msg_comp;
#endif
};
@@ -100,8 +100,16 @@ static int ksz_set_hwtstamp_config(struct ksz_device *dev, int port,
switch (config->tx_type) {
case HWTSTAMP_TX_OFF:
+ prt->ptpmsg_irq[KSZ_SYNC_MSG].ts_en = 0;
+ prt->ptpmsg_irq[KSZ_XDREQ_MSG].ts_en = 0;
+ prt->ptpmsg_irq[KSZ_PDRES_MSG].ts_en = 0;
+ prt->hwts_tx_en = HWTSTAMP_TX_OFF;
+ break;
case HWTSTAMP_TX_ONESTEP_P2P:
- prt->hwts_tx_en = config->tx_type;
+ prt->ptpmsg_irq[KSZ_SYNC_MSG].ts_en = 0;
+ prt->ptpmsg_irq[KSZ_XDREQ_MSG].ts_en = 1;
+ prt->ptpmsg_irq[KSZ_PDRES_MSG].ts_en = 0;
+ prt->hwts_tx_en = HWTSTAMP_TX_ONESTEP_P2P;
break;
default:
return -ERANGE;
@@ -147,6 +155,47 @@ int ksz_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr)
return ret;
}
+void ksz_port_txtstamp(struct dsa_switch *ds, int port,
+ struct sk_buff *skb)
+{
+ struct ksz_device *dev = ds->priv;
+ struct ptp_header *hdr;
+ struct sk_buff *clone;
+ struct ksz_port *prt;
+ unsigned int type;
+ u8 ptp_msg_type;
+
+ prt = &dev->ports[port];
+
+ if (!prt->hwts_tx_en)
+ return;
+
+ type = ptp_classify_raw(skb);
+ if (type == PTP_CLASS_NONE)
+ return;
+
+ hdr = ptp_parse_header(skb, type);
+ if (!hdr)
+ return;
+
+ ptp_msg_type = ptp_get_msgtype(hdr, type);
+
+ switch (ptp_msg_type) {
+ case PTP_MSGTYPE_PDELAY_REQ:
+ break;
+
+ default:
+ return;
+ }
+
+ clone = skb_clone_sk(skb);
+ if (!clone)
+ return;
+
+ /* caching the value to be used in later */
+ KSZ_SKB_CB(skb)->clone = clone;
+}
+
static int _ksz_ptp_gettime(struct ksz_device *dev, struct timespec64 *ts)
{
u32 nanoseconds;
@@ -381,6 +430,49 @@ ktime_t ksz_tstamp_reconstruct(struct dsa_switch *ds, ktime_t tstamp)
return timespec64_to_ktime(ts);
}
+static void ksz_ptp_txtstamp_skb(struct ksz_device *dev,
+ struct ksz_port *prt, struct sk_buff *skb)
+{
+ struct skb_shared_hwtstamps hwtstamps = {};
+ int ret;
+
+ skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+
+ /* timeout must include tstamp latency, IRQ latency and time for
+ * reading the time stamp.
+ */
+ ret = wait_for_completion_timeout(&prt->tstamp_msg_comp,
+ msecs_to_jiffies(100));
+ if (!ret)
+ return;
+
+ hwtstamps.hwtstamp = prt->tstamp_msg;
+ skb_complete_tx_timestamp(skb, &hwtstamps);
+}
+
+#define work_to_xmit_work(w) \
+ container_of((w), struct ksz_deferred_xmit_work, work)
+void ksz_port_deferred_xmit(struct kthread_work *work)
+{
+ struct ksz_deferred_xmit_work *xmit_work = work_to_xmit_work(work);
+ struct sk_buff *clone, *skb = xmit_work->skb;
+ struct dsa_switch *ds = xmit_work->dp->ds;
+ struct ksz_device *dev = ds->priv;
+ struct ksz_port *prt;
+
+ prt = &dev->ports[xmit_work->dp->index];
+
+ clone = KSZ_SKB_CB(skb)->clone;
+
+ reinit_completion(&prt->tstamp_msg_comp);
+
+ dsa_enqueue_skb(skb, skb->dev);
+
+ ksz_ptp_txtstamp_skb(dev, prt, clone);
+
+ kfree(xmit_work);
+}
+
static const struct ptp_clock_info ksz_ptp_caps = {
.owner = THIS_MODULE,
.name = "Microchip Clock",
@@ -436,7 +528,29 @@ void ksz_ptp_clock_unregister(struct dsa_switch *ds)
static irqreturn_t ksz_ptp_msg_thread_fn(int irq, void *dev_id)
{
- return IRQ_NONE;
+ struct ksz_ptp_irq *ptpmsg_irq = dev_id;
+ struct ksz_device *dev;
+ struct ksz_port *port;
+ u32 tstamp_raw;
+ ktime_t tstamp;
+ int ret;
+
+ port = ptpmsg_irq->port;
+ dev = port->ksz_dev;
+
+ if (ptpmsg_irq->ts_en) {
+ ret = ksz_read32(dev, ptpmsg_irq->ts_reg, &tstamp_raw);
+ if (ret)
+ return IRQ_NONE;
+
+ tstamp = ksz_decode_tstamp(tstamp_raw);
+
+ port->tstamp_msg = ksz_tstamp_reconstruct(dev->ds, tstamp);
+
+ complete(&port->tstamp_msg_comp);
+ }
+
+ return IRQ_HANDLED;
}
static irqreturn_t ksz_ptp_irq_thread_fn(int irq, void *dev_id)
@@ -587,6 +701,8 @@ int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p)
REG_PTP_PORT_TX_INT_STATUS__2);
snprintf(ptpirq->name, sizeof(ptpirq->name), "ptp_irq-%d", p);
+ init_completion(&port->tstamp_msg_comp);
+
ptpirq->irq_num = irq_find_mapping(port->pirq.domain, PORT_SRC_PTP_INT);
if (ptpirq->irq_num < 0)
return ptpirq->irq_num;
@@ -28,8 +28,10 @@ int ksz_get_ts_info(struct dsa_switch *ds, int port,
struct ethtool_ts_info *ts);
int ksz_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr);
int ksz_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr);
+void ksz_port_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb);
int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p);
void ksz_ptp_irq_free(struct dsa_switch *ds, u8 p);
+void ksz_port_deferred_xmit(struct kthread_work *work);
ktime_t ksz_tstamp_reconstruct(struct dsa_switch *ds, ktime_t tstamp);
#else
@@ -60,6 +62,9 @@ static inline int ksz_hwtstamp_set(struct dsa_switch *ds, int port,
return -EOPNOTSUPP;
}
+static inline void ksz_port_txtstamp(struct dsa_switch *ds, int port,
+ struct sk_buff *skb) {}
+
static inline int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p)
{
return 0;
@@ -67,6 +72,8 @@ static inline int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p)
static inline void ksz_ptp_irq_free(struct dsa_switch *ds, u8 p) {}
+static inline void ksz_port_deferred_xmit(struct kthread_work *work) {}
+
static inline ktime_t ksz_tstamp_reconstruct(struct dsa_switch *ds, ktime_t tstamp)
{
return 0;
@@ -23,12 +23,26 @@ static inline ktime_t ksz_decode_tstamp(u32 tstamp)
return ns_to_ktime(ns);
}
+struct ksz_deferred_xmit_work {
+ struct dsa_port *dp;
+ struct sk_buff *skb;
+ struct kthread_work work;
+};
+
struct ksz_tagger_data {
+ void (*xmit_work_fn)(struct kthread_work *work);
bool (*hwtstamp_get_state)(struct dsa_switch *ds);
void (*hwtstamp_set_state)(struct dsa_switch *ds, bool on);
ktime_t (*meta_tstamp_handler)(struct dsa_switch *ds, ktime_t tstamp);
};
+struct ksz_skb_cb {
+ struct sk_buff *clone;
+};
+
+#define KSZ_SKB_CB(skb) \
+ ((struct ksz_skb_cb *)((skb)->cb))
+
static inline struct ksz_tagger_data *
ksz_tagger_data(struct dsa_switch *ds)
{
@@ -27,6 +27,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 *
@@ -56,6 +57,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;
}
@@ -63,12 +65,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_get_state = ksz_hwtstamp_get_state;
@@ -238,6 +251,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)
{
@@ -262,7 +310,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)
@@ -318,7 +366,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 = {
@@ -382,7 +430,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 = {