diff options
Diffstat (limited to 'src/packet/packet.cpp')
| -rw-r--r-- | src/packet/packet.cpp | 168 |
1 files changed, 60 insertions, 108 deletions
diff --git a/src/packet/packet.cpp b/src/packet/packet.cpp index 278f6ba..d94fb61 100644 --- a/src/packet/packet.cpp +++ b/src/packet/packet.cpp @@ -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)); |