Commit 85a83a8f authored by David S. Miller's avatar David S. Miller
Browse files

Merge branch 'PTP-driver-refactoring-for-SJA1105-DSA' 



Vladimir Oltean says:

====================
PTP driver refactoring for SJA1105 DSA

This series creates a better separation between the driver core and the
PTP portion. Therefore, users who are not interested in PTP can get a
simpler and smaller driver by compiling it out.

This is in preparation for further patches: SPI transfer timestamping,
synchronizing the hardware clock (as opposed to keeping it
free-running), PPS input/output, etc.
====================

Acked-by: default avatarRichard Cochran <richardcochran@gmail.com>
Signed-off-by: default avatarDavid S. Miller <davem@davemloft.net>
parents a98d62c3 66427778

drivers/net/dsa/sja1105/sja1105.h

+4 −12
Original line number Diff line number Diff line
@@ -21,6 +21,7 @@ 
#define SJA1105_AGEING_TIME_MS(ms)	((ms) / 10)



#include "sja1105_tas.h"

#include "sja1105_ptp.h"



/* Keeps the different addresses between E/T and P/Q/R/S */

struct sja1105_regs {
@@ -71,7 +72,8 @@ struct sja1105_info { 
	const struct sja1105_dynamic_table_ops *dyn_ops;

	const struct sja1105_table_ops *static_ops;

	const struct sja1105_regs *regs;

	int (*ptp_cmd)(const void *ctx, const void *data);

	int (*ptp_cmd)(const struct dsa_switch *ds,

		       const struct sja1105_ptp_cmd *cmd);

	int (*reset_cmd)(const void *ctx, const void *data);

	int (*setup_rgmii_delay)(const void *ctx, int port);

	/* Prototypes from include/net/dsa.h */
@@ -91,26 +93,16 @@ struct sja1105_private { 
	struct spi_device *spidev;

	struct dsa_switch *ds;

	struct sja1105_port ports[SJA1105_NUM_PORTS];

	struct ptp_clock_info ptp_caps;

	struct ptp_clock *clock;

	/* The cycle counter translates the PTP timestamps (based on

	 * a free-running counter) into a software time domain.

	 */

	struct cyclecounter tstamp_cc;

	struct timecounter tstamp_tc;

	struct delayed_work refresh_work;

	/* Serializes all operations on the cycle counter */

	struct mutex ptp_lock;

	/* Serializes transmission of management frames so that

	 * the switch doesn't confuse them with one another.

	 */

	struct mutex mgmt_lock;

	struct sja1105_tagger_data tagger_data;

	struct sja1105_ptp_data ptp_data;

	struct sja1105_tas_data tas_data;

};



#include "sja1105_dynamic_config.h"

#include "sja1105_ptp.h"



struct sja1105_spi_message {

	u64 access;

drivers/net/dsa/sja1105/sja1105_main.c

+4 −230
Original line number Diff line number Diff line
@@ -506,39 +506,6 @@ static int sja1105_init_l2_policing(struct sja1105_private *priv) 
	return 0;

}



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;

}



static int sja1105_static_config_load(struct sja1105_private *priv,

				      struct sja1105_dt_port *ports)

{
@@ -577,9 +544,6 @@ static int sja1105_static_config_load(struct sja1105_private *priv, 
	if (rc < 0)

		return rc;

	rc = sja1105_init_general_params(priv);

	if (rc < 0)

		return rc;

	rc = sja1105_init_avb_params(priv, false);

	if (rc < 0)

		return rc;
@@ -1686,7 +1650,7 @@ static int sja1105_setup(struct dsa_switch *ds) 
		return rc;

	}



	rc = sja1105_ptp_clock_register(priv);

	rc = sja1105_ptp_clock_register(ds);

	if (rc < 0) {

		dev_err(ds->dev, "Failed to register PTP clock: %d\n", rc);

		return rc;
@@ -1728,9 +1692,7 @@ static void sja1105_teardown(struct dsa_switch *ds) 
	struct sja1105_private *priv = ds->priv;



	sja1105_tas_teardown(ds);

	cancel_work_sync(&priv->tagger_data.rxtstamp_work);

	skb_queue_purge(&priv->tagger_data.skb_rxtstamp_queue);

	sja1105_ptp_clock_unregister(priv);

	sja1105_ptp_clock_unregister(ds);

	sja1105_static_config_free(&priv->static_config);

}
@@ -1816,11 +1778,8 @@ static netdev_tx_t sja1105_port_deferred_xmit(struct dsa_switch *ds, int port, 
{

	struct sja1105_private *priv = ds->priv;

	struct sja1105_port *sp = &priv->ports[port];

	struct skb_shared_hwtstamps shwt = {0};

	int slot = sp->mgmt_slot;

	struct sk_buff *clone;

	u64 now, ts;

	int rc;



	/* The tragic fact about the switch having 4x2 slots for installing

	 * management routes is that all of them except one are actually
@@ -1846,27 +1805,8 @@ static netdev_tx_t sja1105_port_deferred_xmit(struct dsa_switch *ds, int port, 
	if (!clone)

		goto out;



	skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;



	mutex_lock(&priv->ptp_lock);

	sja1105_ptp_txtstamp_skb(ds, slot, clone);



	now = priv->tstamp_cc.read(&priv->tstamp_cc);



	rc = sja1105_ptpegr_ts_poll(priv, slot, &ts);

	if (rc < 0) {

		dev_err(ds->dev, "xmit: timed out polling for tstamp\n");

		kfree_skb(clone);

		goto out_unlock_ptp;

	}



	ts = sja1105_tstamp_reconstruct(priv, now, ts);

	ts = timecounter_cyc2time(&priv->tstamp_tc, ts);



	shwt.hwtstamp = ns_to_ktime(ts);

	skb_complete_tx_timestamp(clone, &shwt);



out_unlock_ptp:

	mutex_unlock(&priv->ptp_lock);

out:

	mutex_unlock(&priv->mgmt_lock);

	return NETDEV_TX_OK;
@@ -1896,170 +1836,6 @@ static int sja1105_set_ageing_time(struct dsa_switch *ds, 
	return sja1105_static_config_reload(priv);

}



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

}



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

}



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

}



#define to_tagger(d) \

	container_of((d), struct sja1105_tagger_data, rxtstamp_work)

#define to_sja1105(d) \

	container_of((d), struct sja1105_private, tagger_data)



static void sja1105_rxtstamp_work(struct work_struct *work)

{

	struct sja1105_tagger_data *data = to_tagger(work);

	struct sja1105_private *priv = to_sja1105(data);

	struct sk_buff *skb;

	u64 now;



	mutex_lock(&priv->ptp_lock);



	while ((skb = skb_dequeue(&data->skb_rxtstamp_queue)) != NULL) {

		struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);

		u64 ts;



		now = priv->tstamp_cc.read(&priv->tstamp_cc);



		*shwt = (struct skb_shared_hwtstamps) {0};



		ts = SJA1105_SKB_CB(skb)->meta_tstamp;

		ts = sja1105_tstamp_reconstruct(priv, now, ts);

		ts = timecounter_cyc2time(&priv->tstamp_tc, ts);



		shwt->hwtstamp = ns_to_ktime(ts);

		netif_rx_ni(skb);

	}



	mutex_unlock(&priv->ptp_lock);

}



/* Called from dsa_skb_defer_rx_timestamp */

static 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 *data = &priv->tagger_data;



	if (!test_bit(SJA1105_HWTS_RX_EN, &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(&data->skb_rxtstamp_queue, skb);

	schedule_work(&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.

 */

static 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_port_setup_tc(struct dsa_switch *ds, int port,

				 enum tc_setup_type type,

				 void *type_data)
@@ -2280,9 +2056,6 @@ static int sja1105_probe(struct spi_device *spi) 
	priv->ds = ds;



	tagger_data = &priv->tagger_data;

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



	/* Connections between dsa_port and sja1105_port */

	for (i = 0; i < SJA1105_NUM_PORTS; i++) {
@@ -2292,6 +2065,7 @@ static int sja1105_probe(struct spi_device *spi) 
		sp->dp = &ds->ports[i];

		sp->data = tagger_data;

	}

	mutex_init(&priv->ptp_data.lock);

	mutex_init(&priv->mgmt_lock);



	sja1105_tas_setup(ds);

drivers/net/dsa/sja1105/sja1105_ptp.c

+317 −74
Original line number Diff line number Diff line
@@ -50,21 +50,151 @@ 
#define SJA1105_CC_MULT_NUM		(1 << 9)

#define SJA1105_CC_MULT_DEM		15625



#define ptp_to_sja1105(d) container_of((d), struct sja1105_private, ptp_caps)

#define cc_to_sja1105(d) container_of((d), struct sja1105_private, tstamp_cc)

#define dw_to_sja1105(d) container_of((d), struct sja1105_private, refresh_work)

#define ptp_caps_to_data(d) \

		container_of((d), struct sja1105_ptp_data, caps)

#define cc_to_ptp_data(d) \

		container_of((d), struct sja1105_ptp_data, tstamp_cc)

#define dw_to_ptp_data(d) \

		container_of((d), struct sja1105_ptp_data, refresh_work)

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



struct sja1105_ptp_cmd {

	u64 resptp;       /* reset */

};

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

}



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 (!priv->clock)

	if (!ptp_data->clock)

		return -ENODEV;



	info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
@@ -74,14 +204,14 @@ int sja1105_get_ts_info(struct dsa_switch *ds, int port, 
			 (1 << HWTSTAMP_TX_ON);

	info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |

			   (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT);

	info->phc_index = ptp_clock_index(priv->clock);

	info->phc_index = ptp_clock_index(ptp_data->clock);

	return 0;

}



int sja1105et_ptp_cmd(const void *ctx, const void *data)

int sja1105et_ptp_cmd(const struct dsa_switch *ds,

		      const struct sja1105_ptp_cmd *cmd)

{

	const struct sja1105_ptp_cmd *cmd = data;

	const struct sja1105_private *priv = ctx;

	const struct sja1105_private *priv = ds->priv;

	const struct sja1105_regs *regs = priv->info->regs;

	const int size = SJA1105_SIZE_PTP_CMD;

	u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
@@ -95,10 +225,10 @@ int sja1105et_ptp_cmd(const void *ctx, const void *data) 
				SJA1105_SIZE_PTP_CMD);

}



int sja1105pqrs_ptp_cmd(const void *ctx, const void *data)

int sja1105pqrs_ptp_cmd(const struct dsa_switch *ds,

			const struct sja1105_ptp_cmd *cmd)

{

	const struct sja1105_ptp_cmd *cmd = data;

	const struct sja1105_private *priv = ctx;

	const struct sja1105_private *priv = ds->priv;

	const struct sja1105_regs *regs = priv->info->regs;

	const int size = SJA1105_SIZE_PTP_CMD;

	u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
@@ -126,9 +256,10 @@ int sja1105pqrs_ptp_cmd(const void *ctx, const void *data) 
 * Must be called within one wraparound period of the partial timestamp since

 * it was generated by the MAC.

 */

u64 sja1105_tstamp_reconstruct(struct sja1105_private *priv, u64 now,

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;
@@ -170,8 +301,9 @@ u64 sja1105_tstamp_reconstruct(struct sja1105_private *priv, u64 now, 
 * 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.

 */

int sja1105_ptpegr_ts_poll(struct sja1105_private *priv, int port, u64 *ts)

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;
@@ -214,22 +346,91 @@ int sja1105_ptpegr_ts_poll(struct sja1105_private *priv, int port, u64 *ts) 
	return 0;

}



int sja1105_ptp_reset(struct sja1105_private *priv)

#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 sja1105_ptp_cmd cmd = {0};

	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 now, ts;



		now = ptp_data->tstamp_cc.read(&ptp_data->tstamp_cc);



		*shwt = (struct skb_shared_hwtstamps) {0};



		ts = SJA1105_SKB_CB(skb)->meta_tstamp;

		ts = sja1105_tstamp_reconstruct(ds, now, ts);

		ts = timecounter_cyc2time(&ptp_data->tstamp_tc, ts);



		shwt->hwtstamp = ns_to_ktime(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;

}



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(&priv->ptp_lock);

	mutex_lock(&ptp_data->lock);



	cmd.resptp = 1;

	dev_dbg(ds->dev, "Resetting PTP clock\n");

	rc = priv->info->ptp_cmd(priv, &cmd);

	rc = priv->info->ptp_cmd(ds, &cmd);



	timecounter_init(&priv->tstamp_tc, &priv->tstamp_cc,

	timecounter_init(&ptp_data->tstamp_tc, &ptp_data->tstamp_cc,

			 ktime_to_ns(ktime_get_real()));



	mutex_unlock(&priv->ptp_lock);

	mutex_unlock(&ptp_data->lock);



	return rc;

}
@@ -237,12 +438,12 @@ int sja1105_ptp_reset(struct sja1105_private *priv) 
static int sja1105_ptp_gettime(struct ptp_clock_info *ptp,

			       struct timespec64 *ts)

{

	struct sja1105_private *priv = ptp_to_sja1105(ptp);

	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);

	u64 ns;



	mutex_lock(&priv->ptp_lock);

	ns = timecounter_read(&priv->tstamp_tc);

	mutex_unlock(&priv->ptp_lock);

	mutex_lock(&ptp_data->lock);

	ns = timecounter_read(&ptp_data->tstamp_tc);

	mutex_unlock(&ptp_data->lock);



	*ts = ns_to_timespec64(ns);
@@ -252,25 +453,25 @@ static int sja1105_ptp_gettime(struct ptp_clock_info *ptp, 
static int sja1105_ptp_settime(struct ptp_clock_info *ptp,

			       const struct timespec64 *ts)

{

	struct sja1105_private *priv = ptp_to_sja1105(ptp);

	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);

	u64 ns = timespec64_to_ns(ts);



	mutex_lock(&priv->ptp_lock);

	timecounter_init(&priv->tstamp_tc, &priv->tstamp_cc, ns);

	mutex_unlock(&priv->ptp_lock);

	mutex_lock(&ptp_data->lock);

	timecounter_init(&ptp_data->tstamp_tc, &ptp_data->tstamp_cc, ns);

	mutex_unlock(&ptp_data->lock);



	return 0;

}



static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)

{

	struct sja1105_private *priv = ptp_to_sja1105(ptp);

	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);

	s64 clkrate;



	clkrate = (s64)scaled_ppm * SJA1105_CC_MULT_NUM;

	clkrate = div_s64(clkrate, SJA1105_CC_MULT_DEM);



	mutex_lock(&priv->ptp_lock);

	mutex_lock(&ptp_data->lock);



	/* Force a readout to update the timer *before* changing its frequency.

	 *
@@ -298,29 +499,30 @@ static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 
	 * instead of a compound function made of two segments (one at the old

	 * and the other at the new rate) - introducing some inaccuracy.

	 */

	timecounter_read(&priv->tstamp_tc);

	timecounter_read(&ptp_data->tstamp_tc);



	priv->tstamp_cc.mult = SJA1105_CC_MULT + clkrate;

	ptp_data->tstamp_cc.mult = SJA1105_CC_MULT + clkrate;



	mutex_unlock(&priv->ptp_lock);

	mutex_unlock(&ptp_data->lock);



	return 0;

}



static int sja1105_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)

{

	struct sja1105_private *priv = ptp_to_sja1105(ptp);

	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);



	mutex_lock(&priv->ptp_lock);

	timecounter_adjtime(&priv->tstamp_tc, delta);

	mutex_unlock(&priv->ptp_lock);

	mutex_lock(&ptp_data->lock);

	timecounter_adjtime(&ptp_data->tstamp_tc, delta);

	mutex_unlock(&ptp_data->lock);



	return 0;

}



static u64 sja1105_ptptsclk_read(const struct cyclecounter *cc)

{

	struct sja1105_private *priv = cc_to_sja1105(cc);

	struct sja1105_ptp_data *ptp_data = cc_to_ptp_data(cc);

	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);

	const struct sja1105_regs *regs = priv->info->regs;

	u64 ptptsclk = 0;

	int rc;
@@ -336,15 +538,29 @@ static u64 sja1105_ptptsclk_read(const struct cyclecounter *cc) 
static void sja1105_ptp_overflow_check(struct work_struct *work)

{

	struct delayed_work *dw = to_delayed_work(work);

	struct sja1105_private *priv = dw_to_sja1105(dw);

	struct sja1105_ptp_data *ptp_data = dw_to_ptp_data(dw);

	struct timespec64 ts;



	sja1105_ptp_gettime(&priv->ptp_caps, &ts);

	sja1105_ptp_gettime(&ptp_data->caps, &ts);



	schedule_delayed_work(&priv->refresh_work, SJA1105_REFRESH_INTERVAL);

	schedule_delayed_work(&ptp_data->refresh_work,

			      SJA1105_REFRESH_INTERVAL);

}



static const struct ptp_clock_info sja1105_ptp_caps = {

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;



	/* Set up the cycle counter */

	ptp_data->tstamp_cc = (struct cyclecounter) {

		.read		= sja1105_ptptsclk_read,

		.mask		= CYCLECOUNTER_MASK(64),

		.shift		= SJA1105_CC_SHIFT,

		.mult		= SJA1105_CC_MULT,

	};

	ptp_data->caps = (struct ptp_clock_info) {

		.owner		= THIS_MODULE,

		.name		= "SJA1105 PHC",

		.adjfine	= sja1105_ptp_adjfine,
@@ -354,36 +570,63 @@ static const struct ptp_clock_info sja1105_ptp_caps = { 
		.max_adj	= SJA1105_MAX_ADJ_PPB,

	};



int sja1105_ptp_clock_register(struct sja1105_private *priv)

{

	struct dsa_switch *ds = priv->ds;



	/* Set up the cycle counter */

	priv->tstamp_cc = (struct cyclecounter) {

		.read = sja1105_ptptsclk_read,

		.mask = CYCLECOUNTER_MASK(64),

		.shift = SJA1105_CC_SHIFT,

		.mult = SJA1105_CC_MULT,

	};

	mutex_init(&priv->ptp_lock);

	priv->ptp_caps = sja1105_ptp_caps;

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



	priv->clock = ptp_clock_register(&priv->ptp_caps, ds->dev);

	if (IS_ERR_OR_NULL(priv->clock))

		return PTR_ERR(priv->clock);

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



	INIT_DELAYED_WORK(&priv->refresh_work, sja1105_ptp_overflow_check);

	schedule_delayed_work(&priv->refresh_work, SJA1105_REFRESH_INTERVAL);

	INIT_DELAYED_WORK(&ptp_data->refresh_work, sja1105_ptp_overflow_check);

	schedule_delayed_work(&ptp_data->refresh_work, SJA1105_REFRESH_INTERVAL);



	return sja1105_ptp_reset(priv);

	return sja1105_ptp_reset(ds);

}



void sja1105_ptp_clock_unregister(struct sja1105_private *priv)

void sja1105_ptp_clock_unregister(struct dsa_switch *ds)

{

	if (IS_ERR_OR_NULL(priv->clock))

	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_delayed_work_sync(&priv->refresh_work);

	ptp_clock_unregister(priv->clock);

	priv->clock = NULL;

	cancel_work_sync(&priv->tagger_data.rxtstamp_work);

	skb_queue_purge(&priv->tagger_data.skb_rxtstamp_queue);

	cancel_delayed_work_sync(&ptp_data->refresh_work);

	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 now, ts;

	int rc;



	skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;



	mutex_lock(&ptp_data->lock);



	now = ptp_data->tstamp_cc.read(&ptp_data->tstamp_cc);



	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, now, ts);

	ts = timecounter_cyc2time(&ptp_data->tstamp_tc, ts);



	shwt.hwtstamp = ns_to_ktime(ts);

	skb_complete_tx_timestamp(skb, &shwt);



out:

	mutex_unlock(&ptp_data->lock);

}

drivers/net/dsa/sja1105/sja1105_ptp.h

+60 −24

File changed.

Preview size limit exceeded, changes collapsed.

drivers/net/dsa/sja1105/sja1105_spi.c

+1 −1
Original line number Diff line number Diff line
@@ -495,7 +495,7 @@ int sja1105_static_config_upload(struct sja1105_private *priv) 
		dev_info(dev, "Succeeded after %d tried\n", RETRIES - retries);

	}



	rc = sja1105_ptp_reset(priv);

	rc = sja1105_ptp_reset(priv->ds);

	if (rc < 0)

		dev_err(dev, "Failed to reset PTP clock: %d\n", rc);