Skip to content

Commit

Permalink
Merge branch 'DSA-Felix-PTP'
Browse files Browse the repository at this point in the history 
Yangbo Lu says:

====================
Support PTP clock and hardware timestamping for DSA Felix driver

This patch-set is to support PTP clock and hardware timestamping
for DSA Felix driver. Some functions in ocelot.c/ocelot_board.c
driver were reworked/exported, so that DSA Felix driver was able
to reuse them as much as possible.

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: David S. Miller <[email protected]>
  • Loading branch information
@davem330
davem330 committed Nov 21, 2019
2 parents 8163999 + c0bcf53 commit 2c44713
Show file tree
Hide file tree
Showing 7 changed files with 222 additions and 82 deletions.
89 changes: 89 additions & 0 deletions drivers/net/dsa/ocelot/felix.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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,
Expand All @@ -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)
{
Expand Down Expand Up @@ -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;
Expand All @@ -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:
Expand Down
16 changes: 16 additions & 0 deletions drivers/net/dsa/ocelot/felix_vsc9959.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -282,6 +282,16 @@ static const u32 vsc9959_sys_regmap[] = {
REG_RESERVED(SYS_CM_DATA),
};

static const u32 vsc9959_ptp_regmap[] = {
REG(PTP_PIN_CFG, 0x000000),
REG(PTP_PIN_TOD_SEC_MSB, 0x000004),
REG(PTP_PIN_TOD_SEC_LSB, 0x000008),
REG(PTP_PIN_TOD_NSEC, 0x00000c),
REG(PTP_CFG_MISC, 0x0000a0),
REG(PTP_CLK_CFG_ADJ_CFG, 0x0000a4),
REG(PTP_CLK_CFG_ADJ_FREQ, 0x0000a8),
};

static const u32 vsc9959_gcb_regmap[] = {
REG(GCB_SOFT_RST, 0x000004),
};
Expand All @@ -293,6 +303,7 @@ static const u32 *vsc9959_regmap[] = {
[REW] = vsc9959_rew_regmap,
[SYS] = vsc9959_sys_regmap,
[S2] = vsc9959_s2_regmap,
[PTP] = vsc9959_ptp_regmap,
[GCB] = vsc9959_gcb_regmap,
};

Expand Down Expand Up @@ -330,6 +341,11 @@ static struct resource vsc9959_target_io_res[] = {
.end = 0x00603ff,
.name = "s2",
},
[PTP] = {
.start = 0x0090000,
.end = 0x00900cb,
.name = "ptp",
},
[GCB] = {
.start = 0x0070000,
.end = 0x00701ff,
Expand Down
113 changes: 91 additions & 22 deletions drivers/net/ethernet/mscc/ocelot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -575,6 +575,32 @@ static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
return 0;
}

int ocelot_port_add_txtstamp_skb(struct ocelot_port *ocelot_port,
struct sk_buff *skb)
{
struct skb_shared_info *shinfo = skb_shinfo(skb);
struct ocelot *ocelot = ocelot_port->ocelot;

if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP &&
ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
struct ocelot_skb *oskb =
kzalloc(sizeof(struct ocelot_skb), GFP_ATOMIC);

if (unlikely(!oskb))
return -ENOMEM;

shinfo->tx_flags |= SKBTX_IN_PROGRESS;

oskb->skb = skb;
oskb->id = ocelot_port->ts_id % 4;

list_add_tail(&oskb->head, &ocelot_port->skbs);
return 0;
}
return -ENODATA;
}
EXPORT_SYMBOL(ocelot_port_add_txtstamp_skb);

static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
{
struct ocelot_port_private *priv = netdev_priv(dev);
Expand Down Expand Up @@ -637,31 +663,17 @@ static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
dev->stats.tx_packets++;
dev->stats.tx_bytes += skb->len;

if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP &&
ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
struct ocelot_skb *oskb =
kzalloc(sizeof(struct ocelot_skb), GFP_ATOMIC);

if (unlikely(!oskb))
goto out;

skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;

oskb->skb = skb;
oskb->id = ocelot_port->ts_id % 4;
if (!ocelot_port_add_txtstamp_skb(ocelot_port, skb)) {
ocelot_port->ts_id++;

list_add_tail(&oskb->head, &ocelot_port->skbs);

return NETDEV_TX_OK;
}

out:
dev_kfree_skb_any(skb);
return NETDEV_TX_OK;
}

void ocelot_get_hwtimestamp(struct ocelot *ocelot, struct timespec64 *ts)
static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
struct timespec64 *ts)
{
unsigned long flags;
u32 val;
Expand All @@ -686,7 +698,64 @@ void ocelot_get_hwtimestamp(struct ocelot *ocelot, struct timespec64 *ts)

spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
}
EXPORT_SYMBOL(ocelot_get_hwtimestamp);

void ocelot_get_txtstamp(struct ocelot *ocelot)
{
int budget = OCELOT_PTP_QUEUE_SZ;

while (budget--) {
struct skb_shared_hwtstamps shhwtstamps;
struct list_head *pos, *tmp;
struct sk_buff *skb = NULL;
struct ocelot_skb *entry;
struct ocelot_port *port;
struct timespec64 ts;
u32 val, id, txport;

val = ocelot_read(ocelot, SYS_PTP_STATUS);

/* Check if a timestamp can be retrieved */
if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
break;

WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);

/* Retrieve the ts ID and Tx port */
id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);

/* Retrieve its associated skb */
port = ocelot->ports[txport];

list_for_each_safe(pos, tmp, &port->skbs) {
entry = list_entry(pos, struct ocelot_skb, head);
if (entry->id != id)
continue;

skb = entry->skb;

list_del(pos);
kfree(entry);
}

/* Next ts */
ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);

if (unlikely(!skb))
continue;

/* Get the h/w timestamp */
ocelot_get_hwtimestamp(ocelot, &ts);

/* Set the timestamp into the skb */
memset(&shhwtstamps, 0, sizeof(shhwtstamps));
shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
skb_tstamp_tx(skb, &shhwtstamps);

dev_kfree_skb_any(skb);
}
}
EXPORT_SYMBOL(ocelot_get_txtstamp);

static int ocelot_mc_unsync(struct net_device *dev, const unsigned char *addr)
{
Expand Down Expand Up @@ -1049,15 +1118,14 @@ static int ocelot_get_port_parent_id(struct net_device *dev,
return 0;
}

static int ocelot_hwstamp_get(struct ocelot *ocelot, int port,
struct ifreq *ifr)
int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr)
{
return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0;
}
EXPORT_SYMBOL(ocelot_hwstamp_get);

static int ocelot_hwstamp_set(struct ocelot *ocelot, int port,
struct ifreq *ifr)
int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
{
struct ocelot_port *ocelot_port = ocelot->ports[port];
struct hwtstamp_config cfg;
Expand Down Expand Up @@ -1120,6 +1188,7 @@ static int ocelot_hwstamp_set(struct ocelot *ocelot, int port,

return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
}
EXPORT_SYMBOL(ocelot_hwstamp_set);

static int ocelot_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
{
Expand Down
6 changes: 0 additions & 6 deletions drivers/net/ethernet/mscc/ocelot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,12 +74,6 @@ struct ocelot_port_private {
struct ocelot_port_tc tc;
};

struct ocelot_skb {
struct list_head head;
struct sk_buff *skb;
u8 id;
};

u32 ocelot_port_readl(struct ocelot_port *port, u32 reg);
void ocelot_port_writel(struct ocelot_port *port, u32 val, u32 reg);

Expand Down
Loading

0 comments on commit 2c44713

Please sign in to comment.