From ed8e12580647f65f017380690aab484677c6a19b Mon Sep 17 00:00:00 2001
From: hsluoyz <hsluoyz@qq.com>
Date: Sat, 22 Aug 2015 13:47:29 +0800
Subject: [PATCH] Finished the DLT_NULL protocol support. But there's a problem
 that Wireshark didn't parse the loopback packets right, need fix.

---
 packetWin7/npf/npf/Lo_send.c  | 44 ++++++++++++++++++++++++-----------
 packetWin7/npf/npf/Loopback.c | 18 ++++++++++----
 packetWin7/npf/npf/Loopback.h | 19 +++++++++++++++
 3 files changed, 63 insertions(+), 18 deletions(-)

diff --git a/packetWin7/npf/npf/Lo_send.c b/packetWin7/npf/npf/Lo_send.c
index 6524d9ec..4331e3da 100644
--- a/packetWin7/npf/npf/Lo_send.c
+++ b/packetWin7/npf/npf/Lo_send.c
@@ -56,6 +56,8 @@
 #define HTON_LONG(x)					(((((x)& 0xff)<<24) | ((x)>>24) & 0xff) | \
 										(((x) & 0xff0000)>>8) | (((x) & 0xff00)<<8))
 
+extern ULONG					g_DltNullMode;
+
 static WSK_REGISTRATION         g_WskRegistration;
 static WSK_PROVIDER_NPI         g_WskProvider;
 static WSK_CLIENT_DISPATCH      g_WskDispatch = { MAKE_WSK_VERSION(1, 0), 0, NULL };
@@ -246,25 +248,41 @@ NPF_WSKSendPacket(
 	IN ULONG BuffSize
 	)
 {
-	PETHER_HEADER	pEthernetHdr = (PETHER_HEADER) PacketBuff;
-	NTSTATUS		status = STATUS_UNSUCCESSFUL;
+	PETHER_HEADER		pEthernetHdr = (PETHER_HEADER) PacketBuff;
+	PDLT_NULL_HEADER	pDltNullHdr = (PDLT_NULL_HEADER) PacketBuff;
+	NTSTATUS			status = STATUS_UNSUCCESSFUL;
 
 	TRACE_ENTER();
-
-	PacketBuff = PacketBuff + ETHER_HDR_LEN;
-	BuffSize = BuffSize - ETHER_HDR_LEN;
 	
-	if (pEthernetHdr->ether_type == RtlUshortByteSwap(ETHERTYPE_IP))
-	{
-		status = WSKSendPacketInternal(NPF_LOOPBACK_SEND_TYPE_IPV4, PacketBuff, BuffSize);
-	}
-	else if (pEthernetHdr->ether_type == RtlUshortByteSwap(ETHERTYPE_IPV6))
-	{
-		status = WSKSendPacketInternal(NPF_LOOPBACK_SEND_TYPE_IPV6, PacketBuff, BuffSize);
+	if (g_DltNullMode)
+	{
+		if (pDltNullHdr->null_type == DLTNULLTYPE_IP)
+		{
+			status = WSKSendPacketInternal(NPF_LOOPBACK_SEND_TYPE_IPV4, PacketBuff + DLT_NULL_HDR_LEN, BuffSize - DLT_NULL_HDR_LEN);
+		}
+		else if (pDltNullHdr->null_type == DLTNULLTYPE_IPV6)
+		{
+			status = WSKSendPacketInternal(NPF_LOOPBACK_SEND_TYPE_IPV6, PacketBuff + DLT_NULL_HDR_LEN, BuffSize - DLT_NULL_HDR_LEN);
+		}
+		else
+		{
+			TRACE_MESSAGE1(PACKET_DEBUG_LOUD, "NPF_WSKSendPacket() failed with status 0x%08X, not valid loopback IPv4 or IPv6 packet (DLT_NULL)\n", status);
+		}
 	}
 	else
 	{
-		TRACE_MESSAGE1(PACKET_DEBUG_LOUD, "NPF_WSKSendPacket() failed with status 0x%08X, not valid loopback IPv4 or IPv6 packet\n", status);
+		if (pEthernetHdr->ether_type == RtlUshortByteSwap(ETHERTYPE_IP))
+		{
+			status = WSKSendPacketInternal(NPF_LOOPBACK_SEND_TYPE_IPV4, PacketBuff + ETHER_HDR_LEN, BuffSize - ETHER_HDR_LEN);
+		}
+		else if (pEthernetHdr->ether_type == RtlUshortByteSwap(ETHERTYPE_IPV6))
+		{
+			status = WSKSendPacketInternal(NPF_LOOPBACK_SEND_TYPE_IPV6, PacketBuff + ETHER_HDR_LEN, BuffSize - ETHER_HDR_LEN);
+		}
+		else
+		{
+			TRACE_MESSAGE1(PACKET_DEBUG_LOUD, "NPF_WSKSendPacket() failed with status 0x%08X, not valid loopback IPv4 or IPv6 packet (Ethernet)\n", status);
+		}
 	}
 
 	TRACE_EXIT();
diff --git a/packetWin7/npf/npf/Loopback.c b/packetWin7/npf/npf/Loopback.c
index 81d54e17..75873b50 100644
--- a/packetWin7/npf/npf/Loopback.c
+++ b/packetWin7/npf/npf/Loopback.c
@@ -52,6 +52,7 @@
 // Global variables
 //
 extern POPEN_INSTANCE g_LoopbackOpenGroupHead; // Loopback adapter open_instance group head, this pointer points to one item in g_arrOpen list.
+extern ULONG g_DltNullMode;
 
 // 
 // Callout and sublayer GUIDs
@@ -240,7 +241,7 @@ NPF_NetworkClassify(
 	INT32				iIPv4 = -1;
 	INT32				iDrection = -1;
 	BOOLEAN				bSelfSent = FALSE;
-	PETHER_HEADER		pContiguousData = NULL;
+	PVOID				pContiguousData = NULL;
 	NET_BUFFER*			pNetBuffer = 0;
 	UCHAR				pPacketData[ETHER_HDR_LEN];
 	PNET_BUFFER_LIST	pNetBufferList = (NET_BUFFER_LIST*) layerData;
@@ -393,7 +394,7 @@ NPF_NetworkClassify(
 		goto Exit_WSK_IP_Retreated;
 	}
 
-	bytesRetreatedEthernet = ETHER_HDR_LEN;
+	bytesRetreatedEthernet = g_DltNullMode ? DLT_NULL_HDR_LEN : ETHER_HDR_LEN;
 	status = NdisRetreatNetBufferListDataStart(pClonedNetBufferList,
 		bytesRetreatedEthernet,
 		0,
@@ -414,7 +415,7 @@ NPF_NetworkClassify(
 	while (pNetBuffer)
 	{
 		pContiguousData = NdisGetDataBuffer(pNetBuffer,
-			ETHER_HDR_LEN,
+			bytesRetreatedEthernet,
 			pPacketData,
 			1,
 			0);
@@ -430,8 +431,15 @@ NPF_NetworkClassify(
 		}
 		else
 		{
-			RtlZeroMemory(pContiguousData, ETHER_ADDR_LEN * 2);
-			pContiguousData->ether_type = iIPv4 ? RtlUshortByteSwap(ETHERTYPE_IP) : RtlUshortByteSwap(ETHERTYPE_IPV6);
+			if (g_DltNullMode)
+			{
+				((PDLT_NULL_HEADER) pContiguousData)->null_type = iIPv4 ? DLTNULLTYPE_IP : DLTNULLTYPE_IPV6;
+			}
+			else
+			{
+				RtlZeroMemory(pContiguousData, ETHER_ADDR_LEN * 2);
+				((PETHER_HEADER) pContiguousData)->ether_type = iIPv4 ? RtlUshortByteSwap(ETHERTYPE_IP) : RtlUshortByteSwap(ETHERTYPE_IPV6);
+			}
 		}
 
 		pNetBuffer = pNetBuffer->Next;
diff --git a/packetWin7/npf/npf/Loopback.h b/packetWin7/npf/npf/Loopback.h
index 35936bd2..2ed03802 100644
--- a/packetWin7/npf/npf/Loopback.h
+++ b/packetWin7/npf/npf/Loopback.h
@@ -104,6 +104,25 @@ typedef struct _ETHER_HEADER
 #define ETHERTYPE_IPV6		0x86dd	/* IPv6 */
 #define	ETHERTYPE_LOOPBACK	0x9000	/* used to test interfaces */
 
+/*
+* Structure of a DLT_NULL header.
+*/
+typedef struct _DLT_NULL_HEADER
+{
+	UINT	null_type;
+} DLT_NULL_HEADER, *PDLT_NULL_HEADER;
+
+/*
+* The length of the combined header.
+*/
+#define	DLT_NULL_HDR_LEN	sizeof(DLT_NULL_HEADER)
+
+/*
+* Types in a DLT_NULL (Loopback) header.
+*/
+#define	DLTNULLTYPE_IP		0x0002	/* IP protocol */
+#define	DLTNULLTYPE_IPV6	0x0017	/* IPv6 */
+
 /*
 * Structure of a IPv4 header, based on netinet/ip.h
 * http://openhip.sourceforge.net/doxygen/ip_8h_source.html