1// SPDX-License-Identifier: GPL-2.0
  2/* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com>
  3 */
  4#include <linux/if_vlan.h>
  5#include <linux/dsa/sja1105.h>
  6#include <linux/dsa/8021q.h>
  7#include <linux/packing.h>
  8#include "dsa_priv.h"
  9
 10/* Similar to is_link_local_ether_addr(hdr->h_dest) but also covers PTP */
 11static inline bool sja1105_is_link_local(const struct sk_buff *skb)
 12{
 13	const struct ethhdr *hdr = eth_hdr(skb);
 14	u64 dmac = ether_addr_to_u64(hdr->h_dest);
 15
 16	if (ntohs(hdr->h_proto) == ETH_P_SJA1105_META)
 17		return false;
 18	if ((dmac & SJA1105_LINKLOCAL_FILTER_A_MASK) ==
 19		    SJA1105_LINKLOCAL_FILTER_A)
 20		return true;
 21	if ((dmac & SJA1105_LINKLOCAL_FILTER_B_MASK) ==
 22		    SJA1105_LINKLOCAL_FILTER_B)
 23		return true;
 24	return false;
 25}
 26
 27struct sja1105_meta {
 28	u64 tstamp;
 29	u64 dmac_byte_4;
 30	u64 dmac_byte_3;
 31	u64 source_port;
 32	u64 switch_id;
 33};
 34
 35static void sja1105_meta_unpack(const struct sk_buff *skb,
 36				struct sja1105_meta *meta)
 37{
 38	u8 *buf = skb_mac_header(skb) + ETH_HLEN;
 39
 40	/* UM10944.pdf section 4.2.17 AVB Parameters:
 41	 * Structure of the meta-data follow-up frame.
 42	 * It is in network byte order, so there are no quirks
 43	 * while unpacking the meta frame.
 44	 *
 45	 * Also SJA1105 E/T only populates bits 23:0 of the timestamp
 46	 * whereas P/Q/R/S does 32 bits. Since the structure is the
 47	 * same and the E/T puts zeroes in the high-order byte, use
 48	 * a unified unpacking command for both device series.
 49	 */
 50	packing(buf,     &meta->tstamp,     31, 0, 4, UNPACK, 0);
 51	packing(buf + 4, &meta->dmac_byte_4, 7, 0, 1, UNPACK, 0);
 52	packing(buf + 5, &meta->dmac_byte_3, 7, 0, 1, UNPACK, 0);
 53	packing(buf + 6, &meta->source_port, 7, 0, 1, UNPACK, 0);
 54	packing(buf + 7, &meta->switch_id,   7, 0, 1, UNPACK, 0);
 55}
 56
 57static inline bool sja1105_is_meta_frame(const struct sk_buff *skb)
 58{
 59	const struct ethhdr *hdr = eth_hdr(skb);
 60	u64 smac = ether_addr_to_u64(hdr->h_source);
 61	u64 dmac = ether_addr_to_u64(hdr->h_dest);
 62
 63	if (smac != SJA1105_META_SMAC)
 64		return false;
 65	if (dmac != SJA1105_META_DMAC)
 66		return false;
 67	if (ntohs(hdr->h_proto) != ETH_P_SJA1105_META)
 68		return false;
 69	return true;
 70}
 71
 72static bool sja1105_can_use_vlan_as_tags(const struct sk_buff *skb)
 73{
 74	struct vlan_ethhdr *hdr = vlan_eth_hdr(skb);
 75
 76	if (hdr->h_vlan_proto == htons(ETH_P_SJA1105))
 77		return true;
 78
 79	if (hdr->h_vlan_proto != htons(ETH_P_8021Q))
 80		return false;
 81
 82	return vid_is_dsa_8021q(ntohs(hdr->h_vlan_TCI) & VLAN_VID_MASK);
 83}
 84
 85/* This is the first time the tagger sees the frame on RX.
 86 * Figure out if we can decode it.
 87 */
 88static bool sja1105_filter(const struct sk_buff *skb, struct net_device *dev)
 89{
 90	if (sja1105_can_use_vlan_as_tags(skb))
 91		return true;
 92	if (sja1105_is_link_local(skb))
 93		return true;
 94	if (sja1105_is_meta_frame(skb))
 95		return true;
 96	return false;
 97}
 98
 99/* Calls sja1105_port_deferred_xmit in sja1105_main.c */
100static struct sk_buff *sja1105_defer_xmit(struct sja1105_port *sp,
101					  struct sk_buff *skb)
102{
103	/* Increase refcount so the kfree_skb in dsa_slave_xmit
104	 * won't really free the packet.
105	 */
106	skb_queue_tail(&sp->xmit_queue, skb_get(skb));
107	kthread_queue_work(sp->xmit_worker, &sp->xmit_work);
108
109	return NULL;
110}
111
112static u16 sja1105_xmit_tpid(struct sja1105_port *sp)
113{
114	return sp->xmit_tpid;
115}
116
117static struct sk_buff *sja1105_xmit(struct sk_buff *skb,
118				    struct net_device *netdev)
119{
120	struct dsa_port *dp = dsa_slave_to_port(netdev);
121	u16 tx_vid = dsa_8021q_tx_vid(dp->ds, dp->index);
122	u16 queue_mapping = skb_get_queue_mapping(skb);
123	u8 pcp = netdev_txq_to_tc(netdev, queue_mapping);
124
125	/* Transmitting management traffic does not rely upon switch tagging,
126	 * but instead SPI-installed management routes. Part 2 of this
127	 * is the .port_deferred_xmit driver callback.
128	 */
129	if (unlikely(sja1105_is_link_local(skb)))
130		return sja1105_defer_xmit(dp->priv, skb);
131
132	return dsa_8021q_xmit(skb, netdev, sja1105_xmit_tpid(dp->priv),
133			     ((pcp << VLAN_PRIO_SHIFT) | tx_vid));
134}
135
136static void sja1105_transfer_meta(struct sk_buff *skb,
137				  const struct sja1105_meta *meta)
138{
139	struct ethhdr *hdr = eth_hdr(skb);
140
141	hdr->h_dest[3] = meta->dmac_byte_3;
142	hdr->h_dest[4] = meta->dmac_byte_4;
143	SJA1105_SKB_CB(skb)->meta_tstamp = meta->tstamp;
144}
145
146/* This is a simple state machine which follows the hardware mechanism of
147 * generating RX timestamps:
148 *
149 * After each timestampable skb (all traffic for which send_meta1 and
150 * send_meta0 is true, aka all MAC-filtered link-local traffic) a meta frame
151 * containing a partial timestamp is immediately generated by the switch and
152 * sent as a follow-up to the link-local frame on the CPU port.
153 *
154 * The meta frames have no unique identifier (such as sequence number) by which
155 * one may pair them to the correct timestampable frame.
156 * Instead, the switch has internal logic that ensures no frames are sent on
157 * the CPU port between a link-local timestampable frame and its corresponding
158 * meta follow-up. It also ensures strict ordering between ports (lower ports
159 * have higher priority towards the CPU port). For this reason, a per-port
160 * data structure is not needed/desirable.
161 *
162 * This function pairs the link-local frame with its partial timestamp from the
163 * meta follow-up frame. The full timestamp will be reconstructed later in a
164 * work queue.
165 */
166static struct sk_buff
167*sja1105_rcv_meta_state_machine(struct sk_buff *skb,
168				struct sja1105_meta *meta,
169				bool is_link_local,
170				bool is_meta)
171{
172	struct sja1105_port *sp;
173	struct dsa_port *dp;
174
175	dp = dsa_slave_to_port(skb->dev);
176	sp = dp->priv;
177
178	/* Step 1: A timestampable frame was received.
179	 * Buffer it until we get its meta frame.
180	 */
181	if (is_link_local) {
182		if (!test_bit(SJA1105_HWTS_RX_EN, &sp->data->state))
183			/* Do normal processing. */
184			return skb;
185
186		spin_lock(&sp->data->meta_lock);
187		/* Was this a link-local frame instead of the meta
188		 * that we were expecting?
189		 */
190		if (sp->data->stampable_skb) {
191			dev_err_ratelimited(dp->ds->dev,
192					    "Expected meta frame, is %12llx "
193					    "in the DSA master multicast filter?\n",
194					    SJA1105_META_DMAC);
195			kfree_skb(sp->data->stampable_skb);
196		}
197
198		/* Hold a reference to avoid dsa_switch_rcv
199		 * from freeing the skb.
200		 */
201		sp->data->stampable_skb = skb_get(skb);
202		spin_unlock(&sp->data->meta_lock);
203
204		/* Tell DSA we got nothing */
205		return NULL;
206
207	/* Step 2: The meta frame arrived.
208	 * Time to take the stampable skb out of the closet, annotate it
209	 * with the partial timestamp, and pretend that we received it
210	 * just now (basically masquerade the buffered frame as the meta
211	 * frame, which serves no further purpose).
212	 */
213	} else if (is_meta) {
214		struct sk_buff *stampable_skb;
215
216		/* Drop the meta frame if we're not in the right state
217		 * to process it.
218		 */
219		if (!test_bit(SJA1105_HWTS_RX_EN, &sp->data->state))
220			return NULL;
221
222		spin_lock(&sp->data->meta_lock);
223
224		stampable_skb = sp->data->stampable_skb;
225		sp->data->stampable_skb = NULL;
226
227		/* Was this a meta frame instead of the link-local
228		 * that we were expecting?
229		 */
230		if (!stampable_skb) {
231			dev_err_ratelimited(dp->ds->dev,
232					    "Unexpected meta frame\n");
233			spin_unlock(&sp->data->meta_lock);
234			return NULL;
235		}
236
237		if (stampable_skb->dev != skb->dev) {
238			dev_err_ratelimited(dp->ds->dev,
239					    "Meta frame on wrong port\n");
240			spin_unlock(&sp->data->meta_lock);
241			return NULL;
242		}
243
244		/* Free the meta frame and give DSA the buffered stampable_skb
245		 * for further processing up the network stack.
246		 */
247		kfree_skb(skb);
248		skb = stampable_skb;
249		sja1105_transfer_meta(skb, meta);
250
251		spin_unlock(&sp->data->meta_lock);
252	}
253
254	return skb;
255}
256
257static void sja1105_decode_subvlan(struct sk_buff *skb, u16 subvlan)
258{
259	struct dsa_port *dp = dsa_slave_to_port(skb->dev);
260	struct sja1105_port *sp = dp->priv;
261	u16 vid = sp->subvlan_map[subvlan];
262	u16 vlan_tci;
263
264	if (vid == VLAN_N_VID)
265		return;
266
267	vlan_tci = (skb->priority << VLAN_PRIO_SHIFT) | vid;
268	__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tci);
269}
270
271static struct sk_buff *sja1105_rcv(struct sk_buff *skb,
272				   struct net_device *netdev,
273				   struct packet_type *pt)
274{
275	struct sja1105_meta meta = {0};
276	int source_port, switch_id;
277	struct ethhdr *hdr;
278	u16 tpid, vid, tci;
279	bool is_link_local;
280	u16 subvlan = 0;
281	bool is_tagged;
282	bool is_meta;
283
284	hdr = eth_hdr(skb);
285	tpid = ntohs(hdr->h_proto);
286	is_tagged = (tpid == ETH_P_SJA1105 || tpid == ETH_P_8021Q);
287	is_link_local = sja1105_is_link_local(skb);
288	is_meta = sja1105_is_meta_frame(skb);
289
290	skb->offload_fwd_mark = 1;
291
292	if (is_tagged) {
293		/* Normal traffic path. */
294		skb_push_rcsum(skb, ETH_HLEN);
295		__skb_vlan_pop(skb, &tci);
296		skb_pull_rcsum(skb, ETH_HLEN);
297		skb_reset_network_header(skb);
298		skb_reset_transport_header(skb);
299
300		vid = tci & VLAN_VID_MASK;
301		source_port = dsa_8021q_rx_source_port(vid);
302		switch_id = dsa_8021q_rx_switch_id(vid);
303		skb->priority = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
304		subvlan = dsa_8021q_rx_subvlan(vid);
305	} else if (is_link_local) {
306		/* Management traffic path. Switch embeds the switch ID and
307		 * port ID into bytes of the destination MAC, courtesy of
308		 * the incl_srcpt options.
309		 */
310		source_port = hdr->h_dest[3];
311		switch_id = hdr->h_dest[4];
312		/* Clear the DMAC bytes that were mangled by the switch */
313		hdr->h_dest[3] = 0;
314		hdr->h_dest[4] = 0;
315	} else if (is_meta) {
316		sja1105_meta_unpack(skb, &meta);
317		source_port = meta.source_port;
318		switch_id = meta.switch_id;
319	} else {
320		return NULL;
321	}
322
323	skb->dev = dsa_master_find_slave(netdev, switch_id, source_port);
324	if (!skb->dev) {
325		netdev_warn(netdev, "Couldn't decode source port\n");
326		return NULL;
327	}
328
329	if (subvlan)
330		sja1105_decode_subvlan(skb, subvlan);
331
332	return sja1105_rcv_meta_state_machine(skb, &meta, is_link_local,
333					      is_meta);
334}
335
336static const struct dsa_device_ops sja1105_netdev_ops = {
337	.name = "sja1105",
338	.proto = DSA_TAG_PROTO_SJA1105,
339	.xmit = sja1105_xmit,
340	.rcv = sja1105_rcv,
341	.filter = sja1105_filter,
342	.overhead = VLAN_HLEN,
343};
344
345MODULE_LICENSE("GPL v2");
346MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_SJA1105);
347
348module_dsa_tag_driver(sja1105_netdev_ops);