187 lines
5.4 KiB
Diff
187 lines
5.4 KiB
Diff
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;
|
|
}
|
|
|