[PATCH 1/2] net/hyperv: Fix data corruption in rndis_filter_receive()

@@ -352,8 +352,7 @@ int rndis_filter_receive(struct hv_device *dev,
 {
 	struct netvsc_device *net_dev = hv_get_drvdata(dev);
 	struct rndis_device *rndis_dev;
-	struct rndis_message rndis_msg;
-	struct rndis_message *rndis_hdr;
+	struct rndis_message *rndis_msg;
 	struct net_device *ndev;
 
 	if (!net_dev)

@@ -375,46 +374,32 @@ int rndis_filter_receive(struct hv_device *dev,
 		return -ENODEV;
 	}
 
-	rndis_hdr = pkt->data;
-
-	/* Make sure we got a valid rndis message */
-	if ((rndis_hdr->ndis_msg_type != REMOTE_NDIS_PACKET_MSG) &&
-	    (rndis_hdr->msg_len > sizeof(struct rndis_message))) {
-		netdev_err(ndev, "incoming rndis message buffer overflow "
-			   "detected (got %u, max %zu)..marking it an error!\n",
-			   rndis_hdr->msg_len,
-			   sizeof(struct rndis_message));
-	}
+	rndis_msg = pkt->data;
 
-	memcpy(&rndis_msg, rndis_hdr,
-		(rndis_hdr->msg_len > sizeof(struct rndis_message)) ?
-			sizeof(struct rndis_message) :
-			rndis_hdr->msg_len);
+	dump_rndis_message(dev, rndis_msg);
 
-	dump_rndis_message(dev, &rndis_msg);
-
-	switch (rndis_msg.ndis_msg_type) {
+	switch (rndis_msg->ndis_msg_type) {
 	case REMOTE_NDIS_PACKET_MSG:
 		/* data msg */
-		rndis_filter_receive_data(rndis_dev, &rndis_msg, pkt);
+		rndis_filter_receive_data(rndis_dev, rndis_msg, pkt);
 		break;
 
 	case REMOTE_NDIS_INITIALIZE_CMPLT:
 	case REMOTE_NDIS_QUERY_CMPLT:
 	case REMOTE_NDIS_SET_CMPLT:
 		/* completion msgs */
-		rndis_filter_receive_response(rndis_dev, &rndis_msg);
+		rndis_filter_receive_response(rndis_dev, rndis_msg);
 		break;
 
 	case REMOTE_NDIS_INDICATE_STATUS_MSG:
 		/* notification msgs */
-		rndis_filter_receive_indicate_status(rndis_dev, &rndis_msg);
+		rndis_filter_receive_indicate_status(rndis_dev, rndis_msg);
 		break;
 	default:
 		netdev_err(ndev,
 			"unhandled rndis message (type %u len %u)\n",
-			   rndis_msg.ndis_msg_type,
-			   rndis_msg.msg_len);
+			   rndis_msg->ndis_msg_type,
+			   rndis_msg->msg_len);
 		break;
 	}
 

@@ -49,6 +49,7 @@ struct hv_netvsc_packet {
 
 	struct hv_device *device;
 	bool is_data_pkt;
+	u16 vlan_tci;
 
 	/*
 	 * Valid only for receives when we break a xfer page packet

@@ -926,9 +927,40 @@ struct rndis_oobd {
 struct rndis_per_packet_info {
 	u32 size;
 	u32 type;
-	u32 per_pkt_info_offset;
+	u32 ppi_offset;
+};
+
+enum ndis_per_pkt_info_type {
+	TCPIP_CHKSUM_PKTINFO,
+	IPSEC_PKTINFO,
+	TCP_LARGESEND_PKTINFO,
+	CLASSIFICATION_HANDLE_PKTINFO,
+	NDIS_RESERVED,
+	SG_LIST_PKTINFO,
+	IEEE_8021Q_INFO,
+	ORIGINAL_PKTINFO,
+	PACKET_CANCEL_ID,
+	ORIGINAL_NET_BUFLIST,
+	CACHED_NET_BUFLIST,
+	SHORT_PKT_PADINFO,
+	MAX_PER_PKT_INFO
+};
+
+struct ndis_pkt_8021q_info {
+	union {
+		struct {
+			u32 pri:3; /* User Priority */
+			u32 cfi:1; /* Canonical Format ID */
+			u32 vlanid:12; /* VLAN ID */
+			u32 reserved:16;
+		};
+		u32 value;
+	};
 };
 
+#define NDIS_VLAN_PPI_SIZE (sizeof(struct rndis_per_packet_info) + \
+		sizeof(struct ndis_pkt_8021q_info))
+
 /* Format of Information buffer passed in a SetRequest for the OID */
 /* OID_GEN_RNDIS_CONFIG_PARAMETER. */
 struct rndis_config_parameter_info {

@@ -300,6 +300,7 @@ static int negotiate_nvsp_ver(struct hv_device *device,
 	memset(init_packet, 0, sizeof(struct nvsp_message));
 	init_packet->hdr.msg_type = NVSP_MSG2_TYPE_SEND_NDIS_CONFIG;
 	init_packet->msg.v2_msg.send_ndis_config.mtu = net_device->ndev->mtu;
+	init_packet->msg.v2_msg.send_ndis_config.capability.ieee8021q = 1;
 
 	ret = vmbus_sendpacket(device->channel, init_packet,
 				sizeof(struct nvsp_message),

@@ -341,7 +342,7 @@ static int netvsc_connect_vsp(struct hv_device *device)
 	/* Send the ndis version */
 	memset(init_packet, 0, sizeof(struct nvsp_message));
 
-	ndis_version = 0x00050000;
+	ndis_version = 0x00050001;
 
 	init_packet->hdr.msg_type = NVSP_MSG1_TYPE_SEND_NDIS_VER;
 	init_packet->msg.v1_msg.

@@ -159,7 +159,8 @@ static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
 	/* Allocate a netvsc packet based on # of frags. */
 	packet = kzalloc(sizeof(struct hv_netvsc_packet) +
 			 (num_pages * sizeof(struct hv_page_buffer)) +
-			 sizeof(struct rndis_filter_packet), GFP_ATOMIC);
+			 sizeof(struct rndis_filter_packet) +
+			 NDIS_VLAN_PPI_SIZE, GFP_ATOMIC);
 	if (!packet) {
 		/* out of memory, drop packet */
 		netdev_err(net, "unable to allocate hv_netvsc_packet\n");

@@ -169,6 +170,8 @@ static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
 		return NETDEV_TX_BUSY;
 	}
 
+	packet->vlan_tci = skb->vlan_tci;
+
 	packet->extension = (void *)(unsigned long)packet +
 				sizeof(struct hv_netvsc_packet) +
 				    (num_pages * sizeof(struct hv_page_buffer));

@@ -293,6 +296,7 @@ int netvsc_recv_callback(struct hv_device *device_obj,
 
 	skb->protocol = eth_type_trans(skb, net);
 	skb->ip_summed = CHECKSUM_NONE;
+	skb->vlan_tci = packet->vlan_tci;
 
 	net->stats.rx_packets++;
 	net->stats.rx_bytes += packet->total_data_buflen;

@@ -407,7 +411,7 @@ static int netvsc_probe(struct hv_device *dev,
 
 	/* TODO: Add GSO and Checksum offload */
 	net->hw_features = NETIF_F_SG;
-	net->features = NETIF_F_SG;
+	net->features = NETIF_F_SG | NETIF_F_HW_VLAN_TX;
 
 	SET_ETHTOOL_OPS(net, &ethtool_ops);
 	SET_NETDEV_DEV(net, &dev->device);

@@ -26,6 +26,7 @@
 #include <linux/io.h>
 #include <linux/if_ether.h>
 #include <linux/netdevice.h>
+#include <linux/if_vlan.h>
 
 #include "hyperv_net.h"
 

@@ -303,12 +304,39 @@ static void rndis_filter_receive_indicate_status(struct rndis_device *dev,
 	}
 }
 
+/*
+ * Get the Per-Packet-Info with the specified type
+ * return NULL if not found.
+ */
+static inline void *rndis_get_ppi(struct rndis_packet *rpkt, u32 type)
+{
+	struct rndis_per_packet_info *ppi;
+	int len;
+
+	if (rpkt->per_pkt_info_offset == 0)
+		return NULL;
+
+	ppi = (struct rndis_per_packet_info *)((ulong)rpkt +
+		rpkt->per_pkt_info_offset);
+	len = rpkt->per_pkt_info_len;
+
+	while (len > 0) {
+		if (ppi->type == type)
+			return (void *)((ulong)ppi + ppi->ppi_offset);
+		len -= ppi->size;
+		ppi = (struct rndis_per_packet_info *)((ulong)ppi + ppi->size);
+	}
+
+	return NULL;
+}
+
 static void rndis_filter_receive_data(struct rndis_device *dev,
 				   struct rndis_message *msg,
 				   struct hv_netvsc_packet *pkt)
 {
 	struct rndis_packet *rndis_pkt;
 	u32 data_offset;
+	struct ndis_pkt_8021q_info *vlan;
 
 	rndis_pkt = &msg->msg.pkt;
 

@@ -344,6 +372,14 @@ static void rndis_filter_receive_data(struct rndis_device *dev,
 
 	pkt->is_data_pkt = true;
 
+	vlan = rndis_get_ppi(rndis_pkt, IEEE_8021Q_INFO);
+	if (vlan) {
+		pkt->vlan_tci = VLAN_TAG_PRESENT | vlan->vlanid |
+			(vlan->pri << VLAN_PRIO_SHIFT);
+	} else {
+		pkt->vlan_tci = 0;
+	}
+
 	netvsc_recv_callback(dev->net_dev->dev, pkt);
 }
 

@@ -759,12 +795,15 @@ int rndis_filter_send(struct hv_device *dev,
 	struct rndis_message *rndis_msg;
 	struct rndis_packet *rndis_pkt;
 	u32 rndis_msg_size;
+	bool isvlan = pkt->vlan_tci & VLAN_TAG_PRESENT;
 
 	/* Add the rndis header */
 	filter_pkt = (struct rndis_filter_packet *)pkt->extension;
 
 	rndis_msg = &filter_pkt->msg;
 	rndis_msg_size = RNDIS_MESSAGE_SIZE(struct rndis_packet);
+	if (isvlan)
+		rndis_msg_size += NDIS_VLAN_PPI_SIZE;
 
 	rndis_msg->ndis_msg_type = REMOTE_NDIS_PACKET_MSG;
 	rndis_msg->msg_len = pkt->total_data_buflen +

@@ -772,8 +811,29 @@ int rndis_filter_send(struct hv_device *dev,
 
 	rndis_pkt = &rndis_msg->msg.pkt;
 	rndis_pkt->data_offset = sizeof(struct rndis_packet);
+	if (isvlan)
+		rndis_pkt->data_offset += NDIS_VLAN_PPI_SIZE;
 	rndis_pkt->data_len = pkt->total_data_buflen;
 
+	if (isvlan) {
+		struct rndis_per_packet_info *ppi;
+		struct ndis_pkt_8021q_info *vlan;
+
+		rndis_pkt->per_pkt_info_offset = sizeof(struct rndis_packet);
+		rndis_pkt->per_pkt_info_len = NDIS_VLAN_PPI_SIZE;
+
+		ppi = (struct rndis_per_packet_info *)((ulong)rndis_pkt +
+			rndis_pkt->per_pkt_info_offset);
+		ppi->size = NDIS_VLAN_PPI_SIZE;
+		ppi->type = IEEE_8021Q_INFO;
+		ppi->ppi_offset = sizeof(struct rndis_per_packet_info);
+
+		vlan = (struct ndis_pkt_8021q_info *)((ulong)ppi +
+			ppi->ppi_offset);
+		vlan->vlanid = pkt->vlan_tci & VLAN_VID_MASK;
+		vlan->pri = (pkt->vlan_tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
+	}
+
 	pkt->is_data_pkt = true;
 	pkt->page_buf[0].pfn = virt_to_phys(rndis_msg) >> PAGE_SHIFT;
 	pkt->page_buf[0].offset =