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

/*
 * 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>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 */

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

/* 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(20)

int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port,
			  struct ethtool_ts_info *info)
{
	struct mv88e6xxx_chip *chip = ds->priv;

	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 =
		(1 << HWTSTAMP_FILTER_NONE) |
		(1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) |
		(1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC) |
		(1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ) |
		(1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
		(1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) |
		(1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) |
		(1 << HWTSTAMP_FILTER_PTP_V2_EVENT) |
		(1 << HWTSTAMP_FILTER_PTP_V2_SYNC) |
		(1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ);

	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.
	 */
	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) {
		if (ptp_ops->port_enable)
			ptp_ops->port_enable(chip, port);
	} else {
		if (ptp_ops->port_disable)
			ptp_ops->port_disable(chip, port);
	}
	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 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)
{
	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;
	}

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