1 files changed, 101 insertions, 58 deletions

diff --git a/drivers/net/hyperv/rndis_filter.c b/drivers/net/hyperv/rndis_filter.c
index 133b7fbf8595..d6be64bcefd4 100644
--- a/drivers/net/hyperv/rndis_filter.c
+++ b/drivers/net/hyperv/rndis_filter.c
@@ -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);
 }
 
@@ -352,8 +388,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 +410,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;
 	}
 
@@ -758,66 +779,88 @@ int rndis_filter_open(struct hv_device *dev)
 
 int rndis_filter_close(struct hv_device *dev)
 {
-	struct netvsc_device *netDevice = hv_get_drvdata(dev);
+	struct netvsc_device *nvdev = hv_get_drvdata(dev);
 
-	if (!netDevice)
+	if (!nvdev)
 		return -EINVAL;
 
-	return rndis_filter_close_device(netDevice->extension);
+	return rndis_filter_close_device(nvdev->extension);
 }
 
 int rndis_filter_send(struct hv_device *dev,
 			     struct hv_netvsc_packet *pkt)
 {
 	int ret;
-	struct rndis_filter_packet *filterPacket;
-	struct rndis_message *rndisMessage;
-	struct rndis_packet *rndisPacket;
-	u32 rndisMessageSize;
+	struct rndis_filter_packet *filter_pkt;
+	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 */
-	filterPacket = (struct rndis_filter_packet *)pkt->extension;
-
-	memset(filterPacket, 0, sizeof(struct rndis_filter_packet));
-
-	rndisMessage = &filterPacket->msg;
-	rndisMessageSize = RNDIS_MESSAGE_SIZE(struct rndis_packet);
-
-	rndisMessage->ndis_msg_type = REMOTE_NDIS_PACKET_MSG;
-	rndisMessage->msg_len = pkt->total_data_buflen +
-				      rndisMessageSize;
-
-	rndisPacket = &rndisMessage->msg.pkt;
-	rndisPacket->data_offset = sizeof(struct rndis_packet);
-	rndisPacket->data_len = pkt->total_data_buflen;
+	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 +
+				      rndis_msg_size;
+
+	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(rndisMessage) >> PAGE_SHIFT;
+	pkt->page_buf[0].pfn = virt_to_phys(rndis_msg) >> PAGE_SHIFT;
 	pkt->page_buf[0].offset =
-			(unsigned long)rndisMessage & (PAGE_SIZE-1);
-	pkt->page_buf[0].len = rndisMessageSize;
+			(unsigned long)rndis_msg & (PAGE_SIZE-1);
+	pkt->page_buf[0].len = rndis_msg_size;
 
 	/* Add one page_buf if the rndis msg goes beyond page boundary */
-	if (pkt->page_buf[0].offset + rndisMessageSize > PAGE_SIZE) {
+	if (pkt->page_buf[0].offset + rndis_msg_size > PAGE_SIZE) {
 		int i;
 		for (i = pkt->page_buf_cnt; i > 1; i--)
 			pkt->page_buf[i] = pkt->page_buf[i-1];
 		pkt->page_buf_cnt++;
 		pkt->page_buf[0].len = PAGE_SIZE - pkt->page_buf[0].offset;
 		pkt->page_buf[1].pfn = virt_to_phys((void *)((ulong)
-			rndisMessage + pkt->page_buf[0].len)) >> PAGE_SHIFT;
+			rndis_msg + pkt->page_buf[0].len)) >> PAGE_SHIFT;
 		pkt->page_buf[1].offset = 0;
-		pkt->page_buf[1].len = rndisMessageSize - pkt->page_buf[0].len;
+		pkt->page_buf[1].len = rndis_msg_size - pkt->page_buf[0].len;
 	}
 
 	/* Save the packet send completion and context */
-	filterPacket->completion = pkt->completion.send.send_completion;
-	filterPacket->completion_ctx =
+	filter_pkt->completion = pkt->completion.send.send_completion;
+	filter_pkt->completion_ctx =
 				pkt->completion.send.send_completion_ctx;
 
 	/* Use ours */
 	pkt->completion.send.send_completion = rndis_filter_send_completion;
-	pkt->completion.send.send_completion_ctx = filterPacket;
+	pkt->completion.send.send_completion_ctx = filter_pkt;
 
 	ret = netvsc_send(dev, pkt);
 	if (ret != 0) {
@@ -826,9 +869,9 @@ int rndis_filter_send(struct hv_device *dev,
 		 * above
 		 */
 		pkt->completion.send.send_completion =
-				filterPacket->completion;
+				filter_pkt->completion;
 		pkt->completion.send.send_completion_ctx =
-				filterPacket->completion_ctx;
+				filter_pkt->completion_ctx;
 	}
 
 	return ret;
@@ -836,10 +879,10 @@ int rndis_filter_send(struct hv_device *dev,
 
 static void rndis_filter_send_completion(void *ctx)
 {
-	struct rndis_filter_packet *filterPacket = ctx;
+	struct rndis_filter_packet *filter_pkt = ctx;
 
 	/* Pass it back to the original handler */
-	filterPacket->completion(filterPacket->completion_ctx);
+	filter_pkt->completion(filter_pkt->completion_ctx);
 }