| 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; |
| } |
| |