net: dsa: sja1105: Add a state machine for RX timestamping (f3097be2) · Commits · 戴 / test

drivers/net/dsa/sja1105/sja1105_main.c

+62 −0

Original line number	Diff line number	Diff line
		@@ -1600,6 +1600,14 @@ static int sja1105_setup(struct dsa_switch *ds)
		return sja1105_setup_8021q_tagging(ds, true);
		}

		static void sja1105_teardown(struct dsa_switch *ds)
		{
		struct sja1105_private *priv = ds->priv;

		cancel_work_sync(&priv->tagger_data.rxtstamp_work);
		skb_queue_purge(&priv->tagger_data.skb_rxtstamp_queue);
		}

		static int sja1105_mgmt_xmit(struct dsa_switch *ds, int port, int slot,
		struct sk_buff *skb, bool takets)
		{
		@@ -1747,6 +1755,57 @@ static int sja1105_set_ageing_time(struct dsa_switch *ds,
		return sja1105_static_config_reload(priv);
		}

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

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

		while ((skb = skb_dequeue(&data->skb_rxtstamp_queue)) != NULL) {
		struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
		u64 ts;

		*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 */
		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 (!data->hwts_rx_en)
		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.
		@@ -1766,6 +1825,7 @@ bool sja1105_port_txtstamp(struct dsa_switch *ds, int port,
		static const struct dsa_switch_ops sja1105_switch_ops = {
		.get_tag_protocol = sja1105_get_tag_protocol,
		.setup = sja1105_setup,
		.teardown = sja1105_teardown,
		.set_ageing_time = sja1105_set_ageing_time,
		.phylink_validate = sja1105_phylink_validate,
		.phylink_mac_config = sja1105_mac_config,
		@@ -1787,6 +1847,7 @@ static const struct dsa_switch_ops sja1105_switch_ops = {
		.port_mdb_add = sja1105_mdb_add,
		.port_mdb_del = sja1105_mdb_del,
		.port_deferred_xmit = sja1105_port_deferred_xmit,
		.port_rxtstamp = sja1105_port_rxtstamp,
		.port_txtstamp = sja1105_port_txtstamp,
		};

		@@ -1885,6 +1946,7 @@ static int sja1105_probe(struct spi_device *spi)

		tagger_data = &priv->tagger_data;
		skb_queue_head_init(&tagger_data->skb_rxtstamp_queue);
		INIT_WORK(&tagger_data->rxtstamp_work, sja1105_rxtstamp_work);

		/* Connections between dsa_port and sja1105_port */
		for (i = 0; i < SJA1105_NUM_PORTS; i++) {

include/linux/dsa/sja1105.h

+7 −0

Original line number	Diff line number	Diff line
		@@ -45,6 +45,13 @@ struct sja1105_tagger_data {
		bool hwts_rx_en;
		};

		struct sja1105_skb_cb {
		u32 meta_tstamp;
		};

		#define SJA1105_SKB_CB(skb) \
		((struct sja1105_skb_cb *)DSA_SKB_CB_PRIV(skb))

		struct sja1105_port {
		struct sja1105_tagger_data *data;
		struct dsa_port *dp;

net/dsa/tag_sja1105.c

+120 −1

Original line number	Diff line number	Diff line
		@@ -110,6 +110,124 @@ static struct sk_buff sja1105_xmit(struct sk_buff skb,
		((pcp << VLAN_PRIO_SHIFT) \| tx_vid));
		}

		static void sja1105_transfer_meta(struct sk_buff *skb,
		const struct sja1105_meta *meta)
		{
		struct ethhdr *hdr = eth_hdr(skb);

		hdr->h_dest[3] = meta->dmac_byte_3;
		hdr->h_dest[4] = meta->dmac_byte_4;
		SJA1105_SKB_CB(skb)->meta_tstamp = meta->tstamp;
		}

		/* This is a simple state machine which follows the hardware mechanism of
		* generating RX timestamps:
		*
		* After each timestampable skb (all traffic for which send_meta1 and
		* send_meta0 is true, aka all MAC-filtered link-local traffic) a meta frame
		* containing a partial timestamp is immediately generated by the switch and
		* sent as a follow-up to the link-local frame on the CPU port.
		*
		* The meta frames have no unique identifier (such as sequence number) by which
		* one may pair them to the correct timestampable frame.
		* Instead, the switch has internal logic that ensures no frames are sent on
		* the CPU port between a link-local timestampable frame and its corresponding
		* meta follow-up. It also ensures strict ordering between ports (lower ports
		* have higher priority towards the CPU port). For this reason, a per-port
		* data structure is not needed/desirable.
		*
		* This function pairs the link-local frame with its partial timestamp from the
		* meta follow-up frame. The full timestamp will be reconstructed later in a
		* work queue.
		*/
		static struct sk_buff
		sja1105_rcv_meta_state_machine(struct sk_buff skb,
		struct sja1105_meta *meta,
		bool is_link_local,
		bool is_meta)
		{
		struct sja1105_port *sp;
		struct dsa_port *dp;

		dp = dsa_slave_to_port(skb->dev);
		sp = dp->priv;

		/* Step 1: A timestampable frame was received.
		* Buffer it until we get its meta frame.
		*/
		if (is_link_local && sp->data->hwts_rx_en) {
		spin_lock(&sp->data->meta_lock);
		/* Was this a link-local frame instead of the meta
		* that we were expecting?
		*/
		if (sp->data->stampable_skb) {
		dev_err_ratelimited(dp->ds->dev,
		"Expected meta frame, is %12llx "
		"in the DSA master multicast filter?\n",
		SJA1105_META_DMAC);
		}

		/* Hold a reference to avoid dsa_switch_rcv
		* from freeing the skb.
		*/
		sp->data->stampable_skb = skb_get(skb);
		spin_unlock(&sp->data->meta_lock);

		/* Tell DSA we got nothing */
		return NULL;

		/* Step 2: The meta frame arrived.
		* Time to take the stampable skb out of the closet, annotate it
		* with the partial timestamp, and pretend that we received it
		* just now (basically masquerade the buffered frame as the meta
		* frame, which serves no further purpose).
		*/
		} else if (is_meta) {
		struct sk_buff *stampable_skb;

		spin_lock(&sp->data->meta_lock);

		stampable_skb = sp->data->stampable_skb;
		sp->data->stampable_skb = NULL;

		/* Was this a meta frame instead of the link-local
		* that we were expecting?
		*/
		if (!stampable_skb) {
		dev_err_ratelimited(dp->ds->dev,
		"Unexpected meta frame\n");
		spin_unlock(&sp->data->meta_lock);
		return NULL;
		}

		if (stampable_skb->dev != skb->dev) {
		dev_err_ratelimited(dp->ds->dev,
		"Meta frame on wrong port\n");
		spin_unlock(&sp->data->meta_lock);
		return NULL;
		}

		/* Free the meta frame and give DSA the buffered stampable_skb
		* for further processing up the network stack.
		*/
		kfree_skb(skb);

		skb = skb_copy(stampable_skb, GFP_ATOMIC);
		if (!skb) {
		dev_err_ratelimited(dp->ds->dev,
		"Failed to copy stampable skb\n");
		return NULL;
		}
		sja1105_transfer_meta(skb, meta);
		/* The cached copy will be freed now */
		skb_unref(stampable_skb);

		spin_unlock(&sp->data->meta_lock);
		}

		return skb;
		}

		static struct sk_buff sja1105_rcv(struct sk_buff skb,
		struct net_device *netdev,
		struct packet_type *pt)
		@@ -167,7 +285,8 @@ static struct sk_buff sja1105_rcv(struct sk_buff skb,
		if (is_tagged)
		skb = dsa_8021q_remove_header(skb);

		return skb;
		return sja1105_rcv_meta_state_machine(skb, &meta, is_link_local,
		is_meta);
		}

		static struct dsa_device_ops sja1105_netdev_ops = {

Admin message