[PATCH] net: hsr : Provide fix for HSRv1 supervisor frames decoding

[Lukasz Majewski <lukma@denx.de>]

Provide fix to decode correctly supervisory frames when HSRv1 version of
the HSR protocol is used.

Without this patch console is polluted with:
ksz-switch spi1.0 lan1: hsr_addr_subst_dest: Unknown node

as a result of destination node's A MAC address equals to:
00:00:00:00:00:00.

cat /sys/kernel/debug/hsr/hsr0/node_table
Node Table entries for (HSR) device
MAC-Address-A,    MAC-Address-B,    time_in[A], time_in[B], Address-B
00:00:00:00:00:00 00:10:a1:94:77:30      400bf,       399c,	        0

It was caused by wrong frames decoding in the hsr_handle_sup_frame().

As the supervisor frame is encapsulated in HSRv1 frame:

SKB_I100000000: 01 15 4e 00 01 2d 00 10 a1 94 77 30 89 2f 00 34
SKB_I100000010: 02 59 88 fb 00 01 84 15 17 06 00 10 a1 94 77 30
SKB_I100000020: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
SKB_I100000030: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
SKB_I100000040: 00 00

The code had to be adjusted accordingly and the MAC-Address-A now
has the proper address (the MAC-Address-B now has all 0's).

Signed-off-by: Lukasz Majewski <lukma@denx.de>
---
 net/hsr/hsr_framereg.c | 32 ++++++++++++++++++++++++++++++--
 1 file changed, 30 insertions(+), 2 deletions(-)

diff --git a/net/hsr/hsr_framereg.c b/net/hsr/hsr_framereg.c
index 80fc71daf7ca..85abe052e0a9 100644
--- a/net/hsr/hsr_framereg.c
+++ b/net/hsr/hsr_framereg.c
@@ -300,9 +300,24 @@ void hsr_handle_sup_frame(struct hsr_frame_info *frame)
 
 	ethhdr = (struct ethhdr *)skb_mac_header(skb);
 
-	/* And leave the HSR tag. */
+	 * And leave the HSR tag.
+	 *
+	 * The HSRv1 supervisory frame encapsulates the v0 frame
+	 * with EtherType of 0x88FB
+	 */
 	if (ethhdr->h_proto == htons(ETH_P_HSR)) {
-		pull_size = sizeof(struct ethhdr);
+		if (hsr->prot_version == HSR_V1)
+			/* In the above step the DA, SA and EtherType
+			 * (0x892F - HSRv1) bytes has been removed.
+			 *
+			 * As the HSRv1 has the HSR header added, one need
+			 * to remove path_and_LSDU_size and sequence_nr fields.
+			 *
+			 */
+			pull_size = 4;
+		else
+			pull_size = sizeof(struct hsr_tag);
+
 		skb_pull(skb, pull_size);
 		total_pull_size += pull_size;
 	}
@@ -313,6 +328,19 @@ void hsr_handle_sup_frame(struct hsr_frame_info *frame)
 	total_pull_size += pull_size;
 
 	/* get HSR sup payload */
+	if (hsr->prot_version == HSR_V1) {
+		/* In the HSRv1 supervisor frame, when
+		 * one with EtherType = 0x88FB is extracted, the Node A
+		 * MAC address is preceded with type and length elements of TLV
+		 * data field.
+		 *
+		 * It needs to be removed to get the remote peer MAC address.
+		 */
+		pull_size = sizeof(struct hsr_sup_tlv);
+		skb_pull(skb, pull_size);
+		total_pull_size += pull_size;
+	}
+
 	hsr_sp = (struct hsr_sup_payload *)skb->data;
 
 	/* Merge node_curr (registered on macaddress_B) into node_real */
-- 
2.20.1