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

Add L2TP utils

Browse Source
This commit is contained in:
luwenpeng
2024-06-07 14:47:38 +08:00
parent a50ecb3dbb
commit 4c0ad823d4
5 changed files with 524 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

168
src/packet/packet.cpp
View File

@@ -13,6 +13,7 @@
#include "ipv4_utils.h"
#include "ipv6_utils.h"
#include "mpls_utils.h"
#include "l2tp_utils.h"
#include "vlan_utils.h"
#include "vxlan_utils.h"
@@ -57,13 +58,12 @@ static inline void set_tuple6(const char *data, enum layer_type type, struct tup
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_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_pweth(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_l2tpv2_over_udp(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);
@@ -354,99 +354,6 @@ static inline uint16_t get_gtp_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) == 0 || SEQUENCE_BIT(control) == 0 || 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;
}
uint16_t offset = ntohs(*((uint16_t *)(data + skip_len)));
if (offset == 0)
{
return skip_len + 2;
}
// invalid offset
if (offset > len)
{
return 0;
}
else
{
return offset;
}
}
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)))
@@ -594,11 +501,9 @@ success:
}
}
static inline const char *parse_l2tpv2(struct packet *pkt, const char *data, uint16_t len)
static inline const char *parse_l2tpv2_over_udp(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);
uint16_t hdr_len = calc_udp_l2tpv2_hdr_len(data, len);
if (unlikely(hdr_len == 0 || hdr_len > len))
{
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_TYPE_L2TP);
@@ -612,11 +517,12 @@ static inline const char *parse_l2tpv2(struct packet *pkt, const char *data, uin
}
SET_LAYER(pkt, layer, LAYER_TYPE_L2TP, hdr_len, data, len, 0);
uint16_t control = ntohs(*((uint16_t *)data));
if (CONTROL_BIT(control))
// control message
if (l2tp_hdr_get_type((const struct l2tp_hdr *)data))
{
return layer->pld_ptr;
}
// data message
else
{
return parse_ppp(pkt, layer->pld_ptr, layer->pld_len);
@@ -625,14 +531,60 @@ static inline const char *parse_l2tpv2(struct packet *pkt, const char *data, uin
static inline const char *parse_l2tpv3_over_udp(struct packet *pkt, const char *data, uint16_t len)
{
// TODO
return data;
uint16_t hdr_len = calc_udp_l2tpv3_hdr_len(data, len);
if (unlikely(hdr_len == 0 || hdr_len > len))
{
PACKET_LOG_DATA_INSUFFICIENCY(pkt, 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, 0);
// control message
if (l2tp_hdr_get_type((const struct l2tp_hdr *)data))
{
return layer->pld_ptr;
}
// data message
else
{
// TOOD
return layer->pld_ptr;
}
}
static inline const char *parse_l2tpv3_over_ip(struct packet *pkt, const char *data, uint16_t len)
{
// TODO
return data;
uint16_t hdr_len = calc_ip_l2tpv3_hdr_len(data, len);
if (unlikely(hdr_len == 0 || hdr_len > len))
{
PACKET_LOG_DATA_INSUFFICIENCY(pkt, 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, 0);
// data message
if (ntohl(*((uint32_t *)data)))
{
// TOOD
return layer->pld_ptr;
}
// control message
else
{
return layer->pld_ptr;
}
}
static inline const char *parse_vlan(struct packet *pkt, const char *data, uint16_t len)
@@ -978,11 +930,10 @@ static inline const char *parse_udp(struct packet *pkt, const char *data, uint16
return layer->pld_ptr;
}
uint16_t control = ntohs(*((uint16_t *)layer->pld_ptr));
switch (L2TP_VERSION(control))
switch (l2tp_hdr_get_ver((const struct l2tp_hdr *)layer->pld_ptr))
{
case 2:
return parse_l2tpv2(pkt, layer->pld_ptr, layer->pld_len);
return parse_l2tpv2_over_udp(pkt, layer->pld_ptr, layer->pld_len);
case 3:
return parse_l2tpv3_over_udp(pkt, layer->pld_ptr, layer->pld_len);
default:
@@ -1214,6 +1165,7 @@ void packet_print_str(const struct packet *pkt)
case LAYER_TYPE_HDLC:
break;
case LAYER_TYPE_L2TP:
used = l2tp_hdr_to_str((const struct l2tp_hdr *)layer->hdr_ptr, buffer, sizeof(buffer));
break;
case LAYER_TYPE_VLAN:
used = vlan_hdr_to_str((const struct vlan_hdr *)layer->hdr_ptr, buffer, sizeof(buffer));