Merge pull request #159 in PROCESSOR-SDK/processor-sdk-linux from PLSDK-2497 to processor-sdk-linux-4.19.y

* commit 'd7a1049951aaf67074ddace44a693500569efacb':
  net: hsr-prp: Add support for PTP over VLAN
  net: ethernet: prueth: Add functionality to run PTP over VLAN

drivers/net/ethernet/ti/prueth.c

@@ -180,6 +180,19 @@ static inline int is_hsr_skb(struct sk_buff *skb)
 	return (*(p + 12) == 0x89 && *(p + 13) == 0x2f);
 }
 
+static inline int is_vlan_skb(struct sk_buff *skb)
+{
+	__be16 ethertype;
+
+	if (!skb->data)
+		return 0;
+
+	ethertype = *(skb->data + 12);
+
+	/* FIXME: should use macros to access header fields */
+	return eth_type_vlan(ethertype);
+}
+
 static inline u32 prueth_read_reg(struct prueth *prueth,
 				  enum prueth_mem region,
 				  unsigned int reg)
@@ -581,7 +594,7 @@ static int pruptp_hsr_cut_thru_ts_work_init(struct prueth_emac *emac)
 static int pruptp_rx_timestamp(struct prueth_emac *emac, struct sk_buff *skb)
 {
 	struct prueth *prueth = emac->prueth;
-	bool changed = false;
+	int changed = 0;
 	u32 ts_ofs;
 	u8 ts_msgtype;
 	int ret;
@@ -591,6 +604,10 @@ static int pruptp_rx_timestamp(struct prueth_emac *emac, struct sk_buff *skb)
 	if (!emac_is_ptp_rx_enabled(emac))
 		return -EPERM;
 
+	if (is_vlan_skb(skb)) {
+		skb->data += 4;
+		changed += 4;
+	}
 	if (PRUETH_HAS_HSR(prueth) && is_hsr_skb(skb)) {
 		/* This 6-byte shift is just a trick to skip
 		 * the size of a hsr tag so that the same
@@ -598,13 +615,13 @@ static int pruptp_rx_timestamp(struct prueth_emac *emac, struct sk_buff *skb)
 		 * hsr tagged skbs
 		 */
 		skb->data += 6;
-		changed = true;
+		changed += 6;
 	}
 
 	ts_msgtype = pruptp_ts_msgtype(skb);
 
 	if (changed)
-		skb->data -= 6;
+		skb->data -= changed;
 
 	if ((ts_msgtype != PTP_SYNC_MSG_ID) &&
 	    (ts_msgtype != PTP_PDLY_REQ_MSG_ID) &&
@@ -1675,10 +1692,14 @@ static inline int emac_tx_ts_enqueue(struct prueth_emac *emac,
 {
 	unsigned long flags;
 	struct prueth *prueth = emac->prueth;
-	bool changed = false;
+	int changed = 0;
 	u8 msg_t;
 	struct tx_ev_cb_data *cb;
 
+	if (is_vlan_skb(skb)) {
+		skb->data += 4;
+		changed += 4;
+	}
 	if (PRUETH_HAS_HSR(prueth) && is_hsr_skb(skb)) {
 		/* This 6-byte shift is just a trick to skip
 		 * the size of a hsr tag so that the same
@@ -1686,13 +1707,13 @@ static inline int emac_tx_ts_enqueue(struct prueth_emac *emac,
 		 * hsr tagged skbs
 		 */
 		skb->data += 6;
-		changed = true;
+		changed += 6;
 	}
 
 	msg_t = pruptp_ts_msgtype(skb);
 
 	if (changed)
-		skb->data -= 6;
+		skb->data -= changed;
 
 	if (msg_t > PTP_PDLY_RSP_MSG_ID) {
 		netdev_err(emac->ndev, "invalid msg_t %u\n", msg_t);

net/hsr-prp/hsr_prp_forward.c

@@ -30,17 +30,24 @@ struct hsr_prp_frame_info {
 	struct skb_redundant_info *sred;
 };
 
-static inline int is_hsr_l2ptp(struct sk_buff *skb)
+static inline int is_hsr_l2ptp(struct sk_buff *skb,
+			       struct hsr_prp_frame_info *frame)
 {
 	struct hsr_ethhdr *hsr_ethhdr;
+	unsigned char *pc;
 
-	hsr_ethhdr = (struct hsr_ethhdr *)skb_mac_header(skb);
+	pc = skb_mac_header(skb);
+	if (frame->is_vlan)
+		hsr_ethhdr = (struct hsr_ethhdr *)(pc + VLAN_HLEN);
+	else
+		hsr_ethhdr = (struct hsr_ethhdr *)pc;
 
 	return (hsr_ethhdr->ethhdr.h_proto == htons(ETH_P_HSR) &&
 		hsr_ethhdr->hsr_tag.encap_proto == htons(ETH_P_1588));
 }
 
-static inline int is_hsr_l2ptp_evt(struct sk_buff *skb)
+static inline int is_hsr_l2ptp_evt(struct sk_buff *skb,
+				   struct hsr_prp_frame_info *frame)
 {
 	unsigned char *p;
 
@@ -48,6 +55,8 @@ static inline int is_hsr_l2ptp_evt(struct sk_buff *skb)
 		return 0;
 
 	p = skb->data;
+	if (frame->is_vlan)
+		p += VLAN_HLEN;
 
 	/* FIXME: should use macros to access header fields */
 	return (*(p + 12) == 0x89 && *(p + 13) == 0x2f &&  /* HSR */
@@ -318,7 +327,7 @@ static struct sk_buff *create_tagged_skb(struct sk_buff *skb_o,
 					 struct hsr_prp_port *port)
 {
 	int movelen;
-	unsigned char *dst, *src;
+	unsigned char *dst, *src, *pc;
 	struct sk_buff *skb;
 	struct skb_redundant_info *sred;
 	struct hsr_ethhdr *hsr_ethhdr;
@@ -362,12 +371,14 @@ static struct sk_buff *create_tagged_skb(struct sk_buff *skb_o,
 
 	skb_shinfo(skb)->tx_flags = skb_shinfo(skb_o)->tx_flags;
 	skb->sk = skb_o->sk;
-
 	/* TODO: should check socket option instead? */
-	if (is_hsr_l2ptp(skb)) {
+	if (is_hsr_l2ptp(skb, frame)) {
 		sred = skb_redinfo(skb);
-		/* assumes no vlan */
-		hsr_ethhdr = (struct hsr_ethhdr *)skb_mac_header(skb);
+		pc = skb_mac_header(skb);
+		if (frame->is_vlan)
+			hsr_ethhdr = (struct hsr_ethhdr *)(pc + VLAN_HLEN);
+		else
+			hsr_ethhdr = (struct hsr_ethhdr *)pc;
 		sred->io_port = (PTP_EVT_OUT | BIT(port->type - 1));
 		sred->ethertype = ntohs(hsr_ethhdr->ethhdr.h_proto);
 		s = ntohs(hsr_ethhdr->hsr_tag.path_and_LSDU_size);
@@ -480,6 +491,7 @@ static void stripped_skb_get_shared_info(struct sk_buff *skb_stripped,
 	struct sk_buff *skb_hsr, *skb;
 	struct skb_redundant_info *sred;
 	struct hsr_ethhdr *hsr_ethhdr;
+	unsigned char *pc;
 	u16 s;
 
 	if (port_rcv->priv->prot_version > HSR_V1)
@@ -491,7 +503,7 @@ static void stripped_skb_get_shared_info(struct sk_buff *skb_stripped,
 	skb_hsr = frame->skb_hsr;
 	skb = skb_stripped;
 
-	if (is_hsr_l2ptp_evt(skb_hsr)) {
+	if (is_hsr_l2ptp_evt(skb_hsr, frame)) {
 		/* Rx timestamp */
 		skb_hwtstamps(skb)->hwtstamp = skb_hwtstamps(skb_hsr)->hwtstamp;
 		/* Cut-through tx timestamp */
@@ -499,10 +511,13 @@ static void stripped_skb_get_shared_info(struct sk_buff *skb_stripped,
 			skb_redinfo_hwtstamps(skb_hsr)->hwtstamp;
 	}
 
-	if (is_hsr_l2ptp(skb_hsr)) {
+	if (is_hsr_l2ptp(skb_hsr, frame)) {
 		sred = skb_redinfo(skb);
-		/* assumes no vlan */
-		hsr_ethhdr = (struct hsr_ethhdr *)skb_mac_header(skb_hsr);
+		pc = skb_mac_header(skb);
+		if (frame->is_vlan)
+			hsr_ethhdr = (struct hsr_ethhdr *)(pc + VLAN_HLEN);
+		else
+			hsr_ethhdr = (struct hsr_ethhdr *)pc;
 		sred->io_port = (PTP_MSG_IN | BIT(port_rcv->type - 1));
 		sred->ethertype = ntohs(hsr_ethhdr->ethhdr.h_proto);
 		s = ntohs(hsr_ethhdr->hsr_tag.path_and_LSDU_size);