[mirror_ubuntu-eoan-kernel.git] / drivers / net / dsa / mv88e6xxx / hwtstamp.c

// SPDX-License-Identifier: GPL-2.0-or-later
/*
 * Marvell 88E6xxx Switch hardware timestamping support
 *
 * Copyright (c) 2008 Marvell Semiconductor
 *
 * Copyright (c) 2017 National Instruments
 *      Erik Hons <erik.hons@ni.com>
 *      Brandon Streiff <brandon.streiff@ni.com>
 *      Dane Wagner <dane.wagner@ni.com>
 */

#include "chip.h"
#include "global2.h"
#include "hwtstamp.h"
#include "ptp.h"
#include <linux/ptp_classify.h>

#define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb))

static int mv88e6xxx_port_ptp_read(struct mv88e6xxx_chip *chip, int port,
				   int addr, u16 *data, int len)
{
	if (!chip->info->ops->avb_ops->port_ptp_read)
		return -EOPNOTSUPP;

	return chip->info->ops->avb_ops->port_ptp_read(chip, port, addr,
						       data, len);
}

static int mv88e6xxx_port_ptp_write(struct mv88e6xxx_chip *chip, int port,
				    int addr, u16 data)
{
	if (!chip->info->ops->avb_ops->port_ptp_write)
		return -EOPNOTSUPP;

	return chip->info->ops->avb_ops->port_ptp_write(chip, port, addr,
							data);
}

static int mv88e6xxx_ptp_write(struct mv88e6xxx_chip *chip, int addr,
			       u16 data)
{
	if (!chip->info->ops->avb_ops->ptp_write)
		return -EOPNOTSUPP;

	return chip->info->ops->avb_ops->ptp_write(chip, addr, data);
}

static int mv88e6xxx_ptp_read(struct mv88e6xxx_chip *chip, int addr,
			      u16 *data)
{
	if (!chip->info->ops->avb_ops->ptp_read)
		return -EOPNOTSUPP;

	return chip->info->ops->avb_ops->ptp_read(chip, addr, data, 1);
}

/* TX_TSTAMP_TIMEOUT: This limits the time spent polling for a TX
 * timestamp. When working properly, hardware will produce a timestamp
 * within 1ms. Software may enounter delays due to MDIO contention, so
 * the timeout is set accordingly.
 */
#define TX_TSTAMP_TIMEOUT	msecs_to_jiffies(40)

int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port,
			  struct 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 = ptp_clock_index(chip->ptp_clock);
	info->tx_types =
		(1 << HWTSTAMP_TX_OFF) |
		(1 << HWTSTAMP_TX_ON);
	info->rx_filters = ptp_ops->rx_filters;

	return 0;
}

static int mv88e6xxx_set_hwtstamp_config(struct mv88e6xxx_chip *chip, int port,
					 struct hwtstamp_config *config)
{
	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
	bool tstamp_enable = false;

	/* Prevent the TX/RX paths from trying to interact with the
	 * timestamp hardware while we reconfigure it.
	 */
	clear_bit_unlock(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state);

	/* reserved for future extensions */
	if (config->flags)
		return -EINVAL;

	switch (config->tx_type) {
	case HWTSTAMP_TX_OFF:
		tstamp_enable = false;
		break;
	case HWTSTAMP_TX_ON:
		tstamp_enable = true;
		break;
	default:
		return -ERANGE;
	}

	/* The switch supports timestamping both L2 and L4; one cannot be
	 * disabled independently of the other.
	 */

	if (!(BIT(config->rx_filter) & ptp_ops->rx_filters)) {
		config->rx_filter = HWTSTAMP_FILTER_NONE;
		dev_dbg(chip->dev, "Unsupported rx_filter %d\n",
			config->rx_filter);
		return -ERANGE;
	}

	switch (config->rx_filter) {
	case HWTSTAMP_FILTER_NONE:
		tstamp_enable = false;
		break;
	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
	case HWTSTAMP_FILTER_PTP_V2_EVENT:
	case HWTSTAMP_FILTER_PTP_V2_SYNC:
	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
		config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
		break;
	case HWTSTAMP_FILTER_ALL:
	default:
		config->rx_filter = HWTSTAMP_FILTER_NONE;
		return -ERANGE;
	}

	mutex_lock(&chip->reg_lock);
	if (tstamp_enable) {
		chip->enable_count += 1;
		if (chip->enable_count == 1 && ptp_ops->global_enable)
			ptp_ops->global_enable(chip);
		if (ptp_ops->port_enable)
			ptp_ops->port_enable(chip, port);
	} else {
		if (ptp_ops->port_disable)
			ptp_ops->port_disable(chip, port);
		chip->enable_count -= 1;
		if (chip->enable_count == 0 && ptp_ops->global_disable)
			ptp_ops->global_disable(chip);
	}
	mutex_unlock(&chip->reg_lock);

	/* Once hardware has been configured, enable timestamp checks
	 * in the RX/TX paths.
	 */
	if (tstamp_enable)
		set_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state);

	return 0;
}

int mv88e6xxx_port_hwtstamp_set(struct dsa_switch *ds, int port,
				struct ifreq *ifr)
{
	struct mv88e6xxx_chip *chip = ds->priv;
	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
	struct hwtstamp_config config;
	int err;

	if (!chip->info->ptp_support)
		return -EOPNOTSUPP;

	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
		return -EFAULT;

	err = mv88e6xxx_set_hwtstamp_config(chip, port, &config);
	if (err)
		return err;

	/* Save the chosen configuration to be returned later. */
	memcpy(&ps->tstamp_config, &config, sizeof(config));

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

int mv88e6xxx_port_hwtstamp_get(struct dsa_switch *ds, int port,
				struct ifreq *ifr)
{
	struct mv88e6xxx_chip *chip = ds->priv;
	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
	struct hwtstamp_config *config = &ps->tstamp_config;

	if (!chip->info->ptp_support)
		return -EOPNOTSUPP;

	return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ?
		-EFAULT : 0;
}

/* Get the start of the PTP header in this skb */
static u8 *parse_ptp_header(struct sk_buff *skb, unsigned int type)
{
	u8 *data = skb_mac_header(skb);
	unsigned int offset = 0;

	if (type & PTP_CLASS_VLAN)
		offset += VLAN_HLEN;

	switch (type & PTP_CLASS_PMASK) {
	case PTP_CLASS_IPV4:
		offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
		break;
	case PTP_CLASS_IPV6:
		offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
		break;
	case PTP_CLASS_L2:
		offset += ETH_HLEN;
		break;
	default:
		return NULL;
	}

	/* Ensure that the entire header is present in this packet. */
	if (skb->len + ETH_HLEN < offset + 34)
		return NULL;

	return data + offset;
}

/* Returns a pointer to the PTP header if the caller should time stamp,
 * or NULL if the caller should not.
 */
static u8 *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip, int port,
				   struct sk_buff *skb, unsigned int type)
{
	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
	u8 *hdr;

	if (!chip->info->ptp_support)
		return NULL;

	hdr = parse_ptp_header(skb, type);
	if (!hdr)
		return NULL;

	if (!test_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state))
		return NULL;

	return hdr;
}

static int mv88e6xxx_ts_valid(u16 status)
{
	if (!(status & MV88E6XXX_PTP_TS_VALID))
		return 0;
	if (status & MV88E6XXX_PTP_TS_STATUS_MASK)
		return 0;
	return 1;
}

static int seq_match(struct sk_buff *skb, u16 ts_seqid)
{
	unsigned int type = SKB_PTP_TYPE(skb);
	u8 *hdr = parse_ptp_header(skb, type);
	__be16 *seqid;

	seqid = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID);

	return ts_seqid == ntohs(*seqid);
}

static void mv88e6xxx_get_rxts(struct mv88e6xxx_chip *chip,
			       struct mv88e6xxx_port_hwtstamp *ps,
			       struct sk_buff *skb, u16 reg,
			       struct sk_buff_head *rxq)
{
	u16 buf[4] = { 0 }, status, seq_id;
	struct skb_shared_hwtstamps *shwt;
	struct sk_buff_head received;
	u64 ns, timelo, timehi;
	unsigned long flags;
	int err;

	/* The latched timestamp belongs to one of the received frames. */
	__skb_queue_head_init(&received);
	spin_lock_irqsave(&rxq->lock, flags);
	skb_queue_splice_tail_init(rxq, &received);
	spin_unlock_irqrestore(&rxq->lock, flags);

	mutex_lock(&chip->reg_lock);
	err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
				      reg, buf, ARRAY_SIZE(buf));
	mutex_unlock(&chip->reg_lock);
	if (err)
		pr_err("failed to get the receive time stamp\n");

	status = buf[0];
	timelo = buf[1];
	timehi = buf[2];
	seq_id = buf[3];

	if (status & MV88E6XXX_PTP_TS_VALID) {
		mutex_lock(&chip->reg_lock);
		err = mv88e6xxx_port_ptp_write(chip, ps->port_id, reg, 0);
		mutex_unlock(&chip->reg_lock);
		if (err)
			pr_err("failed to clear the receive status\n");
	}
	/* Since the device can only handle one time stamp at a time,
	 * we purge any extra frames from the queue.
	 */
	for ( ; skb; skb = __skb_dequeue(&received)) {
		if (mv88e6xxx_ts_valid(status) && seq_match(skb, seq_id)) {
			ns = timehi << 16 | timelo;

			mutex_lock(&chip->reg_lock);
			ns = timecounter_cyc2time(&chip->tstamp_tc, ns);
			mutex_unlock(&chip->reg_lock);
			shwt = skb_hwtstamps(skb);
			memset(shwt, 0, sizeof(*shwt));
			shwt->hwtstamp = ns_to_ktime(ns);
			status &= ~MV88E6XXX_PTP_TS_VALID;
		}
		netif_rx_ni(skb);
	}
}

static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip,
				    struct mv88e6xxx_port_hwtstamp *ps)
{
	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
	struct sk_buff *skb;

	skb = skb_dequeue(&ps->rx_queue);

	if (skb)
		mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr0_sts_reg,
				   &ps->rx_queue);

	skb = skb_dequeue(&ps->rx_queue2);
	if (skb)
		mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr1_sts_reg,
				   &ps->rx_queue2);
}

static int is_pdelay_resp(u8 *msgtype)
{
	return (*msgtype & 0xf) == 3;
}

bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port,
			     struct sk_buff *skb, unsigned int type)
{
	struct mv88e6xxx_port_hwtstamp *ps;
	struct mv88e6xxx_chip *chip;
	u8 *hdr;

	chip = ds->priv;
	ps = &chip->port_hwtstamp[port];

	if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
		return false;

	hdr = mv88e6xxx_should_tstamp(chip, port, skb, type);
	if (!hdr)
		return false;

	SKB_PTP_TYPE(skb) = type;

	if (is_pdelay_resp(hdr))
		skb_queue_tail(&ps->rx_queue2, skb);
	else
		skb_queue_tail(&ps->rx_queue, skb);

	ptp_schedule_worker(chip->ptp_clock, 0);

	return true;
}

static int mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip *chip,
				   struct mv88e6xxx_port_hwtstamp *ps)
{
	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
	struct skb_shared_hwtstamps shhwtstamps;
	u16 departure_block[4], status;
	struct sk_buff *tmp_skb;
	u32 time_raw;
	int err;
	u64 ns;

	if (!ps->tx_skb)
		return 0;

	mutex_lock(&chip->reg_lock);
	err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
				      ptp_ops->dep_sts_reg,
				      departure_block,
				      ARRAY_SIZE(departure_block));
	mutex_unlock(&chip->reg_lock);

	if (err)
		goto free_and_clear_skb;

	if (!(departure_block[0] & MV88E6XXX_PTP_TS_VALID)) {
		if (time_is_before_jiffies(ps->tx_tstamp_start +
					   TX_TSTAMP_TIMEOUT)) {
			dev_warn(chip->dev, "p%d: clearing tx timestamp hang\n",
				 ps->port_id);
			goto free_and_clear_skb;
		}
		/* The timestamp should be available quickly, while getting it
		 * is high priority and time bounded to only 10ms. A poll is
		 * warranted so restart the work.
		 */
		return 1;
	}

	/* We have the timestamp; go ahead and clear valid now */
	mutex_lock(&chip->reg_lock);
	mv88e6xxx_port_ptp_write(chip, ps->port_id, ptp_ops->dep_sts_reg, 0);
	mutex_unlock(&chip->reg_lock);

	status = departure_block[0] & MV88E6XXX_PTP_TS_STATUS_MASK;
	if (status != MV88E6XXX_PTP_TS_STATUS_NORMAL) {
		dev_warn(chip->dev, "p%d: tx timestamp overrun\n", ps->port_id);
		goto free_and_clear_skb;
	}

	if (departure_block[3] != ps->tx_seq_id) {
		dev_warn(chip->dev, "p%d: unexpected seq. id\n", ps->port_id);
		goto free_and_clear_skb;
	}

	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
	time_raw = ((u32)departure_block[2] << 16) | departure_block[1];
	mutex_lock(&chip->reg_lock);
	ns = timecounter_cyc2time(&chip->tstamp_tc, time_raw);
	mutex_unlock(&chip->reg_lock);
	shhwtstamps.hwtstamp = ns_to_ktime(ns);

	dev_dbg(chip->dev,
		"p%d: txtstamp %llx status 0x%04x skb ID 0x%04x hw ID 0x%04x\n",
		ps->port_id, ktime_to_ns(shhwtstamps.hwtstamp),
		departure_block[0], ps->tx_seq_id, departure_block[3]);

	/* skb_complete_tx_timestamp() will free up the client to make
	 * another timestamp-able transmit. We have to be ready for it
	 * -- by clearing the ps->tx_skb "flag" -- beforehand.
	 */

	tmp_skb = ps->tx_skb;
	ps->tx_skb = NULL;
	clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
	skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);

	return 0;

free_and_clear_skb:
	dev_kfree_skb_any(ps->tx_skb);
	ps->tx_skb = NULL;
	clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);

	return 0;
}

long mv88e6xxx_hwtstamp_work(struct ptp_clock_info *ptp)
{
	struct mv88e6xxx_chip *chip = ptp_to_chip(ptp);
	struct dsa_switch *ds = chip->ds;
	struct mv88e6xxx_port_hwtstamp *ps;
	int i, restart = 0;

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

		mv88e6xxx_rxtstamp_work(chip, ps);
	}

	return restart ? 1 : -1;
}

bool mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port,
			     struct sk_buff *clone, unsigned int type)
{
	struct mv88e6xxx_chip *chip = ds->priv;
	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
	__be16 *seq_ptr;
	u8 *hdr;

	if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP))
		return false;

	hdr = mv88e6xxx_should_tstamp(chip, port, clone, type);
	if (!hdr)
		return false;

	seq_ptr = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID);

	if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS,
				  &ps->state))
		return false;

	ps->tx_skb = clone;
	ps->tx_tstamp_start = jiffies;
	ps->tx_seq_id = be16_to_cpup(seq_ptr);

	ptp_schedule_worker(chip->ptp_clock, 0);
	return true;
}

int mv88e6165_global_disable(struct mv88e6xxx_chip *chip)
{
	u16 val;
	int err;

	err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val);
	if (err)
		return err;
	val |= MV88E6165_PTP_CFG_DISABLE_PTP;

	return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val);
}

int mv88e6165_global_enable(struct mv88e6xxx_chip *chip)
{
	u16 val;
	int err;

	err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val);
	if (err)
		return err;

	val &= ~(MV88E6165_PTP_CFG_DISABLE_PTP | MV88E6165_PTP_CFG_TSPEC_MASK);

	return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val);
}

int mv88e6352_hwtstamp_port_disable(struct mv88e6xxx_chip *chip, int port)
{
	return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
					MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP);
}

int mv88e6352_hwtstamp_port_enable(struct mv88e6xxx_chip *chip, int port)
{
	return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
					MV88E6XXX_PORT_PTP_CFG0_DISABLE_TSPEC_MATCH);
}

static int mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip *chip, int port)
{
	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];

	ps->port_id = port;

	skb_queue_head_init(&ps->rx_queue);
	skb_queue_head_init(&ps->rx_queue2);

	if (ptp_ops->port_disable)
		return ptp_ops->port_disable(chip, port);

	return 0;
}

int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip)
{
	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
	int err;
	int i;

	/* Disable timestamping on all ports. */
	for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
		err = mv88e6xxx_hwtstamp_port_setup(chip, i);
		if (err)
			return err;
	}

	/* Disable PTP globally */
	if (ptp_ops->global_disable) {
		err = ptp_ops->global_disable(chip);
		if (err)
			return err;
	}

	/* Set the ethertype of L2 PTP messages */
	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_GC_ETYPE, ETH_P_1588);
	if (err)
		return err;

	/* MV88E6XXX_PTP_MSG_TYPE is a mask of PTP message types to
	 * timestamp. This affects all ports that have timestamping enabled,
	 * but the timestamp config is per-port; thus we configure all events
	 * here and only support the HWTSTAMP_FILTER_*_EVENT filter types.
	 */
	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_MSGTYPE,
				  MV88E6XXX_PTP_MSGTYPE_ALL_EVENT);
	if (err)
		return err;

	/* Use ARRIVAL1 for peer delay response messages. */
	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_TS_ARRIVAL_PTR,
				  MV88E6XXX_PTP_MSGTYPE_PDLAY_RES);
	if (err)
		return err;

	/* 88E6341 devices default to timestamping at the PHY, but this has
	 * a hardware issue that results in unreliable timestamps. Force
	 * these devices to timestamp at the MAC.
	 */
	if (chip->info->family == MV88E6XXX_FAMILY_6341) {
		u16 val = MV88E6341_PTP_CFG_UPDATE |
			  MV88E6341_PTP_CFG_MODE_IDX |
			  MV88E6341_PTP_CFG_MODE_TS_AT_MAC;
		err = mv88e6xxx_ptp_write(chip, MV88E6341_PTP_CFG, val);
		if (err)
			return err;
	}

	return 0;
}

void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip)
{
}
Commit	Line	Data
2874c5fd	1	// SPDX-License-Identifier: GPL-2.0-or-later
c6fe0ad2 BS	2	/*
	3	* Marvell 88E6xxx Switch hardware timestamping support
	4	*
	5	* Copyright (c) 2008 Marvell Semiconductor
	6	*
	7	* Copyright (c) 2017 National Instruments
	8	* Erik Hons <erik.hons@ni.com>
	9	* Brandon Streiff <brandon.streiff@ni.com>
	10	* Dane Wagner <dane.wagner@ni.com>
c6fe0ad2 BS	11	*/
	12
	13	#include "chip.h"
	14	#include "global2.h"
	15	#include "hwtstamp.h"
	16	#include "ptp.h"
	17	#include <linux/ptp_classify.h>
	18
	19	#define SKB_PTP_TYPE(__skb) ((unsigned int )((__skb)->cb))
	20
	21	static int mv88e6xxx_port_ptp_read(struct mv88e6xxx_chip *chip, int port,
	22	int addr, u16 *data, int len)
	23	{
	24	if (!chip->info->ops->avb_ops->port_ptp_read)
	25	return -EOPNOTSUPP;
	26
	27	return chip->info->ops->avb_ops->port_ptp_read(chip, port, addr,
	28	data, len);
	29	}
	30
	31	static int mv88e6xxx_port_ptp_write(struct mv88e6xxx_chip *chip, int port,
	32	int addr, u16 data)
	33	{
	34	if (!chip->info->ops->avb_ops->port_ptp_write)
	35	return -EOPNOTSUPP;
	36
	37	return chip->info->ops->avb_ops->port_ptp_write(chip, port, addr,
	38	data);
	39	}
	40
	41	static int mv88e6xxx_ptp_write(struct mv88e6xxx_chip *chip, int addr,
	42	u16 data)
	43	{
	44	if (!chip->info->ops->avb_ops->ptp_write)
	45	return -EOPNOTSUPP;
	46
	47	return chip->info->ops->avb_ops->ptp_write(chip, addr, data);
	48	}
	49
e2294a8b AL	50	static int mv88e6xxx_ptp_read(struct mv88e6xxx_chip *chip, int addr,
	51	u16 *data)
	52	{
	53	if (!chip->info->ops->avb_ops->ptp_read)
	54	return -EOPNOTSUPP;
	55
	56	return chip->info->ops->avb_ops->ptp_read(chip, addr, data, 1);
	57	}
	58
c6fe0ad2 BS	59	/* TX_TSTAMP_TIMEOUT: This limits the time spent polling for a TX
	60	* timestamp. When working properly, hardware will produce a timestamp
	61	* within 1ms. Software may enounter delays due to MDIO contention, so
	62	* the timeout is set accordingly.
	63	*/
df31b74c	64	#define TX_TSTAMP_TIMEOUT msecs_to_jiffies(40)
c6fe0ad2 BS	65
	66	int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port,
	67	struct ethtool_ts_info *info)
	68	{
48cb5e03 AL	69	const struct mv88e6xxx_ptp_ops *ptp_ops;
	70	struct mv88e6xxx_chip *chip;
	71
	72	chip = ds->priv;
	73	ptp_ops = chip->info->ops->ptp_ops;
c6fe0ad2 BS	74
	75	if (!chip->info->ptp_support)
	76	return -EOPNOTSUPP;
	77
	78	info->so_timestamping =
	79	SOF_TIMESTAMPING_TX_HARDWARE \|
	80	SOF_TIMESTAMPING_RX_HARDWARE \|
	81	SOF_TIMESTAMPING_RAW_HARDWARE;
	82	info->phc_index = ptp_clock_index(chip->ptp_clock);
	83	info->tx_types =
	84	(1 << HWTSTAMP_TX_OFF) \|
	85	(1 << HWTSTAMP_TX_ON);
48cb5e03	86	info->rx_filters = ptp_ops->rx_filters;
c6fe0ad2 BS	87
	88	return 0;
	89	}
	90
	91	static int mv88e6xxx_set_hwtstamp_config(struct mv88e6xxx_chip *chip, int port,
	92	struct hwtstamp_config *config)
	93	{
ffc705de	94	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
c6fe0ad2 BS	95	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
c6fe0ad2 BS	96	bool tstamp_enable = false;
c6fe0ad2 BS	97
	98	/* Prevent the TX/RX paths from trying to interact with the
	99	* timestamp hardware while we reconfigure it.
	100	*/
	101	clear_bit_unlock(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state);
	102
	103	/* reserved for future extensions */
	104	if (config->flags)
	105	return -EINVAL;
	106
	107	switch (config->tx_type) {
	108	case HWTSTAMP_TX_OFF:
	109	tstamp_enable = false;
	110	break;
	111	case HWTSTAMP_TX_ON:
	112	tstamp_enable = true;
	113	break;
	114	default:
	115	return -ERANGE;
	116	}
	117
	118	/* The switch supports timestamping both L2 and L4; one cannot be
	119	* disabled independently of the other.
	120	*/
48cb5e03 AL	121
	122	if (!(BIT(config->rx_filter) & ptp_ops->rx_filters)) {
	123	config->rx_filter = HWTSTAMP_FILTER_NONE;
	124	dev_dbg(chip->dev, "Unsupported rx_filter %d\n",
	125	config->rx_filter);
	126	return -ERANGE;
	127	}
	128
c6fe0ad2 BS	129	switch (config->rx_filter) {
	130	case HWTSTAMP_FILTER_NONE:
	131	tstamp_enable = false;
	132	break;
	133	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
	134	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
	135	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
	136	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
	137	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
	138	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
	139	case HWTSTAMP_FILTER_PTP_V2_EVENT:
	140	case HWTSTAMP_FILTER_PTP_V2_SYNC:
	141	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
	142	config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
	143	break;
	144	case HWTSTAMP_FILTER_ALL:
	145	default:
	146	config->rx_filter = HWTSTAMP_FILTER_NONE;
	147	return -ERANGE;
	148	}
	149
ffc705de	150	mutex_lock(&chip->reg_lock);
c6fe0ad2	151	if (tstamp_enable) {
e2294a8b AL	152	chip->enable_count += 1;
	153	if (chip->enable_count == 1 && ptp_ops->global_enable)
	154	ptp_ops->global_enable(chip);
ffc705de AL	155	if (ptp_ops->port_enable)
ffc705de AL	156	ptp_ops->port_enable(chip, port);
c6fe0ad2	157	} else {
ffc705de AL	158	if (ptp_ops->port_disable)
ffc705de AL	159	ptp_ops->port_disable(chip, port);
e2294a8b AL	160	chip->enable_count -= 1;
	161	if (chip->enable_count == 0 && ptp_ops->global_disable)
	162	ptp_ops->global_disable(chip);
c6fe0ad2	163	}
c6fe0ad2 BS	164	mutex_unlock(&chip->reg_lock);
c6fe0ad2 BS	165
c6fe0ad2 BS	166	/* Once hardware has been configured, enable timestamp checks
	167	* in the RX/TX paths.
	168	*/
	169	if (tstamp_enable)
	170	set_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state);
	171
	172	return 0;
	173	}
	174
	175	int mv88e6xxx_port_hwtstamp_set(struct dsa_switch *ds, int port,
	176	struct ifreq *ifr)
	177	{
	178	struct mv88e6xxx_chip *chip = ds->priv;
	179	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
	180	struct hwtstamp_config config;
	181	int err;
	182
	183	if (!chip->info->ptp_support)
	184	return -EOPNOTSUPP;
	185
c6fe0ad2 BS	186	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
	187	return -EFAULT;
	188
	189	err = mv88e6xxx_set_hwtstamp_config(chip, port, &config);
	190	if (err)
	191	return err;
	192
	193	/* Save the chosen configuration to be returned later. */
	194	memcpy(&ps->tstamp_config, &config, sizeof(config));
	195
	196	return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
	197	-EFAULT : 0;
	198	}
	199
	200	int mv88e6xxx_port_hwtstamp_get(struct dsa_switch *ds, int port,
	201	struct ifreq *ifr)
	202	{
	203	struct mv88e6xxx_chip *chip = ds->priv;
	204	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
	205	struct hwtstamp_config *config = &ps->tstamp_config;
	206
	207	if (!chip->info->ptp_support)
	208	return -EOPNOTSUPP;
	209
c6fe0ad2 BS	210	return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ?
	211	-EFAULT : 0;
	212	}
	213
	214	/* Get the start of the PTP header in this skb */
	215	static u8 parse_ptp_header(struct sk_buff skb, unsigned int type)
	216	{
	217	u8 *data = skb_mac_header(skb);
	218	unsigned int offset = 0;
	219
	220	if (type & PTP_CLASS_VLAN)
	221	offset += VLAN_HLEN;
	222
	223	switch (type & PTP_CLASS_PMASK) {
	224	case PTP_CLASS_IPV4:
	225	offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
	226	break;
	227	case PTP_CLASS_IPV6:
	228	offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
	229	break;
	230	case PTP_CLASS_L2:
	231	offset += ETH_HLEN;
	232	break;
	233	default:
	234	return NULL;
	235	}
	236
	237	/* Ensure that the entire header is present in this packet. */
	238	if (skb->len + ETH_HLEN < offset + 34)
	239	return NULL;
	240
	241	return data + offset;
	242	}
	243
	244	/* Returns a pointer to the PTP header if the caller should time stamp,
	245	* or NULL if the caller should not.
	246	*/
	247	static u8 mv88e6xxx_should_tstamp(struct mv88e6xxx_chip chip, int port,
	248	struct sk_buff *skb, unsigned int type)
	249	{
	250	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
	251	u8 *hdr;
	252
	253	if (!chip->info->ptp_support)
	254	return NULL;
	255
c6fe0ad2 BS	256	hdr = parse_ptp_header(skb, type);
	257	if (!hdr)
	258	return NULL;
	259
	260	if (!test_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state))
	261	return NULL;
	262
	263	return hdr;
	264	}
	265
	266	static int mv88e6xxx_ts_valid(u16 status)
	267	{
	268	if (!(status & MV88E6XXX_PTP_TS_VALID))
	269	return 0;
	270	if (status & MV88E6XXX_PTP_TS_STATUS_MASK)
	271	return 0;
	272	return 1;
	273	}
	274
	275	static int seq_match(struct sk_buff *skb, u16 ts_seqid)
	276	{
	277	unsigned int type = SKB_PTP_TYPE(skb);
	278	u8 *hdr = parse_ptp_header(skb, type);
	279	__be16 *seqid;
	280
	281	seqid = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID);
	282
	283	return ts_seqid == ntohs(*seqid);
	284	}
	285
	286	static void mv88e6xxx_get_rxts(struct mv88e6xxx_chip *chip,
	287	struct mv88e6xxx_port_hwtstamp *ps,
	288	struct sk_buff *skb, u16 reg,
	289	struct sk_buff_head *rxq)
	290	{
b2d12101	291	u16 buf[4] = { 0 }, status, seq_id;
c6fe0ad2	292	struct skb_shared_hwtstamps *shwt;
22904823 RC	293	struct sk_buff_head received;
	294	u64 ns, timelo, timehi;
	295	unsigned long flags;
c6fe0ad2 BS	296	int err;
c6fe0ad2 BS	297
22904823 RC	298	/* The latched timestamp belongs to one of the received frames. */
	299	__skb_queue_head_init(&received);
	300	spin_lock_irqsave(&rxq->lock, flags);
	301	skb_queue_splice_tail_init(rxq, &received);
	302	spin_unlock_irqrestore(&rxq->lock, flags);
	303
c6fe0ad2 BS	304	mutex_lock(&chip->reg_lock);
	305	err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
	306	reg, buf, ARRAY_SIZE(buf));
	307	mutex_unlock(&chip->reg_lock);
	308	if (err)
	309	pr_err("failed to get the receive time stamp\n");
	310
	311	status = buf[0];
	312	timelo = buf[1];
	313	timehi = buf[2];
	314	seq_id = buf[3];
	315
	316	if (status & MV88E6XXX_PTP_TS_VALID) {
	317	mutex_lock(&chip->reg_lock);
	318	err = mv88e6xxx_port_ptp_write(chip, ps->port_id, reg, 0);
	319	mutex_unlock(&chip->reg_lock);
	320	if (err)
	321	pr_err("failed to clear the receive status\n");
	322	}
	323	/* Since the device can only handle one time stamp at a time,
	324	* we purge any extra frames from the queue.
	325	*/
22904823	326	for ( ; skb; skb = __skb_dequeue(&received)) {
c6fe0ad2	327	if (mv88e6xxx_ts_valid(status) && seq_match(skb, seq_id)) {
b2d12101	328	ns = timehi << 16 \| timelo;
c6fe0ad2 BS	329
	330	mutex_lock(&chip->reg_lock);
	331	ns = timecounter_cyc2time(&chip->tstamp_tc, ns);
	332	mutex_unlock(&chip->reg_lock);
	333	shwt = skb_hwtstamps(skb);
	334	memset(shwt, 0, sizeof(*shwt));
	335	shwt->hwtstamp = ns_to_ktime(ns);
	336	status &= ~MV88E6XXX_PTP_TS_VALID;
	337	}
	338	netif_rx_ni(skb);
	339	}
	340	}
	341
	342	static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip,
	343	struct mv88e6xxx_port_hwtstamp *ps)
	344	{
ffc705de	345	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
c6fe0ad2 BS	346	struct sk_buff *skb;
	347
	348	skb = skb_dequeue(&ps->rx_queue);
	349
	350	if (skb)
ffc705de	351	mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr0_sts_reg,
c6fe0ad2 BS	352	&ps->rx_queue);
	353
	354	skb = skb_dequeue(&ps->rx_queue2);
	355	if (skb)
ffc705de	356	mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr1_sts_reg,
c6fe0ad2 BS	357	&ps->rx_queue2);
	358	}
	359
	360	static int is_pdelay_resp(u8 *msgtype)
	361	{
	362	return (*msgtype & 0xf) == 3;
	363	}
	364
	365	bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port,
	366	struct sk_buff *skb, unsigned int type)
	367	{
	368	struct mv88e6xxx_port_hwtstamp *ps;
	369	struct mv88e6xxx_chip *chip;
	370	u8 *hdr;
	371
	372	chip = ds->priv;
	373	ps = &chip->port_hwtstamp[port];
	374
	375	if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
	376	return false;
	377
	378	hdr = mv88e6xxx_should_tstamp(chip, port, skb, type);
	379	if (!hdr)
	380	return false;
	381
	382	SKB_PTP_TYPE(skb) = type;
	383
	384	if (is_pdelay_resp(hdr))
	385	skb_queue_tail(&ps->rx_queue2, skb);
	386	else
	387	skb_queue_tail(&ps->rx_queue, skb);
	388
	389	ptp_schedule_worker(chip->ptp_clock, 0);
	390
	391	return true;
	392	}
	393
	394	static int mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip *chip,
	395	struct mv88e6xxx_port_hwtstamp *ps)
	396	{
ffc705de	397	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
c6fe0ad2 BS	398	struct skb_shared_hwtstamps shhwtstamps;
	399	u16 departure_block[4], status;
	400	struct sk_buff *tmp_skb;
	401	u32 time_raw;
	402	int err;
	403	u64 ns;
	404
	405	if (!ps->tx_skb)
	406	return 0;
	407
	408	mutex_lock(&chip->reg_lock);
	409	err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
ffc705de	410	ptp_ops->dep_sts_reg,
c6fe0ad2 BS	411	departure_block,
	412	ARRAY_SIZE(departure_block));
	413	mutex_unlock(&chip->reg_lock);
	414
	415	if (err)
	416	goto free_and_clear_skb;
	417
	418	if (!(departure_block[0] & MV88E6XXX_PTP_TS_VALID)) {
	419	if (time_is_before_jiffies(ps->tx_tstamp_start +
	420	TX_TSTAMP_TIMEOUT)) {
	421	dev_warn(chip->dev, "p%d: clearing tx timestamp hang\n",
	422	ps->port_id);
	423	goto free_and_clear_skb;
	424	}
	425	/* The timestamp should be available quickly, while getting it
	426	* is high priority and time bounded to only 10ms. A poll is
	427	* warranted so restart the work.
	428	*/
	429	return 1;
	430	}
	431
	432	/* We have the timestamp; go ahead and clear valid now */
	433	mutex_lock(&chip->reg_lock);
ffc705de	434	mv88e6xxx_port_ptp_write(chip, ps->port_id, ptp_ops->dep_sts_reg, 0);
c6fe0ad2 BS	435	mutex_unlock(&chip->reg_lock);
	436
	437	status = departure_block[0] & MV88E6XXX_PTP_TS_STATUS_MASK;
	438	if (status != MV88E6XXX_PTP_TS_STATUS_NORMAL) {
	439	dev_warn(chip->dev, "p%d: tx timestamp overrun\n", ps->port_id);
	440	goto free_and_clear_skb;
	441	}
	442
	443	if (departure_block[3] != ps->tx_seq_id) {
	444	dev_warn(chip->dev, "p%d: unexpected seq. id\n", ps->port_id);
	445	goto free_and_clear_skb;
	446	}
	447
	448	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
	449	time_raw = ((u32)departure_block[2] << 16) \| departure_block[1];
	450	mutex_lock(&chip->reg_lock);
	451	ns = timecounter_cyc2time(&chip->tstamp_tc, time_raw);
	452	mutex_unlock(&chip->reg_lock);
	453	shhwtstamps.hwtstamp = ns_to_ktime(ns);
	454
	455	dev_dbg(chip->dev,
	456	"p%d: txtstamp %llx status 0x%04x skb ID 0x%04x hw ID 0x%04x\n",
	457	ps->port_id, ktime_to_ns(shhwtstamps.hwtstamp),
	458	departure_block[0], ps->tx_seq_id, departure_block[3]);
	459
	460	/* skb_complete_tx_timestamp() will free up the client to make
	461	* another timestamp-able transmit. We have to be ready for it
	462	* -- by clearing the ps->tx_skb "flag" -- beforehand.
	463	*/
	464
	465	tmp_skb = ps->tx_skb;
	466	ps->tx_skb = NULL;
	467	clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
	468	skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);
	469
	470	return 0;
	471
	472	free_and_clear_skb:
	473	dev_kfree_skb_any(ps->tx_skb);
	474	ps->tx_skb = NULL;
	475	clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
	476
	477	return 0;
	478	}
	479
	480	long mv88e6xxx_hwtstamp_work(struct ptp_clock_info *ptp)
	481	{
	482	struct mv88e6xxx_chip *chip = ptp_to_chip(ptp);
	483	struct dsa_switch *ds = chip->ds;
	484	struct mv88e6xxx_port_hwtstamp *ps;
	485	int i, restart = 0;
	486
	487	for (i = 0; i < ds->num_ports; i++) {
	488	if (!dsa_is_user_port(ds, i))
	489	continue;
	490
	491	ps = &chip->port_hwtstamp[i];
	492	if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state))
	493	restart \|= mv88e6xxx_txtstamp_work(chip, ps);
	494
	495	mv88e6xxx_rxtstamp_work(chip, ps);
	496	}
	497
	498	return restart ? 1 : -1;
499	}
500
501	bool mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port,
502	struct sk_buff *clone, unsigned int type)
503	{
504	struct mv88e6xxx_chip *chip = ds->priv;
505	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
506	__be16 *seq_ptr;
507	u8 *hdr;
508
509	if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP))
510	return false;
511
512	hdr = mv88e6xxx_should_tstamp(chip, port, clone, type);
513	if (!hdr)
514	return false;
515
516	seq_ptr = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID);
517
518	if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS,
519	&ps->state))
520	return false;
521
522	ps->tx_skb = clone;
523	ps->tx_tstamp_start = jiffies;
524	ps->tx_seq_id = be16_to_cpup(seq_ptr);
525
526	ptp_schedule_worker(chip->ptp_clock, 0);
527	return true;
528	}
529
e2294a8b AL	530	int mv88e6165_global_disable(struct mv88e6xxx_chip *chip)
	531	{
	532	u16 val;
	533	int err;
	534
	535	err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val);
	536	if (err)
	537	return err;
	538	val \|= MV88E6165_PTP_CFG_DISABLE_PTP;
	539
	540	return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val);
	541	}
	542
	543	int mv88e6165_global_enable(struct mv88e6xxx_chip *chip)
	544	{
	545	u16 val;
	546	int err;
	547
	548	err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val);
	549	if (err)
	550	return err;
	551
	552	val &= ~(MV88E6165_PTP_CFG_DISABLE_PTP \| MV88E6165_PTP_CFG_TSPEC_MASK);
	553
	554	return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val);
	555	}
	556
ffc705de AL	557	int mv88e6352_hwtstamp_port_disable(struct mv88e6xxx_chip *chip, int port)
	558	{
	559	return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
	560	MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP);
	561	}
	562
	563	int mv88e6352_hwtstamp_port_enable(struct mv88e6xxx_chip *chip, int port)
	564	{
	565	return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
	566	MV88E6XXX_PORT_PTP_CFG0_DISABLE_TSPEC_MATCH);
	567	}
	568
c6fe0ad2 BS	569	static int mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip *chip, int port)
c6fe0ad2 BS	570	{
ffc705de	571	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
c6fe0ad2 BS	572	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
	573
	574	ps->port_id = port;
	575
	576	skb_queue_head_init(&ps->rx_queue);
	577	skb_queue_head_init(&ps->rx_queue2);
	578
ffc705de AL	579	if (ptp_ops->port_disable)
	580	return ptp_ops->port_disable(chip, port);
	581
	582	return 0;
c6fe0ad2 BS	583	}
	584
	585	int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip)
	586	{
e2294a8b	587	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
c6fe0ad2 BS	588	int err;
	589	int i;
	590
	591	/* Disable timestamping on all ports. */
	592	for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
	593	err = mv88e6xxx_hwtstamp_port_setup(chip, i);
	594	if (err)
	595	return err;
	596	}
	597
e2294a8b AL	598	/* Disable PTP globally */
	599	if (ptp_ops->global_disable) {
	600	err = ptp_ops->global_disable(chip);
	601	if (err)
	602	return err;
	603	}
	604
2dbed245 AL	605	/* Set the ethertype of L2 PTP messages */
	606	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_GC_ETYPE, ETH_P_1588);
	607	if (err)
	608	return err;
	609
c6fe0ad2 BS	610	/* MV88E6XXX_PTP_MSG_TYPE is a mask of PTP message types to
	611	* timestamp. This affects all ports that have timestamping enabled,
	612	* but the timestamp config is per-port; thus we configure all events
	613	* here and only support the HWTSTAMP_FILTER_*_EVENT filter types.
	614	*/
	615	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_MSGTYPE,
	616	MV88E6XXX_PTP_MSGTYPE_ALL_EVENT);
	617	if (err)
	618	return err;
	619
	620	/* Use ARRIVAL1 for peer delay response messages. */
	621	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_TS_ARRIVAL_PTR,
	622	MV88E6XXX_PTP_MSGTYPE_PDLAY_RES);
	623	if (err)
	624	return err;
	625
a2e47134 BS	626	/* 88E6341 devices default to timestamping at the PHY, but this has
	627	* a hardware issue that results in unreliable timestamps. Force
	628	* these devices to timestamp at the MAC.
	629	*/
	630	if (chip->info->family == MV88E6XXX_FAMILY_6341) {
	631	u16 val = MV88E6341_PTP_CFG_UPDATE \|
	632	MV88E6341_PTP_CFG_MODE_IDX \|
	633	MV88E6341_PTP_CFG_MODE_TS_AT_MAC;
	634	err = mv88e6xxx_ptp_write(chip, MV88E6341_PTP_CFG, val);
	635	if (err)
	636	return err;
	637	}
	638
c6fe0ad2 BS	639	return 0;
	640	}
	641
	642	void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip)
	643	{
	644	}