996 lines
33 KiB
C
996 lines
33 KiB
C
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <netinet/icmp6.h>
|
|
#include <netinet/ip_icmp.h>
|
|
#include <linux/ppp_defs.h>
|
|
|
|
#include "log_internal.h"
|
|
#include "packet_helper.h"
|
|
#include "packet_internal.h"
|
|
#include "packet_parser.h"
|
|
|
|
#define PACKET_PARSE_LOG_DEBUG(format, ...) // STELLAR_LOG_DEBUG(__thread_local_logger, "packet parse", format, ##__VA_ARGS__)
|
|
#define PACKET_PARSE_LOG_ERROR(format, ...) STELLAR_LOG_ERROR(__thread_local_logger, "packet parse", format, ##__VA_ARGS__)
|
|
#define PACKET_PARSE_LOG_WARN(format, ...) STELLAR_LOG_WARN(__thread_local_logger, "packet parse", format, ##__VA_ARGS__)
|
|
|
|
#define PACKET_LOG_DATA_INSUFFICIENCY(pkt, layer) \
|
|
{ \
|
|
PACKET_PARSE_LOG_WARN("pkt: %p, layer: %s, data insufficiency", \
|
|
(pkt), layer_proto_to_str(layer)); \
|
|
}
|
|
|
|
#define PACKET_LOG_UNSUPPORT_PROTO(pkt, layer, next_proto) \
|
|
{ \
|
|
PACKET_PARSE_LOG_WARN("pkt: %p, layer: %s, unsupport next proto: %d", \
|
|
(pkt), layer_proto_to_str(layer), (next_proto)); \
|
|
}
|
|
|
|
#define PACKET_LOG_UNSUPPORT_ETHPROTO(pkt, next_proto) \
|
|
{ \
|
|
PACKET_PARSE_LOG_WARN("pkt: %p, layer: L3, unsupport next proto: %d %s", \
|
|
(pkt), (next_proto), eth_proto_to_str(next_proto)); \
|
|
}
|
|
|
|
#define PACKET_LOG_UNSUPPORT_IPPROTO(pkt, next_proto) \
|
|
{ \
|
|
PACKET_PARSE_LOG_WARN("pkt: %p, layer: L4, unsupport next proto: %d %s", \
|
|
(pkt), (next_proto), ip_proto_to_str(next_proto)); \
|
|
}
|
|
|
|
/******************************************************************************
|
|
* Static API
|
|
******************************************************************************/
|
|
|
|
static inline struct layer_internal *get_free_layer(struct packet *pkt);
|
|
|
|
// 数据链路层
|
|
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_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);
|
|
|
|
// 数据链路层 -- 隧道
|
|
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);
|
|
static inline const char *parse_mpls(struct packet *pkt, const char *data, uint16_t len);
|
|
// 网络层
|
|
static inline const char *parse_ipv4(struct packet *pkt, const char *data, uint16_t len);
|
|
static inline const char *parse_ipv6(struct packet *pkt, const char *data, uint16_t len);
|
|
static inline const char *parse_auth(struct packet *pkt, const char *data, uint16_t len);
|
|
// 网络层 -- 隧道
|
|
static inline const char *parse_gre(struct packet *pkt, const char *data, uint16_t len);
|
|
// 传输层
|
|
static inline const char *parse_udp(struct packet *pkt, const char *data, uint16_t len);
|
|
static inline const char *parse_tcp(struct packet *pkt, const char *data, uint16_t len);
|
|
static inline const char *parse_icmp(struct packet *pkt, const char *data, uint16_t len);
|
|
static inline const char *parse_icmp6(struct packet *pkt, const char *data, uint16_t len);
|
|
// 传输层 -- 隧道
|
|
static inline const char *parse_vxlan(struct packet *pkt, const char *data, uint16_t len);
|
|
static inline const char *parse_gtp_u(struct packet *pkt, const char *data, uint16_t len);
|
|
static inline const char *parse_gtp_c(struct packet *pkt, const char *data, uint16_t len);
|
|
// L3/L4
|
|
static inline const char *parse_l3(struct packet *pkt, uint16_t next_proto, const char *data, uint16_t len);
|
|
static inline const char *parse_l4(struct packet *pkt, uint8_t next_proto, const char *data, uint16_t len);
|
|
|
|
/******************************************************************************
|
|
* Private API -- Utils
|
|
******************************************************************************/
|
|
|
|
static inline struct layer_internal *get_free_layer(struct packet *pkt)
|
|
{
|
|
if (pkt->layers_used >= pkt->layers_size)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
return &pkt->layers[pkt->layers_used];
|
|
}
|
|
|
|
#define SET_LAYER(_pkt, _layer, _proto, _hdr_len, _data, _len, _trim) \
|
|
{ \
|
|
(_layer)->proto = (_proto); \
|
|
(_layer)->hdr_offset = (_pkt)->data_len - (_pkt)->trim_len - (_len); \
|
|
(_layer)->hdr_ptr = (_data); \
|
|
(_layer)->hdr_len = (_hdr_len); \
|
|
(_layer)->pld_ptr = (_data) + (_hdr_len); \
|
|
(_layer)->pld_len = (_len) - (_hdr_len) - (_trim); \
|
|
(_pkt)->trim_len += (_trim); \
|
|
(_pkt)->layers_used++; \
|
|
PACKET_PARSE_LOG_DEBUG("layer[%d/%d]: %s, header: {offset: %d, ptr: %p, len: %d}, payload: {ptr: %p, len: %d}", \
|
|
(_pkt)->layers_used - 1, (_pkt)->layers_size, layer_proto_to_str((_proto)), \
|
|
(_layer)->hdr_offset, (_layer)->hdr_ptr, (_layer)->hdr_len, (_layer)->pld_ptr, (_layer)->pld_len); \
|
|
}
|
|
|
|
/******************************************************************************
|
|
* Private API -- Parses
|
|
******************************************************************************/
|
|
|
|
static inline const char *parse_ether(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
if (unlikely(len < sizeof(struct ethhdr)))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_ETHER);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
uint16_t next_proto = eth_hdr_get_proto((const struct ethhdr *)data);
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_ETHER, sizeof(struct ethhdr), data, len, 0);
|
|
|
|
return parse_l3(pkt, next_proto, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
|
|
static inline const char *parse_pweth(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
/*
|
|
* PW Ethernet Control Word
|
|
* 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
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* |0 0 0 0| Reserved | Sequence Number |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* Reference: https://tools.ietf.org/html/rfc4448
|
|
*/
|
|
if (unlikely(len < 4))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_PWETH);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_PWETH, 4, data, len, 0);
|
|
|
|
return parse_ether(pkt, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
|
|
static inline int is_ppp_proto(uint16_t proto)
|
|
{
|
|
// /usr/include/linux/ppp_defs.h.html
|
|
switch (proto)
|
|
{
|
|
case PPP_IP: /* Internet Protocol */
|
|
case PPP_AT: /* AppleTalk Protocol */
|
|
case PPP_IPX: /* IPX protocol */
|
|
case PPP_VJC_COMP: /* VJ compressed TCP */
|
|
case PPP_VJC_UNCOMP: /* VJ uncompressed TCP */
|
|
case PPP_MP: /* Multilink protocol */
|
|
case PPP_IPV6: /* Internet Protocol Version 6 */
|
|
case PPP_COMPFRAG: /* fragment compressed below bundle */
|
|
case PPP_COMP: /* compressed packet */
|
|
case PPP_MPLS_UC: /* Multi Protocol Label Switching - Unicast */
|
|
case PPP_MPLS_MC: /* Multi Protocol Label Switching - Multicast */
|
|
case PPP_IPCP: /* IP Control Protocol */
|
|
case PPP_ATCP: /* AppleTalk Control Protocol */
|
|
case PPP_IPXCP: /* IPX Control Protocol */
|
|
case PPP_IPV6CP: /* IPv6 Control Protocol */
|
|
case PPP_CCPFRAG: /* CCP at link level (below MP bundle) */
|
|
// case PPP_CCP: /* Compression Control Protocol */ (same as PPP_CCPFRAG)
|
|
case PPP_MPLSCP: /* MPLS Control Protocol */
|
|
case PPP_LCP: /* Link Control Protocol */
|
|
case PPP_PAP: /* Password Authentication Protocol */
|
|
case PPP_LQR: /* Link Quality Report protocol */
|
|
case PPP_CHAP: /* Cryptographic Handshake Auth. Protocol */
|
|
case PPP_CBCP: /* Callback Control Protocol */
|
|
return 1;
|
|
default:
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
static inline const char *parse_ppp(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
/*
|
|
* https://datatracker.ietf.org/doc/html/rfc1661#section-2
|
|
* +----------+-------------+---------+
|
|
* | Protocol | Information | Padding |
|
|
* | 8/16 bits| * | * |
|
|
* +----------+-------------+---------+
|
|
*
|
|
* https://datatracker.ietf.org/doc/html/rfc1331#section-3.1
|
|
* +----------+----------+----------+----------+------------
|
|
* | Flag | Address | Control | Protocol | Information
|
|
* | 01111110 | 11111111 | 00000011 | 16 bits | *
|
|
* +----------+----------+----------+----------+------------
|
|
* ---+----------+----------+-----------------
|
|
* | FCS | Flag | Inter-frame Fill
|
|
* | 16 bits | 01111110 | or next Address
|
|
* ---+----------+----------+-----------------
|
|
*/
|
|
if (unlikely(len < 4))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_PPP);
|
|
return data;
|
|
}
|
|
|
|
uint16_t hdr_len = 0;
|
|
uint16_t next_proto = 0;
|
|
struct layer_internal *layer = NULL;
|
|
|
|
// ppp header 1 byte
|
|
next_proto = *((uint8_t *)data);
|
|
if (is_ppp_proto(next_proto))
|
|
{
|
|
hdr_len = 1;
|
|
goto success;
|
|
}
|
|
|
|
// ppp header 2 bytes
|
|
next_proto = ntohs(*((uint16_t *)data));
|
|
if (is_ppp_proto(next_proto))
|
|
{
|
|
hdr_len = 2;
|
|
goto success;
|
|
}
|
|
|
|
// ppp header 4 bytes
|
|
next_proto = ntohs(*((uint16_t *)data + 1));
|
|
hdr_len = 4;
|
|
|
|
success:
|
|
layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_PPP, hdr_len, data, len, 0);
|
|
switch (next_proto)
|
|
{
|
|
case PPP_IP:
|
|
return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
|
|
case PPP_IPV6:
|
|
return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
|
|
default:
|
|
PACKET_LOG_UNSUPPORT_PROTO(pkt, LAYER_PROTO_PPP, next_proto);
|
|
return layer->pld_ptr;
|
|
}
|
|
}
|
|
|
|
static inline const char *parse_l2tpv2_over_udp(struct packet *pkt, const char *data, uint16_t 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_PROTO_L2TP);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_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
|
|
{
|
|
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)
|
|
{
|
|
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_PROTO_L2TP);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_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)
|
|
{
|
|
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_PROTO_L2TP);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_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)
|
|
{
|
|
if (unlikely(len < sizeof(struct vlan_hdr)))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_VLAN);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
uint16_t next_proto = vlan_hdr_get_ethertype((const struct vlan_hdr *)data);
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_VLAN, sizeof(struct vlan_hdr), data, len, 0);
|
|
|
|
return parse_l3(pkt, next_proto, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
|
|
static inline const char *parse_pppoe_ses(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
if (unlikely(len < 6))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_PPPOE);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_PPPOE, 6, data, len, 0);
|
|
|
|
return parse_ppp(pkt, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
|
|
static inline const char *parse_mpls(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
if (unlikely(len < 4))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_MPLS);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
|
|
if (mpls_label_get_bos((const struct mpls_label *)data))
|
|
{
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_MPLS, 4, data, len, 0);
|
|
if (layer->pld_len == 0)
|
|
{
|
|
return layer->pld_ptr;
|
|
}
|
|
uint8_t next_proto = layer->pld_ptr[0] >> 4;
|
|
switch (next_proto)
|
|
{
|
|
case 0:
|
|
// the first four digits of the PW Ethernet control word must be "00000", but the first four digits of Ethernet may also be "0000"
|
|
if (layer->pld_len >= sizeof(struct ethhdr) && is_eth_proto(eth_hdr_get_proto((const struct ethhdr *)layer->pld_ptr)))
|
|
{
|
|
return parse_ether(pkt, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
else
|
|
{
|
|
return parse_pweth(pkt, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
case 4:
|
|
return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
|
|
case 6:
|
|
return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
|
|
default:
|
|
return parse_ether(pkt, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_MPLS, 4, data, len, 0);
|
|
return parse_mpls(pkt, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
}
|
|
|
|
static inline const char *parse_ipv4(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
if (unlikely(len < sizeof(struct ip)))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPV4);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
|
|
const struct ip *hdr = (const struct ip *)data;
|
|
uint8_t version = ip4_hdr_get_version(hdr);
|
|
if (unlikely(version != 4))
|
|
{
|
|
PACKET_PARSE_LOG_ERROR("packet %p ipv4 version %d != 4", pkt, version);
|
|
return data;
|
|
}
|
|
|
|
uint16_t hdr_len = ip4_hdr_get_hdr_len(hdr);
|
|
if (unlikely(hdr_len > len))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPV4);
|
|
return data;
|
|
}
|
|
|
|
uint16_t total_len = ip4_hdr_get_total_len(hdr);
|
|
if (unlikely(total_len > len))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPV4);
|
|
return data;
|
|
}
|
|
if (unlikely(total_len < hdr_len))
|
|
{
|
|
PACKET_PARSE_LOG_ERROR("packet %p ip total_len %d < hdr_len %d", pkt, total_len, hdr_len);
|
|
return data;
|
|
}
|
|
uint16_t trim_len = len - total_len;
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_IPV4, hdr_len, data, len, trim_len);
|
|
|
|
// ip fragmented
|
|
if (ip4_hdr_get_mf_flag(hdr) || ip4_hdr_get_frag_offset(hdr))
|
|
{
|
|
PACKET_PARSE_LOG_DEBUG("packet %p ip layer %p is fragmented", pkt, layer);
|
|
pkt->frag_layer = layer;
|
|
return layer->pld_ptr;
|
|
}
|
|
|
|
uint8_t next_proto = ip4_hdr_get_proto(hdr);
|
|
return parse_l4(pkt, next_proto, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
|
|
static inline const char *parse_ipv6(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
/*
|
|
* IP6 Extension Headers
|
|
*
|
|
* Internet Protocol, Version 6 (IPv6) : https://datatracker.ietf.org/doc/html/rfc2460
|
|
* IP Encapsulating Security Payload (ESP) : https://datatracker.ietf.org/doc/html/rfc2406
|
|
* IP Authentication Header : https://datatracker.ietf.org/doc/html/rfc4302
|
|
*
|
|
* skip next header
|
|
* #define IPPROTO_HOPOPTS 0 // IP6 hop-by-hop options
|
|
* #define IPPROTO_ROUTING 43 // IP6 routing header
|
|
* #define IPPROTO_AH 51 // IP6 Auth Header
|
|
* #define IPPROTO_DSTOPTS 60 // IP6 destination option
|
|
*
|
|
* not skip next header
|
|
* #define IPPROTO_FRAGMENT 44 // IP6 fragmentation header
|
|
* #define IPPROTO_ESP 50 // IP6 Encap Sec. Payload
|
|
* #define IPPROTO_NONE 59 // IP6 no next header
|
|
*/
|
|
|
|
if (unlikely(len < sizeof(struct ip6_hdr)))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPV6);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
const struct ip6_hdr *hdr = (const struct ip6_hdr *)data;
|
|
uint8_t version = ip6_hdr_get_version(hdr);
|
|
if (unlikely(version != 6))
|
|
{
|
|
PACKET_PARSE_LOG_ERROR("packet %p ipv6 version %d != 6", pkt, version);
|
|
return data;
|
|
}
|
|
|
|
uint16_t pld_len = ip6_hdr_get_payload_len(hdr);
|
|
if (unlikely(pld_len + sizeof(struct ip6_hdr) > len))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPV6);
|
|
return data;
|
|
}
|
|
uint8_t next_proto = ip6_hdr_get_next_header(hdr);
|
|
uint16_t hdr_len = sizeof(struct ip6_hdr);
|
|
uint16_t trim_len = len - pld_len - sizeof(struct ip6_hdr);
|
|
const char *next_hdr_ptr = data + hdr_len;
|
|
while (next_proto == IPPROTO_HOPOPTS || next_proto == IPPROTO_ROUTING || next_proto == IPPROTO_AH || next_proto == IPPROTO_DSTOPTS)
|
|
{
|
|
if (unlikely(pld_len < 2))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPV6);
|
|
return data;
|
|
}
|
|
struct ip6_ext *ext = (struct ip6_ext *)next_hdr_ptr;
|
|
uint16_t skip_len = 0;
|
|
if (next_proto == IPPROTO_AH)
|
|
{
|
|
/*
|
|
* https://datatracker.ietf.org/doc/html/rfc4302#section-2
|
|
* For IPv6, the total length of the header must be a multiple of 8-octet units.
|
|
* (Note that although IPv6 [DH98] characterizes AH as an extension header,
|
|
* its length is measured in 32-bit words, not the 64-bit words used by other IPv6 extension headers.)
|
|
*/
|
|
skip_len = ext->ip6e_len * 4 + 8;
|
|
}
|
|
else
|
|
{
|
|
skip_len = ext->ip6e_len * 8 + 8;
|
|
}
|
|
if (unlikely(skip_len > pld_len))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPV6);
|
|
return data;
|
|
}
|
|
hdr_len += skip_len;
|
|
pld_len -= skip_len;
|
|
next_hdr_ptr += skip_len;
|
|
next_proto = ext->ip6e_nxt;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_IPV6, hdr_len, data, len, trim_len);
|
|
|
|
// ipv6 fragment
|
|
if (next_proto == IPPROTO_FRAGMENT)
|
|
{
|
|
PACKET_PARSE_LOG_DEBUG("packet %p ipv6 layer %p is fragmented", pkt, layer);
|
|
pkt->frag_layer = layer;
|
|
return layer->pld_ptr;
|
|
}
|
|
|
|
return parse_l4(pkt, next_proto, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
|
|
static inline const char *parse_auth(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
/*
|
|
* https://datatracker.ietf.org/doc/html/rfc4302#section-2
|
|
* For IPv4: AH not IPv4 option, as an single layer
|
|
* For IPv6: AH as IPv6 extension header
|
|
*
|
|
* AH Format
|
|
*
|
|
* 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
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* | Next Header | Payload Len | RESERVED |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* | Security Parameters Index (SPI) |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* | Sequence Number Field |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* | |
|
|
* + Integrity Check Value-ICV (variable) |
|
|
* | |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
*/
|
|
|
|
if (unlikely(len < 12))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPAH);
|
|
return data;
|
|
}
|
|
/*
|
|
* https://datatracker.ietf.org/doc/html/rfc4302#section-2
|
|
* For IPv4, This 8-bit field specifies the length of AH in 32-bit words (4-byte units), minus "2".
|
|
* Thus, for example, if an integrity algorithm yields a 96-bit authentication value,
|
|
* this length field will be "4" (3 32-bit word fixed fields plus 3 32-bit words for the ICV, minus 2).
|
|
*/
|
|
uint8_t next_proto = ((const uint8_t *)data)[0];
|
|
uint16_t hdr_len = ((const uint8_t *)data)[1] * 4 + 8;
|
|
if (unlikely(len < hdr_len))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPAH);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_IPAH, hdr_len, data, len, 0);
|
|
|
|
return parse_l4(pkt, next_proto, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
|
|
static inline const char *parse_gre(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
uint16_t hdr_len = calc_gre_hdr_len(data, len);
|
|
if (unlikely(hdr_len == 0 || hdr_len > len))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_GRE);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
uint16_t next_proto = peek_gre_proto(data, len);
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_GRE, hdr_len, data, len, 0);
|
|
|
|
return parse_l3(pkt, next_proto, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
|
|
static inline const char *parse_udp(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
if (unlikely(len < sizeof(struct udphdr)))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_UDP);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
const struct udphdr *udp_hdr = (struct udphdr *)data;
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_UDP, sizeof(struct udphdr), data, len, 0);
|
|
|
|
uint16_t src_port = udp_hdr_get_src_port(udp_hdr);
|
|
uint16_t dst_port = udp_hdr_get_dst_port(udp_hdr);
|
|
|
|
if (dst_port == 4789)
|
|
{
|
|
// VXLAN_DPORT 4789
|
|
return parse_vxlan(pkt, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
|
|
if (dst_port == 2152 || src_port == 2152)
|
|
{
|
|
// only GTPv1-U, no GTPv2-U
|
|
return parse_gtp_u(pkt, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
|
|
if (dst_port == 2123 || src_port == 2123)
|
|
{
|
|
// GTPv1-C or GTPv2-C
|
|
return parse_gtp_c(pkt, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
|
|
if (dst_port == 1701 || src_port == 1701)
|
|
{
|
|
// L2TP_DPORT 1701
|
|
if (unlikely(layer->pld_len < 8))
|
|
{
|
|
return layer->pld_ptr;
|
|
}
|
|
|
|
switch (l2tp_hdr_get_ver((const struct l2tp_hdr *)layer->pld_ptr))
|
|
{
|
|
case 2:
|
|
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:
|
|
return layer->pld_ptr;
|
|
}
|
|
}
|
|
|
|
if (dst_port == 3544 || src_port == 3544)
|
|
{
|
|
// Teredo IPv6 tunneling 3544
|
|
if (unlikely(layer->pld_len < sizeof(struct ip6_hdr)))
|
|
{
|
|
return layer->pld_ptr;
|
|
}
|
|
const struct ip6_hdr *ipv6_hdr = (const struct ip6_hdr *)layer->pld_ptr;
|
|
if (ip6_hdr_get_version(ipv6_hdr) != 6)
|
|
{
|
|
return layer->pld_ptr;
|
|
}
|
|
return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
|
|
return layer->pld_ptr;
|
|
}
|
|
|
|
static inline const char *parse_tcp(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
if (unlikely(len < sizeof(struct tcphdr)))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_TCP);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
uint16_t hdr_len = tcp_hdr_get_hdr_len((const struct tcphdr *)data);
|
|
if (unlikely(hdr_len > len))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_TCP);
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_TCP, hdr_len, data, len, 0);
|
|
|
|
return layer->pld_ptr;
|
|
}
|
|
|
|
static inline const char *parse_icmp(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
if (unlikely(len < sizeof(struct icmphdr)))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_ICMP);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_ICMP, sizeof(struct icmphdr), data, len, 0);
|
|
|
|
return layer->pld_ptr;
|
|
}
|
|
|
|
static inline const char *parse_icmp6(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
if (unlikely(len < sizeof(struct icmp6_hdr)))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_ICMP6);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_ICMP6, sizeof(struct icmp6_hdr), data, len, 0);
|
|
|
|
return layer->pld_ptr;
|
|
}
|
|
|
|
static inline const char *parse_vxlan(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
if (unlikely(len < sizeof(struct vxlan_hdr)))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_VXLAN);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_VXLAN, sizeof(struct vxlan_hdr), data, len, 0);
|
|
|
|
return parse_ether(pkt, layer->pld_ptr, layer->pld_len);
|
|
}
|
|
|
|
static inline const char *parse_gtp_u(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
// only GTPv1-U, no GTPv2-U
|
|
uint8_t version = peek_gtp_version(data, len);
|
|
if (unlikely(version != 1))
|
|
{
|
|
return data;
|
|
}
|
|
|
|
uint16_t hdr_len = calc_gtp_hdr_len(data, len);
|
|
if (unlikely(hdr_len == 0 || hdr_len > len))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_GTP_U);
|
|
return data;
|
|
}
|
|
|
|
if (hdr_len == len)
|
|
{
|
|
// only GTP-U header, no payload
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_GTP_U, hdr_len, data, len, 0);
|
|
return layer->pld_ptr;
|
|
}
|
|
else
|
|
{
|
|
uint8_t next_proto = (((const uint8_t *)(data + hdr_len))[0]) >> 4;
|
|
if (next_proto != 4 && next_proto != 6)
|
|
{
|
|
// next_proto is not IPv4 or IPv6, this is not a normal GTP-U packet, fallback to UDP
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_GTP_U, hdr_len, data, len, 0);
|
|
|
|
switch (next_proto)
|
|
{
|
|
case 4:
|
|
return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
|
|
case 6:
|
|
return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
|
|
default:
|
|
PACKET_LOG_UNSUPPORT_PROTO(pkt, LAYER_PROTO_GTP_U, next_proto);
|
|
return layer->pld_ptr;
|
|
}
|
|
}
|
|
}
|
|
|
|
static inline const char *parse_gtp_c(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
// GTPv1-C or GTPv2-C
|
|
uint16_t hdr_len = calc_gtp_hdr_len(data, len);
|
|
if (unlikely(hdr_len == 0 || hdr_len > len))
|
|
{
|
|
PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_GTP_C);
|
|
return data;
|
|
}
|
|
|
|
struct layer_internal *layer = get_free_layer(pkt);
|
|
if (unlikely(layer == NULL))
|
|
{
|
|
return data;
|
|
}
|
|
SET_LAYER(pkt, layer, LAYER_PROTO_GTP_C, hdr_len, data, len, 0);
|
|
|
|
return layer->pld_ptr;
|
|
}
|
|
|
|
static inline const char *parse_l3(struct packet *pkt, uint16_t next_proto, const char *data, uint16_t len)
|
|
{
|
|
switch (next_proto)
|
|
{
|
|
case ETH_P_8021Q:
|
|
case ETH_P_8021AD:
|
|
return parse_vlan(pkt, data, len);
|
|
case ETH_P_IP:
|
|
return parse_ipv4(pkt, data, len);
|
|
case ETH_P_IPV6:
|
|
return parse_ipv6(pkt, data, len);
|
|
case ETH_P_PPP_SES:
|
|
return parse_pppoe_ses(pkt, data, len);
|
|
case ETH_P_MPLS_UC:
|
|
return parse_mpls(pkt, data, len);
|
|
case 0x880b:
|
|
return parse_ppp(pkt, data, len);
|
|
default:
|
|
PACKET_LOG_UNSUPPORT_ETHPROTO(pkt, next_proto);
|
|
return data;
|
|
}
|
|
}
|
|
|
|
static inline const char *parse_l4(struct packet *pkt, uint8_t next_proto, const char *data, uint16_t len)
|
|
{
|
|
pkt->ip_proto = next_proto;
|
|
switch (next_proto)
|
|
{
|
|
case IPPROTO_AH:
|
|
return parse_auth(pkt, data, len);
|
|
case IPPROTO_TCP:
|
|
return parse_tcp(pkt, data, len);
|
|
case IPPROTO_UDP:
|
|
return parse_udp(pkt, data, len);
|
|
case IPPROTO_IPIP:
|
|
return parse_ipv4(pkt, data, len);
|
|
case IPPROTO_IPV6:
|
|
return parse_ipv6(pkt, data, len);
|
|
case IPPROTO_GRE:
|
|
return parse_gre(pkt, data, len);
|
|
case IPPROTO_ICMP:
|
|
return parse_icmp(pkt, data, len);
|
|
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(pkt, next_proto);
|
|
return data;
|
|
}
|
|
}
|
|
|
|
/******************************************************************************
|
|
* Public API
|
|
******************************************************************************/
|
|
|
|
// return innermost payload
|
|
const char *packet_parse(struct packet *pkt, const char *data, uint16_t len)
|
|
{
|
|
pkt->frag_layer = NULL;
|
|
pkt->layers_used = 0;
|
|
pkt->layers_size = PACKET_MAX_LAYERS;
|
|
pkt->data_ptr = data;
|
|
pkt->data_len = len;
|
|
pkt->trim_len = 0;
|
|
pkt->ip_proto = 0;
|
|
|
|
return parse_ether(pkt, data, len);
|
|
}
|
|
|
|
const char *layer_proto_to_str(enum layer_proto proto)
|
|
{
|
|
switch (proto)
|
|
{
|
|
case LAYER_PROTO_ETHER:
|
|
return "ETH";
|
|
case LAYER_PROTO_PWETH:
|
|
return "PWETH";
|
|
case LAYER_PROTO_PPP:
|
|
return "PPP";
|
|
case LAYER_PROTO_L2TP:
|
|
return "L2TP";
|
|
case LAYER_PROTO_VLAN:
|
|
return "VLAN";
|
|
case LAYER_PROTO_PPPOE:
|
|
return "PPPOE";
|
|
case LAYER_PROTO_MPLS:
|
|
return "MPLS";
|
|
case LAYER_PROTO_IPV4:
|
|
return "IPV4";
|
|
case LAYER_PROTO_IPV6:
|
|
return "IPV6";
|
|
case LAYER_PROTO_IPAH:
|
|
return "IPAH";
|
|
case LAYER_PROTO_GRE:
|
|
return "GRE";
|
|
case LAYER_PROTO_UDP:
|
|
return "UDP";
|
|
case LAYER_PROTO_TCP:
|
|
return "TCP";
|
|
case LAYER_PROTO_ICMP:
|
|
return "ICMP";
|
|
case LAYER_PROTO_ICMP6:
|
|
return "ICMP6";
|
|
case LAYER_PROTO_VXLAN:
|
|
return "VXLAN";
|
|
case LAYER_PROTO_GTP_C:
|
|
return "GTP-C";
|
|
case LAYER_PROTO_GTP_U:
|
|
return "GTP-U";
|
|
default:
|
|
return "UNKNOWN";
|
|
}
|
|
} |