Initialise the generic Marvell PTP per-port timestamping layer, and use it to provide the get_ts_info() method. Signed-off-by: Russell King (Oracle) --- drivers/net/dsa/mv88e6xxx/chip.c | 4 + drivers/net/dsa/mv88e6xxx/chip.h | 2 + drivers/net/dsa/mv88e6xxx/hwtstamp.c | 188 ++++++++++++++++++++++++--- drivers/net/dsa/mv88e6xxx/hwtstamp.h | 6 + 4 files changed, 182 insertions(+), 18 deletions(-) diff --git a/drivers/net/dsa/mv88e6xxx/chip.c b/drivers/net/dsa/mv88e6xxx/chip.c index ed170a6b0672..fcb7b542bb27 100644 --- a/drivers/net/dsa/mv88e6xxx/chip.c +++ b/drivers/net/dsa/mv88e6xxx/chip.c @@ -4113,6 +4113,10 @@ static int mv88e6xxx_setup(struct dsa_switch *ds) err = mv88e6xxx_ptp_setup_unlocked(chip); if (err) goto out_hwtstamp; + + err = mv88e6xxx_hwtstamp_setup_unlocked(chip); + if (err) + goto out_hwtstamp; } /* Have to be called without holding the register lock, since diff --git a/drivers/net/dsa/mv88e6xxx/chip.h b/drivers/net/dsa/mv88e6xxx/chip.h index 29543f9312bd..a297b8867225 100644 --- a/drivers/net/dsa/mv88e6xxx/chip.h +++ b/drivers/net/dsa/mv88e6xxx/chip.h @@ -424,6 +424,8 @@ struct mv88e6xxx_chip { /* Per-port timestamping resources. */ struct mv88e6xxx_port_hwtstamp port_hwtstamp[DSA_MAX_PORTS]; + struct marvell_ts ptp_ts[DSA_MAX_PORTS]; + struct marvell_ts_caps ptp_caps; /* Array of port structures. */ struct mv88e6xxx_port ports[DSA_MAX_PORTS]; diff --git a/drivers/net/dsa/mv88e6xxx/hwtstamp.c b/drivers/net/dsa/mv88e6xxx/hwtstamp.c index dc92381d5c07..3e6a0481fc19 100644 --- a/drivers/net/dsa/mv88e6xxx/hwtstamp.c +++ b/drivers/net/dsa/mv88e6xxx/hwtstamp.c @@ -21,21 +21,37 @@ static int mv88e6xxx_port_ptp_read(struct mv88e6xxx_chip *chip, int port, int addr, u16 *data, int len) { + int err; + if (!chip->info->ops->avb_ops->port_ptp_read) return -EOPNOTSUPP; - return chip->info->ops->avb_ops->port_ptp_read(chip, port, addr, + err = chip->info->ops->avb_ops->port_ptp_read(chip, port, addr, data, len); + + dev_printk(KERN_DEBUG, + chip->dev, "%s: port=%d addr=%d, data[0]=%04x len=%d (%d)\n", + __func__, port, addr, data[0], len, err); + + return err; } static int mv88e6xxx_port_ptp_write(struct mv88e6xxx_chip *chip, int port, int addr, u16 data) { + int err; + if (!chip->info->ops->avb_ops->port_ptp_write) return -EOPNOTSUPP; - return chip->info->ops->avb_ops->port_ptp_write(chip, port, addr, + err = chip->info->ops->avb_ops->port_ptp_write(chip, port, addr, data); + + dev_printk(KERN_DEBUG, + chip->dev, "%s: port=%d addr=%d data=%04x (%d)\n", + __func__, port, addr, data, err); + + return err; } static int mv88e6xxx_ptp_write(struct mv88e6xxx_chip *chip, int addr, @@ -66,24 +82,14 @@ static int mv88e6xxx_ptp_read(struct mv88e6xxx_chip *chip, int addr, int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port, struct kernel_ethtool_ts_info *info) { - const struct mv88e6xxx_ptp_ops *ptp_ops; struct mv88e6xxx_chip *chip; chip = ds->priv; - ptp_ops = chip->info->ops->ptp_ops; if (!chip->info->ptp_support) return -EOPNOTSUPP; - info->so_timestamping = - SOF_TIMESTAMPING_TX_HARDWARE | - SOF_TIMESTAMPING_RX_HARDWARE | - SOF_TIMESTAMPING_RAW_HARDWARE; - info->phc_index = marvell_tai_ptp_clock_index(chip->tai); - info->tx_types = - (1 << HWTSTAMP_TX_OFF) | - (1 << HWTSTAMP_TX_ON); - info->rx_filters = ptp_ops->rx_filters; + marvell_ts_info(&chip->ptp_ts[port], info); return 0; } @@ -439,20 +445,31 @@ long mv88e6xxx_hwtstamp_work(struct mv88e6xxx_chip *chip) { struct dsa_switch *ds = chip->ds; struct mv88e6xxx_port_hwtstamp *ps; - int i, restart = 0; + long ret, delay = -1; + int i; for (i = 0; i < ds->num_ports; i++) { if (!dsa_is_user_port(ds, i)) continue; ps = &chip->port_hwtstamp[i]; - if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state)) - restart |= mv88e6xxx_txtstamp_work(chip, ps); + if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state) && + mv88e6xxx_txtstamp_work(chip, ps)) + delay = 1; mv88e6xxx_rxtstamp_work(chip, ps); } - return restart ? 1 : -1; + for (i = 0; i < ds->num_ports; i++) { + if (!dsa_is_user_port(ds, i)) + continue; + + ret = marvell_ts_aux_work(&chip->ptp_ts[i]); + if (ret >= 0 && (delay == -1 || delay > ret)) + delay = ret; + } + + return delay; } void mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port, @@ -551,18 +568,129 @@ static int mv88e6xxx_ts_global_write(struct device *dev, u8 reg, u16 val) return chip->info->ops->avb_ops->ptp_write(chip, reg, val); } +/* The device differences are: + * ts_reg MV88E6165 Others + * TS_ARR0 MV88E6165_PORT_PTP_ARR0_STS MV88E6XXX_PORT_PTP_ARR0_STS + * TS_ARR1 MV88E6165_PORT_PTP_ARR1_STS MV88E6XXX_PORT_PTP_ARR1_STS + * TS_DEP MV88E6165_PORT_PTP_DEP_STS MV88E6XXX_PORT_PTP_DEP_STS + */ +static int mv88e6xxx_ts_port_read_ts(struct device *dev, + struct marvell_hwts *hwts, u8 port, + enum marvell_ts_reg ts_reg) +{ + struct mv88e6xxx_chip *chip = dev_to_chip(dev); + u16 data[4]; + u16 reg; + int ret; + + switch (ts_reg) { + case MARVELL_TS_ARR0: + reg = chip->info->ops->ptp_ops->arr0_sts_reg; + break; + case MARVELL_TS_ARR1: + reg = chip->info->ops->ptp_ops->arr1_sts_reg; + break; + case MARVELL_TS_DEP: + reg = chip->info->ops->ptp_ops->dep_sts_reg; + break; + } + + mv88e6xxx_reg_lock(chip); + /* Read the status, time and sequence registers. If there's a valid + * timestamp, immediately clear the status. + */ + ret = mv88e6xxx_port_ptp_read(chip, port, reg, data, ARRAY_SIZE(data)); + if (ret == 0 && data[0] & MV_STATUS_VALID) + ret = mv88e6xxx_port_ptp_write(chip, port, reg, 0); + mv88e6xxx_reg_unlock(chip); + + if (ret == 0) { + hwts->stat = data[0]; + hwts->time = data[1] | data[2] << 16; + hwts->seq = data[3]; + + ret = !!(hwts->stat & MV_STATUS_VALID); + } + + return ret; +} + +/* PTP_PORT_CONFIG_0 is MV88E6XXX_PORT_PTP_CFG0 + * PTP_PORT_CONFIG_1 is MV88E6XXX_PORT_PTP_CFG1 + * note: nothing sets this register in this driver + * PTP_PORT_CONFIG_2 is MV88E6XXX_PORT_PTP_CFG2 + * note: nothing sets this register in this driver + * MV88E6XXX_PORT_PTP_LED_CFG has no equivalent + * note: nothing sets this register in this driver + * mv88e6165 doesn't have these registers + */ +static int mv88e6xxx_ts_port_write(struct device *dev, u8 port, u8 reg, u16 val) +{ + struct mv88e6xxx_chip *chip = dev_to_chip(dev); + const struct mv88e6xxx_avb_ops *avb_ops; + u16 old; + int err; + + avb_ops = chip->info->ops->avb_ops; + + mv88e6xxx_reg_lock(chip); + err = avb_ops->port_ptp_read(chip, port, reg, &old, 1); + err = avb_ops->port_ptp_write(chip, port, reg, val); + mv88e6xxx_reg_unlock(chip); + + dev_printk(KERN_DEBUG, + dev, "%s: port=%u reg=%u val=%04x, old=%04x (%d)\n", + __func__, port, reg, val, old, err); + + return err; +} + +static int mv88e6xxx_ts_port_modify(struct device *dev, u8 port, u8 reg, + u16 mask, u16 val) +{ + struct mv88e6xxx_chip *chip = dev_to_chip(dev); + const struct mv88e6xxx_avb_ops *avb_ops; + u16 old, data; + int err; + + avb_ops = chip->info->ops->avb_ops; + + mv88e6xxx_reg_lock(chip); + err = avb_ops->port_ptp_read(chip, port, reg, &old, 1); + if (err) + goto out; + + data = (old & ~mask) | val; + err = avb_ops->port_ptp_write(chip, port, reg, data); + + dev_printk(KERN_DEBUG, + dev, "%s: port=%u reg=%u mask=%04x val=%04x, 0x%04x -> 0x%04x (%d)\n", + __func__, port, reg, mask, val, old, data, err); + +out: + mv88e6xxx_reg_unlock(chip); + + return err; +} + static const struct marvell_ts_ops mv88e6xxx_ts_ops = { .ts_global_write = mv88e6xxx_ts_global_write, + .ts_port_read_ts = mv88e6xxx_ts_port_read_ts, + .ts_port_write = mv88e6xxx_ts_port_write, + .ts_port_modify = mv88e6xxx_ts_port_modify, }; int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip) { const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; + unsigned int n_ports = mv88e6xxx_num_ports(chip); int err; int i; + chip->ptp_caps.rx_filters = ptp_ops->rx_filters; + /* Disable timestamping on all ports. */ - for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) { + for (i = 0; i < n_ports; ++i) { err = mv88e6xxx_hwtstamp_port_setup(chip, i); if (err) return err; @@ -611,6 +739,30 @@ int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip) return 0; } +int mv88e6xxx_hwtstamp_setup_unlocked(struct mv88e6xxx_chip *chip) +{ + unsigned int n_ports = mv88e6xxx_num_ports(chip); + int i, err; + + for (i = err = 0; i < n_ports; ++i) { + err = marvell_ts_probe(&chip->ptp_ts[i], chip->dev, chip->tai, + &chip->ptp_caps, &mv88e6xxx_ts_ops, i); + if (err) + break; + } + + if (err) + while (i--) + marvell_ts_remove(&chip->ptp_ts[i]); + + return err; +} + void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip) { + unsigned int n_ports = mv88e6xxx_num_ports(chip); + int i; + + for (i = 0; i < n_ports; i++) + marvell_ts_remove(&chip->ptp_ts[i]); } diff --git a/drivers/net/dsa/mv88e6xxx/hwtstamp.h b/drivers/net/dsa/mv88e6xxx/hwtstamp.h index 747351d59921..f82383764653 100644 --- a/drivers/net/dsa/mv88e6xxx/hwtstamp.h +++ b/drivers/net/dsa/mv88e6xxx/hwtstamp.h @@ -125,6 +125,7 @@ int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port, struct kernel_ethtool_ts_info *info); long mv88e6xxx_hwtstamp_work(struct mv88e6xxx_chip *chip); +int mv88e6xxx_hwtstamp_setup_unlocked(struct mv88e6xxx_chip *chip); int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip); void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip); int mv88e6352_hwtstamp_port_enable(struct mv88e6xxx_chip *chip, int port); @@ -172,6 +173,11 @@ static inline int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip) return 0; } +static inline int mv88e6xxx_hwtstamp_setup_unlocked(struct mv88e6xxx_chip *chip) +{ + return 0; +} + static inline void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip) { } -- 2.47.3