1913 lines
56 KiB
C
1913 lines
56 KiB
C
/*
|
|
* divert.c
|
|
* (C) 2011, all rights reserved,
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* 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
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#ifndef UNICODE
|
|
#define UNICODE
|
|
#endif
|
|
|
|
#include <winsock2.h>
|
|
#include <windows.h>
|
|
#include <winioctl.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <wdfinstaller.h>
|
|
|
|
// #define DIVERT_DEBUG
|
|
|
|
#define DIVERTEXPORT
|
|
#include "divert.h"
|
|
#include "divert_device.h"
|
|
|
|
#define DIVERT_DRIVER_NAME L"divert"
|
|
#define DIVERT_DRIVER_SYS L"\\" DIVERT_DRIVER_NAME L".sys"
|
|
#define DIVERT_DRIVER_INF L"\\" DIVERT_DRIVER_NAME L".inf"
|
|
#define DIVERT_DRIVER_MATCH_DLL L"\\WdfCoInstaller*.dll"
|
|
|
|
/*
|
|
* Filter parsing.
|
|
*/
|
|
typedef enum
|
|
{
|
|
FILTER_TOKEN_ICMP,
|
|
FILTER_TOKEN_ICMP_BODY,
|
|
FILTER_TOKEN_ICMP_CHECKSUM,
|
|
FILTER_TOKEN_ICMP_CODE,
|
|
FILTER_TOKEN_ICMP_TYPE,
|
|
FILTER_TOKEN_ICMPV6,
|
|
FILTER_TOKEN_ICMPV6_BODY,
|
|
FILTER_TOKEN_ICMPV6_CHECKSUM,
|
|
FILTER_TOKEN_ICMPV6_CODE,
|
|
FILTER_TOKEN_ICMPV6_TYPE,
|
|
FILTER_TOKEN_IP,
|
|
FILTER_TOKEN_IP_CHECKSUM,
|
|
FILTER_TOKEN_IP_DF,
|
|
FILTER_TOKEN_IP_DST_ADDR,
|
|
FILTER_TOKEN_IP_FRAG_OFF,
|
|
FILTER_TOKEN_IP_HDR_LENGTH,
|
|
FILTER_TOKEN_IP_ID,
|
|
FILTER_TOKEN_IP_LENGTH,
|
|
FILTER_TOKEN_IP_MF,
|
|
FILTER_TOKEN_IP_PROTOCOL,
|
|
FILTER_TOKEN_IP_SRC_ADDR,
|
|
FILTER_TOKEN_IP_TOS,
|
|
FILTER_TOKEN_IP_TTL,
|
|
FILTER_TOKEN_IPV6,
|
|
FILTER_TOKEN_IPV6_DST_ADDR,
|
|
FILTER_TOKEN_IPV6_FLOW_LABEL,
|
|
FILTER_TOKEN_IPV6_HOP_LIMIT,
|
|
FILTER_TOKEN_IPV6_LENGTH,
|
|
FILTER_TOKEN_IPV6_NEXT_HDR,
|
|
FILTER_TOKEN_IPV6_SRC_ADDR,
|
|
FILTER_TOKEN_IPV6_TRAFFIC_CLASS,
|
|
FILTER_TOKEN_TCP,
|
|
FILTER_TOKEN_TCP_ACK,
|
|
FILTER_TOKEN_TCP_ACK_NUM,
|
|
FILTER_TOKEN_TCP_CHECKSUM,
|
|
FILTER_TOKEN_TCP_DST_PORT,
|
|
FILTER_TOKEN_TCP_FIN,
|
|
FILTER_TOKEN_TCP_HDR_LENGTH,
|
|
FILTER_TOKEN_TCP_PAYLOAD_LENGTH,
|
|
FILTER_TOKEN_TCP_PSH,
|
|
FILTER_TOKEN_TCP_RST,
|
|
FILTER_TOKEN_TCP_SEQ_NUM,
|
|
FILTER_TOKEN_TCP_SRC_PORT,
|
|
FILTER_TOKEN_TCP_SYN,
|
|
FILTER_TOKEN_TCP_URG,
|
|
FILTER_TOKEN_TCP_URG_PTR,
|
|
FILTER_TOKEN_TCP_WINDOW,
|
|
FILTER_TOKEN_UDP,
|
|
FILTER_TOKEN_UDP_CHECKSUM,
|
|
FILTER_TOKEN_UDP_DST_PORT,
|
|
FILTER_TOKEN_UDP_LENGTH,
|
|
FILTER_TOKEN_UDP_PAYLOAD_LENGTH,
|
|
FILTER_TOKEN_UDP_SRC_PORT,
|
|
FILTER_TOKEN_TRUE,
|
|
FILTER_TOKEN_FALSE,
|
|
FILTER_TOKEN_INBOUND,
|
|
FILTER_TOKEN_OUTBOUND,
|
|
FILTER_TOKEN_IF_IDX,
|
|
FILTER_TOKEN_SUB_IF_IDX,
|
|
FILTER_TOKEN_OPEN,
|
|
FILTER_TOKEN_CLOSE,
|
|
FILTER_TOKEN_EQ,
|
|
FILTER_TOKEN_NEQ,
|
|
FILTER_TOKEN_LT,
|
|
FILTER_TOKEN_LEQ,
|
|
FILTER_TOKEN_GT,
|
|
FILTER_TOKEN_GEQ,
|
|
FILTER_TOKEN_NOT,
|
|
FILTER_TOKEN_AND,
|
|
FILTER_TOKEN_OR,
|
|
FILTER_TOKEN_NUMBER,
|
|
FILTER_TOKEN_END,
|
|
} FILTER_TOKEN_KIND;
|
|
|
|
typedef struct
|
|
{
|
|
FILTER_TOKEN_KIND kind;
|
|
UINT32 val[4];
|
|
} FILTER_TOKEN;
|
|
|
|
#define FILTER_TOKEN_MAXLEN 32 // Fits longest IPv6
|
|
|
|
typedef struct
|
|
{
|
|
char *name;
|
|
FILTER_TOKEN_KIND kind;
|
|
} FILTER_TOKEN_NAME, *PFILTER_TOKEN_NAME;
|
|
|
|
/*
|
|
* IPv4/IPv6 pseudo headers.
|
|
*/
|
|
typedef struct
|
|
{
|
|
UINT32 SrcAddr;
|
|
UINT32 DstAddr;
|
|
UINT8 Zero;
|
|
UINT8 Protocol;
|
|
UINT16 Length;
|
|
} DIVERT_PSEUDOHDR, *PDIVERT_PSEUDOHDR;
|
|
|
|
typedef struct
|
|
{
|
|
UINT32 SrcAddr[4];
|
|
UINT32 DstAddr[4];
|
|
UINT32 Length;
|
|
UINT32 NextHdr:8;
|
|
UINT32 Zero:24;
|
|
} DIVERT_PSEUDOV6HDR, *PDIVERT_PSEUDOV6HDR;
|
|
|
|
/*
|
|
* Misc.
|
|
*/
|
|
#define UINT8_MAX 0xFF
|
|
#define UINT32_MAX 0xFFFFFFFF
|
|
#define IPPROTO_ICMP 1
|
|
#define IPPROTO_TCP 6
|
|
#define IPPROTO_UDP 17
|
|
#define IPPROTO_ICMPV6 58
|
|
|
|
/*
|
|
* Prototypes.
|
|
*/
|
|
static HMODULE DivertLoadCoInstaller(LPWSTR divert_dll);
|
|
static BOOLEAN DivertDriverFiles(LPWSTR *divert_dir_ptr,
|
|
LPWSTR *divert_sys_ptr, LPWSTR *divert_inf_ptr, LPWSTR *divert_dll_ptr);
|
|
static BOOLEAN DivertDriverInstall(VOID);
|
|
static BOOL DivertCompileFilter(const char *filter_str,
|
|
divert_ioctl_filter_t filter, UINT8 *fp);
|
|
static int DivertFilterTokenNameCompare(const void *a, const void *b);
|
|
static BOOL DivertTokenizeFilter(const char *filter, FILTER_TOKEN *tokens,
|
|
UINT8 tokensmax);
|
|
static BOOL DivertParseIPv4Address(char *str, UINT32 *addr_ptr);
|
|
static BOOL DivertParseIPv6Address(char *str, UINT32 *addr_ptr);
|
|
static BOOL DivertParseFilter(FILTER_TOKEN *tokens, UINT8 *tp,
|
|
divert_ioctl_filter_t filter, UINT8 *fp, FILTER_TOKEN_KIND op);
|
|
static void DivertFilterNegate(divert_ioctl_filter_t filter, UINT8 s, UINT8 e);
|
|
static void DivertFilterUpdate(divert_ioctl_filter_t filter, UINT8 s, UINT8 e,
|
|
UINT8 success, UINT8 failure);
|
|
static void DivertInitPseudoHeader(PDIVERT_IPHDR ip_header,
|
|
PDIVERT_PSEUDOHDR pseudo_header, UINT8 protocol, UINT len);
|
|
static void DivertInitPseudoHeaderV6(PDIVERT_IPV6HDR ipv6_header,
|
|
PDIVERT_PSEUDOV6HDR pseudov6_header, UINT8 protocol, UINT len);
|
|
static UINT16 DivertHelperCalcChecksum(PVOID pseudo_header,
|
|
UINT16 pseudo_header_len, PVOID data, UINT len);
|
|
|
|
#ifdef DIVERT_DEBUG
|
|
static void DivertFilterDump(divert_ioctl_filter_t filter, UINT8 len);
|
|
#endif
|
|
|
|
/*
|
|
* Co-installer functions.
|
|
*/
|
|
PFN_WDFPREDEVICEINSTALLEX pfnWdfPreDeviceInstallEx;
|
|
PFN_WDFPOSTDEVICEINSTALL pfnWdfPostDeviceInstall;
|
|
|
|
/*
|
|
* Dll Entry
|
|
*/
|
|
extern BOOL APIENTRY DivertDllEntry(HANDLE module, DWORD reason,
|
|
LPVOID reserved)
|
|
{
|
|
return TRUE;
|
|
}
|
|
|
|
/*
|
|
* Load the co-installer functions.
|
|
*/
|
|
static HMODULE DivertLoadCoInstaller(LPWSTR divert_dll)
|
|
{
|
|
HMODULE library = LoadLibrary(divert_dll);
|
|
|
|
if (library == NULL)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
pfnWdfPreDeviceInstallEx = (PFN_WDFPREDEVICEINSTALLEX)GetProcAddress(
|
|
library, "WdfPreDeviceInstallEx");
|
|
if (pfnWdfPreDeviceInstallEx == NULL)
|
|
{
|
|
goto DivertLoadInstallerError;
|
|
}
|
|
pfnWdfPostDeviceInstall = (PFN_WDFPOSTDEVICEINSTALL)GetProcAddress(
|
|
library, "WdfPostDeviceInstall");
|
|
if (pfnWdfPostDeviceInstall == NULL)
|
|
{
|
|
goto DivertLoadInstallerError;
|
|
}
|
|
|
|
return library;
|
|
|
|
DivertLoadInstallerError:
|
|
|
|
FreeLibrary(library);
|
|
return NULL;
|
|
}
|
|
|
|
/*
|
|
* Locate the Divert driver files.
|
|
*/
|
|
static BOOLEAN DivertDriverFiles(LPWSTR *divert_dir_ptr,
|
|
LPWSTR *divert_sys_ptr, LPWSTR *divert_inf_ptr, LPWSTR *divert_dll_ptr)
|
|
{
|
|
DWORD length, l2;
|
|
HANDLE find;
|
|
WIN32_FIND_DATA find_data;
|
|
LPWSTR divert_dir, divert_sys, divert_inf, divert_dll;
|
|
|
|
// Construct the filenames from the current directory name
|
|
length = GetCurrentDirectory(0, NULL);
|
|
length--;
|
|
divert_dir = (WCHAR *)malloc((length+1)*sizeof(WCHAR));
|
|
divert_sys = (WCHAR *)malloc((length+wcslen(DIVERT_DRIVER_SYS)+1)*
|
|
sizeof(WCHAR));
|
|
divert_inf = (WCHAR *)malloc((length+wcslen(DIVERT_DRIVER_INF)+1)*
|
|
sizeof(WCHAR));
|
|
divert_dll = (WCHAR *)malloc((length+wcslen(DIVERT_DRIVER_MATCH_DLL)+1)*
|
|
sizeof(WCHAR));
|
|
if (divert_dir == NULL || divert_sys == NULL || divert_inf == NULL ||
|
|
divert_dll == NULL)
|
|
{
|
|
goto DivertDriverFilesError;
|
|
}
|
|
if (GetCurrentDirectory(length+1, divert_dir) != length)
|
|
{
|
|
SetLastError(ERROR_FILE_NOT_FOUND);
|
|
goto DivertDriverFilesError;
|
|
}
|
|
wcscpy(divert_sys, divert_dir);
|
|
wcscpy(divert_inf, divert_dir);
|
|
wcscpy(divert_dll, divert_dir);
|
|
wcscpy(divert_sys + length, DIVERT_DRIVER_SYS);
|
|
wcscpy(divert_inf + length, DIVERT_DRIVER_INF);
|
|
wcscpy(divert_dll + length, DIVERT_DRIVER_MATCH_DLL);
|
|
|
|
// Check the the files exist; and find the co-installer filename.
|
|
find = FindFirstFile(divert_sys, &find_data);
|
|
if (find == INVALID_HANDLE_VALUE)
|
|
{
|
|
goto DivertDriverFilesError;
|
|
}
|
|
FindClose(find);
|
|
find = FindFirstFile(divert_inf, &find_data);
|
|
if (find == INVALID_HANDLE_VALUE)
|
|
{
|
|
goto DivertDriverFilesError;
|
|
}
|
|
FindClose(find);
|
|
find = FindFirstFile(divert_dll, &find_data);
|
|
free(divert_dll);
|
|
if (find == INVALID_HANDLE_VALUE)
|
|
{
|
|
goto DivertDriverFilesError;
|
|
}
|
|
FindClose(find);
|
|
divert_dll = (WCHAR *)malloc((length+wcslen(find_data.cFileName)+2)*
|
|
sizeof(WCHAR));
|
|
if (divert_dll == NULL)
|
|
{
|
|
goto DivertDriverFilesError;
|
|
}
|
|
wcscpy(divert_dll, divert_dir);
|
|
divert_dll[length] = L'\\';
|
|
wcscpy(divert_dll + length + 1, find_data.cFileName);
|
|
|
|
*divert_dir_ptr = divert_dir;
|
|
*divert_sys_ptr = divert_sys;
|
|
*divert_inf_ptr = divert_inf;
|
|
*divert_dll_ptr = divert_dll;
|
|
return TRUE;
|
|
|
|
DivertDriverFilesError:
|
|
free(divert_dir);
|
|
free(divert_sys);
|
|
free(divert_inf);
|
|
free(divert_dll);
|
|
return FALSE;
|
|
}
|
|
|
|
/*
|
|
* Install the Divert driver.
|
|
*/
|
|
static BOOLEAN DivertDriverInstall(VOID)
|
|
{
|
|
static BOOLEAN installed = FALSE;
|
|
DWORD err;
|
|
SC_HANDLE manager = NULL, service = NULL;
|
|
WDF_COINSTALLER_INSTALL_OPTIONS client_options;
|
|
LPWSTR divert_dir = NULL, divert_sys = NULL, divert_inf = NULL,
|
|
divert_dll = NULL;
|
|
HMODULE library = NULL;
|
|
|
|
// Do nothing if the driver is already installed:
|
|
if (installed)
|
|
{
|
|
return TRUE;
|
|
}
|
|
|
|
// Open the service manager:
|
|
manager = OpenSCManager(NULL, NULL, SC_MANAGER_ALL_ACCESS);
|
|
if (manager == NULL)
|
|
{
|
|
goto DivertDriverInstallExit;
|
|
}
|
|
|
|
// Check if the divert service already exists; if so, start it.
|
|
service = OpenService(manager, DIVERT_DRIVER_NAME, SERVICE_ALL_ACCESS);
|
|
if (service != NULL)
|
|
{
|
|
if (!StartService(service, 0, NULL))
|
|
{
|
|
err = GetLastError();
|
|
installed = (err == ERROR_SERVICE_ALREADY_RUNNING);
|
|
goto DivertDriverInstallExit;
|
|
}
|
|
installed = TRUE;
|
|
goto DivertDriverInstallExit;
|
|
}
|
|
|
|
// Get driver files:
|
|
if (!DivertDriverFiles(&divert_dir, &divert_sys, &divert_inf, &divert_dll))
|
|
{
|
|
return FALSE;
|
|
}
|
|
|
|
// Load the co-installer:
|
|
if (DivertLoadCoInstaller(divert_dll) == NULL)
|
|
{
|
|
return FALSE;
|
|
}
|
|
|
|
// Pre-install:
|
|
WDF_COINSTALLER_INSTALL_OPTIONS_INIT(&client_options);
|
|
err = pfnWdfPreDeviceInstallEx(divert_inf, L"divert.NT.Wdf",
|
|
&client_options);
|
|
if (err != ERROR_SUCCESS)
|
|
{
|
|
SetLastError(err);
|
|
goto DivertDriverInstallExit;
|
|
}
|
|
|
|
// Create the service:
|
|
service = CreateService(manager, DIVERT_DRIVER_NAME, DIVERT_DRIVER_NAME,
|
|
SERVICE_ALL_ACCESS, SERVICE_KERNEL_DRIVER, SERVICE_DEMAND_START,
|
|
SERVICE_ERROR_NORMAL, divert_sys, NULL, NULL, NULL, NULL, NULL);
|
|
if (service == NULL && GetLastError() != ERROR_SERVICE_EXISTS)
|
|
{
|
|
goto DivertDriverInstallExit;
|
|
}
|
|
|
|
// Post-install:
|
|
err = pfnWdfPostDeviceInstall(divert_inf, NULL);
|
|
if (err != ERROR_SUCCESS)
|
|
{
|
|
SetLastError(err);
|
|
goto DivertDriverInstallExit;
|
|
}
|
|
FreeLibrary(library);
|
|
library = NULL;
|
|
|
|
// Start the service:
|
|
if (!StartService(service, 0, NULL))
|
|
{
|
|
err = GetLastError();
|
|
installed = (err == ERROR_SERVICE_ALREADY_RUNNING);
|
|
goto DivertDriverInstallExit;
|
|
}
|
|
|
|
installed = TRUE;
|
|
|
|
DivertDriverInstallExit:
|
|
free(divert_dir);
|
|
free(divert_sys);
|
|
free(divert_inf);
|
|
free(divert_dll);
|
|
if (service != NULL)
|
|
{
|
|
CloseServiceHandle(service);
|
|
}
|
|
if (manager != NULL)
|
|
{
|
|
CloseServiceHandle(manager);
|
|
}
|
|
if (library != NULL)
|
|
{
|
|
FreeLibrary(library);
|
|
}
|
|
return installed;
|
|
}
|
|
|
|
/*
|
|
* Open a handle to the Divert device.
|
|
*/
|
|
extern HANDLE DivertOpen(const char *filter)
|
|
{
|
|
struct
|
|
{
|
|
struct divert_message_s header;
|
|
struct divert_ioctl_filter_s filter[DIVERT_FILTER_MAXLEN];
|
|
} ioctl;
|
|
UINT8 filter_len;
|
|
DWORD err, iolen;
|
|
HANDLE handle;
|
|
|
|
// Parse the filter:
|
|
if (!DivertCompileFilter(filter, ioctl.filter, &filter_len))
|
|
{
|
|
SetLastError(ERROR_INVALID_PARAMETER);
|
|
return INVALID_HANDLE_VALUE;
|
|
}
|
|
|
|
#ifdef DIVERT_DEBUG
|
|
DivertFilterDump(ioctl.filter, filter_len);
|
|
#endif
|
|
|
|
// Attempt to open the Divert device:
|
|
handle = CreateFile(L"\\\\.\\Divert", GENERIC_READ | GENERIC_WRITE,
|
|
0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, INVALID_HANDLE_VALUE);
|
|
if (handle == INVALID_HANDLE_VALUE)
|
|
{
|
|
err = GetLastError();
|
|
if (err != ERROR_FILE_NOT_FOUND && err != ERROR_PATH_NOT_FOUND)
|
|
{
|
|
return INVALID_HANDLE_VALUE;
|
|
}
|
|
|
|
// Open failed because the device isn't installed; install it now.
|
|
if (!DivertDriverInstall())
|
|
{
|
|
SetLastError(ERROR_OPEN_FAILED);
|
|
return INVALID_HANDLE_VALUE;
|
|
}
|
|
handle = CreateFile(L"\\\\.\\Divert", GENERIC_READ | GENERIC_WRITE, 0,
|
|
NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, INVALID_HANDLE_VALUE);
|
|
if (handle == INVALID_HANDLE_VALUE)
|
|
{
|
|
return INVALID_HANDLE_VALUE;
|
|
}
|
|
}
|
|
|
|
// Set the filter:
|
|
ioctl.header.version = DIVERT_VERSION;
|
|
ioctl.header.magic = DIVERT_MAGIC;
|
|
ioctl.header.reserved = 0x0;
|
|
if (!DeviceIoControl(handle, IOCTL_DIVERT_SET_FILTER, &ioctl,
|
|
sizeof(struct divert_message_s) +
|
|
filter_len*sizeof(struct divert_ioctl_filter_s), NULL, 0, &iolen,
|
|
NULL))
|
|
{
|
|
CloseHandle(handle);
|
|
return INVALID_HANDLE_VALUE;
|
|
}
|
|
|
|
// Success!
|
|
return handle;
|
|
}
|
|
|
|
/*
|
|
* Receive a packet from the Divert device.
|
|
*/
|
|
extern BOOL DivertRecv(HANDLE handle, PDIVERT_PACKET pPacket, UINT packetLen,
|
|
UINT *readlen)
|
|
{
|
|
divert_message_t message;
|
|
DWORD readlen0;
|
|
|
|
if (!ReadFile(handle, (PVOID)pPacket, (DWORD)packetLen, &readlen0, NULL))
|
|
{
|
|
return FALSE;
|
|
}
|
|
if (readlen0 <= sizeof(DIVERT_PACKET))
|
|
{
|
|
SetLastError(ERROR_INVALID_DATA);
|
|
return FALSE;
|
|
}
|
|
message = (divert_message_t)pPacket->Reserved;
|
|
if (message->magic != DIVERT_MAGIC ||
|
|
message->version != DIVERT_VERSION)
|
|
{
|
|
SetLastError(ERROR_INVALID_DATA);
|
|
return FALSE;
|
|
}
|
|
if (readlen != NULL)
|
|
{
|
|
*readlen = readlen0;
|
|
}
|
|
return TRUE;
|
|
}
|
|
|
|
/*
|
|
* Send (inject) a packet to the Divert device.
|
|
*/
|
|
extern BOOL DivertSend(HANDLE handle, PDIVERT_PACKET pPacket, UINT packetLen,
|
|
UINT *writelen)
|
|
{
|
|
divert_message_t message;
|
|
DWORD writelen0;
|
|
|
|
if (packetLen <= sizeof(DIVERT_PACKET))
|
|
{
|
|
SetLastError(ERROR_INVALID_PARAMETER);
|
|
return FALSE;
|
|
}
|
|
message = (divert_message_t)pPacket->Reserved;
|
|
message->magic = DIVERT_MAGIC;
|
|
message->version = DIVERT_VERSION;
|
|
message->reserved = 0x0;
|
|
if (writelen == NULL)
|
|
{
|
|
writelen = &writelen0;
|
|
}
|
|
return WriteFile(handle, (PVOID)pPacket, (DWORD)packetLen,
|
|
(DWORD *)writelen, NULL);
|
|
}
|
|
|
|
/*
|
|
* Close a handle to the Divert device.
|
|
*/
|
|
extern BOOL DivertClose(HANDLE handle)
|
|
{
|
|
return CloseHandle(handle);
|
|
}
|
|
|
|
/*
|
|
* Compile a filter.
|
|
*/
|
|
static BOOL DivertCompileFilter(const char *filter_str,
|
|
divert_ioctl_filter_t filter, UINT8 *fp)
|
|
{
|
|
FILTER_TOKEN tokens[DIVERT_FILTER_MAXLEN*3];
|
|
UINT8 tp;
|
|
|
|
if (!DivertTokenizeFilter(filter_str, tokens, DIVERT_FILTER_MAXLEN*3-1))
|
|
{
|
|
return FALSE;
|
|
}
|
|
|
|
tp = 0;
|
|
*fp = 0;
|
|
if (!DivertParseFilter(tokens, &tp, filter, fp, FILTER_TOKEN_AND))
|
|
{
|
|
return FALSE;
|
|
}
|
|
if (tokens[tp].kind != FILTER_TOKEN_END)
|
|
{
|
|
return FALSE;
|
|
}
|
|
return TRUE;
|
|
}
|
|
|
|
/*
|
|
* Compare two FILTER_TOKEN_NAMEs.
|
|
*/
|
|
static int DivertFilterTokenNameCompare(const void *a, const void *b)
|
|
{
|
|
PFILTER_TOKEN_NAME na = (PFILTER_TOKEN_NAME)a;
|
|
PFILTER_TOKEN_NAME nb = (PFILTER_TOKEN_NAME)b;
|
|
return strcmp(na->name, nb->name);
|
|
}
|
|
|
|
/*
|
|
* Tokenize the given filter string.
|
|
*/
|
|
static BOOL DivertTokenizeFilter(const char *filter, FILTER_TOKEN *tokens,
|
|
UINT8 tokensmax)
|
|
{
|
|
static const FILTER_TOKEN_NAME token_names[] =
|
|
{
|
|
{"and", FILTER_TOKEN_AND},
|
|
{"false", FILTER_TOKEN_FALSE},
|
|
{"icmp", FILTER_TOKEN_ICMP},
|
|
{"icmp.Body", FILTER_TOKEN_ICMP_BODY},
|
|
{"icmp.Checksum", FILTER_TOKEN_ICMP_CHECKSUM},
|
|
{"icmp.Code", FILTER_TOKEN_ICMP_CODE},
|
|
{"icmp.Type", FILTER_TOKEN_ICMP_TYPE},
|
|
{"icmpv6", FILTER_TOKEN_ICMPV6},
|
|
{"icmpv6.Body", FILTER_TOKEN_ICMPV6_BODY},
|
|
{"icmpv6.Checksum", FILTER_TOKEN_ICMPV6_CHECKSUM},
|
|
{"icmpv6.Code", FILTER_TOKEN_ICMPV6_CODE},
|
|
{"icmpv6.Type", FILTER_TOKEN_ICMPV6_TYPE},
|
|
{"ifIdx", FILTER_TOKEN_IF_IDX},
|
|
{"inbound", FILTER_TOKEN_INBOUND},
|
|
{"ip", FILTER_TOKEN_IP},
|
|
{"ip.Checksum", FILTER_TOKEN_IP_CHECKSUM},
|
|
{"ip.DF", FILTER_TOKEN_IP_DF},
|
|
{"ip.DstAddr", FILTER_TOKEN_IP_DST_ADDR},
|
|
{"ip.FragOff", FILTER_TOKEN_IP_FRAG_OFF},
|
|
{"ip.HdrLength", FILTER_TOKEN_IP_HDR_LENGTH},
|
|
{"ip.Id", FILTER_TOKEN_IP_ID},
|
|
{"ip.Length", FILTER_TOKEN_IP_LENGTH},
|
|
{"ip.MF", FILTER_TOKEN_IP_MF},
|
|
{"ip.Protocol", FILTER_TOKEN_IP_PROTOCOL},
|
|
{"ip.SrcAddr", FILTER_TOKEN_IP_SRC_ADDR},
|
|
{"ip.TOS", FILTER_TOKEN_IP_TOS},
|
|
{"ip.TTL", FILTER_TOKEN_IP_TTL},
|
|
{"ipv6", FILTER_TOKEN_IPV6},
|
|
{"ipv6.DstAddr", FILTER_TOKEN_IPV6_DST_ADDR},
|
|
{"ipv6.FlowLabel", FILTER_TOKEN_IPV6_FLOW_LABEL},
|
|
{"ipv6.HopLimit", FILTER_TOKEN_IPV6_HOP_LIMIT},
|
|
{"ipv6.Length", FILTER_TOKEN_IPV6_LENGTH},
|
|
{"ipv6.NextHdr", FILTER_TOKEN_IPV6_NEXT_HDR},
|
|
{"ipv6.SrcAddr", FILTER_TOKEN_IPV6_SRC_ADDR},
|
|
{"ipv6.TrafficClass", FILTER_TOKEN_IPV6_TRAFFIC_CLASS},
|
|
{"not", FILTER_TOKEN_NOT},
|
|
{"or", FILTER_TOKEN_OR},
|
|
{"outbound", FILTER_TOKEN_OUTBOUND},
|
|
{"subIfIdx", FILTER_TOKEN_SUB_IF_IDX},
|
|
{"tcp", FILTER_TOKEN_TCP},
|
|
{"tcp.Ack", FILTER_TOKEN_TCP_ACK},
|
|
{"tcp.AckNum", FILTER_TOKEN_TCP_ACK_NUM},
|
|
{"tcp.Checksum", FILTER_TOKEN_TCP_CHECKSUM},
|
|
{"tcp.DstPort", FILTER_TOKEN_TCP_DST_PORT},
|
|
{"tcp.Fin", FILTER_TOKEN_TCP_FIN},
|
|
{"tcp.HdrLength", FILTER_TOKEN_TCP_HDR_LENGTH},
|
|
{"tcp.PayloadLength", FILTER_TOKEN_TCP_PAYLOAD_LENGTH},
|
|
{"tcp.Psh", FILTER_TOKEN_TCP_PSH},
|
|
{"tcp.Rst", FILTER_TOKEN_TCP_RST},
|
|
{"tcp.SeqNum", FILTER_TOKEN_TCP_SEQ_NUM},
|
|
{"tcp.SrcPort", FILTER_TOKEN_TCP_SRC_PORT},
|
|
{"tcp.Syn", FILTER_TOKEN_TCP_SYN},
|
|
{"tcp.Urg", FILTER_TOKEN_TCP_URG},
|
|
{"tcp.UrgPtr", FILTER_TOKEN_TCP_URG_PTR},
|
|
{"tcp.Window", FILTER_TOKEN_TCP_WINDOW},
|
|
{"true", FILTER_TOKEN_TRUE},
|
|
{"udp", FILTER_TOKEN_UDP},
|
|
{"udp.Checksum", FILTER_TOKEN_UDP_CHECKSUM},
|
|
{"udp.DstPort", FILTER_TOKEN_UDP_DST_PORT},
|
|
{"udp.Length", FILTER_TOKEN_UDP_LENGTH},
|
|
{"udp.PayloadLength", FILTER_TOKEN_UDP_PAYLOAD_LENGTH},
|
|
{"udp.SrcPort", FILTER_TOKEN_UDP_SRC_PORT},
|
|
};
|
|
FILTER_TOKEN_NAME key, *result;
|
|
char c;
|
|
char token[FILTER_TOKEN_MAXLEN];
|
|
UINT i = 0, j;
|
|
UINT8 tp = 0;
|
|
|
|
while (TRUE)
|
|
{
|
|
if (tp >= tokensmax-1)
|
|
{
|
|
return FALSE;
|
|
}
|
|
memset(tokens[tp].val, 0, sizeof(tokens[tp].val));
|
|
while (isspace(filter[i]))
|
|
{
|
|
i++;
|
|
}
|
|
c = filter[i++];
|
|
switch (c)
|
|
{
|
|
case '\0':
|
|
tokens[tp].kind = FILTER_TOKEN_END;
|
|
return TRUE;
|
|
case '(':
|
|
tokens[tp++].kind = FILTER_TOKEN_OPEN;
|
|
continue;
|
|
case ')':
|
|
tokens[tp++].kind = FILTER_TOKEN_CLOSE;
|
|
continue;
|
|
case '!':
|
|
if (filter[i] == '=')
|
|
{
|
|
i++;
|
|
tokens[tp++].kind = FILTER_TOKEN_NEQ;
|
|
}
|
|
else
|
|
{
|
|
tokens[tp++].kind = FILTER_TOKEN_NOT;
|
|
}
|
|
continue;
|
|
case '=':
|
|
if (filter[i] == '=')
|
|
{
|
|
i++;
|
|
}
|
|
tokens[tp++].kind = FILTER_TOKEN_EQ;
|
|
continue;
|
|
case '<':
|
|
if (filter[i] == '=')
|
|
{
|
|
i++;
|
|
tokens[tp++].kind = FILTER_TOKEN_LEQ;
|
|
}
|
|
else
|
|
{
|
|
tokens[tp++].kind = FILTER_TOKEN_LT;
|
|
}
|
|
continue;
|
|
case '>':
|
|
if (filter[i] == '=')
|
|
{
|
|
i++;
|
|
tokens[tp++].kind = FILTER_TOKEN_GEQ;
|
|
}
|
|
else
|
|
{
|
|
tokens[tp++].kind = FILTER_TOKEN_GT;
|
|
}
|
|
continue;
|
|
case '&':
|
|
if (filter[i++] != '&')
|
|
{
|
|
return FALSE;
|
|
}
|
|
tokens[tp++].kind = FILTER_TOKEN_AND;
|
|
continue;
|
|
case '|':
|
|
if (filter[i++] != '|')
|
|
{
|
|
return FALSE;
|
|
}
|
|
tokens[tp++].kind = FILTER_TOKEN_OR;
|
|
continue;
|
|
default:
|
|
break;
|
|
}
|
|
token[0] = c;
|
|
if (isalnum(c) || c == '.' || c == ':')
|
|
{
|
|
UINT32 num;
|
|
char *end;
|
|
for (j = 1; j < FILTER_TOKEN_MAXLEN && (isalnum(filter[i]) ||
|
|
filter[i] == '.' || filter[i] == ':'); j++, i++)
|
|
{
|
|
token[j] = filter[i];
|
|
}
|
|
if (j >= FILTER_TOKEN_MAXLEN)
|
|
{
|
|
return FALSE;
|
|
}
|
|
token[j] = '\0';
|
|
|
|
// Check for symbol:
|
|
key.name = token;
|
|
result = (PFILTER_TOKEN_NAME)bsearch((const void *)&key,
|
|
token_names, sizeof(token_names) / sizeof(FILTER_TOKEN_NAME),
|
|
sizeof(FILTER_TOKEN_NAME), DivertFilterTokenNameCompare);
|
|
if (result != NULL)
|
|
{
|
|
tokens[tp++].kind = result->kind;
|
|
continue;
|
|
}
|
|
|
|
// Check for base 10 number:
|
|
errno = 0;
|
|
num = strtoul(token, &end, 10);
|
|
if (errno == 0 && *end == '\0')
|
|
{
|
|
tokens[tp].kind = FILTER_TOKEN_NUMBER;
|
|
tokens[tp].val[0] = num;
|
|
tp++;
|
|
continue;
|
|
}
|
|
|
|
// Check for base 16 number:
|
|
errno = 0;
|
|
num = strtoul(token, &end, 16);
|
|
if (errno == 0 && *end == '\0')
|
|
{
|
|
tokens[tp].kind = FILTER_TOKEN_NUMBER;
|
|
tokens[tp].val[0] = num;
|
|
tp++;
|
|
continue;
|
|
}
|
|
|
|
// Check for IPv4 address:
|
|
if (DivertParseIPv4Address(token, tokens[tp].val))
|
|
{
|
|
tokens[tp].kind = FILTER_TOKEN_NUMBER;
|
|
tp++;
|
|
continue;
|
|
}
|
|
|
|
// Check for IPv6 address:
|
|
if (DivertParseIPv6Address(token, tokens[tp].val))
|
|
{
|
|
tokens[tp].kind = FILTER_TOKEN_NUMBER;
|
|
tp++;
|
|
continue;
|
|
}
|
|
|
|
return FALSE;
|
|
}
|
|
else
|
|
{
|
|
return FALSE;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Parse an IPv4 address.
|
|
*/
|
|
static BOOL DivertParseIPv4Address(char *str, UINT32 *addr_ptr)
|
|
{
|
|
UINT32 addr = 0;
|
|
UINT part, i;
|
|
|
|
errno = 0;
|
|
for (i = 0; i < 4; i++)
|
|
{
|
|
part = strtoul(str, &str, 10);
|
|
if (errno != 0 || part > UINT8_MAX)
|
|
{
|
|
return FALSE;
|
|
}
|
|
if (i != 3 && *str++ != '.')
|
|
{
|
|
return FALSE;
|
|
}
|
|
addr |= part << (8*(3-i));
|
|
}
|
|
*addr_ptr = addr;
|
|
return TRUE;
|
|
}
|
|
|
|
/*
|
|
* Parse an IPv6 address.
|
|
*/
|
|
static BOOL DivertParseIPv6Address(char *str, UINT32 *addr_ptr)
|
|
{
|
|
UINT16 addr[8] = {0};
|
|
UINT part;
|
|
UINT i, j;
|
|
BOOL end = FALSE;
|
|
|
|
if (*str == ':')
|
|
{
|
|
str++;
|
|
}
|
|
|
|
for (i = 0, j = 7; i < 8; i++)
|
|
{
|
|
if (*str == ':')
|
|
{
|
|
if (end)
|
|
{
|
|
return FALSE;
|
|
}
|
|
end = TRUE;
|
|
str++;
|
|
}
|
|
errno = 0;
|
|
part = strtoul(str, &str, 16);
|
|
if (part > UINT32_MAX || errno != 0 || (*str != ':' && *str != '\0'))
|
|
{
|
|
return FALSE;
|
|
}
|
|
if (!end)
|
|
{
|
|
addr[7 - i] = (UINT16)part;
|
|
}
|
|
else
|
|
{
|
|
addr[7 - j--] = (UINT16)part;
|
|
}
|
|
if (*str == '\0')
|
|
{
|
|
if (!end)
|
|
{
|
|
return FALSE;
|
|
}
|
|
break;
|
|
}
|
|
str++;
|
|
}
|
|
|
|
if (*str != '\0')
|
|
{
|
|
return FALSE;
|
|
}
|
|
|
|
if (end)
|
|
{
|
|
j++;
|
|
for (i = 7; j < i; j++, i--)
|
|
{
|
|
UINT16 tmp = addr[7 - i];
|
|
addr[7 - i] = addr[7 - j];
|
|
addr[7 - j] = tmp;
|
|
}
|
|
}
|
|
memcpy(addr_ptr, addr, sizeof(addr));
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
/*
|
|
* Parse the given filter.
|
|
*/
|
|
static BOOL DivertParseFilter(FILTER_TOKEN *tokens, UINT8 *tp,
|
|
divert_ioctl_filter_t filter, UINT8 *fp, FILTER_TOKEN_KIND op)
|
|
{
|
|
BOOL testop, fused, result, negate;
|
|
FILTER_TOKEN token;
|
|
UINT8 t, f, s, tmp;
|
|
s = *fp;
|
|
|
|
DivertParseFilterNext:
|
|
|
|
testop = TRUE;
|
|
fused = TRUE;
|
|
negate = FALSE;
|
|
token = tokens[*tp];
|
|
|
|
*tp = *tp + 1;
|
|
f = *fp;
|
|
if (f >= DIVERT_FILTER_MAXLEN)
|
|
{
|
|
return FALSE;
|
|
}
|
|
filter[f].success = DIVERT_FILTER_RESULT_ACCEPT;
|
|
filter[f].failure = DIVERT_FILTER_RESULT_REJECT;
|
|
filter[f].arg[1] = 0;
|
|
filter[f].arg[2] = 0;
|
|
filter[f].arg[3] = 0;
|
|
if (token.kind == FILTER_TOKEN_NOT)
|
|
{
|
|
negate = TRUE;
|
|
token = tokens[*tp];
|
|
*tp = *tp + 1;
|
|
}
|
|
switch (token.kind)
|
|
{
|
|
case FILTER_TOKEN_OPEN:
|
|
result = DivertParseFilter(tokens, tp, filter, fp,
|
|
FILTER_TOKEN_AND);
|
|
result = (result? (tokens[*tp].kind == FILTER_TOKEN_CLOSE): FALSE);
|
|
if (!result)
|
|
{
|
|
return FALSE;
|
|
}
|
|
*tp = *tp + 1;
|
|
testop = FALSE;
|
|
fused = FALSE;
|
|
break;
|
|
case FILTER_TOKEN_TRUE: case FILTER_TOKEN_FALSE:
|
|
filter[f].field = DIVERT_FILTER_FIELD_ZERO;
|
|
filter[f].test = DIVERT_FILTER_TEST_EQ;
|
|
filter[f].arg[0] = (token.kind == FILTER_TOKEN_FALSE);
|
|
testop = FALSE;
|
|
break;
|
|
case FILTER_TOKEN_OUTBOUND:
|
|
filter[f].field = DIVERT_FILTER_FIELD_OUTBOUND;
|
|
break;
|
|
case FILTER_TOKEN_INBOUND:
|
|
filter[f].field = DIVERT_FILTER_FIELD_INBOUND;
|
|
break;
|
|
case FILTER_TOKEN_IF_IDX:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IFIDX;
|
|
break;
|
|
case FILTER_TOKEN_SUB_IF_IDX:
|
|
filter[f].field = DIVERT_FILTER_FIELD_SUBIFIDX;
|
|
break;
|
|
case FILTER_TOKEN_IP:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IP;
|
|
break;
|
|
case FILTER_TOKEN_IPV6:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IPV6;
|
|
break;
|
|
case FILTER_TOKEN_ICMP:
|
|
filter[f].field = DIVERT_FILTER_FIELD_ICMP;
|
|
break;
|
|
case FILTER_TOKEN_ICMPV6:
|
|
filter[f].field = DIVERT_FILTER_FIELD_ICMPV6;
|
|
break;
|
|
case FILTER_TOKEN_TCP:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP;
|
|
break;
|
|
case FILTER_TOKEN_UDP:
|
|
filter[f].field = DIVERT_FILTER_FIELD_UDP;
|
|
break;
|
|
case FILTER_TOKEN_IP_HDR_LENGTH:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IP_HDRLENGTH;
|
|
break;
|
|
case FILTER_TOKEN_IP_TOS:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IP_TOS;
|
|
break;
|
|
case FILTER_TOKEN_IP_LENGTH:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IP_LENGTH;
|
|
break;
|
|
case FILTER_TOKEN_IP_ID:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IP_ID;
|
|
break;
|
|
case FILTER_TOKEN_IP_DF:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IP_DF;
|
|
break;
|
|
case FILTER_TOKEN_IP_MF:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IP_MF;
|
|
break;
|
|
case FILTER_TOKEN_IP_FRAG_OFF:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IP_FRAGOFF;
|
|
break;
|
|
case FILTER_TOKEN_IP_TTL:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IP_TTL;
|
|
break;
|
|
case FILTER_TOKEN_IP_PROTOCOL:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IP_PROTOCOL;
|
|
break;
|
|
case FILTER_TOKEN_IP_CHECKSUM:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IP_CHECKSUM;
|
|
break;
|
|
case FILTER_TOKEN_IP_SRC_ADDR:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IP_SRCADDR;
|
|
break;
|
|
case FILTER_TOKEN_IP_DST_ADDR:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IP_DSTADDR;
|
|
break;
|
|
case FILTER_TOKEN_IPV6_TRAFFIC_CLASS:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IPV6_TRAFFICCLASS;
|
|
break;
|
|
case FILTER_TOKEN_IPV6_FLOW_LABEL:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IPV6_FLOWLABEL;
|
|
break;
|
|
case FILTER_TOKEN_IPV6_LENGTH:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IPV6_LENGTH;
|
|
break;
|
|
case FILTER_TOKEN_IPV6_NEXT_HDR:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IPV6_NEXTHDR;
|
|
break;
|
|
case FILTER_TOKEN_IPV6_HOP_LIMIT:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IPV6_HOPLIMIT;
|
|
break;
|
|
case FILTER_TOKEN_IPV6_SRC_ADDR:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IPV6_SRCADDR;
|
|
break;
|
|
case FILTER_TOKEN_IPV6_DST_ADDR:
|
|
filter[f].field = DIVERT_FILTER_FIELD_IPV6_DSTADDR;
|
|
break;
|
|
case FILTER_TOKEN_ICMP_TYPE:
|
|
filter[f].field = DIVERT_FILTER_FIELD_ICMP_TYPE;
|
|
break;
|
|
case FILTER_TOKEN_ICMP_CODE:
|
|
filter[f].field = DIVERT_FILTER_FIELD_ICMP_CODE;
|
|
break;
|
|
case FILTER_TOKEN_ICMP_CHECKSUM:
|
|
filter[f].field = DIVERT_FILTER_FIELD_ICMP_CHECKSUM;
|
|
break;
|
|
case FILTER_TOKEN_ICMP_BODY:
|
|
filter[f].field = DIVERT_FILTER_FIELD_ICMP_BODY;
|
|
break;
|
|
case FILTER_TOKEN_ICMPV6_TYPE:
|
|
filter[f].field = DIVERT_FILTER_FIELD_ICMPV6_TYPE;
|
|
break;
|
|
case FILTER_TOKEN_ICMPV6_CODE:
|
|
filter[f].field = DIVERT_FILTER_FIELD_ICMPV6_CODE;
|
|
break;
|
|
case FILTER_TOKEN_ICMPV6_CHECKSUM:
|
|
filter[f].field = DIVERT_FILTER_FIELD_ICMPV6_CHECKSUM;
|
|
break;
|
|
case FILTER_TOKEN_ICMPV6_BODY:
|
|
filter[f].field = DIVERT_FILTER_FIELD_ICMPV6_BODY;
|
|
break;
|
|
case FILTER_TOKEN_TCP_SRC_PORT:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_SRCPORT;
|
|
break;
|
|
case FILTER_TOKEN_TCP_DST_PORT:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_DSTPORT;
|
|
break;
|
|
case FILTER_TOKEN_TCP_SEQ_NUM:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_SEQNUM;
|
|
break;
|
|
case FILTER_TOKEN_TCP_ACK_NUM:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_ACKNUM;
|
|
break;
|
|
case FILTER_TOKEN_TCP_HDR_LENGTH:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_HDRLENGTH;
|
|
break;
|
|
case FILTER_TOKEN_TCP_URG:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_URG;
|
|
break;
|
|
case FILTER_TOKEN_TCP_ACK:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_ACK;
|
|
break;
|
|
case FILTER_TOKEN_TCP_PSH:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_PSH;
|
|
break;
|
|
case FILTER_TOKEN_TCP_RST:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_RST;
|
|
break;
|
|
case FILTER_TOKEN_TCP_SYN:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_SYN;
|
|
break;
|
|
case FILTER_TOKEN_TCP_FIN:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_FIN;
|
|
break;
|
|
case FILTER_TOKEN_TCP_WINDOW:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_WINDOW;
|
|
break;
|
|
case FILTER_TOKEN_TCP_CHECKSUM:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_CHECKSUM;
|
|
break;
|
|
case FILTER_TOKEN_TCP_URG_PTR:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_URGPTR;
|
|
break;
|
|
case FILTER_TOKEN_TCP_PAYLOAD_LENGTH:
|
|
filter[f].field = DIVERT_FILTER_FIELD_TCP_PAYLOADLENGTH;
|
|
break;
|
|
case FILTER_TOKEN_UDP_SRC_PORT:
|
|
filter[f].field = DIVERT_FILTER_FIELD_UDP_SRCPORT;
|
|
break;
|
|
case FILTER_TOKEN_UDP_DST_PORT:
|
|
filter[f].field = DIVERT_FILTER_FIELD_UDP_DSTPORT;
|
|
break;
|
|
case FILTER_TOKEN_UDP_LENGTH:
|
|
filter[f].field = DIVERT_FILTER_FIELD_UDP_LENGTH;
|
|
break;
|
|
case FILTER_TOKEN_UDP_CHECKSUM:
|
|
filter[f].field = DIVERT_FILTER_FIELD_UDP_CHECKSUM;
|
|
break;
|
|
case FILTER_TOKEN_UDP_PAYLOAD_LENGTH:
|
|
filter[f].field = DIVERT_FILTER_FIELD_UDP_PAYLOADLENGTH;
|
|
break;
|
|
default:
|
|
return FALSE;
|
|
}
|
|
|
|
if (fused)
|
|
{
|
|
*fp = f+1;
|
|
}
|
|
|
|
if (testop)
|
|
{
|
|
token = tokens[*tp];
|
|
if (!negate)
|
|
{
|
|
switch (token.kind)
|
|
{
|
|
case FILTER_TOKEN_EQ:
|
|
filter[f].test = DIVERT_FILTER_TEST_EQ;
|
|
break;
|
|
case FILTER_TOKEN_NEQ:
|
|
filter[f].test = DIVERT_FILTER_TEST_NEQ;
|
|
break;
|
|
case FILTER_TOKEN_LT:
|
|
filter[f].test = DIVERT_FILTER_TEST_LT;
|
|
break;
|
|
case FILTER_TOKEN_LEQ:
|
|
filter[f].test = DIVERT_FILTER_TEST_LEQ;
|
|
break;
|
|
case FILTER_TOKEN_GT:
|
|
filter[f].test = DIVERT_FILTER_TEST_GT;
|
|
break;
|
|
case FILTER_TOKEN_GEQ:
|
|
filter[f].test = DIVERT_FILTER_TEST_GEQ;
|
|
break;
|
|
default:
|
|
filter[f].test = DIVERT_FILTER_TEST_NEQ;
|
|
filter[f].arg[0] = 0;
|
|
testop = FALSE;
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
switch (token.kind)
|
|
{
|
|
case FILTER_TOKEN_EQ:
|
|
filter[f].test = DIVERT_FILTER_TEST_NEQ;
|
|
break;
|
|
case FILTER_TOKEN_NEQ:
|
|
filter[f].test = DIVERT_FILTER_TEST_EQ;
|
|
break;
|
|
case FILTER_TOKEN_LT:
|
|
filter[f].test = DIVERT_FILTER_TEST_GEQ;
|
|
break;
|
|
case FILTER_TOKEN_LEQ:
|
|
filter[f].test = DIVERT_FILTER_TEST_GT;
|
|
break;
|
|
case FILTER_TOKEN_GT:
|
|
filter[f].test = DIVERT_FILTER_TEST_LEQ;
|
|
break;
|
|
case FILTER_TOKEN_GEQ:
|
|
filter[f].test = DIVERT_FILTER_TEST_LT;
|
|
break;
|
|
default:
|
|
filter[f].test = DIVERT_FILTER_TEST_EQ;
|
|
filter[f].arg[0] = 0;
|
|
testop = FALSE;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (testop)
|
|
{
|
|
*tp = *tp + 1;
|
|
token = tokens[*tp];
|
|
*tp = *tp + 1;
|
|
if (token.kind != FILTER_TOKEN_NUMBER)
|
|
{
|
|
return FALSE;
|
|
}
|
|
filter[f].arg[0] = token.val[0];
|
|
filter[f].arg[1] = token.val[1];
|
|
filter[f].arg[2] = token.val[2];
|
|
filter[f].arg[3] = token.val[3];
|
|
}
|
|
}
|
|
|
|
token = tokens[*tp];
|
|
if (token.kind != FILTER_TOKEN_AND && token.kind != FILTER_TOKEN_OR)
|
|
{
|
|
return TRUE;
|
|
}
|
|
if (op < token.kind)
|
|
{
|
|
op = token.kind;
|
|
f = s;
|
|
}
|
|
*tp = *tp + 1;
|
|
switch (token.kind)
|
|
{
|
|
case FILTER_TOKEN_AND:
|
|
DivertFilterUpdate(filter, f, *fp, *fp,
|
|
DIVERT_FILTER_RESULT_REJECT);
|
|
goto DivertParseFilterNext;
|
|
case FILTER_TOKEN_OR:
|
|
DivertFilterUpdate(filter, f, *fp, DIVERT_FILTER_RESULT_ACCEPT,
|
|
*fp);
|
|
goto DivertParseFilterNext;
|
|
}
|
|
return TRUE;
|
|
}
|
|
|
|
/*
|
|
* Negate a filter.
|
|
*/
|
|
static void DivertFilterNegate(divert_ioctl_filter_t filter, UINT8 s, UINT8 e)
|
|
{
|
|
// This is easy; simple swap REJECTs and ACCEPTs
|
|
DivertFilterUpdate(filter, s, e, DIVERT_FILTER_RESULT_REJECT,
|
|
DIVERT_FILTER_RESULT_ACCEPT);
|
|
}
|
|
|
|
/*
|
|
* Update success.
|
|
*/
|
|
static void DivertFilterUpdate(divert_ioctl_filter_t filter, UINT8 s, UINT8 e,
|
|
UINT8 success, UINT8 failure)
|
|
{
|
|
UINT8 i;
|
|
|
|
for (i = s; i < e; i++)
|
|
{
|
|
switch (filter[i].success)
|
|
{
|
|
case DIVERT_FILTER_RESULT_ACCEPT:
|
|
filter[i].success = success;
|
|
break;
|
|
case DIVERT_FILTER_RESULT_REJECT:
|
|
filter[i].success = failure;
|
|
break;
|
|
}
|
|
switch (filter[i].failure)
|
|
{
|
|
case DIVERT_FILTER_RESULT_ACCEPT:
|
|
filter[i].failure = success;
|
|
break;
|
|
case DIVERT_FILTER_RESULT_REJECT:
|
|
filter[i].failure = failure;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
#ifdef DIVERT_DEBUG
|
|
/*
|
|
* Print a filter (debugging).
|
|
*/
|
|
static void DivertFilterDump(divert_ioctl_filter_t filter, UINT8 len)
|
|
{
|
|
UINT8 i;
|
|
|
|
for (i = 0; i < len; i++)
|
|
{
|
|
printf("label_%u:\n\tif (", i);
|
|
switch (filter[i].field)
|
|
{
|
|
case DIVERT_FILTER_FIELD_ZERO:
|
|
printf("zero ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_INBOUND:
|
|
printf("inbound ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_OUTBOUND:
|
|
printf("outbound ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IFIDX:
|
|
printf("ifIdx ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_SUBIFIDX:
|
|
printf("subIfIdx ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IP:
|
|
printf("ip ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IPV6:
|
|
printf("ipv6 ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_ICMP:
|
|
printf("icmp ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_ICMPV6:
|
|
printf("icmpv6 ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP:
|
|
printf("tcp ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_UDP:
|
|
printf("udp ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IP_HDRLENGTH:
|
|
printf("ip.HdrLength ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IP_TOS:
|
|
printf("ip.TOS ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IP_LENGTH:
|
|
printf("ip.Length ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IP_ID:
|
|
printf("ip.Id ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IP_DF:
|
|
printf("ip.DF ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IP_MF:
|
|
printf("ip.MF ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IP_FRAGOFF:
|
|
printf("ip.FragOff ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IP_TTL:
|
|
printf("ip.TTL ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IP_PROTOCOL:
|
|
printf("ip.Protocol ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IP_CHECKSUM:
|
|
printf("ip.Checksum ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IP_SRCADDR:
|
|
printf("ip.SrcAddr ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IP_DSTADDR:
|
|
printf("ip.DstAddr ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IPV6_TRAFFICCLASS:
|
|
printf("ipv6.TrafficClass ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IPV6_FLOWLABEL:
|
|
printf("ipv6.FlowLabel ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IPV6_LENGTH:
|
|
printf("ipv6.Length ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IPV6_NEXTHDR:
|
|
printf("ipv6.NextHdr ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IPV6_HOPLIMIT:
|
|
printf("ipv6.HopLimit ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IPV6_SRCADDR:
|
|
printf("ipv6.SrcAddr ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_IPV6_DSTADDR:
|
|
printf("ipv6.DstAddr ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_ICMP_TYPE:
|
|
printf("icmp.Type ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_ICMP_CODE:
|
|
printf("icmp.Code ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_ICMP_CHECKSUM:
|
|
printf("icmp.Checksum ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_ICMP_BODY:
|
|
printf("icmp.Body ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_ICMPV6_TYPE:
|
|
printf("icmpv6.Type ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_ICMPV6_CODE:
|
|
printf("icmpv6.Code ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_ICMPV6_CHECKSUM:
|
|
printf("icmpv6.Checksum ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_ICMPV6_BODY:
|
|
printf("icmpv6.Body ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_SRCPORT:
|
|
printf("tcp.SrcPort ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_DSTPORT:
|
|
printf("tcp.DstPort ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_SEQNUM:
|
|
printf("tcp.SeqNum ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_ACKNUM:
|
|
printf("tcp.AckNum ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_HDRLENGTH:
|
|
printf("tcp.HdrLength ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_URG:
|
|
printf("tcp.Urg ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_ACK:
|
|
printf("tcp.Ack ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_PSH:
|
|
printf("tcp.Psh ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_RST:
|
|
printf("tcp.Rst ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_SYN:
|
|
printf("tcp.Syn ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_FIN:
|
|
printf("tcp.Fin ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_WINDOW:
|
|
printf("tcp.Window ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_CHECKSUM:
|
|
printf("tcp.Checksum ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_URGPTR:
|
|
printf("tcp.UrgPtr ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_TCP_PAYLOADLENGTH:
|
|
printf("tcp.PayloadLength " );
|
|
break;
|
|
case DIVERT_FILTER_FIELD_UDP_SRCPORT:
|
|
printf("udp.SrcPort ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_UDP_DSTPORT:
|
|
printf("udp.DstPort ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_UDP_LENGTH:
|
|
printf("udp.Length ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_UDP_CHECKSUM:
|
|
printf("udp.Checksum ");
|
|
break;
|
|
case DIVERT_FILTER_FIELD_UDP_PAYLOADLENGTH:
|
|
printf("udp.PayloadLength ");
|
|
break;
|
|
default:
|
|
printf("unknown.Field ");
|
|
break;
|
|
}
|
|
switch (filter[i].test)
|
|
{
|
|
case DIVERT_FILTER_TEST_EQ:
|
|
printf("== ");
|
|
break;
|
|
case DIVERT_FILTER_TEST_NEQ:
|
|
printf("!= ");
|
|
break;
|
|
case DIVERT_FILTER_TEST_LT:
|
|
printf("< ");
|
|
break;
|
|
case DIVERT_FILTER_TEST_LEQ:
|
|
printf("<= ");
|
|
break;
|
|
case DIVERT_FILTER_TEST_GT:
|
|
printf("> ");
|
|
break;
|
|
case DIVERT_FILTER_TEST_GEQ:
|
|
printf(">= ");
|
|
break;
|
|
default:
|
|
printf("?? ");
|
|
break;
|
|
}
|
|
printf("%u)\n", filter[i].arg[0]);
|
|
switch (filter[i].success)
|
|
{
|
|
case DIVERT_FILTER_RESULT_ACCEPT:
|
|
printf("\t\treturn ACCEPT;\n");
|
|
break;
|
|
case DIVERT_FILTER_RESULT_REJECT:
|
|
printf("\t\treturn REJECT;\n");
|
|
break;
|
|
default:
|
|
printf("\t\tgoto label_%u;\n", filter[i].success);
|
|
break;
|
|
}
|
|
printf("\telse\n");
|
|
switch (filter[i].failure)
|
|
{
|
|
case DIVERT_FILTER_RESULT_ACCEPT:
|
|
printf("\t\treturn ACCEPT;\n");
|
|
break;
|
|
case DIVERT_FILTER_RESULT_REJECT:
|
|
printf("\t\treturn REJECT;\n");
|
|
break;
|
|
default:
|
|
printf("\t\tgoto label_%u;\n", filter[i].failure);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif /* DIVERT_DEBUG */
|
|
|
|
/****************************************************************************/
|
|
/* DIVERT HELPER IMPLEMENTATION */
|
|
/****************************************************************************/
|
|
|
|
/*
|
|
* Parse IPv4/IPv6/ICMP/ICMPv6/TCP/UDP headers from a raw packet.
|
|
*/
|
|
extern BOOL DivertHelperParse(PDIVERT_PACKET pPacket, UINT packetLen,
|
|
PDIVERT_IPHDR *ppIpHdr, PDIVERT_IPV6HDR *ppIpv6Hdr,
|
|
PDIVERT_ICMPHDR *ppIcmpHdr, PDIVERT_ICMPV6HDR *ppIcmpv6Hdr,
|
|
PDIVERT_TCPHDR *ppTcpHdr, PDIVERT_UDPHDR *ppUdpHdr, PVOID *ppData,
|
|
UINT *pDataLen)
|
|
{
|
|
PDIVERT_IPHDR ip_header = NULL;
|
|
PDIVERT_IPV6HDR ipv6_header = NULL;
|
|
PDIVERT_ICMPHDR icmp_header = NULL;
|
|
PDIVERT_ICMPV6HDR icmpv6_header = NULL;
|
|
PDIVERT_TCPHDR tcp_header = NULL;
|
|
PDIVERT_UDPHDR udp_header = NULL;
|
|
UINT16 header_len;
|
|
UINT8 trans_proto;
|
|
PVOID data = NULL;
|
|
UINT data_len = 0;
|
|
BOOL success;
|
|
|
|
if (pPacket == NULL ||
|
|
packetLen < sizeof(DIVERT_PACKET) + sizeof(UINT8))
|
|
{
|
|
goto DivertHelperParseExit;
|
|
}
|
|
data = DIVERT_PACKET_DATA(pPacket);
|
|
data_len = packetLen - sizeof(DIVERT_PACKET);
|
|
|
|
ip_header = (PDIVERT_IPHDR)data;
|
|
switch (ip_header->Version)
|
|
{
|
|
case 4:
|
|
if (data_len < sizeof(DIVERT_IPHDR) ||
|
|
ip_header->HdrLength < 5 ||
|
|
data_len < ip_header->HdrLength*sizeof(UINT32) ||
|
|
ntohs(ip_header->Length) != data_len)
|
|
{
|
|
ip_header = NULL;
|
|
goto DivertHelperParseExit;
|
|
}
|
|
trans_proto = ip_header->Protocol;
|
|
header_len = ip_header->HdrLength*sizeof(UINT32);
|
|
data = (PVOID)((UINT8 *)data + header_len);
|
|
data_len -= header_len;
|
|
break;
|
|
case 6:
|
|
ip_header = NULL;
|
|
ipv6_header = (PDIVERT_IPV6HDR)data;
|
|
if (data_len < sizeof(DIVERT_IPV6HDR) ||
|
|
ntohs(ipv6_header->Length) !=
|
|
data_len - sizeof(DIVERT_IPV6HDR))
|
|
{
|
|
ipv6_header = NULL;
|
|
goto DivertHelperParseExit;
|
|
}
|
|
trans_proto = ipv6_header->NextHdr;
|
|
data = (PVOID)((UINT8 *)data + sizeof(DIVERT_IPV6HDR));
|
|
data_len -= sizeof(DIVERT_IPV6HDR);
|
|
break;
|
|
default:
|
|
ip_header = NULL;
|
|
goto DivertHelperParseExit;
|
|
}
|
|
|
|
switch (trans_proto)
|
|
{
|
|
case IPPROTO_TCP:
|
|
tcp_header = (PDIVERT_TCPHDR)data;
|
|
if (data_len < sizeof(DIVERT_TCPHDR) ||
|
|
tcp_header->HdrLength < 5 ||
|
|
data_len < tcp_header->HdrLength*sizeof(UINT32))
|
|
{
|
|
tcp_header = NULL;
|
|
goto DivertHelperParseExit;
|
|
}
|
|
header_len = tcp_header->HdrLength*sizeof(UINT32);
|
|
data = ((UINT8 *)data + header_len);
|
|
data_len -= header_len;
|
|
break;
|
|
case IPPROTO_UDP:
|
|
udp_header = (PDIVERT_UDPHDR)data;
|
|
if (data_len < sizeof(DIVERT_UDPHDR) ||
|
|
ntohs(udp_header->Length) != data_len)
|
|
{
|
|
udp_header = NULL;
|
|
goto DivertHelperParseExit;
|
|
}
|
|
data = ((UINT8 *)data + sizeof(DIVERT_UDPHDR));
|
|
data_len -= sizeof(DIVERT_UDPHDR);
|
|
break;
|
|
case IPPROTO_ICMP:
|
|
icmp_header = (PDIVERT_ICMPHDR)data;
|
|
if (ip_header == NULL ||
|
|
data_len < sizeof(DIVERT_ICMPHDR))
|
|
{
|
|
icmp_header = NULL;
|
|
goto DivertHelperParseExit;
|
|
}
|
|
data = ((UINT8 *)data + sizeof(DIVERT_ICMPHDR));
|
|
data_len -= sizeof(DIVERT_ICMPHDR);
|
|
break;
|
|
case IPPROTO_ICMPV6:
|
|
icmpv6_header = (PDIVERT_ICMPV6HDR)data;
|
|
if (ipv6_header == NULL ||
|
|
data_len < sizeof(DIVERT_ICMPV6HDR))
|
|
{
|
|
icmpv6_header = NULL;
|
|
goto DivertHelperParseExit;
|
|
}
|
|
data = ((UINT8 *)data + sizeof(DIVERT_ICMPV6HDR));
|
|
data_len -= sizeof(DIVERT_ICMPV6HDR);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
if (data_len == 0)
|
|
{
|
|
data = NULL;
|
|
}
|
|
|
|
DivertHelperParseExit:
|
|
success = TRUE;
|
|
if (ppIpHdr != NULL)
|
|
{
|
|
*ppIpHdr = ip_header;
|
|
success = success && (ip_header != NULL);
|
|
}
|
|
if (ppIpv6Hdr != NULL)
|
|
{
|
|
*ppIpv6Hdr = ipv6_header;
|
|
success = success && (ipv6_header != NULL);
|
|
}
|
|
if (ppIcmpHdr != NULL)
|
|
{
|
|
*ppIcmpHdr = icmp_header;
|
|
success = success && (icmp_header != NULL);
|
|
}
|
|
if (ppIcmpv6Hdr != NULL)
|
|
{
|
|
*ppIcmpv6Hdr = icmpv6_header;
|
|
success = success && (icmpv6_header != NULL);
|
|
}
|
|
if (ppTcpHdr != NULL)
|
|
{
|
|
*ppTcpHdr = tcp_header;
|
|
success = success && (tcp_header != NULL);
|
|
}
|
|
if (ppUdpHdr != NULL)
|
|
{
|
|
*ppUdpHdr = udp_header;
|
|
success = success && (udp_header != NULL);
|
|
}
|
|
if (ppData != NULL)
|
|
{
|
|
*ppData = data;
|
|
success = success && (data != NULL);
|
|
}
|
|
if (pDataLen != NULL)
|
|
{
|
|
*pDataLen = data_len;
|
|
}
|
|
return success;
|
|
}
|
|
|
|
/*
|
|
* Calculate IPv4/IPv6/ICMP/ICMPv6/TCP/UDP checksums.
|
|
*/
|
|
extern UINT DivertHelperCalcChecksums(PDIVERT_PACKET pPacket, UINT packetLen,
|
|
UINT64 flags)
|
|
{
|
|
DIVERT_PSEUDOHDR pseudo_header;
|
|
DIVERT_PSEUDOV6HDR pseudov6_header;
|
|
BOOL have_pseudo_header = FALSE, have_v6pseudo_header = FALSE;
|
|
PDIVERT_IPHDR ip_header;
|
|
PDIVERT_IPV6HDR ipv6_header;
|
|
PDIVERT_ICMPHDR icmp_header;
|
|
PDIVERT_ICMPV6HDR icmpv6_header;
|
|
PDIVERT_TCPHDR tcp_header;
|
|
PDIVERT_UDPHDR udp_header;
|
|
UINT payload_len, checksum_len;
|
|
UINT count = 0;
|
|
|
|
DivertHelperParse(pPacket, packetLen, &ip_header, &ipv6_header,
|
|
&icmp_header, &icmpv6_header, &tcp_header, &udp_header, NULL,
|
|
&payload_len);
|
|
|
|
if (ip_header != NULL && !(flags & DIVERT_HELPER_NO_IP_CHECKSUM))
|
|
{
|
|
ip_header->Checksum = 0;
|
|
ip_header->Checksum = DivertHelperCalcChecksum(NULL, 0,
|
|
ip_header, ip_header->HdrLength*sizeof(UINT32));
|
|
count++;
|
|
}
|
|
|
|
if (icmp_header != NULL)
|
|
{
|
|
if (flags & DIVERT_HELPER_NO_ICMP_CHECKSUM)
|
|
{
|
|
return count;
|
|
}
|
|
icmp_header->Checksum = 0;
|
|
icmp_header->Checksum = DivertHelperCalcChecksum(NULL, 0,
|
|
icmp_header, payload_len + sizeof(DIVERT_ICMPHDR));
|
|
count++;
|
|
return count;
|
|
}
|
|
|
|
if (icmpv6_header != NULL)
|
|
{
|
|
if (flags & DIVERT_HELPER_NO_ICMPV6_CHECKSUM)
|
|
{
|
|
return count;
|
|
}
|
|
checksum_len = payload_len + sizeof(DIVERT_ICMPV6HDR);
|
|
DivertInitPseudoHeaderV6(ipv6_header, &pseudov6_header,
|
|
IPPROTO_ICMPV6, checksum_len);
|
|
icmpv6_header->Checksum = 0;
|
|
icmpv6_header->Checksum = DivertHelperCalcChecksum(&pseudov6_header,
|
|
sizeof(pseudov6_header), icmpv6_header, checksum_len);
|
|
count++;
|
|
return count;
|
|
}
|
|
|
|
if (tcp_header != NULL)
|
|
{
|
|
if (flags & DIVERT_HELPER_NO_TCP_CHECKSUM)
|
|
{
|
|
return count;
|
|
}
|
|
checksum_len = payload_len + tcp_header->HdrLength*sizeof(UINT32);
|
|
if (ip_header != NULL)
|
|
{
|
|
DivertInitPseudoHeader(ip_header, &pseudo_header, IPPROTO_TCP,
|
|
checksum_len);
|
|
tcp_header->Checksum = 0;
|
|
tcp_header->Checksum = DivertHelperCalcChecksum(&pseudo_header,
|
|
sizeof(pseudo_header), tcp_header, checksum_len);
|
|
}
|
|
else
|
|
{
|
|
DivertInitPseudoHeaderV6(ipv6_header, &pseudov6_header,
|
|
IPPROTO_TCP, checksum_len);
|
|
tcp_header->Checksum = 0;
|
|
tcp_header->Checksum = DivertHelperCalcChecksum(&pseudov6_header,
|
|
sizeof(pseudov6_header), tcp_header, checksum_len);
|
|
}
|
|
count++;
|
|
return count;
|
|
}
|
|
|
|
if (udp_header != NULL)
|
|
{
|
|
if (flags & DIVERT_HELPER_NO_UDP_CHECKSUM)
|
|
{
|
|
return count;
|
|
}
|
|
checksum_len = payload_len + sizeof(DIVERT_UDPHDR);
|
|
if (ip_header != NULL)
|
|
{
|
|
DivertInitPseudoHeader(ip_header, &pseudo_header, IPPROTO_UDP,
|
|
checksum_len);
|
|
udp_header->Checksum = 0;
|
|
udp_header->Checksum = DivertHelperCalcChecksum(&pseudo_header,
|
|
sizeof(pseudo_header), udp_header, checksum_len);
|
|
if (udp_header->Checksum == 0)
|
|
{
|
|
udp_header->Checksum = 0xFFFF;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
DivertInitPseudoHeaderV6(ipv6_header, &pseudov6_header,
|
|
IPPROTO_UDP, checksum_len);
|
|
udp_header->Checksum = 0;
|
|
udp_header->Checksum = DivertHelperCalcChecksum(&pseudov6_header,
|
|
sizeof(pseudov6_header), udp_header, checksum_len);
|
|
}
|
|
count++;
|
|
}
|
|
return count;
|
|
}
|
|
|
|
/*
|
|
* Initialize the IP pseudo header.
|
|
*/
|
|
static void DivertInitPseudoHeader(PDIVERT_IPHDR ip_header,
|
|
PDIVERT_PSEUDOHDR pseudo_header, UINT8 protocol, UINT len)
|
|
{
|
|
pseudo_header->SrcAddr = ip_header->SrcAddr;
|
|
pseudo_header->DstAddr = ip_header->DstAddr;
|
|
pseudo_header->Zero = 0;
|
|
pseudo_header->Protocol = protocol;
|
|
pseudo_header->Length = htons((UINT16)len);
|
|
}
|
|
|
|
/*
|
|
* Initialize the IPv6 pseudo header.
|
|
*/
|
|
static void DivertInitPseudoHeaderV6(PDIVERT_IPV6HDR ipv6_header,
|
|
PDIVERT_PSEUDOV6HDR pseudov6_header, UINT8 protocol, UINT len)
|
|
{
|
|
memcpy(pseudov6_header->SrcAddr, ipv6_header->SrcAddr,
|
|
sizeof(pseudov6_header->SrcAddr));
|
|
memcpy(pseudov6_header->DstAddr, ipv6_header->DstAddr,
|
|
sizeof(pseudov6_header->DstAddr));
|
|
pseudov6_header->Length = htonl((UINT32)len);
|
|
pseudov6_header->NextHdr = protocol;
|
|
pseudov6_header->Zero = 0;
|
|
}
|
|
|
|
/*
|
|
* Generic checksum computation.
|
|
*/
|
|
static UINT16 DivertHelperCalcChecksum(PVOID pseudo_header,
|
|
UINT16 pseudo_header_len, PVOID data, UINT len)
|
|
{
|
|
register const UINT16 *data16 = (const UINT16 *)pseudo_header;
|
|
register size_t len16 = pseudo_header_len >> 1;
|
|
register UINT32 sum = 0;
|
|
size_t i;
|
|
|
|
// Pseudo header:
|
|
for (i = 0; i < len16; i++)
|
|
{
|
|
sum += (UINT32)data16[i];
|
|
}
|
|
|
|
// Main data:
|
|
data16 = (const UINT16 *)data;
|
|
len16 = len >> 1;
|
|
for (i = 0; i < len16; i++)
|
|
{
|
|
sum += (UINT32)data16[i];
|
|
}
|
|
|
|
if (len & 0x1)
|
|
{
|
|
const UINT8 *data8 = (const UINT8 *)data;
|
|
sum += (UINT16)data8[len-1];
|
|
}
|
|
|
|
sum = (sum & 0xFFFF) + (sum >> 16);
|
|
sum += (sum >> 16);
|
|
sum = ~sum;
|
|
return (UINT16)sum;
|
|
}
|
|
|