WSL2-Linux-Kernel/drivers/net/dsa/sja1105/sja1105_ptp.c

// SPDX-License-Identifier: GPL-2.0
/* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com>
 */
#include <linux/spi/spi.h>
#include "sja1105.h"

/* The adjfine API clamps ppb between [-32,768,000, 32,768,000], and
 * therefore scaled_ppm between [-2,147,483,648, 2,147,483,647].
 * Set the maximum supported ppb to a round value smaller than the maximum.
 *
 * Percentually speaking, this is a +/- 0.032x adjustment of the
 * free-running counter (0.968x to 1.032x).
 */
#define SJA1105_MAX_ADJ_PPB		32000000
#define SJA1105_SIZE_PTP_CMD		4

/*            This range is actually +/- SJA1105_MAX_ADJ_PPB
 *            divided by 1000 (ppb -> ppm) and with a 16-bit
 *            "fractional" part (actually fixed point).
 *                                    |
 *                                    v
 * Convert scaled_ppm from the +/- ((10^6) << 16) range
 * into the +/- (1 << 31) range.
 *
 * This forgoes a "ppb" numeric representation (up to NSEC_PER_SEC)
 * and defines the scaling factor between scaled_ppm and the actual
 * frequency adjustments of the PHC.
 *
 *   ptpclkrate = scaled_ppm * 2^31 / (10^6 * 2^16)
 *   simplifies to
 *   ptpclkrate = scaled_ppm * 2^9 / 5^6
 */
#define SJA1105_CC_MULT_NUM		(1 << 9)
#define SJA1105_CC_MULT_DEM		15625
#define SJA1105_CC_MULT			0x80000000

enum sja1105_ptp_clk_mode {
	PTP_ADD_MODE = 1,
	PTP_SET_MODE = 0,
};

#define ptp_caps_to_data(d) \
		container_of((d), struct sja1105_ptp_data, caps)
#define ptp_data_to_sja1105(d) \
		container_of((d), struct sja1105_private, ptp_data)

static int sja1105_init_avb_params(struct sja1105_private *priv,
				   bool on)
{
	struct sja1105_avb_params_entry *avb;
	struct sja1105_table *table;

	table = &priv->static_config.tables[BLK_IDX_AVB_PARAMS];

	/* Discard previous AVB Parameters Table */
	if (table->entry_count) {
		kfree(table->entries);
		table->entry_count = 0;
	}

	/* Configure the reception of meta frames only if requested */
	if (!on)
		return 0;

	table->entries = kcalloc(SJA1105_MAX_AVB_PARAMS_COUNT,
				 table->ops->unpacked_entry_size, GFP_KERNEL);
	if (!table->entries)
		return -ENOMEM;

	table->entry_count = SJA1105_MAX_AVB_PARAMS_COUNT;

	avb = table->entries;

	avb->destmeta = SJA1105_META_DMAC;
	avb->srcmeta  = SJA1105_META_SMAC;

	return 0;
}

/* Must be called only with priv->tagger_data.state bit
 * SJA1105_HWTS_RX_EN cleared
 */
static int sja1105_change_rxtstamping(struct sja1105_private *priv,
				      bool on)
{
	struct sja1105_general_params_entry *general_params;
	struct sja1105_table *table;
	int rc;

	table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS];
	general_params = table->entries;
	general_params->send_meta1 = on;
	general_params->send_meta0 = on;

	rc = sja1105_init_avb_params(priv, on);
	if (rc < 0)
		return rc;

	/* Initialize the meta state machine to a known state */
	if (priv->tagger_data.stampable_skb) {
		kfree_skb(priv->tagger_data.stampable_skb);
		priv->tagger_data.stampable_skb = NULL;
	}

	return sja1105_static_config_reload(priv, SJA1105_RX_HWTSTAMPING);
}

int sja1105_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr)
{
	struct sja1105_private *priv = ds->priv;
	struct hwtstamp_config config;
	bool rx_on;
	int rc;

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

	switch (config.tx_type) {
	case HWTSTAMP_TX_OFF:
		priv->ports[port].hwts_tx_en = false;
		break;
	case HWTSTAMP_TX_ON:
		priv->ports[port].hwts_tx_en = true;
		break;
	default:
		return -ERANGE;
	}

	switch (config.rx_filter) {
	case HWTSTAMP_FILTER_NONE:
		rx_on = false;
		break;
	default:
		rx_on = true;
		break;
	}

	if (rx_on != test_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state)) {
		clear_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state);

		rc = sja1105_change_rxtstamping(priv, rx_on);
		if (rc < 0) {
			dev_err(ds->dev,
				"Failed to change RX timestamping: %d\n", rc);
			return rc;
		}
		if (rx_on)
			set_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state);
	}

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

int sja1105_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr)
{
	struct sja1105_private *priv = ds->priv;
	struct hwtstamp_config config;

	config.flags = 0;
	if (priv->ports[port].hwts_tx_en)
		config.tx_type = HWTSTAMP_TX_ON;
	else
		config.tx_type = HWTSTAMP_TX_OFF;
	if (test_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state))
		config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
	else
		config.rx_filter = HWTSTAMP_FILTER_NONE;

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

int sja1105_get_ts_info(struct dsa_switch *ds, int port,
			struct ethtool_ts_info *info)
{
	struct sja1105_private *priv = ds->priv;
	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;

	/* Called during cleanup */
	if (!ptp_data->clock)
		return -ENODEV;

	info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
				SOF_TIMESTAMPING_RX_HARDWARE |
				SOF_TIMESTAMPING_RAW_HARDWARE;
	info->tx_types = (1 << HWTSTAMP_TX_OFF) |
			 (1 << HWTSTAMP_TX_ON);
	info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
			   (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT);
	info->phc_index = ptp_clock_index(ptp_data->clock);
	return 0;
}

void sja1105et_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
			       enum packing_op op)
{
	const int size = SJA1105_SIZE_PTP_CMD;
	/* No need to keep this as part of the structure */
	u64 valid = 1;

	sja1105_packing(buf, &valid,           31, 31, size, op);
	sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
	sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
	sja1105_packing(buf, &cmd->resptp,      2,  2, size, op);
	sja1105_packing(buf, &cmd->corrclk4ts,  1,  1, size, op);
	sja1105_packing(buf, &cmd->ptpclkadd,   0,  0, size, op);
}

void sja1105pqrs_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
				 enum packing_op op)
{
	const int size = SJA1105_SIZE_PTP_CMD;
	/* No need to keep this as part of the structure */
	u64 valid = 1;

	sja1105_packing(buf, &valid,           31, 31, size, op);
	sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
	sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
	sja1105_packing(buf, &cmd->resptp,      3,  3, size, op);
	sja1105_packing(buf, &cmd->corrclk4ts,  2,  2, size, op);
	sja1105_packing(buf, &cmd->ptpclkadd,   0,  0, size, op);
}

int sja1105_ptp_commit(struct dsa_switch *ds, struct sja1105_ptp_cmd *cmd,
		       sja1105_spi_rw_mode_t rw)
{
	const struct sja1105_private *priv = ds->priv;
	const struct sja1105_regs *regs = priv->info->regs;
	u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
	int rc;

	if (rw == SPI_WRITE)
		priv->info->ptp_cmd_packing(buf, cmd, PACK);

	rc = sja1105_xfer_buf(priv, SPI_WRITE, regs->ptp_control, buf,
			      SJA1105_SIZE_PTP_CMD);

	if (rw == SPI_READ)
		priv->info->ptp_cmd_packing(buf, cmd, UNPACK);

	return rc;
}

/* The switch returns partial timestamps (24 bits for SJA1105 E/T, which wrap
 * around in 0.135 seconds, and 32 bits for P/Q/R/S, wrapping around in 34.35
 * seconds).
 *
 * This receives the RX or TX MAC timestamps, provided by hardware as
 * the lower bits of the cycle counter, sampled at the time the timestamp was
 * collected.
 *
 * To reconstruct into a full 64-bit-wide timestamp, the cycle counter is
 * read and the high-order bits are filled in.
 *
 * Must be called within one wraparound period of the partial timestamp since
 * it was generated by the MAC.
 */
static u64 sja1105_tstamp_reconstruct(struct dsa_switch *ds, u64 now,
				      u64 ts_partial)
{
	struct sja1105_private *priv = ds->priv;
	u64 partial_tstamp_mask = CYCLECOUNTER_MASK(priv->info->ptp_ts_bits);
	u64 ts_reconstructed;

	ts_reconstructed = (now & ~partial_tstamp_mask) | ts_partial;

	/* Check lower bits of current cycle counter against the timestamp.
	 * If the current cycle counter is lower than the partial timestamp,
	 * then wraparound surely occurred and must be accounted for.
	 */
	if ((now & partial_tstamp_mask) <= ts_partial)
		ts_reconstructed -= (partial_tstamp_mask + 1);

	return ts_reconstructed;
}

/* Reads the SPI interface for an egress timestamp generated by the switch
 * for frames sent using management routes.
 *
 * SJA1105 E/T layout of the 4-byte SPI payload:
 *
 * 31    23    15    7     0
 * |     |     |     |     |
 * +-----+-----+-----+     ^
 *          ^              |
 *          |              |
 *  24-bit timestamp   Update bit
 *
 *
 * SJA1105 P/Q/R/S layout of the 8-byte SPI payload:
 *
 * 31    23    15    7     0     63    55    47    39    32
 * |     |     |     |     |     |     |     |     |     |
 *                         ^     +-----+-----+-----+-----+
 *                         |                 ^
 *                         |                 |
 *                    Update bit    32-bit timestamp
 *
 * Notice that the update bit is in the same place.
 * To have common code for E/T and P/Q/R/S for reading the timestamp,
 * we need to juggle with the offset and the bit indices.
 */
static int sja1105_ptpegr_ts_poll(struct dsa_switch *ds, int port, u64 *ts)
{
	struct sja1105_private *priv = ds->priv;
	const struct sja1105_regs *regs = priv->info->regs;
	int tstamp_bit_start, tstamp_bit_end;
	int timeout = 10;
	u8 packed_buf[8];
	u64 update;
	int rc;

	do {
		rc = sja1105_xfer_buf(priv, SPI_READ, regs->ptpegr_ts[port],
				      packed_buf, priv->info->ptpegr_ts_bytes);
		if (rc < 0)
			return rc;

		sja1105_unpack(packed_buf, &update, 0, 0,
			       priv->info->ptpegr_ts_bytes);
		if (update)
			break;

		usleep_range(10, 50);
	} while (--timeout);

	if (!timeout)
		return -ETIMEDOUT;

	/* Point the end bit to the second 32-bit word on P/Q/R/S,
	 * no-op on E/T.
	 */
	tstamp_bit_end = (priv->info->ptpegr_ts_bytes - 4) * 8;
	/* Shift the 24-bit timestamp on E/T to be collected from 31:8.
	 * No-op on P/Q/R/S.
	 */
	tstamp_bit_end += 32 - priv->info->ptp_ts_bits;
	tstamp_bit_start = tstamp_bit_end + priv->info->ptp_ts_bits - 1;

	*ts = 0;

	sja1105_unpack(packed_buf, ts, tstamp_bit_start, tstamp_bit_end,
		       priv->info->ptpegr_ts_bytes);

	return 0;
}

/* Caller must hold ptp_data->lock */
static int sja1105_ptpclkval_read(struct sja1105_private *priv, u64 *ticks,
				  struct ptp_system_timestamp *ptp_sts)
{
	const struct sja1105_regs *regs = priv->info->regs;

	return sja1105_xfer_u64(priv, SPI_READ, regs->ptpclkval, ticks,
				ptp_sts);
}

/* Caller must hold ptp_data->lock */
static int sja1105_ptpclkval_write(struct sja1105_private *priv, u64 ticks,
				   struct ptp_system_timestamp *ptp_sts)
{
	const struct sja1105_regs *regs = priv->info->regs;

	return sja1105_xfer_u64(priv, SPI_WRITE, regs->ptpclkval, &ticks,
				ptp_sts);
}

#define rxtstamp_to_tagger(d) \
	container_of((d), struct sja1105_tagger_data, rxtstamp_work)
#define tagger_to_sja1105(d) \
	container_of((d), struct sja1105_private, tagger_data)

static void sja1105_rxtstamp_work(struct work_struct *work)
{
	struct sja1105_tagger_data *tagger_data = rxtstamp_to_tagger(work);
	struct sja1105_private *priv = tagger_to_sja1105(tagger_data);
	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
	struct dsa_switch *ds = priv->ds;
	struct sk_buff *skb;

	mutex_lock(&ptp_data->lock);

	while ((skb = skb_dequeue(&tagger_data->skb_rxtstamp_queue)) != NULL) {
		struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
		u64 ticks, ts;
		int rc;

		rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
		if (rc < 0) {
			dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
			kfree_skb(skb);
			continue;
		}

		*shwt = (struct skb_shared_hwtstamps) {0};

		ts = SJA1105_SKB_CB(skb)->meta_tstamp;
		ts = sja1105_tstamp_reconstruct(ds, ticks, ts);

		shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
		netif_rx_ni(skb);
	}

	mutex_unlock(&ptp_data->lock);
}

/* Called from dsa_skb_defer_rx_timestamp */
bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port,
			   struct sk_buff *skb, unsigned int type)
{
	struct sja1105_private *priv = ds->priv;
	struct sja1105_tagger_data *tagger_data = &priv->tagger_data;

	if (!test_bit(SJA1105_HWTS_RX_EN, &tagger_data->state))
		return false;

	/* We need to read the full PTP clock to reconstruct the Rx
	 * timestamp. For that we need a sleepable context.
	 */
	skb_queue_tail(&tagger_data->skb_rxtstamp_queue, skb);
	schedule_work(&tagger_data->rxtstamp_work);
	return true;
}

/* Called from dsa_skb_tx_timestamp. This callback is just to make DSA clone
 * the skb and have it available in DSA_SKB_CB in the .port_deferred_xmit
 * callback, where we will timestamp it synchronously.
 */
bool sja1105_port_txtstamp(struct dsa_switch *ds, int port,
			   struct sk_buff *skb, unsigned int type)
{
	struct sja1105_private *priv = ds->priv;
	struct sja1105_port *sp = &priv->ports[port];

	if (!sp->hwts_tx_en)
		return false;

	return true;
}

static int sja1105_ptp_reset(struct dsa_switch *ds)
{
	struct sja1105_private *priv = ds->priv;
	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
	struct sja1105_ptp_cmd cmd = ptp_data->cmd;
	int rc;

	mutex_lock(&ptp_data->lock);

	cmd.resptp = 1;

	dev_dbg(ds->dev, "Resetting PTP clock\n");
	rc = sja1105_ptp_commit(ds, &cmd, SPI_WRITE);

	sja1105_tas_clockstep(priv->ds);

	mutex_unlock(&ptp_data->lock);

	return rc;
}

/* Caller must hold ptp_data->lock */
int __sja1105_ptp_gettimex(struct dsa_switch *ds, u64 *ns,
			   struct ptp_system_timestamp *ptp_sts)
{
	struct sja1105_private *priv = ds->priv;
	u64 ticks;
	int rc;

	rc = sja1105_ptpclkval_read(priv, &ticks, ptp_sts);
	if (rc < 0) {
		dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
		return rc;
	}

	*ns = sja1105_ticks_to_ns(ticks);

	return 0;
}

static int sja1105_ptp_gettimex(struct ptp_clock_info *ptp,
				struct timespec64 *ts,
				struct ptp_system_timestamp *ptp_sts)
{
	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
	u64 now = 0;
	int rc;

	mutex_lock(&ptp_data->lock);

	rc = __sja1105_ptp_gettimex(priv->ds, &now, ptp_sts);
	*ts = ns_to_timespec64(now);

	mutex_unlock(&ptp_data->lock);

	return rc;
}

/* Caller must hold ptp_data->lock */
static int sja1105_ptp_mode_set(struct sja1105_private *priv,
				enum sja1105_ptp_clk_mode mode)
{
	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;

	if (ptp_data->cmd.ptpclkadd == mode)
		return 0;

	ptp_data->cmd.ptpclkadd = mode;

	return sja1105_ptp_commit(priv->ds, &ptp_data->cmd, SPI_WRITE);
}

/* Write to PTPCLKVAL while PTPCLKADD is 0 */
int __sja1105_ptp_settime(struct dsa_switch *ds, u64 ns,
			  struct ptp_system_timestamp *ptp_sts)
{
	struct sja1105_private *priv = ds->priv;
	u64 ticks = ns_to_sja1105_ticks(ns);
	int rc;

	rc = sja1105_ptp_mode_set(priv, PTP_SET_MODE);
	if (rc < 0) {
		dev_err(priv->ds->dev, "Failed to put PTPCLK in set mode\n");
		return rc;
	}

	rc = sja1105_ptpclkval_write(priv, ticks, ptp_sts);

	sja1105_tas_clockstep(priv->ds);

	return rc;
}

static int sja1105_ptp_settime(struct ptp_clock_info *ptp,
			       const struct timespec64 *ts)
{
	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
	u64 ns = timespec64_to_ns(ts);
	int rc;

	mutex_lock(&ptp_data->lock);

	rc = __sja1105_ptp_settime(priv->ds, ns, NULL);

	mutex_unlock(&ptp_data->lock);

	return rc;
}

static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
{
	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
	const struct sja1105_regs *regs = priv->info->regs;
	u32 clkrate32;
	s64 clkrate;
	int rc;

	clkrate = (s64)scaled_ppm * SJA1105_CC_MULT_NUM;
	clkrate = div_s64(clkrate, SJA1105_CC_MULT_DEM);

	/* Take a +/- value and re-center it around 2^31. */
	clkrate = SJA1105_CC_MULT + clkrate;
	WARN_ON(abs(clkrate) >= GENMASK_ULL(31, 0));
	clkrate32 = clkrate;

	mutex_lock(&ptp_data->lock);

	rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptpclkrate, &clkrate32,
			      NULL);

	sja1105_tas_adjfreq(priv->ds);

	mutex_unlock(&ptp_data->lock);

	return rc;
}

/* Write to PTPCLKVAL while PTPCLKADD is 1 */
int __sja1105_ptp_adjtime(struct dsa_switch *ds, s64 delta)
{
	struct sja1105_private *priv = ds->priv;
	s64 ticks = ns_to_sja1105_ticks(delta);
	int rc;

	rc = sja1105_ptp_mode_set(priv, PTP_ADD_MODE);
	if (rc < 0) {
		dev_err(priv->ds->dev, "Failed to put PTPCLK in add mode\n");
		return rc;
	}

	rc = sja1105_ptpclkval_write(priv, ticks, NULL);

	sja1105_tas_clockstep(priv->ds);

	return rc;
}

static int sja1105_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
{
	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
	int rc;

	mutex_lock(&ptp_data->lock);

	rc = __sja1105_ptp_adjtime(priv->ds, delta);

	mutex_unlock(&ptp_data->lock);

	return rc;
}

int sja1105_ptp_clock_register(struct dsa_switch *ds)
{
	struct sja1105_private *priv = ds->priv;
	struct sja1105_tagger_data *tagger_data = &priv->tagger_data;
	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;

	ptp_data->caps = (struct ptp_clock_info) {
		.owner		= THIS_MODULE,
		.name		= "SJA1105 PHC",
		.adjfine	= sja1105_ptp_adjfine,
		.adjtime	= sja1105_ptp_adjtime,
		.gettimex64	= sja1105_ptp_gettimex,
		.settime64	= sja1105_ptp_settime,
		.max_adj	= SJA1105_MAX_ADJ_PPB,
	};

	skb_queue_head_init(&tagger_data->skb_rxtstamp_queue);
	INIT_WORK(&tagger_data->rxtstamp_work, sja1105_rxtstamp_work);
	spin_lock_init(&tagger_data->meta_lock);

	ptp_data->clock = ptp_clock_register(&ptp_data->caps, ds->dev);
	if (IS_ERR_OR_NULL(ptp_data->clock))
		return PTR_ERR(ptp_data->clock);

	ptp_data->cmd.corrclk4ts = true;
	ptp_data->cmd.ptpclkadd = PTP_SET_MODE;

	return sja1105_ptp_reset(ds);
}

void sja1105_ptp_clock_unregister(struct dsa_switch *ds)
{
	struct sja1105_private *priv = ds->priv;
	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;

	if (IS_ERR_OR_NULL(ptp_data->clock))
		return;

	cancel_work_sync(&priv->tagger_data.rxtstamp_work);
	skb_queue_purge(&priv->tagger_data.skb_rxtstamp_queue);
	ptp_clock_unregister(ptp_data->clock);
	ptp_data->clock = NULL;
}

void sja1105_ptp_txtstamp_skb(struct dsa_switch *ds, int slot,
			      struct sk_buff *skb)
{
	struct sja1105_private *priv = ds->priv;
	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
	struct skb_shared_hwtstamps shwt = {0};
	u64 ticks, ts;
	int rc;

	skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;

	mutex_lock(&ptp_data->lock);

	rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
	if (rc < 0) {
		dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
		kfree_skb(skb);
		goto out;
	}

	rc = sja1105_ptpegr_ts_poll(ds, slot, &ts);
	if (rc < 0) {
		dev_err(ds->dev, "timed out polling for tstamp\n");
		kfree_skb(skb);
		goto out;
	}

	ts = sja1105_tstamp_reconstruct(ds, ticks, ts);

	shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
	skb_complete_tx_timestamp(skb, &shwt);

out:
	mutex_unlock(&ptp_data->lock);
}