gfwleak/stellar-stellar
Archived
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

packet parser support L2TPv2

Browse Source
This commit is contained in:
luwenpeng
2024-05-24 16:14:20 +08:00
parent 570c93e616
commit 6c1f9d390c
5 changed files with 468 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

197
src/packet/packet.cpp
View File

@@ -20,28 +20,28 @@
#define likely(expr) __builtin_expect((expr), 1)
#define unlikely(expr) __builtin_expect((expr), 0)
#define PACKET_LOG_DATA_INSUFFICIENCY(type) \
{ \
PACKET_LOG_ERROR("layer: %s, data insufficiency", \
layer_type_to_str((type))); \
#define PACKET_LOG_DATA_INSUFFICIENCY(type) \
{ \
PACKET_LOG_WARN("layer: %s, data insufficiency", \
layer_type_to_str((type))); \
}
#define PACKET_LOG_UNSUPPORT_PROTO(tag, next_proto) \
{ \
PACKET_LOG_ERROR("%s: unsupport next proto %d", \
(tag), (next_proto)); \
#define PACKET_LOG_UNSUPPORT_PROTO(tag, next_proto) \
{ \
PACKET_LOG_WARN("%s: unsupport next proto %d", \
(tag), (next_proto)); \
}
#define PACKET_LOG_UNSUPPORT_ETHPROTO(tag, next_proto) \
{ \
PACKET_LOG_ERROR("%s: unsupport next proto %d: %s", \
(tag), (next_proto), ethproto_to_str(next_proto)); \
}
#define PACKET_LOG_UNSUPPORT_IPPROTO(tag, next_proto) \
#define PACKET_LOG_UNSUPPORT_ETHPROTO(tag, next_proto) \
{ \
PACKET_LOG_ERROR("%s: unsupport next proto %d: %s", \
(tag), (next_proto), ipproto_to_str(next_proto)); \
PACKET_LOG_WARN("%s: unsupport next eth proto %d: %s", \
(tag), (next_proto), ethproto_to_str(next_proto)); \
}
#define PACKET_LOG_UNSUPPORT_IPPROTO(tag, next_proto) \
{ \
PACKET_LOG_WARN("%s: unsupport next ip proto %d: %s", \
(tag), (next_proto), ipproto_to_str(next_proto)); \
}
/******************************************************************************
@@ -61,10 +61,15 @@ static inline struct packet_layer *get_free_layer(struct packet *pkt);
static inline uint16_t get_gtp_hdr_len(const char *data, uint16_t len);
static inline uint16_t get_gre_hdr_len(const char *data, uint16_t len);
static inline uint16_t get_l2tpv2_hdr_len(const char *data, uint16_t len);
// 数据链路层
static inline const char *parse_ether(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_ppp(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_l2tpv2(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_l2tpv3_over_udp(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_l2tpv3_over_ip(struct packet *pkt, const char *data, uint16_t len);
// 数据链路层 -- 隧道
static inline const char *parse_vlan(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_pppoe_ses(struct packet *pkt, const char *data, uint16_t len);
@@ -291,6 +296,8 @@ static inline const char *layer_type_to_str(enum layer_type type)
return "PPP";
case LAYER_TYPE_HDLC:
return "HDLC";
case LAYER_TYPE_L2TP:
return "L2TP";
case LAYER_TYPE_VLAN:
return "VLAN";
case LAYER_TYPE_PPPOE:
@@ -307,6 +314,10 @@ static inline const char *layer_type_to_str(enum layer_type type)
return "UDP";
case LAYER_TYPE_TCP:
return "TCP";
case LAYER_TYPE_ICMP:
return "ICMP";
case LAYER_TYPE_ICMP6:
return "ICMP6";
case LAYER_TYPE_VXLAN:
return "VXLAN";
case LAYER_TYPE_GTPV1_U:
@@ -646,6 +657,85 @@ static inline uint16_t get_gre_hdr_len(const char *data, uint16_t len)
return hdr_offset;
}
static inline uint16_t get_l2tpv2_hdr_len(const char *data, uint16_t len)
{
/*
* Layer Two Tunneling Protocol "L2TP"
* https://datatracker.ietf.org/doc/html/rfc2661
*
* 0 1 2 3
* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* |T|L|x|x|S|x|O|P|x|x|x|x| Ver | Length (opt) |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* | Tunnel ID | Session ID |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* | Ns (opt) | Nr (opt) |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* | Offset Size (opt) | Offset pad... (opt)
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
*
* Figure 3.1 L2TP Message Header
*/
#define CONTROL_BIT(msg_info) (msg_info & 0x8000) // Type bit control = 1 data = 0
#define LENGTH_BIT(msg_info) (msg_info & 0x4000) // Length bit = 1
#define SEQUENCE_BIT(msg_info) (msg_info & 0x0800) // SEQUENCE bit = 1 Ns and Nr fields
#define OFFSET_BIT(msg_info) (msg_info & 0x0200) // Offset
#define PRIORITY_BIT(msg_info) (msg_info & 0x0100) // Priority
#define L2TP_VERSION(msg_info) (msg_info & 0x000f) // Version of l2tp
if (unlikely(len < 6))
{
return 0;
}
uint16_t control = ntohs(*((uint16_t *)data));
if (L2TP_VERSION(control) != 2)
{
return 0;
}
if (CONTROL_BIT(control))
{
if (LENGTH_BIT(control) != 1 || SEQUENCE_BIT(control) != 1 || OFFSET_BIT(control) != 0 || PRIORITY_BIT(control) != 0)
{
return 0;
}
else
{
return ntohs(*((uint16_t *)(data + 2)));
}
}
else
{
uint16_t skip_len = 2;
if (LENGTH_BIT(control))
{
skip_len += 2; // skip length field
}
skip_len += 2; // skip tunnel id field
skip_len += 2; // skip session id field
if (SEQUENCE_BIT(control))
{
skip_len += 2; // skip ns field
skip_len += 2; // skip nr field
}
if (OFFSET_BIT(control))
{
if (skip_len + 2 > len)
{
return 0;
}
return ntohs(*((uint16_t *)(data + skip_len)));
}
else
{
return skip_len;
}
}
}
static inline const char *parse_ether(struct packet *pkt, const char *data, uint16_t len)
{
if (unlikely(len < sizeof(struct ethhdr)))
@@ -695,6 +785,47 @@ static inline const char *parse_ppp(struct packet *pkt, const char *data, uint16
}
}
static inline const char *parse_l2tpv2(struct packet *pkt, const char *data, uint16_t len)
{
#define CONTROL_BIT(msg_info) (msg_info & 0x8000) // Type bit control = 1 data = 0
uint16_t hdr_len = get_l2tpv2_hdr_len(data, len);
if (unlikely(hdr_len == 0 || hdr_len > len))
{
PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_L2TP);
return data;
}
struct packet_layer *layer = get_free_layer(pkt);
if (unlikely(layer == NULL))
{
return data;
}
SET_LAYER(pkt, layer, LAYER_TYPE_L2TP, hdr_len, data, len);
uint16_t control = ntohs(*((uint16_t *)data));
if (CONTROL_BIT(control))
{
return layer->pld_ptr;
}
else
{
return parse_ppp(pkt, layer->pld_ptr, layer->pld_len);
}
}
static inline const char *parse_l2tpv3_over_udp(struct packet *pkt, const char *data, uint16_t len)
{
// TODO
return data;
}
static inline const char *parse_l2tpv3_over_ip(struct packet *pkt, const char *data, uint16_t len)
{
// TODO
return data;
}
static inline const char *parse_vlan(struct packet *pkt, const char *data, uint16_t len)
{
struct vlan_hdr
@@ -742,7 +873,7 @@ static inline const char *parse_pppoe_ses(struct packet *pkt, const char *data,
switch (next_proto)
{
// TESTED
// TESTED
case PPPOE_TYPE_IPV4:
return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
case PPPOE_TYPE_IPV6:
@@ -882,7 +1013,7 @@ static inline const char *parse_ipv4(struct packet *pkt, const char *data, uint1
// ip fragmented
if (ipv4_hdr_get_mf_flag(hdr) || ipv4_hdr_get_frag_offset(hdr))
{
PACKET_LOG_DEBUG("ip is fragmented");
PACKET_LOG_WARN("ip is fragmented");
pkt->frag_layer = layer;
return layer->pld_ptr;
}
@@ -910,11 +1041,13 @@ static inline const char *parse_ipv6(struct packet *pkt, const char *data, uint1
// ipv6 fragment
if (next_proto == IPPROTO_FRAGMENT)
{
PACKET_LOG_DEBUG("ipv6 is fragmented");
PACKET_LOG_WARN("ipv6 is fragmented");
pkt->frag_layer = layer;
return layer->pld_ptr;
}
// TODO parse ipv6 extension headers
// TESTED
return parse_l4(pkt, next_proto, layer->pld_ptr, layer->pld_len);
}
@@ -986,6 +1119,27 @@ static inline const char *parse_udp(struct packet *pkt, const char *data, uint16
return parse_gtpv1_u(pkt, layer->pld_ptr, layer->pld_len);
}
if (udp_hdr_get_dst_port(hdr) == 1701)
{
// L2TP_DPORT 1701
if (unlikely(layer->pld_len < 8))
{
return layer->pld_ptr;
}
uint16_t control = ntohs(*((uint16_t *)layer->pld_ptr));
switch (L2TP_VERSION(control))
{
case 2:
// TESTED
return parse_l2tpv2(pkt, layer->pld_ptr, layer->pld_len);
case 3:
return parse_l2tpv3_over_udp(pkt, layer->pld_ptr, layer->pld_len);
default:
return layer->pld_ptr;
}
}
return layer->pld_ptr;
}
@@ -1159,6 +1313,9 @@ static inline const char *parse_l4(struct packet *pkt, uint8_t next_proto, const
// TESTED
case IPPROTO_ICMPV6:
return parse_icmp6(pkt, data, len);
case 115:
// L2TP
return parse_l2tpv3_over_ip(pkt, data, len);
default:
PACKET_LOG_UNSUPPORT_IPPROTO("l4", next_proto);
return data;