diff options
| -rw-r--r-- | drivers/net/dsa/ocelot/felix.c | 89 | ||||
| -rw-r--r-- | net/dsa/tag_ocelot.c | 14 |
2 files changed, 102 insertions, 1 deletions
diff --git a/drivers/net/dsa/ocelot/felix.c b/drivers/net/dsa/ocelot/felix.c index ce3637b504dd..167e41549cdd 100644 --- 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_switch *ds) 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_switch_ops = { .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_dev *pdev, 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_dev *pdev, err_register_ds: kfree(ds); err_alloc_ds: +err_alloc_irq: err_alloc_felix: kfree(felix); err_dma: diff --git a/net/dsa/tag_ocelot.c b/net/dsa/tag_ocelot.c index 078d4790669d..8e3e7283d430 100644 --- a/net/dsa/tag_ocelot.c +++ b/net/dsa/tag_ocelot.c @@ -137,9 +137,11 @@ static struct sk_buff *ocelot_xmit(struct sk_buff *skb, 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(struct sk_buff *skb, 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; } |