// Build the Attribute list
BUF *SstpBuildAttributeList(LIST *o, USHORT message_type)
{
UINT i;
BUF *b;
USHORT us;
// Validate arguments
if (o == NULL)
{
return NULL;
}
b = NewBuf();
us = Endian16(message_type);
WriteBuf(b, &us, sizeof(USHORT));
us = Endian16((USHORT)LIST_NUM(o));
WriteBuf(b, &us, sizeof(USHORT));
for (i = 0;i < LIST_NUM(o);i++)
{
SSTP_ATTRIBUTE *a = LIST_DATA(o, i);
BUF *ab = SstpBuildAttribute(a);
if (ab != NULL)
{
WriteBufBuf(b, ab);
FreeBuf(ab);
}
}
return b;
}
// Received an ARP packet
void L3RecvArp(L3IF *f, PKT *p)
{
ARPV4_HEADER *a;
// Validate arguments
if (f == NULL || p == NULL)
{
return;
}
a = p->L3.ARPv4Header;
if (Endian16(a->HardwareType) != ARP_HARDWARE_TYPE_ETHERNET ||
Endian16(a->ProtocolType) != MAC_PROTO_IPV4 ||
a->HardwareSize != 6 || a->ProtocolSize != 4)
{
return;
}
if (Cmp(a->SrcAddress, p->MacAddressSrc, 6) != 0)
{
return;
}
switch (Endian16(a->Operation))
{
case ARP_OPERATION_REQUEST:
// ARP request arrives
L3RecvArpRequest(f, p);
break;
case ARP_OPERATION_RESPONSE:
// ARP response arrives
L3RecvArpResponse(f, p);
break;
}
}
// Build the Attribute
BUF *SstpBuildAttribute(SSTP_ATTRIBUTE *a)
{
UCHAR uc;
USHORT us;
BUF *b;
// Validate arguments
if (a == NULL)
{
return NULL;
}
b = NewBuf();
// Reserved
uc = 0;
WriteBuf(b, &uc, sizeof(UCHAR));
// Attribute ID
uc = a->AttributeId;
WriteBuf(b, &uc, sizeof(UCHAR));
// LengthPacket
a->TotalLength = a->DataSize + 4;
us = (USHORT)a->TotalLength;
us = Endian16(us);
WriteBuf(b, &us, sizeof(USHORT));
// Data
WriteBuf(b, a->Data, a->DataSize);
return b;
}
// TAG VLAN パース
bool ParsePacketTAGVLAN(PKT *p, UCHAR *buf, UINT size)
{
USHORT vlan_ushort;
// 引数チェック
if (p == NULL || buf == NULL)
{
return false;
}
// サイズをチェック
if (size < sizeof(TAGVLAN_HEADER))
{
return false;
}
// TAG VLAN ヘッダ
p->L3.TagVlanHeader = (TAGVLAN_HEADER *)buf;
p->TypeL3 = L3_TAGVLAN;
buf += sizeof(TAGVLAN_HEADER);
size -= sizeof(TAGVLAN_HEADER);
vlan_ushort = Endian16(*((USHORT *)p->L3.TagVlanHeader->Data));
vlan_ushort = vlan_ushort & 0xFFF;
p->VlanId = vlan_ushort;
return true;
}
// パケットに VLAN タグを埋め込む
void VLanInsertTag(void **packet_data, UINT *packet_size, UINT vlan_id)
{
UINT dest_size;
UCHAR *dest_data;
UINT src_size;
UCHAR *src_data;
USHORT vlan_ushort = Endian16(((USHORT)vlan_id) & 0xFFF);
// 引数チェック
if (packet_data == NULL || *packet_data == NULL || packet_size == NULL ||
*packet_size < 14 || vlan_id == 0)
{
return;
}
src_size = *packet_size;
src_data = (UCHAR *)(*packet_data);
dest_size = src_size + 4;
dest_data = Malloc(dest_size);
dest_data[12] = 0x81;
dest_data[13] = 0x00;
Copy(&dest_data[14], &vlan_ushort, sizeof(USHORT));
Copy(&dest_data[0], &src_data[0], 12);
Copy(&dest_data[16], &src_data[12], src_size - 12);
*packet_size = dest_size;
*packet_data = dest_data;
Free(src_data);
}
// Send an L2 packet immediately
void L3SendL2Now(L3IF *f, UCHAR *dest_mac, UCHAR *src_mac, USHORT protocol, void *data, UINT size)
{
UCHAR *buf;
MAC_HEADER *mac_header;
PKT *p;
// Validate arguments
if (f == NULL || dest_mac == NULL || src_mac == NULL || data == NULL)
{
return;
}
// Buffer creation
buf = Malloc(MAC_HEADER_SIZE + size);
// MAC header
mac_header = (MAC_HEADER *)&buf[0];
Copy(mac_header->DestAddress, dest_mac, 6);
Copy(mac_header->SrcAddress, src_mac, 6);
mac_header->Protocol = Endian16(protocol);
// Copy data
Copy(&buf[sizeof(MAC_HEADER)], data, size);
// Size
size += sizeof(MAC_HEADER);
// Packet generation
p = ZeroMalloc(sizeof(PKT));
p->PacketData = buf;
p->PacketSize = size;
// Add to the queue
InsertQueue(f->SendQueue, p);
}
// Send the IP packet immediately
void L3SendIpNow(L3IF *f, L3ARPENTRY *a, L3PACKET *p)
{
// Validate arguments
if (f == NULL || p == NULL)
{
return;
}
L3SendL2Now(f, a != NULL ? a->MacAddress : broadcast, f->MacAddress, Endian16(p->Packet->MacHeader->Protocol),
p->Packet->L3.PointerL3, p->Packet->PacketSize - sizeof(MAC_HEADER));
}
// UDP パース
bool ParseUDP(PKT *p, UCHAR *buf, UINT size)
{
USHORT src_port, dst_port;
// 引数チェック
if (p == NULL || buf == NULL)
{
return false;
}
// サイズをチェック
if (size < sizeof(UDP_HEADER))
{
// サイズが不正
return false;
}
// UDP ヘッダ
p->L4.UDPHeader = (UDP_HEADER *)buf;
p->TypeL4 = L4_UDP;
buf += sizeof(UDP_HEADER);
size -= sizeof(UDP_HEADER);
// ポート番号をチェック
src_port = Endian16(p->L4.UDPHeader->SrcPort);
dst_port = Endian16(p->L4.UDPHeader->DstPort);
if ((src_port == 67 && dst_port == 68) ||
(src_port == 68 && dst_port == 67))
{
if (p->TypeL3 == L3_IPV4)
{
// DHCP パケットを発見
ParseDHCPv4(p, buf, size);
}
}
return true;
}
// Send an ARP request packet
void L3SendArpRequestNow(L3IF *f, UINT dest_ip)
{
ARPV4_HEADER arp;
// Validate arguments
if (f == NULL)
{
return;
}
// Build an ARP header
arp.HardwareType = Endian16(ARP_HARDWARE_TYPE_ETHERNET);
arp.ProtocolType = Endian16(MAC_PROTO_IPV4);
arp.HardwareSize = 6;
arp.ProtocolSize = 4;
arp.Operation = Endian16(ARP_OPERATION_REQUEST);
Copy(arp.SrcAddress, f->MacAddress, 6);
arp.SrcIP = f->IpAddress;
Zero(&arp.TargetAddress, 6);
arp.TargetIP = dest_ip;
// Transmission
L3SendL2Now(f, broadcast, f->MacAddress, MAC_PROTO_ARPV4, &arp, sizeof(arp));
}
// Send an ARP response packet
void L3SendArpResponseNow(L3IF *f, UCHAR *dest_mac, UINT dest_ip, UINT src_ip)
{
ARPV4_HEADER arp;
// Validate arguments
if (f == NULL || dest_mac == NULL)
{
return;
}
// Build a header
arp.HardwareType = Endian16(ARP_HARDWARE_TYPE_ETHERNET);
arp.ProtocolType = Endian16(MAC_PROTO_IPV4);
arp.HardwareSize = 6;
arp.ProtocolSize = 4;
arp.Operation = Endian16(ARP_OPERATION_RESPONSE);
Copy(arp.SrcAddress, f->MacAddress, 6);
Copy(arp.TargetAddress, dest_mac, 6);
arp.SrcIP = src_ip;
arp.TargetIP = dest_ip;
// Transmission
L3SendL2Now(f, dest_mac, f->MacAddress, MAC_PROTO_ARPV4, &arp, sizeof(arp));
}
// Building the SSTP packet
BUF *SstpBuildPacket(SSTP_PACKET *p)
{
BUF *b;
UCHAR uc;
USHORT us;
// Validate arguments
if (p == NULL)
{
return NULL;
}
b = NewBuf();
if (p->IsControl)
{
BUF *ab;
if (p->Data != NULL)
{
Free(p->Data);
}
ab = SstpBuildAttributeList(p->AttibuteList, p->MessageType);
p->Data = ab->Buf;
p->DataSize = ab->Size;
Free(ab);
}
// Version
uc = SSTP_VERSION_1;
WriteBuf(b, &uc, sizeof(UCHAR));
// Flag
uc = p->IsControl ? 1 : 0;
WriteBuf(b, &uc, sizeof(UCHAR));
// Length Packet
us = Endian16(p->DataSize + 4);
WriteBuf(b, &us, sizeof(USHORT));
// Data
WriteBuf(b, p->Data, p->DataSize);
return b;
}
// パケットから VLAN タグを取り除く
bool VLanRemoveTag(void **packet_data, UINT *packet_size, UINT vlan_id)
{
bool has_vlan_tag = false;
UCHAR *src_data;
UINT src_size;
// 引数チェック
if (packet_data == NULL || *packet_data == NULL || packet_size == NULL ||
*packet_size < 14)
{
return false;
}
src_data = (UCHAR *)(*packet_data);
src_size = *packet_size;
if (src_data[12] == 0x81 && src_data[13] == 0x00)
{
if (src_size >= 18)
{
USHORT vlan_ushort;
vlan_ushort = Endian16(*((USHORT *)&src_data[14]));
vlan_ushort = vlan_ushort & 0xFFF;
if (vlan_id == 0 || (vlan_ushort == vlan_id))
{
UINT dest_size = src_size - 4;
UINT i;
for (i = 12;i < dest_size;i++)
{
src_data[i] = src_data[i + 4];
}
*packet_size = dest_size;
return true;
}
}
}
return false;
}
// Send
void UdpAccelSend(UDP_ACCEL *a, UCHAR *data, UINT data_size, bool compressed, UINT max_size, bool high_priority)
{
UCHAR tmp[UDP_ACCELERATION_TMP_BUF_SIZE];
UCHAR *buf;
UINT size;
UCHAR key[UDP_ACCELERATION_PACKET_KEY_SIZE];
UINT64 ui64;
USHORT us;
UCHAR c;
UINT current_size;
UINT ui32;
bool fatal_error = false;
UINT r;
// Validate arguments
if (a == NULL || (data_size != 0 && data == NULL))
{
return;
}
if (max_size == 0)
{
max_size = INFINITE;
}
buf = tmp;
size = 0;
// IV
if (a->PlainTextMode == false)
{
// IV
Copy(buf, a->NextIv, UDP_ACCELERATION_PACKET_IV_SIZE);
buf += UDP_ACCELERATION_PACKET_IV_SIZE;
size += UDP_ACCELERATION_PACKET_IV_SIZE;
// Calculate the key
UdpAccelCalcKey(key, a->MyKey, a->NextIv);
if (false)
{
char tmp1[256];
char tmp2[256];
char tmp3[256];
BinToStr(tmp1, sizeof(tmp1), a->MyKey, sizeof(a->MyKey));
BinToStr(tmp2, sizeof(tmp2), a->NextIv, UDP_ACCELERATION_PACKET_IV_SIZE);
BinToStr(tmp3, sizeof(tmp3), key, sizeof(key));
Debug("My Key : %s\n"
"IV : %s\n"
"Comm Key: %s\n",
tmp1, tmp2, tmp3);
}
}
// Cookie
ui32 = Endian32(a->YourCookie);
Copy(buf, &ui32, sizeof(UINT));
buf += sizeof(UINT);
size += sizeof(UINT);
// My Tick
ui64 = Endian64(a->Now == 0 ? 1ULL : a->Now);
Copy(buf, &ui64, sizeof(UINT64));
buf += sizeof(UINT64);
size += sizeof(UINT64);
// Your Tick
ui64 = Endian64(a->LastRecvYourTick);
Copy(buf, &ui64, sizeof(UINT64));
buf += sizeof(UINT64);
size += sizeof(UINT64);
// Size
us = Endian16(data_size);
Copy(buf, &us, sizeof(USHORT));
buf += sizeof(USHORT);
size += sizeof(USHORT);
// Compress Flag
c = (compressed ? 1 : 0);
Copy(buf, &c, sizeof(UCHAR));
buf += sizeof(UCHAR);
size += sizeof(UCHAR);
// Data
if (data_size >= 1)
{
Copy(buf, data, data_size);
buf += data_size;
size += data_size;
}
if (a->PlainTextMode == false)
{
static UCHAR zero[UDP_ACCELERATION_PACKET_IV_SIZE] = {0};
CRYPT *c;
current_size = UDP_ACCELERATION_PACKET_IV_SIZE + sizeof(UINT) + sizeof(UINT64) * 2 +
sizeof(USHORT) + sizeof(UCHAR) + data_size + UDP_ACCELERATION_PACKET_IV_SIZE;
if (current_size < max_size)
{
// Padding
UCHAR pad[UDP_ACCELERATION_MAX_PADDING_SIZE];
UINT pad_size = MIN(max_size - current_size, UDP_ACCELERATION_MAX_PADDING_SIZE);
pad_size = rand() % pad_size;
Zero(pad, sizeof(pad));
Copy(buf, pad, pad_size);
buf += pad_size;
size += pad_size;
}
// Verify
Copy(buf, zero, UDP_ACCELERATION_PACKET_IV_SIZE);
buf += UDP_ACCELERATION_PACKET_IV_SIZE;
size += UDP_ACCELERATION_PACKET_IV_SIZE;
// Encryption
c = NewCrypt(key, UDP_ACCELERATION_PACKET_KEY_SIZE);
Encrypt(c, tmp + UDP_ACCELERATION_PACKET_IV_SIZE, tmp + UDP_ACCELERATION_PACKET_IV_SIZE, size - UDP_ACCELERATION_PACKET_IV_SIZE);
FreeCrypt(c);
// Next Iv
Copy(a->NextIv, buf - UDP_ACCELERATION_PACKET_IV_SIZE, UDP_ACCELERATION_PACKET_IV_SIZE);
}
// Send
SetSockHighPriority(a->UdpSock, high_priority);
r = SendTo(a->UdpSock, &a->YourIp, a->YourPort, tmp, size);
if (r == 0 && a->UdpSock->IgnoreSendErr == false)
{
fatal_error = true;
Debug("Error: SendTo: %r %u %u\n", &a->YourIp, a->YourPort, size);
WHERE;
}
if (data_size == 0)
{
if (UdpAccelIsSendReady(a, true) == false)
{
if ((a->YourPortByNatTServer != 0) && (a->YourPort != a->YourPortByNatTServer))
{
r = SendTo(a->UdpSock, &a->YourIp, a->YourPortByNatTServer, tmp, size);
if (r == 0 && a->UdpSock->IgnoreSendErr == false)
{
fatal_error = true;
WHERE;
}
}
}
}
if (data_size == 0)
{
if (IsZeroIP(&a->YourIp2) == false && CmpIpAddr(&a->YourIp, &a->YourIp2) != 0)
{
if (UdpAccelIsSendReady(a, true) == false)
{
// When the KeepAlive, if the opponent may be behind a NAT,
// send the packet to the IP address of outside of the NAT
r = SendTo(a->UdpSock, &a->YourIp2, a->YourPort, tmp, size);
if (r == 0 && a->UdpSock->IgnoreSendErr == false)
{
fatal_error = true;
WHERE;
}
if ((a->YourPortByNatTServer != 0) && (a->YourPort != a->YourPortByNatTServer))
{
r = SendTo(a->UdpSock, &a->YourIp2, a->YourPortByNatTServer, tmp, size);
if (r == 0 && a->UdpSock->IgnoreSendErr == false)
{
fatal_error = true;
WHERE;
}
}
}
}
}
if (fatal_error)
{
a->FatalError = true;
WHERE;
}
//Debug("UDP Send: %u\n", size);
}
// ICMPv6 パース
bool ParseICMPv6(PKT *p, UCHAR *buf, UINT size)
{
ICMPV6_HEADER_INFO icmp_info;
ICMP_HEADER *icmp;
ICMP_ECHO *echo;
UINT msg_size;
// 引数チェック
if (p == NULL || buf == NULL)
{
return false;
}
Zero(&icmp_info, sizeof(icmp_info));
if (size < sizeof(ICMP_HEADER))
{
return false;
}
icmp = (ICMP_HEADER *)buf;
p->L4.ICMPHeader = icmp;
msg_size = size - sizeof(ICMP_HEADER);
icmp_info.Type = icmp->Type;
icmp_info.Code = icmp->Code;
icmp_info.Data = ((UCHAR *)buf) + sizeof(ICMP_HEADER);
icmp_info.DataSize = msg_size;
switch (icmp_info.Type)
{
case ICMPV6_TYPE_ECHO_REQUEST:
case ICMPV6_TYPE_ECHO_RESPONSE:
// ICMP Echo Request / Response
if (icmp_info.DataSize < sizeof(ICMP_ECHO))
{
return false;
}
echo = (ICMP_ECHO *)icmp_info.Data;
icmp_info.EchoHeader.Identifier = Endian16(echo->Identifier);
icmp_info.EchoHeader.SeqNo = Endian16(echo->SeqNo);
icmp_info.EchoData = (UCHAR *)echo + sizeof(ICMP_ECHO);
icmp_info.EchoDataSize = icmp_info.DataSize - sizeof(ICMP_ECHO);
break;
case ICMPV6_TYPE_ROUTER_SOLICIATION:
// ルータ要請
if (icmp_info.DataSize < sizeof(ICMPV6_ROUTER_SOLICIATION_HEADER))
{
return false;
}
icmp_info.Headers.RouterSoliciationHeader =
(ICMPV6_ROUTER_SOLICIATION_HEADER *)(((UCHAR *)icmp_info.Data));
if (ParseICMPv6Options(&icmp_info.OptionList, ((UCHAR *)icmp_info.Headers.HeaderPointer) + sizeof(ICMPV6_ROUTER_SOLICIATION_HEADER),
icmp_info.DataSize - sizeof(ICMPV6_ROUTER_SOLICIATION_HEADER)) == false)
{
return false;
}
break;
case ICMPV6_TYPE_ROUTER_ADVERTISEMENT:
// ルータ広告
if (icmp_info.DataSize < sizeof(ICMPV6_ROUTER_ADVERTISEMENT_HEADER))
{
return false;
}
icmp_info.Headers.RouterAdvertisementHeader =
(ICMPV6_ROUTER_ADVERTISEMENT_HEADER *)(((UCHAR *)icmp_info.Data));
if (ParseICMPv6Options(&icmp_info.OptionList, ((UCHAR *)icmp_info.Headers.HeaderPointer) + sizeof(ICMPV6_ROUTER_ADVERTISEMENT_HEADER),
icmp_info.DataSize - sizeof(ICMPV6_ROUTER_ADVERTISEMENT_HEADER)) == false)
{
return false;
}
break;
case ICMPV6_TYPE_NEIGHBOR_SOLICIATION:
// 近隣要請
if (icmp_info.DataSize < sizeof(ICMPV6_NEIGHBOR_SOLICIATION_HEADER))
{
return false;
}
icmp_info.Headers.NeighborSoliciationHeader =
(ICMPV6_NEIGHBOR_SOLICIATION_HEADER *)(((UCHAR *)icmp_info.Data));
if (ParseICMPv6Options(&icmp_info.OptionList, ((UCHAR *)icmp_info.Headers.HeaderPointer) + sizeof(ICMPV6_NEIGHBOR_SOLICIATION_HEADER),
icmp_info.DataSize - sizeof(ICMPV6_NEIGHBOR_SOLICIATION_HEADER)) == false)
{
return false;
}
break;
case ICMPV6_TYPE_NEIGHBOR_ADVERTISEMENT:
// 近隣広告
if (icmp_info.DataSize < sizeof(ICMPV6_NEIGHBOR_ADVERTISEMENT_HEADER))
{
return false;
}
icmp_info.Headers.NeighborAdvertisementHeader =
(ICMPV6_NEIGHBOR_ADVERTISEMENT_HEADER *)(((UCHAR *)icmp_info.Data));
if (ParseICMPv6Options(&icmp_info.OptionList, ((UCHAR *)icmp_info.Headers.HeaderPointer) + sizeof(ICMPV6_NEIGHBOR_ADVERTISEMENT_HEADER),
icmp_info.DataSize - sizeof(ICMPV6_NEIGHBOR_ADVERTISEMENT_HEADER)) == false)
{
return false;
}
break;
}
p->TypeL4 = L4_ICMPV6;
Copy(&p->ICMPv6HeaderPacketInfo, &icmp_info, sizeof(ICMPV6_HEADER_INFO));
return true;
}
// IPv6 パケットヘッダ部のビルド
BUF *BuildIPv6PacketHeader(IPV6_HEADER_PACKET_INFO *info, UINT *bytes_before_payload)
{
BUF *b;
QUEUE *q;
UINT bbp = 0;
// 引数チェック
if (info == NULL)
{
return NULL;
}
b = NewBuf();
q = NewQueueFast();
// オプションヘッダの一覧を作成
if (info->HopHeader != NULL)
{
InsertQueueInt(q, IPV6_HEADER_HOP);
}
if (info->EndPointHeader != NULL)
{
InsertQueueInt(q, IPV6_HEADER_ENDPOINT);
}
if (info->RoutingHeader != NULL)
{
InsertQueueInt(q, IPV6_HEADER_ROUTING);
}
if (info->FragmentHeader != NULL)
{
InsertQueueInt(q, IPV6_HEADER_FRAGMENT);
}
InsertQueueInt(q, info->Protocol);
// IPv6 ヘッダ
info->IPv6Header->NextHeader = IPv6GetNextHeaderFromQueue(q);
WriteBuf(b, info->IPv6Header, sizeof(IPV6_HEADER));
// ホップバイホップオプションヘッダ
if (info->HopHeader != NULL)
{
BuildAndAddIPv6PacketOptionHeader(b, info->HopHeader,
IPv6GetNextHeaderFromQueue(q), info->HopHeaderSize);
}
// 終点オプションヘッダ
if (info->EndPointHeader != NULL)
{
BuildAndAddIPv6PacketOptionHeader(b, info->EndPointHeader,
IPv6GetNextHeaderFromQueue(q), info->EndPointHeaderSize);
}
// ルーティングヘッダ
if (info->RoutingHeader != NULL)
{
BuildAndAddIPv6PacketOptionHeader(b, info->RoutingHeader,
IPv6GetNextHeaderFromQueue(q), info->RoutingHeaderSize);
}
// フラグメントヘッダ
if (info->FragmentHeader != NULL)
{
info->FragmentHeader->NextHeader = IPv6GetNextHeaderFromQueue(q);
WriteBuf(b, info->FragmentHeader, sizeof(IPV6_FRAGMENT_HEADER));
}
bbp = b->Size;
if (info->FragmentHeader == NULL)
{
bbp += sizeof(IPV6_FRAGMENT_HEADER);
}
// ペイロード
if (info->Protocol != IPV6_HEADER_NONE)
{
WriteBuf(b, info->Payload, info->PayloadSize);
}
ReleaseQueue(q);
SeekBuf(b, 0, 0);
// ペイロード長さ
((IPV6_HEADER *)b->Buf)->PayloadLength = Endian16(b->Size - (USHORT)sizeof(IPV6_HEADER));
if (bytes_before_payload != NULL)
{
// ペイロードの直前までの長さ (ただしフラグメントヘッダは必ず含まれると仮定)
// を計算する
*bytes_before_payload = bbp;
}
return b;
}
bool ParsePacketL2Ex(PKT *p, UCHAR *buf, UINT size, bool no_l3)
{
UINT i;
bool b1, b2;
USHORT type_id_16;
// 引数チェック
if (p == NULL || buf == NULL)
{
return false;
}
// サイズをチェック
if (size < sizeof(MAC_HEADER))
{
return false;
}
// MAC ヘッダ
p->MacHeader = (MAC_HEADER *)buf;
buf += sizeof(MAC_HEADER);
size -= sizeof(MAC_HEADER);
// MAC ヘッダの解析
p->BroadcastPacket = true;
b1 = true;
b2 = true;
for (i = 0;i < 6;i++)
{
if (p->MacHeader->DestAddress[i] != 0xff)
{
p->BroadcastPacket = false;
}
if (p->MacHeader->SrcAddress[i] != 0xff)
{
b1 = false;
}
if (p->MacHeader->SrcAddress[i] != 0x00)
{
b2 = false;
}
}
if (b1 || b2 || (Cmp(p->MacHeader->SrcAddress, p->MacHeader->DestAddress, 6) == 0))
{
p->InvalidSourcePacket = true;
}
else
{
p->InvalidSourcePacket = false;
}
if (p->MacHeader->DestAddress[0] & 0x01)
{
p->BroadcastPacket = true;
}
// L3 パケットのパース
type_id_16 = Endian16(p->MacHeader->Protocol);
if (type_id_16 > 1500)
{
// 通常の Ethernet フレーム
switch (type_id_16)
{
case MAC_PROTO_ARPV4: // ARPv4
if (no_l3)
{
return true;
}
return ParsePacketARPv4(p, buf, size);
case MAC_PROTO_IPV4: // IPv4
if (no_l3)
{
return true;
}
return ParsePacketIPv4(p, buf, size);
case MAC_PROTO_IPV6: // IPv6
if (no_l3)
{
return true;
}
return ParsePacketIPv6(p, buf, size);
default: // 不明
if (type_id_16 == p->VlanTypeID)
{
// VLAN
return ParsePacketTAGVLAN(p, buf, size);
}
else
{
return true;
}
}
}
else
{
// IEEE 802.3 の古い (パケットのペイロード長が書いてある) フレーム
// (BPDU 等で使われていることがある)
UINT length = (UINT)type_id_16;
LLC_HEADER *llc;
// 長さが残っているかどうかチェック
if (size < length || size < sizeof(LLC_HEADER))
{
return true;
}
// LLC ヘッダを読む
llc = (LLC_HEADER *)buf;
buf += sizeof(LLC_HEADER);
size -= sizeof(LLC_HEADER);
// DSAP, SSAP の値によってプロトコルを判定する
if (llc->Dsap == LLC_DSAP_BPDU && llc->Ssap == LLC_SSAP_BPDU)
{
// BPDU (Spanning Tree) である
return ParsePacketBPDU(p, buf, size);
}
else
{
// 不明なプロトコルである
return true;
}
}
}
// Process IP packets
void L3RecvIp(L3IF *f, PKT *p, bool self)
{
IPV4_HEADER *ip;
UINT header_size;
UINT next_hop = 0;
L3IF *dst;
L3PACKET *packet;
UINT new_ttl = 0;
// Validate arguments
if (f == NULL || p == NULL)
{
return;
}
ip = p->L3.IPv4Header;
header_size = IPV4_GET_HEADER_LEN(p->L3.IPv4Header) * 4;
// Calculate the checksum
if (IpCheckChecksum(ip) == false)
{
// The checksum does not match
goto FREE_PACKET;
}
// Register in the ARP table
L3KnownArp(f, ip->SrcIP, p->MacAddressSrc);
if (p->BroadcastPacket)
{
// Not to route in the case of broadcast packet
goto FREE_PACKET;
}
// Calculate the TTL
if (ip->TimeToLive >= 1)
{
new_ttl = ip->TimeToLive - 1;
}
else
{
new_ttl = 0;
}
if (new_ttl == 0)
{
if (ip->DstIP != f->IpAddress)
{
UINT src_packet_size = p->PacketSize - sizeof(MAC_HEADER);
UINT icmp_packet_total_size = sizeof(MAC_HEADER) + sizeof(IPV4_HEADER) + sizeof(ICMP_HEADER) + 4 + header_size + 8;
UCHAR *buf;
IPV4_HEADER *ipv4;
ICMP_HEADER *icmpv4;
UCHAR *data;
PKT *pkt;
UINT data_size = MIN(p->PacketSize - header_size, header_size + 8);
// Generate an ICMP message that means that the TTL has expired
buf = ZeroMalloc(icmp_packet_total_size);
ipv4 = (IPV4_HEADER *)(buf + sizeof(MAC_HEADER));
icmpv4 = (ICMP_HEADER *)(buf + sizeof(MAC_HEADER) + sizeof(IPV4_HEADER));
data = buf + sizeof(MAC_HEADER) + sizeof(IPV4_HEADER) + sizeof(ICMP_HEADER) + 4;
IPV4_SET_VERSION(ipv4, 4);
IPV4_SET_HEADER_LEN(ipv4, sizeof(IPV4_HEADER) / 4);
ipv4->TotalLength = Endian16((USHORT)(icmp_packet_total_size - sizeof(MAC_HEADER)));
ipv4->TimeToLive = 0xff;
ipv4->Protocol = IP_PROTO_ICMPV4;
ipv4->SrcIP = f->IpAddress;
ipv4->DstIP = ip->SrcIP;
ipv4->Checksum = IpChecksum(ipv4, sizeof(IPV4_HEADER));
icmpv4->Type = 11;
Copy(data, ip, data_size);
icmpv4->Checksum = IpChecksum(icmpv4, sizeof(ICMP_HEADER) + data_size + 4);
buf[12] = 0x08;
buf[13] = 0x00;
pkt = ParsePacket(buf, icmp_packet_total_size);
if (pkt == NULL)
{
Free(buf);
}
else
{
L3RecvIp(f, pkt, true);
}
// Discard the packet body whose the TTL has expired
goto FREE_PACKET;
}
}
// Rewrite the TTL
p->L3.IPv4Header->TimeToLive = (UCHAR)new_ttl;
// Get the interface corresponding to the destination IP address
dst = L3GetNextIf(f->Switch, ip->DstIP, &next_hop);
if (dst == NULL && self == false)
{
UINT src_packet_size = p->PacketSize - sizeof(MAC_HEADER);
UINT icmp_packet_total_size = sizeof(MAC_HEADER) + sizeof(IPV4_HEADER) + sizeof(ICMP_HEADER) + 4 + header_size + 8;
UCHAR *buf;
IPV4_HEADER *ipv4;
ICMP_HEADER *icmpv4;
UCHAR *data;
PKT *pkt;
UINT data_size = MIN(p->PacketSize - header_size, header_size + 8);
// Respond with ICMP that indicates that no route can be found
buf = ZeroMalloc(icmp_packet_total_size);
ipv4 = (IPV4_HEADER *)(buf + sizeof(MAC_HEADER));
icmpv4 = (ICMP_HEADER *)(buf + sizeof(MAC_HEADER) + sizeof(IPV4_HEADER));
data = buf + sizeof(MAC_HEADER) + sizeof(IPV4_HEADER) + sizeof(ICMP_HEADER) + 4;
IPV4_SET_VERSION(ipv4, 4);
IPV4_SET_HEADER_LEN(ipv4, sizeof(IPV4_HEADER) / 4);
ipv4->TotalLength = Endian16((USHORT)(icmp_packet_total_size - sizeof(MAC_HEADER)));
ipv4->TimeToLive = 0xff;
ipv4->Protocol = IP_PROTO_ICMPV4;
ipv4->SrcIP = f->IpAddress;
ipv4->DstIP = ip->SrcIP;
ipv4->Checksum = IpChecksum(ipv4, sizeof(IPV4_HEADER));
icmpv4->Type = 3;
Copy(data, ip, data_size);
icmpv4->Checksum = IpChecksum(icmpv4, sizeof(ICMP_HEADER) + data_size + 4);
buf[12] = 0x08;
buf[13] = 0x00;
pkt = ParsePacket(buf, icmp_packet_total_size);
if (pkt == NULL)
{
Free(buf);
}
else
{
L3RecvIp(f, pkt, true);
}
// Discard the packet body whose route can not be found
goto FREE_PACKET;
}
if (dst != NULL && ip->DstIP == dst->IpAddress)
{
bool free_packet = true;
// IP packet addressed to myself has arrived
if (p->TypeL4 == L4_ICMPV4)
{
ICMP_HEADER *icmp = p->L4.ICMPHeader;
if (icmp->Type == ICMP_TYPE_ECHO_REQUEST)
{
// Reply by rewriting the source and destination of the IP packet
UINT src_ip, dst_ip;
src_ip = p->L3.IPv4Header->DstIP;
dst_ip = p->L3.IPv4Header->SrcIP;
p->L3.IPv4Header->DstIP = dst_ip;
p->L3.IPv4Header->SrcIP = src_ip;
ip->TimeToLive = 0xff;
// Recalculates the checksum
ip->FlagsAndFlagmentOffset[0] = ip->FlagsAndFlagmentOffset[1] = 0;
icmp->Checksum = 0;
icmp->Type = ICMP_TYPE_ECHO_RESPONSE;
icmp->Checksum = IpChecksum(icmp, p->PacketSize - sizeof(MAC_HEADER) - header_size);
dst = L3GetNextIf(f->Switch, ip->DstIP, &next_hop);
free_packet = false;
}
}
if (free_packet)
{
goto FREE_PACKET;
}
}
if (dst == NULL)
{
// The destination does not exist
goto FREE_PACKET;
}
// Recalculate the IP checksum
ip->Checksum = 0;
ip->Checksum = IpChecksum(ip, header_size);
// Treat as a Layer-3 packet
packet = ZeroMalloc(sizeof(L3PACKET));
packet->Expire = Tick64() + IP_WAIT_FOR_ARP_TIMEOUT;
packet->NextHopIp = next_hop;
packet->Packet = p;
// Store to the destination session
L3StoreIpPacketToIf(f, dst, packet);
return;
FREE_PACKET:
// Release the packet
Free(p->PacketData);
FreePacket(p);
return;
}
// Beacon transmission
void L3PollingBeacon(L3IF *f)
{
// Validate arguments
if (f == NULL)
{
return;
}
if (f->LastBeaconSent == 0 ||
(f->LastBeaconSent + BEACON_SEND_INTERVAL) <= Tick64())
{
UINT dest_ip;
UCHAR *udp_buf;
UINT udp_buf_size;
ARPV4_HEADER arp;
IPV4_HEADER *ip;
UDP_HEADER *udp;
static char beacon_str[] =
"PacketiX VPN Virtual Layer-3 Switch Beacon";
// Send an UDP
dest_ip = (f->IpAddress & f->SubnetMask) | (~f->SubnetMask);
udp_buf_size = sizeof(IPV4_HEADER) + sizeof(UDP_HEADER) + sizeof(beacon_str);
udp_buf = ZeroMalloc(udp_buf_size);
ip = (IPV4_HEADER *)udp_buf;
udp = (UDP_HEADER *)(udp_buf + sizeof(IPV4_HEADER));
udp->DstPort = Endian16(7);
udp->SrcPort = Endian16(7);
udp->PacketLength = Endian16(sizeof(UDP_HEADER) + sizeof(beacon_str));
Copy(udp_buf + sizeof(IPV4_HEADER) + sizeof(UDP_HEADER), beacon_str, sizeof(beacon_str));
udp->Checksum = CalcChecksumForIPv4(f->IpAddress, dest_ip, 0x11, udp, sizeof(UDP_HEADER) + sizeof(beacon_str), 0);
ip->DstIP = dest_ip;
IPV4_SET_VERSION(ip, 4);
IPV4_SET_HEADER_LEN(ip, (IP_HEADER_SIZE / 4));
ip->TypeOfService = DEFAULT_IP_TOS;
ip->TotalLength = Endian16((USHORT)(udp_buf_size));
ip->TimeToLive = DEFAULT_IP_TTL;
ip->Protocol = IP_PROTO_UDP;
ip->SrcIP = f->IpAddress;
ip->Checksum = IpChecksum(ip, IP_HEADER_SIZE);
L3SendL2Now(f, broadcast, f->MacAddress, MAC_PROTO_IPV4, udp_buf, udp_buf_size);
Free(udp_buf);
// Build the ARP header
arp.HardwareType = Endian16(ARP_HARDWARE_TYPE_ETHERNET);
arp.ProtocolType = Endian16(MAC_PROTO_IPV4);
arp.HardwareSize = 6;
arp.ProtocolSize = 4;
arp.Operation = Endian16(ARP_OPERATION_RESPONSE);
Copy(arp.SrcAddress, f->MacAddress, 6);
arp.SrcIP = f->IpAddress;
arp.TargetAddress[0] =
arp.TargetAddress[1] =
arp.TargetAddress[2] =
arp.TargetAddress[3] =
arp.TargetAddress[4] =
arp.TargetAddress[5] = 0xff;
arp.TargetIP = dest_ip;
// Transmission
L3SendL2Now(f, broadcast, f->MacAddress, MAC_PROTO_ARPV4, &arp, sizeof(arp));
f->LastBeaconSent = Tick64();
}
}
// Attempts Radius authentication (with specifying retry interval and multiple server)
bool RadiusLogin(CONNECTION *c, char *server, UINT port, UCHAR *secret, UINT secret_size, wchar_t *username, char *password, UINT interval, UCHAR *mschap_v2_server_response_20)
{
UCHAR random[MD5_SIZE];
UCHAR id;
BUF *encrypted_password = NULL;
BUF *user_name = NULL;
//IP ip;
bool ret = false;
TOKEN_LIST *token;
UINT i;
LIST *ip_list;
IPC_MSCHAP_V2_AUTHINFO mschap;
bool is_mschap;
char client_ip_str[MAX_SIZE];
static UINT packet_id = 0;
// Validate arguments
if (server == NULL || port == 0 || (secret_size != 0 && secret == NULL) || username == NULL || password == NULL)
{
return false;
}
Zero(client_ip_str, sizeof(client_ip_str));
if (c != NULL && c->FirstSock != NULL)
{
IPToStr(client_ip_str, sizeof(client_ip_str), &c->FirstSock->RemoteIP);
}
// Parse the MS-CHAP v2 authentication data
Zero(&mschap, sizeof(mschap));
is_mschap = ParseAndExtractMsChapV2InfoFromPassword(&mschap, password);
// Split the server into tokens
token = ParseToken(server, " ,;\t");
// Get the IP address of the server
ip_list = NewListFast(NULL);
for(i = 0; i < token->NumTokens; i++)
{
IP *tmp_ip = Malloc(sizeof(IP));
if (GetIP(tmp_ip, token->Token[i]))
{
Add(ip_list, tmp_ip);
}
else if (GetIPEx(tmp_ip, token->Token[i], true))
{
Add(ip_list, tmp_ip);
}
else
{
Free(tmp_ip);
}
}
FreeToken(token);
if(LIST_NUM(ip_list) == 0)
{
ReleaseList(ip_list);
return false;
}
// Random number generation
Rand(random, sizeof(random));
// ID generation
id = (UCHAR)(packet_id % 254 + 1);
packet_id++;
if (is_mschap == false)
{
// Encrypt the password
encrypted_password = RadiusEncryptPassword(password, random, secret, secret_size);
if (encrypted_password == NULL)
{
// Encryption failure
ReleaseList(ip_list);
return false;
}
}
// Generate the user name packet
user_name = RadiusCreateUserName(username);
if (user_name != NULL)
{
// Generate a password packet
BUF *user_password = (is_mschap ? NULL : RadiusCreateUserPassword(encrypted_password->Buf, encrypted_password->Size));
BUF *nas_id = RadiusCreateNasId(CEDAR_SERVER_STR);
if (is_mschap || user_password != NULL)
{
UINT64 start;
UINT64 next_send_time;
UCHAR tmp[MAX_SIZE];
UINT recv_buf_size = 32768;
UCHAR *recv_buf = MallocEx(recv_buf_size, true);
// Generate an UDP packet
BUF *p = NewBuf();
UCHAR type = 1;
SOCK *sock;
USHORT sz = 0;
UINT pos = 0;
BOOL *finish = ZeroMallocEx(sizeof(BOOL) * LIST_NUM(ip_list), true);
Zero(tmp, sizeof(tmp));
WriteBuf(p, &type, 1);
WriteBuf(p, &id, 1);
WriteBuf(p, &sz, 2);
WriteBuf(p, random, 16);
WriteBuf(p, user_name->Buf, user_name->Size);
if (is_mschap == false)
{
UINT ui;
// PAP
WriteBuf(p, user_password->Buf, user_password->Size);
WriteBuf(p, nas_id->Buf, nas_id->Size);
// Service-Type
ui = Endian32(2);
RadiusAddValue(p, 6, 0, 0, &ui, sizeof(ui));
// NAS-Port-Type
ui = Endian32(5);
RadiusAddValue(p, 61, 0, 0, &ui, sizeof(ui));
// Tunnel-Type
ui = Endian32(1);
RadiusAddValue(p, 64, 0, 0, &ui, sizeof(ui));
// Tunnel-Medium-Type
ui = Endian32(1);
RadiusAddValue(p, 65, 0, 0, &ui, sizeof(ui));
// Calling-Station-Id
RadiusAddValue(p, 31, 0, 0, client_ip_str, StrLen(client_ip_str));
// Tunnel-Client-Endpoint
RadiusAddValue(p, 66, 0, 0, client_ip_str, StrLen(client_ip_str));
}
else
{
// MS-CHAP v2
static UINT session_id = 0;
USHORT us;
UINT ui;
char *ms_ras_version = "MSRASV5.20";
UCHAR ms_chapv2_response[50];
// Acct-Session-Id
us = Endian16(session_id % 254 + 1);
session_id++;
RadiusAddValue(p, 44, 0, 0, &us, sizeof(us));
// NAS-IP-Address
if (c != NULL && c->FirstSock != NULL && c->FirstSock->IPv6 == false)
{
ui = IPToUINT(&c->FirstSock->LocalIP);
RadiusAddValue(p, 4, 0, 0, &ui, sizeof(ui));
}
// Service-Type
ui = Endian32(2);
RadiusAddValue(p, 6, 0, 0, &ui, sizeof(ui));
// MS-RAS-Vendor
ui = Endian32(311);
RadiusAddValue(p, 26, 311, 9, &ui, sizeof(ui));
// MS-RAS-Version
RadiusAddValue(p, 26, 311, 18, ms_ras_version, StrLen(ms_ras_version));
// NAS-Port-Type
ui = Endian32(5);
RadiusAddValue(p, 61, 0, 0, &ui, sizeof(ui));
// Tunnel-Type
ui = Endian32(1);
RadiusAddValue(p, 64, 0, 0, &ui, sizeof(ui));
// Tunnel-Medium-Type
ui = Endian32(1);
RadiusAddValue(p, 65, 0, 0, &ui, sizeof(ui));
// Calling-Station-Id
RadiusAddValue(p, 31, 0, 0, client_ip_str, StrLen(client_ip_str));
// Tunnel-Client-Endpoint
RadiusAddValue(p, 66, 0, 0, client_ip_str, StrLen(client_ip_str));
// MS-RAS-Client-Version
RadiusAddValue(p, 26, 311, 35, ms_ras_version, StrLen(ms_ras_version));
// MS-RAS-Client-Name
RadiusAddValue(p, 26, 311, 34, client_ip_str, StrLen(client_ip_str));
// MS-CHAP-Challenge
RadiusAddValue(p, 26, 311, 11, mschap.MsChapV2_ServerChallenge, sizeof(mschap.MsChapV2_ServerChallenge));
// MS-CHAP2-Response
Zero(ms_chapv2_response, sizeof(ms_chapv2_response));
Copy(ms_chapv2_response + 2, mschap.MsChapV2_ClientChallenge, 16);
Copy(ms_chapv2_response + 2 + 16 + 8, mschap.MsChapV2_ClientResponse, 24);
RadiusAddValue(p, 26, 311, 25, ms_chapv2_response, sizeof(ms_chapv2_response));
// NAS-ID
WriteBuf(p, nas_id->Buf, nas_id->Size);
}
SeekBuf(p, 0, 0);
WRITE_USHORT(((UCHAR *)p->Buf) + 2, (USHORT)p->Size);
// Create a socket
sock = NewUDPEx(0, IsIP6(LIST_DATA(ip_list, pos)));
// Transmission process start
start = Tick64();
if(interval < RADIUS_RETRY_INTERVAL)
{
interval = RADIUS_RETRY_INTERVAL;
}
else if(interval > RADIUS_RETRY_TIMEOUT)
{
interval = RADIUS_RETRY_TIMEOUT;
}
next_send_time = start + (UINT64)interval;
while (true)
{
UINT server_port;
UINT recv_size;
//IP server_ip;
SOCKSET set;
UINT64 now;
SEND_RETRY:
//SendTo(sock, &ip, port, p->Buf, p->Size);
SendTo(sock, LIST_DATA(ip_list, pos), port, p->Buf, p->Size);
Debug("send to host:%u\n", pos);
next_send_time = Tick64() + (UINT64)interval;
RECV_RETRY:
now = Tick64();
if (next_send_time <= now)
{
// Switch the host to refer
pos++;
pos = pos % LIST_NUM(ip_list);
goto SEND_RETRY;
}
if ((start + RADIUS_RETRY_TIMEOUT) < now)
{
// Time-out
break;
}
InitSockSet(&set);
AddSockSet(&set, sock);
Select(&set, (UINT)(next_send_time - now), NULL, NULL);
recv_size = RecvFrom(sock, LIST_DATA(ip_list, pos), &server_port, recv_buf, recv_buf_size);
if (recv_size == 0)
{
Debug("Radius recv_size 0\n");
finish[pos] = TRUE;
for(i = 0; i < LIST_NUM(ip_list); i++)
{
if(finish[i] == FALSE)
{
// Switch the host to refer
pos++;
pos = pos % LIST_NUM(ip_list);
goto SEND_RETRY;
}
}
// Failure
break;
}
else if (recv_size == SOCK_LATER)
{
// Waiting
goto RECV_RETRY;
}
else
{
// Check such as the IP address
if (/*Cmp(&server_ip, &ip, sizeof(IP)) != 0 || */server_port != port)
{
goto RECV_RETRY;
}
// Success
if (recv_buf[0] == 2)
{
ret = true;
if (is_mschap && mschap_v2_server_response_20 != NULL)
{
// Cutting corners Zurukko
UCHAR signature[] = {0x1A, 0x33, 0x00, 0x00, 0x01, 0x37, 0x1A, 0x2D, 0x00, 0x53, 0x3D, };
UINT i = SearchBin(recv_buf, 0, recv_buf_size, signature, sizeof(signature));
if (i == INFINITE || ((i + sizeof(signature) + 40) > recv_buf_size))
{
ret = false;
}
else
{
char tmp[MAX_SIZE];
BUF *b;
Zero(tmp, sizeof(tmp));
Copy(tmp, recv_buf + i + sizeof(signature), 40);
b = StrToBin(tmp);
if (b != NULL && b->Size == 20)
{
WHERE;
Copy(mschap_v2_server_response_20, b->Buf, 20);
}
else
{
WHERE;
ret = false;
}
FreeBuf(b);
}
}
}
break;
}
}
Free(finish);
// Release the socket
ReleaseSock(sock);
FreeBuf(p);
FreeBuf(user_password);
Free(recv_buf);
}
FreeBuf(nas_id);
FreeBuf(user_name);
}
// Release the ip_list
for(i = 0; i < LIST_NUM(ip_list); i++)
{
IP *tmp_ip = LIST_DATA(ip_list, i);
Free(tmp_ip);
}
ReleaseList(ip_list);
// Release the memory
FreeBuf(encrypted_password);
return ret;
}
UINT EthGetPacketLinux(ETH *e, void **data)
{
int s, ret;
UCHAR tmp[UNIX_ETH_TMP_BUFFER_SIZE];
struct iovec msg_iov;
struct msghdr msg_header;
struct cmsghdr *cmsg;
union
{
struct cmsghdr cmsg;
char buf[CMSG_SPACE(sizeof(struct my_tpacket_auxdata))];
} cmsg_buf;
// Validate arguments
if (e == NULL || data == NULL)
{
return INFINITE;
}
if (e->Tap != NULL)
{
#ifndef NO_VLAN
// tap mode
void *buf;
UINT size;
if (VLanGetNextPacket(e->Tap, &buf, &size) == false)
{
return INFINITE;
}
*data = buf;
return size;
#else // NO_VLAN
return INFINITE;
#endif
}
s = e->Socket;
if (s == INVALID_SOCKET)
{
return INFINITE;
}
// Read
msg_iov.iov_base = tmp;
msg_iov.iov_len = sizeof(tmp);
msg_header.msg_name = NULL;
msg_header.msg_namelen = 0;
msg_header.msg_iov = &msg_iov;
msg_header.msg_iovlen = 1;
if (e->Linux_IsAuxDataSupported)
{
memset(&cmsg_buf, 0, sizeof(cmsg_buf));
msg_header.msg_control = &cmsg_buf;
msg_header.msg_controllen = sizeof(cmsg_buf);
}
else
{
msg_header.msg_control = NULL;
msg_header.msg_controllen = 0;
}
msg_header.msg_flags = 0;
ret = recvmsg(s, &msg_header, 0);
if (ret == 0 || (ret == -1 && errno == EAGAIN))
{
// No packet
*data = NULL;
return 0;
}
else if (ret == -1 || ret > sizeof(tmp))
{
// Error
*data = NULL;
e->Socket = INVALID_SOCKET;
return INFINITE;
}
else
{
bool flag = false;
USHORT api_vlan_id = 0;
USHORT api_vlan_tpid = 0;
if (e->Linux_IsAuxDataSupported)
{
for (cmsg = CMSG_FIRSTHDR(&msg_header); cmsg; cmsg = CMSG_NXTHDR(&msg_header, cmsg))
{
struct my_tpacket_auxdata *aux;
UINT len;
USHORT vlan_tpid = 0x8100;
USHORT vlan_id = 0;
if (cmsg->cmsg_len < CMSG_LEN(sizeof(struct my_tpacket_auxdata)) ||
cmsg->cmsg_level != SOL_PACKET ||
cmsg->cmsg_type != MY_PACKET_AUXDATA)
{
continue;
}
aux = (struct my_tpacket_auxdata *)CMSG_DATA(cmsg);
if (aux != NULL)
{
if (aux->tp_vlan_tci != 0)
{
vlan_id = aux->tp_vlan_tci;
}
}
if (vlan_id != 0)
{
api_vlan_id = vlan_id;
api_vlan_tpid = vlan_tpid;
break;
}
}
if (api_vlan_id != 0 && api_vlan_tpid != 0)
{
// VLAN ID has been received with PACKET_AUXDATA.
// Insert the tag.
USHORT vlan_id_ne = Endian16(api_vlan_id);
USHORT vlan_tpid_ne = Endian16(api_vlan_tpid);
if (ret >= 14)
{
if (*((USHORT *)(tmp + 12)) != vlan_tpid_ne)
{
*data = MallocFast(ret + 4);
Copy(*data, tmp, 12);
Copy(((UCHAR *)*data) + 12, &vlan_tpid_ne, 2);
Copy(((UCHAR *)*data) + 14, &vlan_id_ne, 2);
Copy(((UCHAR *)*data) + 16, tmp + 12, ret - 12);
flag = true;
ret += 4;
}
}
}
}
// Success to read a packet (No VLAN)
if (flag == false)
{
*data = MallocFast(ret);
Copy(*data, tmp, ret);
}
return ret;
}
return 0;
}
