net: prp: add packet handling support

DAN-P nodes are to receive traditional IP packets as well
PRP (Parallel Redundancy Protocol) tagged (trailer) packets.
This patch adds logic to format L2 frames from network
stack to add a trailer and send it as duplicates over the
slave interfaces. At the ingress, it strips the trailer,
do duplicate detection and rejection and forward a stripped
frame up the network stack. Unlike HSR, PRP device should
accept frames from Singly Attached Nodes (SAN) and mark
the link where the frame came from in the node table. As Supervisor
frame LSDU size is same for HSR and PRP, rename the constant for
the same.

Signed-off-by: Murali Karicheri <m-karicheri2@ti.com>

net/hsr-prp/hsr_prp_device.c

@@ -287,7 +287,7 @@ static void send_supervision_frame(struct hsr_prp_port *master,
 	if (proto_ver == HSR_V1) {
 		hsr_tag = skb_put(skb, sizeof(struct hsr_tag));
 		hsr_tag->encap_proto = htons(ETH_P_PRP);
-		set_hsr_tag_LSDU_size(hsr_tag, HSR_V1_SUP_LSDUSIZE);
+		set_hsr_tag_LSDU_size(hsr_tag, HSR_PRP_V1_SUP_LSDUSIZE);
 	}
 
 	hsr_stag = skb_put(skb, sizeof(struct hsr_prp_sup_tag));
@@ -328,7 +328,7 @@ static void send_supervision_frame(struct hsr_prp_port *master,
 		tail = skb_tail_pointer(skb) - HSR_PRP_HLEN;
 		rct = (struct prp_rct *)tail;
 		rct->PRP_suffix = htons(ETH_P_PRP);
-		set_prp_LSDU_size(rct, HSR_V1_SUP_LSDUSIZE);
+		set_prp_LSDU_size(rct, HSR_PRP_V1_SUP_LSDUSIZE);
 		rct->sequence_nr = htons(master->priv->sequence_nr);
 		master->priv->sequence_nr++;
 	}
@@ -478,10 +478,6 @@ int hsr_prp_dev_finalize(struct net_device *hsr_prp_dev,
 	struct hsr_prp_port *port;
 	int res;
 
-	/* PRP not supported yet */
-	if (protocol_version == PRP_V1)
-		return -EPROTONOSUPPORT;
-
 	priv = netdev_priv(hsr_prp_dev);
 	INIT_LIST_HEAD(&priv->ports);
 	INIT_LIST_HEAD(&priv->node_db);
@@ -497,6 +493,12 @@ int hsr_prp_dev_finalize(struct net_device *hsr_prp_dev,
 		return res;
 
 	priv->prot_version = protocol_version;
+	if (priv->prot_version == PRP_V1) {
+		/* For PRP, lan_id has most significant 3 bits holding
+		 * the net_id of PRP_LAN_ID
+		 */
+		priv->net_id = PRP_LAN_ID << 1;
+	}
 
 	spin_lock_init(&priv->seqnr_lock);
 	/* Overflow soon to find bugs easier: */

net/hsr-prp/hsr_prp_forward.c

@@ -18,6 +18,7 @@ struct hsr_prp_node;
 struct hsr_prp_frame_info {
 	struct sk_buff *skb_std;
 	struct sk_buff *skb_hsr;
+	struct sk_buff *skb_prp;
 	struct hsr_prp_port *port_rcv;
 	struct hsr_prp_node *node_src;
 	u16 sequence_nr;
@@ -25,6 +26,7 @@ struct hsr_prp_frame_info {
 	bool is_vlan;
 	bool is_local_dest;
 	bool is_local_exclusive;
+	bool is_from_san;
 };
 
 /* The uses I can see for these HSR supervision frames are:
@@ -85,8 +87,8 @@ static bool is_supervision_frame(struct hsr_prp_priv *priv, struct sk_buff *skb)
 	return true;
 }
 
-static struct sk_buff *create_stripped_skb(struct sk_buff *skb_in,
-					   struct hsr_prp_frame_info *frame)
+static struct sk_buff *create_stripped_skb_hsr(struct sk_buff *skb_in,
+					       struct hsr_prp_frame_info *frame)
 {
 	struct sk_buff *skb;
 	int copylen;
@@ -118,22 +120,91 @@ static struct sk_buff *create_stripped_skb(struct sk_buff *skb_in,
 static struct sk_buff *frame_get_stripped_skb(struct hsr_prp_frame_info *frame,
 					      struct hsr_prp_port *port)
 {
-	if (!frame->skb_std)
-		frame->skb_std = create_stripped_skb(frame->skb_hsr, frame);
+	if (!frame->skb_std) {
+		if (frame->skb_hsr) {
+			frame->skb_std =
+				create_stripped_skb_hsr(frame->skb_hsr, frame);
+		} else if (frame->skb_prp) {
+			/* trim the skb by len - HSR_PRP_HLEN to exclude
+			 * RCT
+			 */
+			skb_trim(frame->skb_prp,
+				 frame->skb_prp->len - HSR_PRP_HLEN);
+			frame->skb_std =
+				__pskb_copy(frame->skb_prp,
+					    skb_headroom(frame->skb_prp),
+					    GFP_ATOMIC);
+
+		} else {
+			/* Unexpected */
+			WARN_ONCE(1, "%s:%d: Unexpected frame received (port_src %s)\n",
+				  __FILE__, __LINE__, port->dev->name);
+			return NULL;
+		}
+	}
+
 	return skb_clone(frame->skb_std, GFP_ATOMIC);
 }
 
-static void hsr_fill_tag(struct sk_buff *skb, struct hsr_prp_frame_info *frame,
-			 struct hsr_prp_port *port, u8 proto_version)
+static void prp_set_lan_id(struct prp_rct *trailor,
+			   struct hsr_prp_port *port)
 {
-	struct hsr_ethhdr *hsr_ethhdr;
 	int lane_id;
-	int lsdu_size;
 
 	if (port->type == HSR_PRP_PT_SLAVE_A)
 		lane_id = 0;
 	else
 		lane_id = 1;
+	/* Add net_id in the upper 3 bits of lane_id */
+	lane_id |= port->priv->net_id;
+
+	set_prp_lan_id(trailor, lane_id);
+}
+
+/* Tailroom for PRP rct should have been created before calling this */
+static void prp_fill_rct(struct sk_buff *skb,
+			 struct hsr_prp_frame_info *frame,
+			 struct hsr_prp_port *port)
+{
+	struct prp_rct *trailor;
+	int lsdu_size;
+
+	if (!skb)
+		return;
+
+	if (frame->is_vlan)
+		skb_put_padto(skb, VLAN_ETH_ZLEN);
+	else
+		skb_put_padto(skb, ETH_ZLEN);
+
+	trailor = (struct prp_rct *)skb_put(skb, HSR_PRP_HLEN);
+	lsdu_size = skb->len - 14;
+	if (frame->is_vlan)
+		lsdu_size -= 4;
+	prp_set_lan_id(trailor, port);
+	set_prp_LSDU_size(trailor, lsdu_size);
+	trailor->sequence_nr = htons(frame->sequence_nr);
+	trailor->PRP_suffix = htons(ETH_P_PRP);
+}
+
+static void hsr_set_path_id(struct hsr_ethhdr *hsr_ethhdr,
+			    struct hsr_prp_port *port)
+{
+	int path_id;
+
+	if (port->type == HSR_PRP_PT_SLAVE_A)
+		path_id = 0;
+	else
+		path_id = 1;
+
+	set_hsr_tag_path(&hsr_ethhdr->hsr_tag, path_id);
+}
+
+static void hsr_fill_tag(struct sk_buff *skb, struct hsr_prp_frame_info *frame,
+			 struct hsr_prp_port *port, u8 proto_version)
+{
+	struct hsr_ethhdr *hsr_ethhdr;
+	int lsdu_size;
 
 	lsdu_size = skb->len - 14;
 	if (frame->is_vlan)
@@ -141,7 +212,7 @@ static void hsr_fill_tag(struct sk_buff *skb, struct hsr_prp_frame_info *frame,
 
 	hsr_ethhdr = (struct hsr_ethhdr *)skb_mac_header(skb);
 
-	set_hsr_tag_path(&hsr_ethhdr->hsr_tag, lane_id);
+	hsr_set_path_id(hsr_ethhdr, port);
 	set_hsr_tag_LSDU_size(&hsr_ethhdr->hsr_tag, lsdu_size);
 	hsr_ethhdr->hsr_tag.sequence_nr = htons(frame->sequence_nr);
 	hsr_ethhdr->hsr_tag.encap_proto = hsr_ethhdr->ethhdr.h_proto;
@@ -157,6 +228,14 @@ static struct sk_buff *create_tagged_skb(struct sk_buff *skb_o,
 	unsigned char *dst, *src;
 	struct sk_buff *skb;
 
+	if (port->priv->prot_version > HSR_V1) {
+		skb = skb_copy_expand(skb_o, 0,
+				      skb_tailroom(skb_o) + HSR_PRP_HLEN,
+				      GFP_ATOMIC);
+		prp_fill_rct(skb, frame, port);
+		return skb;
+	}
+
 	/* Create the new skb with enough headroom to fit the HSR tag */
 	skb = __pskb_copy(skb_o,
 			  skb_headroom(skb_o) + HSR_PRP_HLEN, GFP_ATOMIC);
@@ -187,8 +266,26 @@ static struct sk_buff *create_tagged_skb(struct sk_buff *skb_o,
 static struct sk_buff *frame_get_tagged_skb(struct hsr_prp_frame_info *frame,
 					    struct hsr_prp_port *port)
 {
-	if (frame->skb_hsr)
+	if (frame->skb_hsr) {
+		struct hsr_ethhdr *hsr_ethhdr =
+			(struct hsr_ethhdr *)skb_mac_header(frame->skb_hsr);
+
+		/* set the lane id properly */
+		hsr_set_path_id(hsr_ethhdr, port);
 		return skb_clone(frame->skb_hsr, GFP_ATOMIC);
+	}
+
+	if (frame->skb_prp) {
+		struct prp_rct *trailor = skb_get_PRP_rct(frame->skb_prp);
+
+		if (trailor) {
+			prp_set_lan_id(trailor, port);
+		} else {
+			WARN_ONCE(!trailor, "errored PRP skb");
+			return NULL;
+		}
+		return skb_clone(frame->skb_prp, GFP_ATOMIC);
+	}
 
 	if (port->type != HSR_PRP_PT_SLAVE_A &&
 	    port->type != HSR_PRP_PT_SLAVE_B) {
@@ -236,6 +333,7 @@ static int slave_xmit(struct sk_buff *skb, struct hsr_prp_port *port,
 /* Forward the frame through all devices except:
  * - Back through the receiving device
  * - If it's a HSR frame: through a device where it has passed before
+ * - if it's a PRP frame: through another PRP slave device (no bridge)
  * - To the local HSR master only if the frame is directly addressed to it, or
  *   a non-supervision multicast or broadcast frame.
  *
@@ -247,7 +345,7 @@ static int slave_xmit(struct sk_buff *skb, struct hsr_prp_port *port,
 static void hsr_prp_forward_do(struct hsr_prp_frame_info *frame)
 {
 	struct hsr_prp_port *port;
-	struct sk_buff *skb;
+	struct sk_buff *skb = NULL;
 
 	hsr_prp_for_each_port(frame->port_rcv->priv, port) {
 		/* Don't send frame back the way it came */
@@ -263,24 +361,46 @@ static void hsr_prp_forward_do(struct hsr_prp_frame_info *frame)
 		    frame->is_local_exclusive)
 			continue;
 
-		/* Don't send frame over port where it has been sent before */
-		if (hsr_prp_register_frame_out(port, frame->node_src,
+		/* Don't send frame over port where it has been sent before.
+		 * Also fro SAN, this shouldn't be done.
+		 */
+		if (!frame->is_from_san &&
+		    hsr_prp_register_frame_out(port, frame->node_src,
 					       frame->sequence_nr))
 			continue;
 
 		if (frame->is_supervision && port->type == HSR_PRP_PT_MASTER) {
-			hsr_prp_handle_sup_frame(frame->skb_hsr,
-						 frame->node_src,
-						 frame->port_rcv);
+			if (frame->skb_hsr)
+				skb = frame->skb_hsr;
+			else if (frame->skb_prp)
+				skb = frame->skb_prp;
+
+			if (skb)
+				hsr_prp_handle_sup_frame(skb,
+							 frame->node_src,
+							 frame->port_rcv);
 			continue;
 		}
 
+		if (port->priv->prot_version == PRP_V1 &&
+		    ((frame->port_rcv->type == HSR_PRP_PT_SLAVE_A &&
+		    port->type ==  HSR_PRP_PT_SLAVE_B) ||
+		    (frame->port_rcv->type == HSR_PRP_PT_SLAVE_B &&
+		    port->type ==  HSR_PRP_PT_SLAVE_A)))
+			continue;
+
 		if (port->type != HSR_PRP_PT_MASTER)
 			skb = frame_get_tagged_skb(frame, port);
 		else
 			skb = frame_get_stripped_skb(frame, port);
+
 		if (!skb) {
-			/* FIXME: Record the dropped frame? */
+			if (frame->port_rcv->type == HSR_PRP_PT_MASTER) {
+				struct net_device *master_dev =
+				hsr_prp_get_port(port->priv,
+						 HSR_PRP_PT_MASTER)->dev;
+				master_dev->stats.rx_dropped++;
+			}
 			continue;
 		}
 
@@ -314,34 +434,78 @@ static void check_local_dest(struct hsr_prp_priv *priv, struct sk_buff *skb,
 static int fill_frame_info(struct hsr_prp_frame_info *frame,
 			   struct sk_buff *skb, struct hsr_prp_port *port)
 {
+	struct hsr_prp_priv *priv = port->priv;
 	struct ethhdr *ethhdr;
+	struct hsr_vlan_ethhdr *vlan_hdr;
 	unsigned long irqflags;
+	u16 proto;
 
+	memset(frame, 0, sizeof(*frame));
 	frame->is_supervision = is_supervision_frame(port->priv, skb);
-	frame->node_src = hsr_prp_get_node(port, skb, frame->is_supervision);
+	frame->node_src = hsr_prp_get_node(&priv->node_db, skb,
+					   frame->is_supervision,
+					   port->type);
 	if (!frame->node_src)
 		return -1; /* Unknown node and !is_supervision, or no mem */
 
 	ethhdr = (struct ethhdr *)skb_mac_header(skb);
 	frame->is_vlan = false;
-	if (ethhdr->h_proto == htons(ETH_P_8021Q)) {
+	proto = ethhdr->h_proto;
+
+	if (proto == htons(ETH_P_8021Q)) {
 		frame->is_vlan = true;
 		/* FIXME: */
 		WARN_ONCE(1, "HSR: VLAN not yet supported");
 	}
-	if (ethhdr->h_proto == htons(ETH_P_PRP) ||
-	    ethhdr->h_proto == htons(ETH_P_HSR)) {
-		frame->skb_std = NULL;
-		frame->skb_hsr = skb;
-		frame->sequence_nr = hsr_get_skb_sequence_nr(skb);
+
+	if (frame->is_vlan) {
+		vlan_hdr = (struct hsr_vlan_ethhdr *)ethhdr;
+		proto = vlan_hdr->vlanhdr.h_vlan_encapsulated_proto;
+	}
+
+	frame->is_from_san = false;
+	if (proto == htons(ETH_P_PRP) || proto == htons(ETH_P_HSR)) {
+		struct prp_rct *rct = skb_get_PRP_rct(skb);
+
+		if (rct &&
+		    prp_check_lsdu_size(skb, rct, frame->is_supervision)) {
+			frame->skb_hsr = NULL;
+			frame->skb_std = NULL;
+			frame->skb_prp = skb;
+			frame->sequence_nr = prp_get_skb_sequence_nr(rct);
+		} else {
+			frame->skb_std = NULL;
+			frame->skb_prp = NULL;
+			frame->skb_hsr = skb;
+			frame->sequence_nr = hsr_get_skb_sequence_nr(skb);
+		}
 	} else {
-		frame->skb_std = skb;
-		frame->skb_hsr = NULL;
-		/* Sequence nr for the master node */
-		spin_lock_irqsave(&port->priv->seqnr_lock, irqflags);
-		frame->sequence_nr = port->priv->sequence_nr;
-		port->priv->sequence_nr++;
-		spin_unlock_irqrestore(&port->priv->seqnr_lock, irqflags);
+		struct prp_rct *rct = skb_get_PRP_rct(skb);
+
+		if (rct &&
+		    prp_check_lsdu_size(skb, rct, frame->is_supervision)) {
+			frame->skb_hsr = NULL;
+			frame->skb_std = NULL;
+			frame->skb_prp = skb;
+			frame->sequence_nr = prp_get_skb_sequence_nr(rct);
+			frame->is_from_san = false;
+		} else {
+			frame->skb_hsr = NULL;
+			frame->skb_prp = NULL;
+			frame->skb_std = skb;
+
+			if (port->type != HSR_PRP_PT_MASTER) {
+				frame->is_from_san = true;
+			} else {
+				/* Sequence nr for the master node */
+				spin_lock_irqsave(&port->priv->seqnr_lock,
+						  irqflags);
+				frame->sequence_nr = port->priv->sequence_nr;
+				port->priv->sequence_nr++;
+				spin_unlock_irqrestore(&port->priv->seqnr_lock,
+						       irqflags);
+			}
+		}
 	}
 
 	frame->port_rcv = port;
@@ -363,6 +527,12 @@ void hsr_prp_forward_skb(struct sk_buff *skb, struct hsr_prp_port *port)
 
 	if (fill_frame_info(&frame, skb, port) < 0)
 		goto out_drop;
+
+	/* Only accept packets for the protocol we have been configured */
+	if ((frame.skb_hsr && port->priv->prot_version == PRP_V1) ||
+	    (frame.skb_prp && port->priv->prot_version <= HSR_V1))
+		goto out_drop;
+
 	hsr_prp_register_frame_in(frame.node_src, port, frame.sequence_nr);
 	hsr_prp_forward_do(&frame);
 	/* Gets called for ingress frames as well as egress from master port.
@@ -375,6 +545,8 @@ void hsr_prp_forward_skb(struct sk_buff *skb, struct hsr_prp_port *port)
 
 	if (frame.skb_hsr)
 		kfree_skb(frame.skb_hsr);
+	if (frame.skb_prp)
+		kfree_skb(frame.skb_prp);
 	if (frame.skb_std)
 		kfree_skb(frame.skb_std);
 	return;

net/hsr-prp/hsr_prp_framereg.c

@@ -126,7 +126,8 @@ void hsr_prp_del_node(struct list_head *self_node_db)
  */
 struct hsr_prp_node *hsr_prp_add_node(struct list_head *node_db,
 				      unsigned char addr[],
-				      u16 seq_out)
+				      u16 seq_out, bool san,
+				      enum hsr_prp_port_type rx_port)
 {
 	struct hsr_prp_node *node;
 	unsigned long now;
@@ -146,6 +147,13 @@ struct hsr_prp_node *hsr_prp_add_node(struct list_head *node_db,
 		node->time_in[i] = now;
 	for (i = 0; i < HSR_PRP_PT_PORTS; i++)
 		node->seq_out[i] = seq_out;
+	if (san) {
+		/* Mark if the SAN node is over LAN_A or LAN_B */
+		if (rx_port == HSR_PRP_PT_SLAVE_A)
+			node->san_a = true;
+		else if (rx_port == HSR_PRP_PT_SLAVE_B)
+			node->san_b = true;
+	}
 
 	list_add_tail_rcu(&node->mac_list, node_db);
 
@@ -154,13 +162,15 @@ struct hsr_prp_node *hsr_prp_add_node(struct list_head *node_db,
 
 /* Get the hsr_node from which 'skb' was sent.
  */
-struct hsr_prp_node *hsr_prp_get_node(struct hsr_prp_port *port,
+struct hsr_prp_node *hsr_prp_get_node(struct list_head *node_db,
 				      struct sk_buff *skb,
-				      bool is_sup)
+				      bool is_sup,
+				      enum hsr_prp_port_type rx_port)
 {
-	struct list_head *node_db = &port->priv->node_db;
 	struct hsr_prp_node *node;
 	struct ethhdr *ethhdr;
+	struct prp_rct *rct;
+	bool san = false;
 	u16 seq_out;
 
 	if (!skb_mac_header_was_set(skb))
@@ -175,8 +185,9 @@ struct hsr_prp_node *hsr_prp_get_node(struct hsr_prp_port *port,
 			return node;
 	}
 
-	/* Everyone may create a node entry, connected node to a HSR device. */
-
+	/* Everyone may create a node entry, connected node to a HSR/PRP
+	 * device.
+	 */
 	if (ethhdr->h_proto == htons(ETH_P_PRP) ||
 	    ethhdr->h_proto == htons(ETH_P_HSR)) {
 		/* Use the existing sequence_nr from the tag as starting point
@@ -184,15 +195,18 @@ struct hsr_prp_node *hsr_prp_get_node(struct hsr_prp_port *port,
 		 */
 		seq_out = hsr_get_skb_sequence_nr(skb) - 1;
 	} else {
-		/* this is called also for frames from master port and
-		 * so warn only for non master ports
-		 */
-		if (port->type != HSR_PRP_PT_MASTER)
-			WARN_ONCE(1, "%s: Non-HSR frame\n", __func__);
-		seq_out = HSR_PRP_SEQNR_START;
+		rct = skb_get_PRP_rct(skb);
+		if (rct && prp_check_lsdu_size(skb, rct, is_sup)) {
+			seq_out = prp_get_skb_sequence_nr(rct);
+		} else {
+			if (rx_port != HSR_PRP_PT_MASTER)
+				san = true;
+			seq_out = HSR_PRP_SEQNR_START;
+		}
 	}
 
-	return hsr_prp_add_node(node_db, ethhdr->h_source, seq_out);
+	return hsr_prp_add_node(node_db, ethhdr->h_source, seq_out,
+				san, rx_port);
 }
 
 /* Use the Supervision frame's info about an eventual macaddress_B for merging
@@ -229,7 +243,8 @@ void hsr_prp_handle_sup_frame(struct sk_buff *skb,
 	if (!node_real)
 		/* No frame received from AddrA of this node yet */
 		node_real = hsr_prp_add_node(node_db, sp->macaddress_A,
-					     HSR_PRP_SEQNR_START - 1);
+					     HSR_PRP_SEQNR_START - 1, true,
+					     port_rcv->type);
 	if (!node_real)
 		goto done; /* No mem */
 	if (node_real == node_curr)

net/hsr-prp/hsr_prp_framereg.h

@@ -14,9 +14,12 @@ struct hsr_prp_node;
 
 void hsr_prp_del_node(struct list_head *self_node_db);
 struct hsr_prp_node *hsr_prp_add_node(struct list_head *node_db,
-				      unsigned char addr[], u16 seq_out);
-struct hsr_prp_node *hsr_prp_get_node(struct hsr_prp_port *port,
-				      struct sk_buff *skb, bool is_sup);
+				      unsigned char addr[], u16 seq_out,
+				      bool san,
+				      enum hsr_prp_port_type rx_port);
+struct hsr_prp_node *hsr_prp_get_node(struct list_head *node_db,
+				      struct sk_buff *skb, bool is_sup,
+				      enum hsr_prp_port_type rx_port);
 void hsr_prp_handle_sup_frame(struct sk_buff *skb,
 			      struct hsr_prp_node *node_curr,
 			      struct hsr_prp_port *port);
@@ -56,6 +59,9 @@ struct hsr_prp_node {
 	enum hsr_prp_port_type	addr_B_port;
 	unsigned long		time_in[HSR_PRP_PT_PORTS];
 	bool			time_in_stale[HSR_PRP_PT_PORTS];
+	/* if the node is a SAN */
+	bool			san_a;
+	bool			san_b;
 	u16			seq_out[HSR_PRP_PT_PORTS];
 	struct rcu_head		rcu_head;
 };

net/hsr-prp/hsr_prp_main.h

@@ -10,6 +10,7 @@
 
 #include <linux/netdevice.h>
 #include <linux/list.h>
+#include <linux/if_vlan.h>
 
 /* Time constants as specified in the HSR specification (IEC-62439-3 2010)
  * Table 8.
@@ -54,7 +55,7 @@ struct hsr_tag {
 
 #define HSR_PRP_HLEN	6
 
-#define HSR_V1_SUP_LSDUSIZE		52
+#define HSR_PRP_V1_SUP_LSDUSIZE		52
 
 /* The helper functions below assumes that 'path' occupies the 4 most
  * significant bits of the 16-bit field shared by 'path' and 'LSDU_size' (or
@@ -93,8 +94,13 @@ struct hsr_ethhdr {
 	struct hsr_tag	hsr_tag;
 } __packed;
 
-/* HSR Supervision Frame data types.
- * Field names as defined in the IEC:2010 standard for HSR.
+struct hsr_vlan_ethhdr {
+	struct vlan_ethhdr vlanhdr;
+	struct hsr_tag	hsr_tag;
+} __packed;
+
+/* HSR/PRP Supervision Frame data types.
+ * Field names as defined in the IEC:2012 standard for HSR.
  */
 struct hsr_prp_sup_tag {
 	__be16		path_and_HSR_ver;
@@ -160,6 +166,16 @@ struct prp_rct {
 	__be16		PRP_suffix;
 } __packed;
 
+static inline u16 get_prp_LSDU_size(struct prp_rct *rct)
+{
+	return ntohs(rct->lan_id_and_LSDU_size) & 0x0FFF;
+}
+
+static inline void set_prp_lan_id(struct prp_rct *rct, u16 lan_id)
+{
+	rct->lan_id_and_LSDU_size = htons((ntohs(rct->lan_id_and_LSDU_size) &
+					  0x0FFF) | (lan_id << 12));
+}
 static inline void set_prp_LSDU_size(struct prp_rct *rct, u16 LSDU_size)
 {
 	rct->lan_id_and_LSDU_size = htons(
@@ -191,6 +207,12 @@ struct hsr_prp_priv {
 #define HSR_V0	0
 #define HSR_V1	1
 #define PRP_V1	2
+#define PRP_LAN_ID	0x5     /* 0x1010 for A and 0x1011 for B. Bit 0 is set
+				 * based on SLAVE_A or SLAVE_B
+				 */
+	u8 net_id;		/* for PRP, it occupies most significant 3 bits
+				 * of lan_id
+				 */
 	unsigned char		sup_multicast_addr[ETH_ALEN];
 #ifdef	CONFIG_DEBUG_FS
 	struct dentry *node_tbl_root;
@@ -209,10 +231,55 @@ static inline u16 hsr_get_skb_sequence_nr(struct sk_buff *skb)
 {
 	struct hsr_ethhdr *hsr_ethhdr;
 
+	/* TODO will not work when vlan hdr is present */
 	hsr_ethhdr = (struct hsr_ethhdr *)skb_mac_header(skb);
+
 	return ntohs(hsr_ethhdr->hsr_tag.sequence_nr);
 }
 
+static inline struct prp_rct *skb_get_PRP_rct(struct sk_buff *skb)
+{
+	unsigned char *tail = skb_tail_pointer(skb) - HSR_PRP_HLEN;
+
+	struct prp_rct *rct = (struct prp_rct *)tail;
+
+	if (rct->PRP_suffix == htons(ETH_P_PRP))
+		return rct;
+
+	return NULL;
+}
+
+/* Assume caller has confirmed this skb is PRP suffixed */
+static inline u16 prp_get_skb_sequence_nr(struct prp_rct *rct)
+{
+	return ntohs(rct->sequence_nr);
+}
+
+static inline u16 get_prp_lan_id(struct prp_rct *rct)
+{
+	return ntohs(rct->lan_id_and_LSDU_size) >> 12;
+}
+
+/* assume there is a valid rct */
+static inline bool prp_check_lsdu_size(struct sk_buff *skb,
+				       struct prp_rct *rct,
+				       bool is_sup)
+{
+	struct ethhdr *ethhdr;
+	int expected_lsdu_size;
+
+	if (is_sup) {
+		expected_lsdu_size = HSR_PRP_V1_SUP_LSDUSIZE;
+	} else {
+		ethhdr = (struct ethhdr *)skb_mac_header(skb);
+		expected_lsdu_size = skb->len - 14;
+		if (ethhdr->h_proto == htons(ETH_P_8021Q))
+			expected_lsdu_size -= 4;
+	}
+
+	return (expected_lsdu_size == get_prp_LSDU_size(rct));
+}
+
 int hsr_prp_register_notifier(u8 proto);
 void hsr_prp_unregister_notifier(u8 proto);
 

net/hsr-prp/hsr_prp_slave.c

@@ -18,27 +18,50 @@ static rx_handler_result_t handle_frame(struct sk_buff **pskb)
 {
 	struct sk_buff *skb = *pskb;
 	struct hsr_prp_port *port;
+	struct hsr_prp_priv *priv;
+
 	u16 protocol;
 
+	rcu_read_lock(); /* hsr->node_db, hsr->ports */
+	port = hsr_prp_port_get_rcu(skb->dev);
+	priv = port->priv;
+
 	if (!skb_mac_header_was_set(skb)) {
 		WARN_ONCE(1, "%s: skb invalid", __func__);
-		return RX_HANDLER_PASS;
+		goto finish_pass;
 	}
 
-	rcu_read_lock(); /* hsr->node_db, hsr->ports */
-	port = hsr_prp_port_get_rcu(skb->dev);
-
-	if (hsr_prp_addr_is_self(port->priv, eth_hdr(skb)->h_source)) {
+	if (hsr_prp_addr_is_self(priv, eth_hdr(skb)->h_source)) {
 		/* Directly kill frames sent by ourselves */
 		kfree_skb(skb);
 		goto finish_consume;
 	}
 
+	/* For HSR, non tagged frames are expected, but for PRP
+	 * there could be non tagged frames as well.
+	 */
 	protocol = eth_hdr(skb)->h_proto;
-	if (protocol != htons(ETH_P_PRP) && protocol != htons(ETH_P_HSR))
+	if (protocol != htons(ETH_P_PRP) &&
+	    protocol != htons(ETH_P_HSR) &&
+	    port->priv->prot_version <= HSR_V1)
 		goto finish_pass;
 
-	skb_push(skb, ETH_HLEN);
+	/* Frame is a HSR or PRP frame or frame form a SAN. For
+	 * PRP, only supervisor frame will have a PRP protocol.
+	 */
+	if (protocol == htons(ETH_P_HSR) || protocol == htons(ETH_P_PRP))
+		skb_push(skb, ETH_HLEN);
+
+	/* Not sure why we have to do this as some frames
+	 * don't have the skb->data pointing to mac header for PRP case
+	 */
+	if (skb_mac_header(skb) != skb->data) {
+		skb_push(skb, ETH_HLEN);
+
+		/* do one more check and bail out */
+		if (skb_mac_header(skb) != skb->data)
+			goto finish_consume;
+	}
 
 	hsr_prp_forward_skb(skb, port);