diff --git a/src/Mayaqua/TcpIp.c b/src/Mayaqua/TcpIp.c index 8dba45e2..63ebe57e 100644 --- a/src/Mayaqua/TcpIp.c +++ b/src/Mayaqua/TcpIp.c @@ -2057,43 +2057,15 @@ bool ParsePacketL2Ex(PKT *p, UCHAR *buf, UINT size, bool no_l3, bool no_l3_l4_ex if (type_id_16 > 1500) { - // Ordinary Ethernet frame - switch (type_id_16) + if (type_id_16 == MAC_PROTO_TAGVLAN) { - case MAC_PROTO_ARPV4: // ARPv4 - if (no_l3 || no_l3_l4_except_icmpv6) - { - return true; - } - - return ParsePacketARPv4(p, buf, size); - - case MAC_PROTO_IPV4: // IPv4 - if (no_l3 || no_l3_l4_except_icmpv6) - { - return true; - } - - return ParsePacketIPv4(p, buf, size); - - case MAC_PROTO_IPV6: // IPv6 - if (no_l3) - { - return true; - } - - return ParsePacketIPv6(p, buf, size, no_l3_l4_except_icmpv6); - - default: // Unknown - if (type_id_16 == p->VlanTypeID) - { - // VLAN - return ParsePacketTAGVLAN(p, buf, size); - } - else - { - return true; - } + // Parse VLAN frame + return ParsePacketTAGVLAN(p, buf, size, no_l3, no_l3_l4_except_icmpv6); + } + else + { + // Parse Ordinary Ethernet frame + return ParsePacketL3(p, buf, size, type_id_16, no_l3, no_l3_l4_except_icmpv6); } } else @@ -2128,10 +2100,44 @@ bool ParsePacketL2Ex(PKT *p, UCHAR *buf, UINT size, bool no_l3, bool no_l3_l4_ex } } +bool ParsePacketL3(PKT *p, UCHAR *buf, UINT size, USHORT proto, bool no_l3, bool no_l3_l4_except_icmpv6) +{ + switch (proto) + { + case MAC_PROTO_ARPV4: // ARPv4 + if (no_l3 || no_l3_l4_except_icmpv6) + { + return true; + } + + return ParsePacketARPv4(p, buf, size); + + case MAC_PROTO_IPV4: // IPv4 + if (no_l3 || no_l3_l4_except_icmpv6) + { + return true; + } + + return ParsePacketIPv4(p, buf, size); + + case MAC_PROTO_IPV6: // IPv6 + if (no_l3) + { + return true; + } + + return ParsePacketIPv6(p, buf, size, no_l3_l4_except_icmpv6); + + default: // Unknown + return true; + } +} + // TAG VLAN parsing -bool ParsePacketTAGVLAN(PKT *p, UCHAR *buf, UINT size) +bool ParsePacketTAGVLAN(PKT *p, UCHAR *buf, UINT size, bool no_l3, bool no_l3_l4_except_icmpv6) { USHORT vlan_ushort; + USHORT proto_ushort; // Validate arguments if (p == NULL || buf == NULL) { @@ -2151,12 +2157,17 @@ bool ParsePacketTAGVLAN(PKT *p, UCHAR *buf, UINT size) buf += sizeof(TAGVLAN_HEADER); size -= sizeof(TAGVLAN_HEADER); - vlan_ushort = READ_USHORT(p->L3.TagVlanHeader->Data); + vlan_ushort = READ_USHORT(p->L3.TagVlanHeader->TagID); vlan_ushort = vlan_ushort & 0xFFF; p->VlanId = vlan_ushort; - return true; + proto_ushort = READ_USHORT(p->L3.TagVlanHeader->Protocol); + proto_ushort = proto_ushort & 0xFFFF; + + + // Parse the L3 packet + return ParsePacketL3(p, buf, size, proto_ushort, no_l3, no_l3_l4_except_icmpv6); } // BPDU Parsing diff --git a/src/Mayaqua/TcpIp.h b/src/Mayaqua/TcpIp.h index aa5c42a8..17aa16c2 100644 --- a/src/Mayaqua/TcpIp.h +++ b/src/Mayaqua/TcpIp.h @@ -87,7 +87,8 @@ struct ARPV4_HEADER // Tagged VLAN header struct TAGVLAN_HEADER { - UCHAR Data[2]; // Data + UCHAR TagID[2]; // TagID + UCHAR Protocol[2]; // Protocol } GCC_PACKED; // IPv4 header @@ -762,10 +763,11 @@ void FreePacketTCPv4(PKT *p); void FreePacketICMPv4(PKT *p); void FreePacketDHCPv4(PKT *p); bool ParsePacketL2Ex(PKT *p, UCHAR *buf, UINT size, bool no_l3, bool no_l3_l4_except_icmpv6); +bool ParsePacketL3(PKT *p, UCHAR *buf, UINT size, USHORT proto, bool no_l3, bool no_l3_l4_except_icmpv6); bool ParsePacketARPv4(PKT *p, UCHAR *buf, UINT size); bool ParsePacketIPv4(PKT *p, UCHAR *buf, UINT size); bool ParsePacketBPDU(PKT *p, UCHAR *buf, UINT size); -bool ParsePacketTAGVLAN(PKT *p, UCHAR *buf, UINT size); +bool ParsePacketTAGVLAN(PKT *p, UCHAR *buf, UINT size, bool no_l3, bool no_l3_l4_except_icmpv6); bool ParseICMPv4(PKT *p, UCHAR *buf, UINT size); bool ParseICMPv6(PKT *p, UCHAR *buf, UINT size); bool ParseTCP(PKT *p, UCHAR *buf, UINT size);