--- /dev/null
+From 34bfb24b8ff5af09b014ea8530e1e8d89bb2a155 Mon Sep 17 00:00:00 2001
+From: Yangbo Lu <yangbo.lu@nxp.com>
+Date: Wed, 20 Nov 2019 16:23:18 +0800
+Subject: [PATCH] net: dsa: ocelot: add hardware timestamping support for Felix
+
+This patch is to reuse ocelot functions as possible to enable PTP
+clock and to support hardware timestamping on Felix.
+On TX path, timestamping works on packet which requires timestamp.
+The injection header will be configured accordingly, and skb clone
+requires timestamp will be added into a list. The TX timestamp
+is final handled in threaded interrupt handler when PTP timestamp
+FIFO is ready.
+On RX path, timestamping is always working. The RX timestamp could
+be got from extraction header.
+
+Signed-off-by: Yangbo Lu <yangbo.lu@nxp.com>
+Signed-off-by: David S. Miller <davem@davemloft.net>
+---
+ drivers/net/dsa/ocelot/felix.c | 89 ++++++++++++++++++++++++++++++++++++++++++
+ net/dsa/tag_ocelot.c | 14 ++++++-
+ 2 files changed, 102 insertions(+), 1 deletion(-)
+
+--- a/drivers/net/dsa/ocelot/felix.c
++++ b/drivers/net/dsa/ocelot/felix.c
+@@ -3,6 +3,7 @@
+ */
+ #include <uapi/linux/if_bridge.h>
+ #include <soc/mscc/ocelot.h>
++#include <linux/packing.h>
+ #include <linux/module.h>
+ #include <linux/pci.h>
+ #include <linux/of.h>
+@@ -303,6 +304,62 @@ static void felix_teardown(struct dsa_sw
+ ocelot_deinit(ocelot);
+ }
+
++static int felix_hwtstamp_get(struct dsa_switch *ds, int port,
++ struct ifreq *ifr)
++{
++ struct ocelot *ocelot = ds->priv;
++
++ return ocelot_hwstamp_get(ocelot, port, ifr);
++}
++
++static int felix_hwtstamp_set(struct dsa_switch *ds, int port,
++ struct ifreq *ifr)
++{
++ struct ocelot *ocelot = ds->priv;
++
++ return ocelot_hwstamp_set(ocelot, port, ifr);
++}
++
++static bool felix_rxtstamp(struct dsa_switch *ds, int port,
++ struct sk_buff *skb, unsigned int type)
++{
++ struct skb_shared_hwtstamps *shhwtstamps;
++ struct ocelot *ocelot = ds->priv;
++ u8 *extraction = skb->data - ETH_HLEN - OCELOT_TAG_LEN;
++ u32 tstamp_lo, tstamp_hi;
++ struct timespec64 ts;
++ u64 tstamp, val;
++
++ ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
++ tstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
++
++ packing(extraction, &val, 116, 85, OCELOT_TAG_LEN, UNPACK, 0);
++ tstamp_lo = (u32)val;
++
++ tstamp_hi = tstamp >> 32;
++ if ((tstamp & 0xffffffff) < tstamp_lo)
++ tstamp_hi--;
++
++ tstamp = ((u64)tstamp_hi << 32) | tstamp_lo;
++
++ shhwtstamps = skb_hwtstamps(skb);
++ memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
++ shhwtstamps->hwtstamp = tstamp;
++ return false;
++}
++
++bool felix_txtstamp(struct dsa_switch *ds, int port,
++ struct sk_buff *clone, unsigned int type)
++{
++ struct ocelot *ocelot = ds->priv;
++ struct ocelot_port *ocelot_port = ocelot->ports[port];
++
++ if (!ocelot_port_add_txtstamp_skb(ocelot_port, clone))
++ return true;
++
++ return false;
++}
++
+ static const struct dsa_switch_ops felix_switch_ops = {
+ .get_tag_protocol = felix_get_tag_protocol,
+ .setup = felix_setup,
+@@ -325,12 +382,33 @@ static const struct dsa_switch_ops felix
+ .port_vlan_filtering = felix_vlan_filtering,
+ .port_vlan_add = felix_vlan_add,
+ .port_vlan_del = felix_vlan_del,
++ .port_hwtstamp_get = felix_hwtstamp_get,
++ .port_hwtstamp_set = felix_hwtstamp_set,
++ .port_rxtstamp = felix_rxtstamp,
++ .port_txtstamp = felix_txtstamp,
+ };
+
+ static struct felix_info *felix_instance_tbl[] = {
+ [FELIX_INSTANCE_VSC9959] = &felix_info_vsc9959,
+ };
+
++static irqreturn_t felix_irq_handler(int irq, void *data)
++{
++ struct ocelot *ocelot = (struct ocelot *)data;
++
++ /* The INTB interrupt is used for both PTP TX timestamp interrupt
++ * and preemption status change interrupt on each port.
++ *
++ * - Get txtstamp if have
++ * - TODO: handle preemption. Without handling it, driver may get
++ * interrupt storm.
++ */
++
++ ocelot_get_txtstamp(ocelot);
++
++ return IRQ_HANDLED;
++}
++
+ static int felix_pci_probe(struct pci_dev *pdev,
+ const struct pci_device_id *id)
+ {
+@@ -372,6 +450,16 @@ static int felix_pci_probe(struct pci_de
+
+ pci_set_master(pdev);
+
++ err = devm_request_threaded_irq(&pdev->dev, pdev->irq, NULL,
++ &felix_irq_handler, IRQF_ONESHOT,
++ "felix-intb", ocelot);
++ if (err) {
++ dev_err(&pdev->dev, "Failed to request irq\n");
++ goto err_alloc_irq;
++ }
++
++ ocelot->ptp = 1;
++
+ ds = kzalloc(sizeof(struct dsa_switch), GFP_KERNEL);
+ if (!ds) {
+ err = -ENOMEM;
+@@ -396,6 +484,7 @@ static int felix_pci_probe(struct pci_de
+ err_register_ds:
+ kfree(ds);
+ err_alloc_ds:
++err_alloc_irq:
+ err_alloc_felix:
+ kfree(felix);
+ err_dma:
+--- a/net/dsa/tag_ocelot.c
++++ b/net/dsa/tag_ocelot.c
+@@ -137,9 +137,11 @@ static struct sk_buff *ocelot_xmit(struc
+ struct net_device *netdev)
+ {
+ struct dsa_port *dp = dsa_slave_to_port(netdev);
+- u64 bypass, dest, src, qos_class;
++ u64 bypass, dest, src, qos_class, rew_op;
+ struct dsa_switch *ds = dp->ds;
+ int port = dp->index;
++ struct ocelot *ocelot = ds->priv;
++ struct ocelot_port *ocelot_port = ocelot->ports[port];
+ u8 *injection;
+
+ if (unlikely(skb_cow_head(skb, OCELOT_TAG_LEN) < 0)) {
+@@ -161,6 +163,16 @@ static struct sk_buff *ocelot_xmit(struc
+ packing(injection, &src, 46, 43, OCELOT_TAG_LEN, PACK, 0);
+ packing(injection, &qos_class, 19, 17, OCELOT_TAG_LEN, PACK, 0);
+
++ if (ocelot->ptp && (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
++ rew_op = ocelot_port->ptp_cmd;
++ if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
++ rew_op |= (ocelot_port->ts_id % 4) << 3;
++ ocelot_port->ts_id++;
++ }
++
++ packing(injection, &rew_op, 125, 117, OCELOT_TAG_LEN, PACK, 0);
++ }
++
+ return skb;
+ }
+
projects / openwrt / openwrt.git / blobdiff