diff options
Diffstat (limited to 'src/Wfp/Wfp.c')
| -rw-r--r-- | src/Wfp/Wfp.c | 1282 |
1 files changed, 1282 insertions, 0 deletions
diff --git a/src/Wfp/Wfp.c b/src/Wfp/Wfp.c new file mode 100644 index 00000000..5ef8c1d3 --- /dev/null +++ b/src/Wfp/Wfp.c @@ -0,0 +1,1282 @@ +// SoftEther VPN Source Code
+// Windows Filtering Platform Callout Driver for Capturing IPsec Packets on Windows Vista / 7 / Server 2008
+//
+// SoftEther VPN Server, Client and Bridge are free software under GPLv2.
+//
+// Copyright (c) 2012-2014 Daiyuu Nobori.
+// Copyright (c) 2012-2014 SoftEther VPN Project, University of Tsukuba, Japan.
+// Copyright (c) 2012-2014 SoftEther Corporation.
+//
+// All Rights Reserved.
+//
+// http://www.softether.org/
+//
+// Author: Daiyuu Nobori
+// Comments: Tetsuo Sugiyama, Ph.D.
+//
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 2 as published by the Free Software Foundation.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License version 2
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+// IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+// CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+// TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+//
+// THE LICENSE AGREEMENT IS ATTACHED ON THE SOURCE-CODE PACKAGE
+// AS "LICENSE.TXT" FILE. READ THE TEXT FILE IN ADVANCE TO USE THE SOFTWARE.
+//
+//
+// THIS SOFTWARE IS DEVELOPED IN JAPAN, AND DISTRIBUTED FROM JAPAN,
+// UNDER JAPANESE LAWS. YOU MUST AGREE IN ADVANCE TO USE, COPY, MODIFY,
+// MERGE, PUBLISH, DISTRIBUTE, SUBLICENSE, AND/OR SELL COPIES OF THIS
+// SOFTWARE, THAT ANY JURIDICAL DISPUTES WHICH ARE CONCERNED TO THIS
+// SOFTWARE OR ITS CONTENTS, AGAINST US (SOFTETHER PROJECT, SOFTETHER
+// CORPORATION, DAIYUU NOBORI OR OTHER SUPPLIERS), OR ANY JURIDICAL
+// DISPUTES AGAINST US WHICH ARE CAUSED BY ANY KIND OF USING, COPYING,
+// MODIFYING, MERGING, PUBLISHING, DISTRIBUTING, SUBLICENSING, AND/OR
+// SELLING COPIES OF THIS SOFTWARE SHALL BE REGARDED AS BE CONSTRUED AND
+// CONTROLLED BY JAPANESE LAWS, AND YOU MUST FURTHER CONSENT TO
+// EXCLUSIVE JURISDICTION AND VENUE IN THE COURTS SITTING IN TOKYO,
+// JAPAN. YOU MUST WAIVE ALL DEFENSES OF LACK OF PERSONAL JURISDICTION
+// AND FORUM NON CONVENIENS. PROCESS MAY BE SERVED ON EITHER PARTY IN
+// THE MANNER AUTHORIZED BY APPLICABLE LAW OR COURT RULE.
+//
+// USE ONLY IN JAPAN. DO NOT USE IT IN OTHER COUNTRIES. IMPORTING THIS
+// SOFTWARE INTO OTHER COUNTRIES IS AT YOUR OWN RISK. SOME COUNTRIES
+// PROHIBIT ENCRYPTED COMMUNICATIONS. USING THIS SOFTWARE IN OTHER
+// COUNTRIES MIGHT BE RESTRICTED.
+//
+//
+// DEAR SECURITY EXPERTS
+// ---------------------
+//
+// If you find a bug or a security vulnerability please kindly inform us
+// about the problem immediately so that we can fix the security problem
+// to protect a lot of users around the world as soon as possible.
+//
+// Our e-mail address for security reports is:
+// softether-vpn-security [at] softether.org
+//
+// Please note that the above e-mail address is not a technical support
+// inquiry address. If you need technical assistance, please visit
+// http://www.softether.org/ and ask your question on the users forum.
+//
+// Thank you for your cooperation.
+
+
+// Wfp.c
+// WFP Callout Driver
+
+#include <GlobalConst.h>
+
+#define WFP_DEVICE_DRIVER
+
+#include "WfpInner.h"
+#include "Wfp.h"
+
+static WFP_CTX *wfp = NULL;
+
+// Dispatch function
+NTSTATUS DriverDispatch(DEVICE_OBJECT *device_object, IRP *irp)
+{
+ NTSTATUS ret = STATUS_SUCCESS;
+ IO_STACK_LOCATION *stack;
+ void *buf;
+ bool ok;
+ // Validate arguments
+ if (wfp == NULL || device_object == NULL || irp == NULL || wfp->Halting)
+ {
+ return NDIS_STATUS_FAILURE;
+ }
+
+ // Get the IRP stack
+ stack = IoGetCurrentIrpStackLocation(irp);
+
+ // Initialize the number of bytes
+ irp->IoStatus.Information = 0;
+ irp->IoStatus.Status = STATUS_SUCCESS;
+
+ buf = irp->UserBuffer;
+
+ if (wfp->Halting != FALSE)
+ {
+ // Device driver is terminating
+ irp->IoStatus.Information = STATUS_UNSUCCESSFUL;
+ IoCompleteRequest(irp, IO_NO_INCREMENT);
+ return STATUS_UNSUCCESSFUL;
+ }
+
+ ok = false;
+
+ // Branch to each operation
+ switch (stack->MajorFunction)
+ {
+ case IRP_MJ_CREATE: // Open
+ ok = true;
+ break;
+
+ case IRP_MJ_CLOSE: // Close
+ ok = true;
+ break;
+
+ case IRP_MJ_READ: // Read
+ ResetEvent(wfp->Event);
+ break;
+
+ case IRP_MJ_WRITE: // Write
+ if ((stack->Parameters.Write.Length % sizeof(WFP_LOCAL_IP)) == 0)
+ {
+ UINT size = MIN(WFP_MAX_LOCAL_IP_COUNT * sizeof(WFP_LOCAL_IP), stack->Parameters.Write.Length);
+ UCHAR *copied_buf = Malloc(size);
+ UCHAR *old_buf;
+ Copy(copied_buf, buf, size);
+
+ SpinLock(wfp->LocalIPListLock);
+ {
+ old_buf = wfp->LocalIPListData;
+ wfp->LocalIPListData = copied_buf;
+ wfp->LocalIPListSize = size;
+ }
+ SpinUnlock(wfp->LocalIPListLock);
+
+ if (old_buf != NULL)
+ {
+ Free(old_buf);
+ }
+ }
+ irp->IoStatus.Information = stack->Parameters.Write.Length;
+ ok = true;
+ break;
+ }
+
+ if (ok == false)
+ {
+ irp->IoStatus.Status = STATUS_UNSUCCESSFUL;
+ ret = STATUS_UNSUCCESSFUL;
+ }
+
+ IoCompleteRequest(irp, IO_NO_INCREMENT);
+
+ return ret;
+}
+
+// Function to finish the insertion of the packet
+void NTAPI CalloutInjectionCompleted(void *context, NET_BUFFER_LIST *net_buffer_list, BOOLEAN dispatch_level)
+{
+ WFP_INJECTED_PACKET_CONTEXT *ctx = (WFP_INJECTED_PACKET_CONTEXT *)context;
+
+ if (ctx == NULL)
+ {
+ return;
+ }
+
+ FreeInjectionCtx(ctx);
+}
+
+// Release the injection data
+void FreeInjectionCtx(WFP_INJECTED_PACKET_CONTEXT *ctx)
+{
+ // Validate arguments
+ if (ctx == NULL)
+ {
+ return;
+ }
+
+ if (ctx->CurrentNetBuffer != NULL)
+ {
+ Copy(ctx->CurrentNetBuffer, &ctx->OriginalNetBufferData, sizeof(NET_BUFFER));
+ }
+
+ if (ctx->AllocatedNetBufferList != NULL)
+ {
+ FwpsFreeCloneNetBufferList0(ctx->AllocatedNetBufferList, 0);
+ }
+
+ if (ctx->AllocatedMdl != NULL)
+ {
+ NdisFreeMdl(ctx->AllocatedMdl);
+ }
+
+ if (ctx->AllocatedMemory != NULL)
+ {
+ Free(ctx->AllocatedMemory);
+ }
+
+ Free(ctx);
+}
+
+// Calculate the checksum
+USHORT IpChecksum(void *buf, UINT size)
+{
+ int sum = 0;
+ USHORT *addr = (USHORT *)buf;
+ int len = (int)size;
+ USHORT *w = addr;
+ int nleft = len;
+ USHORT answer = 0;
+
+ while (nleft > 1)
+ {
+ sum += *w++;
+ nleft -= 2;
+ }
+
+ if (nleft == 1)
+ {
+ *(UCHAR *)(&answer) = *(UCHAR *)w;
+ sum += answer;
+ }
+
+ sum = (sum >> 16) + (sum & 0xffff);
+ sum += (sum >> 16);
+
+ answer = ~sum;
+
+ return answer;
+}
+
+// Modify the IPsec ESP packet
+UCHAR *ModificationOfIPsecESPPacket(UCHAR *ip_packet, UINT ip_packet_size, UINT ip_header_size, UINT *dst_size_ptr, bool isv6)
+{
+ UINT ip_and_udp_header_size = ip_header_size + sizeof(WFP_UDP_HEADER);
+ UINT udp_packet_size;
+ UINT udp_payload_size;
+ UINT dst_udp_payload_size;
+ WFP_UDP_HEADER *src_udp;
+ WFP_UDP_HEADER *dst_udp;
+ UCHAR *src_udp_payload;
+ UCHAR *dst_data;
+ UINT dst_size;
+ // Validate arguments
+ if (ip_packet == NULL || ip_packet == 0 || ip_header_size == 0 || dst_size_ptr == NULL)
+ {
+ return NULL;
+ }
+
+ if (ip_packet_size <= ip_and_udp_header_size)
+ {
+ // There is no UDP header
+ return NULL;
+ }
+
+ // Get the UDP header
+ src_udp = (WFP_UDP_HEADER *)(ip_packet + ip_header_size);
+ udp_packet_size = Endian16(src_udp->PacketLength);
+ if (udp_packet_size < sizeof(WFP_UDP_HEADER))
+ {
+ // There is no UDP payload
+ return NULL;
+ }
+
+ // Get the UDP payload size
+ udp_payload_size = udp_packet_size - sizeof(WFP_UDP_HEADER);
+
+ if (ip_packet_size < (ip_and_udp_header_size + udp_payload_size))
+ {
+ // There is no UDP payload
+ return NULL;
+ }
+
+ // Get the UDP payload
+ src_udp_payload = ip_packet + ip_and_udp_header_size;
+
+ if (udp_payload_size < sizeof(UINT))
+ {
+ // The size of the UDP payload is less than 5 bytes
+ return NULL;
+ }
+
+ dst_udp_payload_size = udp_payload_size + sizeof(UINT) * 3;
+ if ((dst_udp_payload_size + sizeof(WFP_UDP_HEADER)) > 0xffff)
+ {
+ // UDP payload size overflows the 16bit
+ return NULL;
+ }
+
+ // Build a new packet
+ dst_size = ip_and_udp_header_size + sizeof(UINT) * 3 + udp_payload_size;
+ if (dst_size > 0xffff)
+ {
+ // IP total size overflows the 16bit
+ return NULL;
+ }
+
+ dst_data = Malloc(dst_size);
+ if (dst_data == NULL)
+ {
+ // Memory allocation failure
+ return NULL;
+ }
+
+ // Copy the IP header + UDP header
+ Copy(dst_data, ip_packet, ip_and_udp_header_size);
+
+ // Copy the original payload
+ Copy(dst_data + ip_and_udp_header_size + sizeof(UINT) * 3, src_udp_payload, udp_payload_size);
+
+ // Insert a tag
+ *((UINT *)(dst_data + ip_and_udp_header_size + sizeof(UINT) * 0)) = 0;
+ *((UINT *)(dst_data + ip_and_udp_header_size + sizeof(UINT) * 1)) = WFP_ESP_PACKET_TAG_1;
+ *((UINT *)(dst_data + ip_and_udp_header_size + sizeof(UINT) * 2)) = WFP_ESP_PACKET_TAG_2;
+
+ // Adjust the new IP header
+ if (isv6 == false)
+ {
+ WFP_IPV4_HEADER *ip = (WFP_IPV4_HEADER *)dst_data;
+
+ ip->TotalLength = Endian16(dst_size);
+ ip->Checksum = 0;
+ ip->Checksum = IpChecksum(ip, ip_header_size);
+ }
+ else
+ {
+ WFP_IPV6_HEADER *ip = (WFP_IPV6_HEADER *)dst_data;
+
+ ip->PayloadLength = Endian16(dst_size);
+ }
+
+ // Adjust the new UDP header
+ dst_udp = (WFP_UDP_HEADER *)(dst_data + ip_header_size);
+ dst_udp->Checksum = 0;
+ dst_udp->PacketLength = Endian16((USHORT)(dst_udp_payload_size + sizeof(WFP_UDP_HEADER)));
+
+ *dst_size_ptr = dst_size;
+ return dst_data;
+}
+
+// Insert the packet into the stack
+bool InjectPacket(HANDLE hInjection, NET_BUFFER_LIST *nbl, UCHAR *dst_data, UINT dst_size, const FWPS_INCOMING_VALUES0* inFixedValues, const FWPS_INCOMING_METADATA_VALUES0* inMetaValues)
+{
+ WFP_INJECTED_PACKET_CONTEXT *ctx;
+ bool block = false;
+ // Validate arguments
+ if (hInjection == NULL || nbl == NULL || dst_data == NULL || dst_size == 0 || inMetaValues == NULL || inFixedValues == NULL)
+ {
+ return false;
+ }
+
+ ctx = ZeroMalloc(sizeof(WFP_INJECTED_PACKET_CONTEXT));
+
+ if (ctx != NULL)
+ {
+ // Generate a modified packet
+ ctx->AllocatedMemory = dst_data;
+
+ if (dst_data != NULL)
+ {
+ NET_BUFFER_LIST *net_buffer_list;
+ NTSTATUS ret;
+
+ // Clone the original NET_BUFFER_LIST
+ ret = FwpsAllocateCloneNetBufferList0(nbl, NULL, NULL, 0, &net_buffer_list);
+
+ if (OK(ret) && net_buffer_list != NULL)
+ {
+ NET_BUFFER *net_buffer = NET_BUFFER_LIST_FIRST_NB(net_buffer_list);
+
+ ctx->AllocatedNetBufferList = net_buffer_list;
+ ctx->CurrentNetBuffer = net_buffer;
+
+ if (net_buffer != NULL)
+ {
+ MDL *mdl = NdisAllocateMdl(wfp->hNdis, dst_data, dst_size);
+ if (mdl != NULL)
+ {
+ NTSTATUS ret;
+
+ ctx->AllocatedMdl = mdl;
+
+ Copy(&ctx->OriginalNetBufferData, net_buffer, sizeof(NET_BUFFER));
+
+ NET_BUFFER_FIRST_MDL(net_buffer) = mdl;
+ NET_BUFFER_DATA_LENGTH(net_buffer) = dst_size;
+ NET_BUFFER_DATA_OFFSET(net_buffer) = 0;
+ NET_BUFFER_CURRENT_MDL(net_buffer) = mdl;
+ NET_BUFFER_CURRENT_MDL_OFFSET(net_buffer) = 0;
+
+ // Insert packets of receiving direction
+ ret = FwpsInjectNetworkReceiveAsync0(hInjection, NULL,
+ 0,
+ (inMetaValues->currentMetadataValues & FWPS_METADATA_FIELD_COMPARTMENT_ID ? inMetaValues->compartmentId : UNSPECIFIED_COMPARTMENT_ID),
+ inFixedValues->incomingValue[FWPS_FIELD_INBOUND_IPPACKET_V4_INTERFACE_INDEX].value.uint32,
+ inFixedValues->incomingValue[FWPS_FIELD_INBOUND_IPPACKET_V4_SUB_INTERFACE_INDEX].value.uint32,
+ net_buffer_list,
+ CalloutInjectionCompleted,
+ (HANDLE)ctx);
+
+ if (NG(ret))
+ {
+ //CRUSH_WHERE;
+ }
+ else
+ {
+ block = true;
+ }
+ }
+ else
+ {
+ //CRUSH_WHERE;
+ }
+ }
+ else
+ {
+ //CRUSH_WHERE;
+ }
+ }
+ else
+ {
+ //CRUSH_WHERE;
+ }
+ }
+ else
+ {
+ //CRUSH_WHERE;
+ }
+
+ if (block == false)
+ {
+ FreeInjectionCtx(ctx);
+ }
+ }
+
+ return block;
+}
+
+// Function to be notified of arriving packet
+void NTAPI CalloutClassify(const FWPS_INCOMING_VALUES0* inFixedValues,
+ const FWPS_INCOMING_METADATA_VALUES0* inMetaValues,
+ void* layerData,
+ const FWPS_FILTER0* filter,
+ UINT64 flowContext,
+ FWPS_CLASSIFY_OUT0* classifyOut)
+{
+ NET_BUFFER_LIST *nbl = layerData;
+ FWPS_PACKET_INJECTION_STATE injecton_state;
+ bool block = false;
+ HANDLE hInjection = NULL;
+ UINT ip_header_len = 0;
+ bool isv6 = false;
+
+ if (wfp->Halting || nbl == NULL)
+ {
+ classifyOut->actionType = FWP_ACTION_CONTINUE;
+ return;
+ }
+
+ switch (inFixedValues->layerId)
+ {
+ case FWPS_LAYER_INBOUND_IPPACKET_V4:
+ hInjection = wfp->hInjectionIPv4;
+ ip_header_len = sizeof(WFP_IPV4_HEADER);
+ break;
+
+ case FWPS_LAYER_INBOUND_IPPACKET_V6:
+ hInjection = wfp->hInjectionIPv6;
+ ip_header_len = sizeof(WFP_IPV6_HEADER);
+ isv6 = true;
+ break;
+ }
+
+ if (hInjection != NULL)
+ {
+ injecton_state = FwpsQueryPacketInjectionState0(hInjection, nbl, NULL);
+
+ if (injecton_state == FWPS_PACKET_INJECTED_BY_SELF || injecton_state == FWPS_PACKET_PREVIOUSLY_INJECTED_BY_SELF)
+ {
+ //SetEvent(wfp->Event);
+ classifyOut->actionType = FWP_ACTION_CONTINUE; // continue
+ return;
+ }
+
+ if (nbl != NULL && NET_BUFFER_LIST_NEXT_NBL(nbl) == NULL)
+ {
+ NET_BUFFER *nb = NET_BUFFER_LIST_FIRST_NB(nbl);
+
+ if (nb != NULL && NET_BUFFER_NEXT_NB(nb) == NULL)
+ {
+ if (OK(NdisRetreatNetBufferDataStart(nb, inMetaValues->ipHeaderSize, 0, NULL)))
+ {
+ WFP_IPV4_HEADER *ipv4;
+ WFP_IPV6_HEADER *ipv6;
+ UCHAR *alloc_buf = Malloc(ip_header_len);
+
+ ipv4 = NdisGetDataBuffer(nb, ip_header_len, alloc_buf, 1, 0);
+ ipv6 = (WFP_IPV6_HEADER *)ipv4;
+
+ if (ipv4 != NULL)
+ {
+ if ((isv6 == false && ipv4->Protocol == WFP_ESP_RAW_PROTOCOL_ID))
+ {
+ if (IsIPv4AddressInList(&ipv4->DstIP))
+ {
+ UINT src_size = NET_BUFFER_DATA_LENGTH(nb);
+ UCHAR *src_data = Malloc(src_size);
+
+ if (src_data != NULL)
+ {
+ UCHAR *src_ptr = NdisGetDataBuffer(nb, src_size, src_data, 1, 0);
+
+ if (src_ptr != NULL)
+ {
+ UINT dst_size = src_size;
+ UCHAR *dst_data = Malloc(dst_size);
+
+ if (dst_data != NULL)
+ {
+ WFP_IPV4_HEADER *ipv4;
+
+ Copy(dst_data, src_ptr, dst_size);
+ ipv4 = (WFP_IPV4_HEADER *)dst_data;
+
+ ipv4->Protocol = WFP_ESP_RAW_PROTOCOL_ID_DST;
+ ipv4->Checksum = 0;
+ ipv4->Checksum = IpChecksum(ipv4, inMetaValues->ipHeaderSize);
+
+ block = InjectPacket(hInjection, nbl, dst_data, dst_size, inFixedValues, inMetaValues);
+ }
+ }
+
+ Free(src_data);
+ }
+ }
+ }
+ else if ((isv6 && ipv6->NextHeader == WFP_ESP_RAW_PROTOCOL_ID))
+ {
+ if (IsIPv6AddressInList(&ipv6->DestAddress))
+ {
+ UINT src_size = NET_BUFFER_DATA_LENGTH(nb);
+ UCHAR *src_data = Malloc(src_size);
+
+ if (src_data != NULL)
+ {
+ UCHAR *src_ptr = NdisGetDataBuffer(nb, src_size, src_data, 1, 0);
+
+ if (src_ptr != NULL)
+ {
+ UINT dst_size = src_size;
+ UCHAR *dst_data = Malloc(dst_size);
+
+ if (dst_data != NULL)
+ {
+ WFP_IPV6_HEADER *ipv6;
+
+ Copy(dst_data, src_ptr, dst_size);
+ ipv6 = (WFP_IPV6_HEADER *)dst_data;
+
+ ipv6->NextHeader = WFP_ESP_RAW_PROTOCOL_ID_DST;
+
+ block = InjectPacket(hInjection, nbl, dst_data, dst_size, inFixedValues, inMetaValues);
+ }
+ }
+
+ Free(src_data);
+ }
+ }
+ }
+
+ if ((isv6 == false && ipv4->Protocol == WFP_IP_PROTO_UDP) ||
+ (isv6 && ipv6->NextHeader == WFP_IP_PROTO_UDP))
+ {
+ UINT ip_and_udp_header_len = inMetaValues->ipHeaderSize + sizeof(WFP_UDP_HEADER);
+ UCHAR *ptr;
+ UCHAR *alloc_buf = Malloc(ip_and_udp_header_len);
+
+ ptr = NdisGetDataBuffer(nb, ip_and_udp_header_len, alloc_buf, 1, 0);
+ if (ptr != NULL)
+ {
+ WFP_UDP_HEADER *udp = (WFP_UDP_HEADER *)(ptr + inMetaValues->ipHeaderSize);
+
+ if (Endian16(udp->DstPort) == 4500)
+ {
+ if ((isv6 == false && IsIPv4AddressInList(&ipv4->DstIP)) ||
+ (isv6 && IsIPv6AddressInList(&ipv6->DestAddress)))
+ {
+ UINT packet_size = NET_BUFFER_DATA_LENGTH(nb);
+ UCHAR *packet_buf_allocated = Malloc(packet_size);
+ UCHAR *packet_data = NdisGetDataBuffer(nb, packet_size, packet_buf_allocated, 1, 0);
+
+ if (packet_data != NULL)
+ {
+ UCHAR *udp_payload = packet_data + ip_and_udp_header_len;
+ UINT udp_payload_size = packet_size - ip_and_udp_header_len;
+
+ if (udp_payload_size >= 4)
+ {
+ UINT *i = (UINT *)udp_payload;
+ if (*i != 0)
+ {
+ // Generate a modified packet
+ UINT dst_size;
+ UCHAR *dst_data = ModificationOfIPsecESPPacket(packet_data, packet_size,
+ inMetaValues->ipHeaderSize, &dst_size, isv6);
+
+ block = InjectPacket(hInjection, nbl, dst_data, dst_size, inFixedValues, inMetaValues);
+ }
+ }
+ }
+ else
+ {
+ //CRUSH_WHERE;
+ }
+
+ Free(packet_buf_allocated);
+ }
+ }
+ }
+ else
+ {
+ //CRUSH_WHERE;
+ }
+
+ Free(alloc_buf);
+ }
+ }
+
+ Free(alloc_buf);
+
+ NdisAdvanceNetBufferDataStart(nb, inMetaValues->ipHeaderSize, false, NULL);
+ }
+ else
+ {
+ //CRUSH_WHERE;
+ }
+
+ nb = NET_BUFFER_NEXT_NB(nb);
+ }
+
+ nbl = NET_BUFFER_LIST_NEXT_NBL(nbl);
+ }
+ }
+
+ classifyOut->actionType = FWP_ACTION_CONTINUE;
+
+ if (block)
+ {
+ // Block the packet
+ classifyOut->actionType = FWP_ACTION_BLOCK;
+ classifyOut->rights &= ~FWPS_RIGHT_ACTION_WRITE;
+ classifyOut->flags |= FWPS_CLASSIFY_OUT_FLAG_ABSORB;
+ //SetEvent(wfp->Event);
+ }
+}
+
+// Function to receive notification from the WFP
+NTSTATUS NTAPI CalloutNotify(FWPS_CALLOUT_NOTIFY_TYPE notifyType,
+ const GUID* filterKey, FWPS_FILTER0* filter)
+{
+ //Crush(1,0,0,0);
+ return 0;
+}
+
+// Scan whether the specified IP address is in the local IP address list
+bool IsIPAddressInList(struct WFP_LOCAL_IP *ip)
+{
+ bool ret = false;
+ // Validate arguments
+ if (ip == NULL)
+ {
+ return false;
+ }
+
+ SpinLock(wfp->LocalIPListLock);
+ {
+ if (wfp->LocalIPListData != NULL)
+ {
+ UINT num = wfp->LocalIPListSize / sizeof(WFP_LOCAL_IP);
+ WFP_LOCAL_IP *o = (WFP_LOCAL_IP *)wfp->LocalIPListData;
+ UINT i;
+
+ for (i = 0;i < num;i++)
+ {
+ if (Cmp(&o[i], ip, sizeof(WFP_LOCAL_IP)) == 0)
+ {
+ ret = true;
+ break;
+ }
+ }
+ }
+ }
+ SpinUnlock(wfp->LocalIPListLock);
+
+ return ret;
+}
+bool IsIPv4AddressInList(void *addr)
+{
+ WFP_LOCAL_IP ip;
+ // Validate arguments
+ if (addr == NULL)
+ {
+ return false;
+ }
+
+ Zero(&ip, sizeof(ip));
+ ip.IpVersion = 4;
+ Copy(ip.IpAddress.IPv4Address, addr, 4);
+
+ return IsIPAddressInList(&ip);
+}
+bool IsIPv6AddressInList(void *addr)
+{
+ WFP_LOCAL_IP ip;
+ // Validate arguments
+ if (addr == NULL)
+ {
+ return false;
+ }
+
+ Zero(&ip, sizeof(ip));
+ ip.IpVersion = 6;
+ Copy(ip.IpAddress.IPv6Address, addr, 16);
+
+ return IsIPAddressInList(&ip);
+}
+
+// Win32 driver entry point
+NTSTATUS DriverEntry(DRIVER_OBJECT *driver_object, UNICODE_STRING *registry_path)
+{
+ NTSTATUS ret;
+ FWPM_SESSION0 t;
+
+ if (wfp != NULL)
+ {
+ return STATUS_UNSUCCESSFUL;
+ }
+
+ wfp = ZeroMalloc(sizeof(WFP_CTX));
+
+ RtlInitUnicodeString(&wfp->DeviceName, WFP_DEVICE_NAME);
+ RtlInitUnicodeString(&wfp->DeviceNameWin32, WFP_DEVICE_NAME_WIN32);
+ // Create a device
+ ret = IoCreateDevice(driver_object, 0, &wfp->DeviceName, FILE_DEVICE_NETWORK, 0, false, &wfp->DeviceObject);
+ if (NG(ret))
+ {
+ return ret;
+ }
+
+ // Open the NDIS handle
+ wfp->hNdis = NdisAllocateGenericObject(driver_object, MEMPOOL_TAG, 0);
+ if (wfp->hNdis == NULL)
+ {
+ DriverUnload(driver_object);
+ return STATUS_UNSUCCESSFUL;
+ }
+
+ // Create a symbolic device for Win32
+ ret = IoCreateSymbolicLink(&wfp->DeviceNameWin32, &wfp->DeviceName);
+ if (NG(ret))
+ {
+ DriverUnload(driver_object);
+ return ret;
+ }
+
+ driver_object->DriverUnload = DriverUnload;
+
+ // Create an Event
+ wfp->Event = NewEvent(WFP_EVENT_NAME);
+ if (wfp->Event == NULL)
+ {
+ DriverUnload(driver_object);
+ return STATUS_UNSUCCESSFUL;
+ }
+
+ // Open the WFP engine
+ Zero(&t, sizeof(t));
+ t.flags = FWPM_SESSION_FLAG_DYNAMIC;
+ ret = FwpmEngineOpen0(NULL, RPC_C_AUTHN_DEFAULT, NULL, &t, &wfp->hEngine);
+ if (NG(ret))
+ {
+ DriverUnload(driver_object);
+ return ret;
+ }
+ else
+ {
+ // Register itself as a Callout Driver
+ FWPS_CALLOUT0 s;
+// FWPM_CALLOUT0 callout;
+
+ Zero(&s, sizeof(s));
+ s.calloutKey = GUID_WFP_CALLOUT_DRIVER_V4;
+ s.classifyFn = CalloutClassify;
+ s.notifyFn = CalloutNotify;
+
+ ret = FwpsCalloutRegister0(wfp->DeviceObject, &s, &wfp->CalloutIdIPv4);
+ if (NG(ret))
+ {
+ DriverUnload(driver_object);
+ return ret;
+ }
+
+ Zero(&s, sizeof(s));
+ s.calloutKey = GUID_WFP_CALLOUT_DRIVER_V6;
+ s.classifyFn = CalloutClassify;
+ s.notifyFn = CalloutNotify;
+
+ ret = FwpsCalloutRegister0(wfp->DeviceObject, &s, &wfp->CalloutIdIPv6);
+ if (NG(ret))
+ {
+ DriverUnload(driver_object);
+ return ret;
+ }
+
+ /*// Create the Callout Driver (IPv4)
+ Zero(&callout, sizeof(callout));
+ callout.calloutKey = GUID_WFP_CALLOUT_DRIVER_V4;
+ callout.applicableLayer = FWPM_LAYER_INBOUND_IPPACKET_V4;
+ callout.displayData.name = WFP_DRIVER_TITLE_V4;
+ ret = FwpmCalloutAdd0(wfp->hEngine, &callout, NULL, &wfp->CalloutObjIdIPv4);
+
+ // Create the Callout Driver (IPv6)
+ Zero(&callout, sizeof(callout));
+ callout.calloutKey = GUID_WFP_CALLOUT_DRIVER_V6;
+ callout.applicableLayer = FWPM_LAYER_INBOUND_IPPACKET_V6;
+ callout.displayData.name = WFP_DRIVER_TITLE_V6;
+ ret = FwpmCalloutAdd0(wfp->hEngine, &callout, NULL, &wfp->CalloutObjIdIPv6);*/
+
+ // Create an injection handle
+ FwpsInjectionHandleCreate0(AF_INET, FWPS_INJECTION_TYPE_NETWORK, &wfp->hInjectionIPv4);
+ if (NG(ret))
+ {
+ wfp->hInjectionIPv4 = NULL;
+ }
+ ret = FwpsInjectionHandleCreate0(AF_INET6, FWPS_INJECTION_TYPE_NETWORK,&wfp->hInjectionIPv6);
+ if (NG(ret))
+ {
+ wfp->hInjectionIPv6 = NULL;
+ }
+ }
+
+ // Create a lock
+ wfp->LocalIPListLock = NewSpinLock();
+
+ // Specify a service function
+ driver_object->MajorFunction[IRP_MJ_CREATE] =
+ driver_object->MajorFunction[IRP_MJ_CLOSE] =
+ driver_object->MajorFunction[IRP_MJ_READ] =
+ driver_object->MajorFunction[IRP_MJ_WRITE] = DriverDispatch;
+
+ return STATUS_SUCCESS;
+}
+
+// Unload the driver
+void DriverUnload(DRIVER_OBJECT *driver_object)
+{
+ // Validate arguments
+ if (wfp == NULL || driver_object == NULL)
+ {
+ return;
+ }
+
+ wfp->Halting = true;
+
+ // Delete the lock
+ FreeSpinLock(wfp->LocalIPListLock);
+
+ // Delete the injection handle
+ if (wfp->hInjectionIPv4 != NULL)
+ {
+ FwpsInjectionHandleDestroy0(wfp->hInjectionIPv4);
+ }
+ if (wfp->hInjectionIPv6 != NULL)
+ {
+ FwpsInjectionHandleDestroy0(wfp->hInjectionIPv6);
+ }
+
+ // Delete the Callout Object
+ if (wfp->hEngine != NULL)
+ {
+ //FwpmCalloutDeleteByKey0(wfp->hEngine, &GUID_WFP_CALLOUT_DRIVER_V4);
+ //FwpmCalloutDeleteByKey0(wfp->hEngine, &GUID_WFP_CALLOUT_DRIVER_V6);
+ }
+
+ if (wfp->CalloutIdIPv4 != 0)
+ {
+ // Delete the registration of Callout Driver (IPv4)
+ FwpsCalloutUnregisterById0(wfp->CalloutIdIPv4);
+ }
+
+ if (wfp->CalloutIdIPv6 != 0)
+ {
+ // Delete the registration of Callout Driver (IPv6)
+ FwpsCalloutUnregisterById0(wfp->CalloutIdIPv6);
+ }
+
+ FreeEvent(wfp->Event);
+
+ IoDeleteSymbolicLink(&wfp->DeviceNameWin32);
+
+ if (wfp->DeviceObject != NULL)
+ {
+ IoDeleteDevice(wfp->DeviceObject);
+ }
+
+ if (wfp->hEngine != NULL)
+ {
+ FwpmEngineClose0(wfp->hEngine);
+ }
+
+ if (wfp->LocalIPListData != NULL)
+ {
+ Free(wfp->LocalIPListData);
+ }
+
+ // Close the NDIS handle
+ if (wfp->hNdis != NULL)
+ {
+ NdisFreeGenericObject(wfp->hNdis);
+ }
+
+ Free(wfp);
+
+ wfp = NULL;
+}
+
+// Create an Event
+EVENT *NewEvent(wchar_t *name)
+{
+ EVENT *e;
+ KEVENT *ke;
+ HANDLE h;
+ UNICODE_STRING name_str;
+ // Validate arguments
+ if (name == NULL)
+ {
+ return NULL;
+ }
+
+ RtlInitUnicodeString(&name_str, name);
+
+ ke = IoCreateNotificationEvent(&name_str, &h);
+ if (ke == NULL)
+ {
+ return NULL;
+ }
+
+ KeInitializeEvent(ke, NotificationEvent, false);
+ KeClearEvent(ke);
+
+ e = ZeroMalloc(sizeof(EVENT));
+
+ e->EventObj = ke;
+ e->Handle = h;
+
+ return e;
+}
+
+// Delete the event
+void FreeEvent(EVENT *e)
+{
+ // Validate arguments
+ if (e == NULL)
+ {
+ return;
+ }
+
+ ZwClose(e->Handle);
+
+ Free(e);
+}
+
+// Set the event
+void SetEvent(EVENT *e)
+{
+ // Validate arguments
+ if (e == NULL)
+ {
+ return;
+ }
+
+ KeSetEvent(e->EventObj, 0, false);
+}
+
+// Reset the event
+void ResetEvent(EVENT *e)
+{
+ // Validate arguments
+ if (e == NULL)
+ {
+ return;
+ }
+
+ KeResetEvent(e->EventObj);
+}
+
+// Allocate the memory
+void *Malloc(UINT size)
+{
+ void *p;
+
+ p = ExAllocatePoolWithTag(NonPagedPool, size + sizeof(UINT), MEMPOOL_TAG);
+ *((UINT *)p) = size;
+
+ return ((UCHAR *)p) + sizeof(UINT);
+}
+void *ZeroMalloc(UINT size)
+{
+ void *p = Malloc(size);
+ Zero(p, size);
+ return p;
+}
+
+// Change the memory block size
+void *ReAlloc(void *p, UINT size)
+{
+ void *ret;
+ UINT oldsize;
+ // Validate arguments
+ if (p == NULL)
+ {
+ return NULL;
+ }
+
+ ret = Malloc(size);
+ if (ret == NULL)
+ {
+ Free(p);
+ return NULL;
+ }
+
+ oldsize = GetMemSize(p);
+
+ Copy(ret, p, MIN(size, oldsize));
+
+ Free(p);
+
+ return ret;
+}
+
+// Copy memory
+void Copy(void *dst, void *src, UINT size)
+{
+ // Validate arguments
+ if (dst == NULL || src == NULL || size == 0)
+ {
+ return;
+ }
+
+ memcpy(dst, src, size);
+}
+
+// Get the memory block size
+UINT GetMemSize(void *p)
+{
+ // Validate arguments
+ if (p == NULL)
+ {
+ return 0;
+ }
+
+ return *(UINT *)(((UCHAR *)p) - sizeof(UINT));
+}
+
+// Release the memory
+void Free(void *p)
+{
+ // Validate arguments
+ if (p == NULL)
+ {
+ return;
+ }
+
+ p = ((UCHAR *)p) - sizeof(UINT);
+
+ ExFreePoolWithTag(p, MEMPOOL_TAG);
+}
+
+// Clear the memory to zero
+void Zero(void *p, UINT size)
+{
+ // Validate arguments
+ if (p == NULL || size == 0)
+ {
+ return;
+ }
+
+ memset(p, 0, size);
+}
+
+// Comparison of memory
+UINT Cmp(void *p1, void *p2, UINT size)
+{
+ UCHAR *c1 = (UCHAR *)p1;
+ UCHAR *c2 = (UCHAR *)p2;
+ UINT i;
+
+ for (i = 0;i < size;i++)
+ {
+ if (c1[i] != c2[i])
+ {
+ if (c1[i] > c2[i])
+ {
+ return 1;
+ }
+ else
+ {
+ return -1;
+ }
+ }
+ }
+
+ return 0;
+}
+
+// Create a spin lock
+SPINLOCK *NewSpinLock()
+{
+ SPINLOCK *s = ZeroMalloc(sizeof(SPINLOCK));
+
+ KeInitializeSpinLock(&s->SpinLock);
+
+ return s;
+}
+
+// Lock
+void SpinLock(SPINLOCK *s)
+{
+ // Validate arguments
+ if (s == NULL)
+ {
+ return;
+ }
+
+ KeAcquireSpinLock(&s->SpinLock, &s->OldIrql);
+}
+
+// Unlock
+void SpinUnlock(SPINLOCK *s)
+{
+ // Validate arguments
+ if (s == NULL)
+ {
+ return;
+ }
+
+ KeReleaseSpinLock(&s->SpinLock, s->OldIrql);
+}
+
+// Release the spin lock
+void FreeSpinLock(SPINLOCK *s)
+{
+ // Validate arguments
+ if (s == NULL)
+ {
+ return;
+ }
+
+ Free(s);
+}
+
+// Sleep
+void Sleep(int milliSeconds)
+{
+ PKTIMER timer = ZeroMalloc(sizeof(KTIMER));
+ LARGE_INTEGER duetime;
+
+ duetime.QuadPart = (__int64)milliSeconds * -10000;
+ KeInitializeTimerEx(timer, NotificationTimer);
+ KeSetTimerEx(timer, duetime, 0, NULL);
+
+ KeWaitForSingleObject (timer, Executive, KernelMode, FALSE, NULL);
+
+ Free(timer);
+}
+
+// 16-bit swap
+USHORT Swap16(USHORT value)
+{
+ USHORT r;
+ ((BYTE *)&r)[0] = ((BYTE *)&value)[1];
+ ((BYTE *)&r)[1] = ((BYTE *)&value)[0];
+ return r;
+}
+
+// 32-bit swap
+UINT Swap32(UINT value)
+{
+ UINT r;
+ ((BYTE *)&r)[0] = ((BYTE *)&value)[3];
+ ((BYTE *)&r)[1] = ((BYTE *)&value)[2];
+ ((BYTE *)&r)[2] = ((BYTE *)&value)[1];
+ ((BYTE *)&r)[3] = ((BYTE *)&value)[0];
+ return r;
+}
+
+// 64-bit swap
+UINT64 Swap64(UINT64 value)
+{
+ UINT64 r;
+ ((BYTE *)&r)[0] = ((BYTE *)&value)[7];
+ ((BYTE *)&r)[1] = ((BYTE *)&value)[6];
+ ((BYTE *)&r)[2] = ((BYTE *)&value)[5];
+ ((BYTE *)&r)[3] = ((BYTE *)&value)[4];
+ ((BYTE *)&r)[4] = ((BYTE *)&value)[3];
+ ((BYTE *)&r)[5] = ((BYTE *)&value)[2];
+ ((BYTE *)&r)[6] = ((BYTE *)&value)[1];
+ ((BYTE *)&r)[7] = ((BYTE *)&value)[0];
+ return r;
+}
+
+// Endian conversion 16bit
+USHORT Endian16(USHORT src)
+{
+ int x = 1;
+ if (*((char *)&x))
+ {
+ return Swap16(src);
+ }
+ else
+ {
+ return src;
+ }
+}
+
+// Endian conversion 32bit
+UINT Endian32(UINT src)
+{
+ int x = 1;
+ if (*((char *)&x))
+ {
+ return Swap32(src);
+ }
+ else
+ {
+ return src;
+ }
+}
+
+// Endian conversion 64bit
+UINT64 Endian64(UINT64 src)
+{
+ int x = 1;
+ if (*((char *)&x))
+ {
+ return Swap64(src);
+ }
+ else
+ {
+ return src;
+ }
+}
+
+// Crash
+void Crush(UINT a, UINT b, UINT c, UINT d)
+{
+ KeBugCheckEx(0x00000061, (ULONG_PTR)a, (ULONG_PTR)b, (ULONG_PTR)c, (ULONG_PTR)d);
+}
+
+// Developed by SoftEther VPN Project at University of Tsukuba in Japan.
+// Department of Computer Science has dozens of overly-enthusiastic geeks.
+// Join us: http://www.tsukuba.ac.jp/english/admission/
|