支持-g参数, 可以按最内层IP,PORT过滤, 支持BPF过滤规则.
This commit is contained in:
lijia
826
net_common.c
Normal file
826
net_common.c
Normal file
@@ -0,0 +1,826 @@
|
||||
|
||||
#include <linux/if_ether.h>
|
||||
#include <net/if_arp.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <netinet/in.h>
|
||||
#include <assert.h>
|
||||
#include <ctype.h>
|
||||
|
||||
#include "netdissect-stdinc.h"
|
||||
#include "netdissect.h"
|
||||
#include "stream_base.h"
|
||||
#include "mesa_net.h"
|
||||
#include "ip.h"
|
||||
#include "ip6.h"
|
||||
#include "tcp.h"
|
||||
#include "udp.h"
|
||||
#include "ppp.h"
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
static int eth_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type);
|
||||
static int vlan8021q_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type);
|
||||
static int ipv4_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type);
|
||||
static int ipv6_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type);
|
||||
|
||||
static int treat_vlan_as_mac_in_mac_sw = 1;
|
||||
|
||||
/* ascii<69>ַ<EFBFBD>ת16<31><36><EFBFBD><EFBFBD> */
|
||||
char MESA_ascii_to_hex(char ascii)
|
||||
{
|
||||
char c = 0;
|
||||
|
||||
switch(ascii)
|
||||
{
|
||||
case '0':
|
||||
case '1':
|
||||
case '2':
|
||||
case '3':
|
||||
case '4':
|
||||
case '5':
|
||||
case '6':
|
||||
case '7':
|
||||
case '8':
|
||||
case '9':
|
||||
c = ascii - 0x30;
|
||||
break;
|
||||
|
||||
case 'a':
|
||||
case 'b':
|
||||
case 'c':
|
||||
case 'd':
|
||||
case 'e':
|
||||
case 'f':
|
||||
c = 10 + ascii - 0x61;
|
||||
break;
|
||||
|
||||
case 'A':
|
||||
case 'B':
|
||||
case 'C':
|
||||
case 'D':
|
||||
case 'E':
|
||||
case 'F':
|
||||
c = 10 + ascii - 0x41;
|
||||
break;
|
||||
}
|
||||
|
||||
return c;
|
||||
}
|
||||
|
||||
|
||||
|
||||
static int arp_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type)
|
||||
{
|
||||
/* arpЭ<70>鲻<EFBFBD><E9B2BB><EFBFBD><EFBFBD><EFBFBD>κ<EFBFBD><CEBA>ϲ<EFBFBD><CFB2><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD> */
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
static int gtp_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type)
|
||||
{
|
||||
const struct gtp_hdr *gh = (struct gtp_hdr *)raw_data;
|
||||
int opt_len = 0; /* <20><>ѡ<EFBFBD><EFBFBD><EEB3A4> */
|
||||
const unsigned char *next_ip_layer_hdr;
|
||||
int skip_len;
|
||||
|
||||
if(ADDR_TYPE_GPRS_TUNNEL == expect_layer_type){
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(gh->flags & 0x2){
|
||||
opt_len += sizeof(int); /* sequence */
|
||||
}
|
||||
|
||||
next_ip_layer_hdr = (unsigned char *)raw_data + opt_len + sizeof(struct gtp_hdr);
|
||||
|
||||
if((*next_ip_layer_hdr & 0x40) == 0x40){
|
||||
skip_len = ipv4_jump_to_layer((char *)next_ip_layer_hdr, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
|
||||
}else if((*next_ip_layer_hdr & 0x60) == 0x60){
|
||||
skip_len = ipv6_jump_to_layer((char *)next_ip_layer_hdr, __ADDR_TYPE_IP_PAIR_V6, expect_layer_type);
|
||||
}else{
|
||||
////sapp_runtime_log(20, "TODO: jmp unsupport type in GTP, 0x%x!\n", (*next_ip_layer_hdr));
|
||||
return -1;
|
||||
}
|
||||
|
||||
return opt_len + sizeof(struct gtp_hdr) + skip_len;
|
||||
}
|
||||
|
||||
static int udp_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type)
|
||||
{
|
||||
const struct mesa_udp_hdr *uh = (const struct mesa_udp_hdr *)raw_data;
|
||||
unsigned short usport, udport;
|
||||
int skip_len;
|
||||
|
||||
if(ADDR_TYPE_UDP == expect_layer_type){
|
||||
return 0;
|
||||
}
|
||||
|
||||
usport = ntohs(uh->uh_sport);
|
||||
udport = ntohs(uh->uh_dport);
|
||||
|
||||
if((2152 == usport) && (2152 == udport)){
|
||||
skip_len = gtp_jump_to_layer(raw_data+sizeof(struct mesa_udp_hdr), ADDR_TYPE_UDP, expect_layer_type);
|
||||
}else if((3544 == usport) || (3544 == udport)){
|
||||
;
|
||||
//TODO
|
||||
//skip_len = teredo_jump_to_layer(raw_data+sizeof(struct mesa_udp_hdr), ADDR_TYPE_UDP, expect_layer_type);
|
||||
}else{
|
||||
/* <20><><EFBFBD><EFBFBD>UDP<44><50><EFBFBD>Ͳ<EFBFBD>֧<EFBFBD><D6A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת */
|
||||
return -1;
|
||||
}
|
||||
|
||||
return skip_len + sizeof(struct mesa_udp_hdr);
|
||||
}
|
||||
|
||||
static int ipv4_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type)
|
||||
{
|
||||
struct ip *p_ip_hdr = (struct ip *)raw_data;
|
||||
int skip_len = 0;
|
||||
int ip_hdr_len = IP_HL(p_ip_hdr) * 4;
|
||||
//const char *next_layer_data = raw_data + ip_hdr_len;
|
||||
|
||||
if(raw_layer_type == expect_layer_type){
|
||||
return 0;
|
||||
}
|
||||
|
||||
switch(p_ip_hdr->ip_p){
|
||||
case IPPROTO_TCP:
|
||||
if(ADDR_TYPE_TCP == expect_layer_type){
|
||||
skip_len = 0;
|
||||
break;
|
||||
}else{
|
||||
skip_len = -1; /* tcp <20><>֮<EFBFBD>ϲ<EFBFBD><CFB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD> */
|
||||
}
|
||||
break;
|
||||
|
||||
case IPPROTO_UDP:
|
||||
if(ADDR_TYPE_UDP == expect_layer_type){
|
||||
skip_len = 0;
|
||||
break;
|
||||
}else{
|
||||
skip_len = udp_jump_to_layer(raw_data+ip_hdr_len, ADDR_TYPE_UDP, expect_layer_type);
|
||||
}
|
||||
break;
|
||||
|
||||
case IPPROTO_IPV6:
|
||||
if(__ADDR_TYPE_IP_PAIR_V6 == expect_layer_type){
|
||||
skip_len = 0;
|
||||
break;
|
||||
}else{
|
||||
skip_len = ipv6_jump_to_layer(raw_data+ip_hdr_len, __ADDR_TYPE_IP_PAIR_V6, expect_layer_type);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
skip_len = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
if(skip_len < 0){
|
||||
return -1;
|
||||
}
|
||||
|
||||
return skip_len + sizeof(struct ip);
|
||||
}
|
||||
|
||||
static int ipv6_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type)
|
||||
{
|
||||
const struct mesa_ip6_hdr *a_packet = (const struct mesa_ip6_hdr *)raw_data;
|
||||
UINT8 next_hdr_type = a_packet->ip6_nxt_hdr;
|
||||
UINT8 *next_hdr_ptr = (UINT8 *)a_packet + sizeof(struct mesa_ip6_hdr);
|
||||
int skip_len = 0;
|
||||
int offset_to_ip6 = 0;
|
||||
|
||||
if(raw_layer_type == expect_layer_type){
|
||||
return 0;
|
||||
}
|
||||
|
||||
while(1){
|
||||
offset_to_ip6 = 0;
|
||||
switch(next_hdr_type)
|
||||
{
|
||||
case 0: //NEXTHDR_HOP:
|
||||
case 43://NEXTHDR_ROUTING:
|
||||
case 51://NEXTHDR_AUTH:
|
||||
case 60://NEXTHDR_DEST:
|
||||
offset_to_ip6 = (*(next_hdr_ptr + 1))*8 + 8; /* ѡ<><EFBFBD><EEB3A4><EFBFBD><EFBFBD>8<EFBFBD>ֽ<EFBFBD>Ϊ<EFBFBD><CEAA>λ */
|
||||
break;
|
||||
|
||||
case 4://NEXTHDR_IPIP:
|
||||
if(__ADDR_TYPE_IP_PAIR_V4 == expect_layer_type){
|
||||
skip_len = next_hdr_ptr - (UINT8 *)raw_data;
|
||||
}else{
|
||||
skip_len = ipv4_jump_to_layer((const char *)next_hdr_ptr, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
|
||||
if(skip_len < 0){
|
||||
return -1;
|
||||
}else{
|
||||
return skip_len + next_hdr_ptr - (UINT8 *)raw_data;
|
||||
}
|
||||
}
|
||||
goto done;
|
||||
break;
|
||||
|
||||
case 59://NEXTHDR_NONE:
|
||||
skip_len = -1;
|
||||
goto done;
|
||||
break;
|
||||
|
||||
case 58://NEXTHDR_ICMP: /* IMCP<43><50><EFBFBD>ٳ<EFBFBD><D9B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD> */
|
||||
skip_len = -1;
|
||||
goto done;
|
||||
break;
|
||||
|
||||
case 6://NEXTHDR_TCP:
|
||||
if(ADDR_TYPE_TCP == expect_layer_type){
|
||||
skip_len = next_hdr_ptr - (UINT8 *)raw_data;
|
||||
}else{
|
||||
skip_len = -1;
|
||||
}
|
||||
goto done;
|
||||
break;
|
||||
|
||||
case 17://NEXTHDR_UDP:
|
||||
if(ADDR_TYPE_UDP == expect_layer_type){
|
||||
skip_len = next_hdr_ptr - (UINT8 *)raw_data;
|
||||
}else{
|
||||
skip_len = -1;
|
||||
}
|
||||
goto done;
|
||||
break;
|
||||
|
||||
case 44:///NEXTHDR_FRAGMENT:
|
||||
offset_to_ip6 = 8; // 8 == sizeof(struct ipv6_frag_hdr);
|
||||
break;
|
||||
|
||||
case 50://NEXTHDR_ESP:
|
||||
skip_len = -1;
|
||||
goto done;
|
||||
|
||||
default:
|
||||
printf("TODO:jmp Unknown IPv6 header type:0x%x!\n", next_hdr_type);
|
||||
skip_len = -1;
|
||||
goto done;
|
||||
break;
|
||||
}
|
||||
|
||||
next_hdr_type = *next_hdr_ptr;
|
||||
next_hdr_ptr += offset_to_ip6;
|
||||
}
|
||||
|
||||
done:
|
||||
if(skip_len < 0){
|
||||
return -1;
|
||||
}
|
||||
|
||||
return skip_len;
|
||||
}
|
||||
|
||||
static int ppp_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type)
|
||||
{
|
||||
int skip_len = 0;
|
||||
struct mesa_pppoe_session_hdr *pppoe_ses_hdr;
|
||||
char *next_hdr;
|
||||
|
||||
if(raw_layer_type == expect_layer_type){
|
||||
return 0;
|
||||
}
|
||||
pppoe_ses_hdr = (struct mesa_pppoe_session_hdr *)raw_data;
|
||||
next_hdr = (char *)raw_data + sizeof(struct mesa_pppoe_session_hdr);
|
||||
|
||||
switch(ntohs(pppoe_ses_hdr->ppp_protocol)){
|
||||
case PPP_PROTOCOL_IPv4:
|
||||
if(__ADDR_TYPE_IP_PAIR_V4 == expect_layer_type){
|
||||
break;
|
||||
}else{
|
||||
skip_len = ipv4_jump_to_layer(next_hdr, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
|
||||
}
|
||||
break;
|
||||
|
||||
case PPP_IPV6:
|
||||
if(__ADDR_TYPE_IP_PAIR_V6 == expect_layer_type){
|
||||
break;
|
||||
}else{
|
||||
skip_len = ipv6_jump_to_layer(next_hdr, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
|
||||
}
|
||||
break;
|
||||
|
||||
case PPP_COMP:
|
||||
case PPP_CCP:
|
||||
case PPP_IPCP:
|
||||
case PPP_PAP:
|
||||
case PPP_CHAP:
|
||||
case 0xC025: ///PPP_LQR:
|
||||
case PPP_PROTOCOL_LCP:
|
||||
|
||||
/* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD>ò<EFBFBD>Э<EFBFBD><D0AD> */
|
||||
skip_len = -1;
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("TODO: jmp unsupport ppp pro:0x%x!\n", ntohs(pppoe_ses_hdr->ppp_protocol));
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
if(skip_len < 0){
|
||||
return -1;
|
||||
}
|
||||
|
||||
return skip_len + sizeof(struct mesa_pppoe_session_hdr);
|
||||
}
|
||||
|
||||
|
||||
static int mpls_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type)
|
||||
{
|
||||
int skip_len = 0;
|
||||
const struct mesa_mpls_hdr *mpls_hdr = (const struct mesa_mpls_hdr *)raw_data;
|
||||
const char *next_layer_data = raw_data + sizeof(struct mesa_mpls_hdr);
|
||||
|
||||
if(raw_layer_type == expect_layer_type){
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(0 == mpls_hdr->mpls_bls){ /* <20><>MPLSջ<53><D5BB>, <20>ݹ<EFBFBD><DDB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||||
return mpls_jump_to_layer(next_layer_data, ADDR_TYPE_MPLS, expect_layer_type) + sizeof(struct mesa_mpls_hdr);
|
||||
}
|
||||
|
||||
/* MPLSû<53><C3BB><EFBFBD>ֶα<D6B6>ʶ<EFBFBD><CAB6>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ʲô, <20><><EFBFBD>²<EFBFBD><C2B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>IP<49><50><EFBFBD><EFBFBD> */
|
||||
if((*next_layer_data & 0x40) == 0x40){
|
||||
skip_len = ipv4_jump_to_layer(next_layer_data, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
|
||||
}else if((*next_layer_data & 0x60) == 0x60){
|
||||
skip_len = ipv6_jump_to_layer(next_layer_data, __ADDR_TYPE_IP_PAIR_V6, expect_layer_type);
|
||||
}else{
|
||||
//sapp_runtime_log(20, "TODO: jmp unsupport type in MPLS, 0x%x!\n", (unsigned char)(*next_layer_data));
|
||||
return -1;
|
||||
}
|
||||
|
||||
return skip_len + sizeof(struct mesa_mpls_hdr); /* mpls header is 4 byte */
|
||||
}
|
||||
|
||||
static int __common_eth_type_dispatch(UINT16 eth_type, const char *next_layer_data, int raw_layer_type, int expect_layer_type)
|
||||
{
|
||||
int skip_len = 0;
|
||||
|
||||
switch(eth_type){
|
||||
case ETH_P_ARP:
|
||||
if(ADDR_TYPE_ARP == expect_layer_type){
|
||||
break;
|
||||
}else{
|
||||
skip_len = arp_jump_to_layer(next_layer_data, ADDR_TYPE_ARP, expect_layer_type);
|
||||
}
|
||||
break;
|
||||
|
||||
case ETH_P_8021Q:
|
||||
if(ADDR_TYPE_VLAN == expect_layer_type){
|
||||
break;
|
||||
}else{
|
||||
if(treat_vlan_as_mac_in_mac_sw){
|
||||
skip_len = eth_jump_to_layer(next_layer_data, ADDR_TYPE_MAC, expect_layer_type);
|
||||
}else{
|
||||
skip_len = vlan8021q_jump_to_layer(next_layer_data, ADDR_TYPE_VLAN, expect_layer_type);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case ETH_P_IP:
|
||||
if(__ADDR_TYPE_IP_PAIR_V4 == expect_layer_type){
|
||||
break;
|
||||
}else{
|
||||
skip_len = ipv4_jump_to_layer(next_layer_data, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
|
||||
}
|
||||
break;
|
||||
|
||||
case ETH_P_IPV6:
|
||||
if(__ADDR_TYPE_IP_PAIR_V6 == expect_layer_type){
|
||||
break;
|
||||
}else{
|
||||
skip_len = ipv6_jump_to_layer(next_layer_data, __ADDR_TYPE_IP_PAIR_V6, expect_layer_type);
|
||||
}
|
||||
break;
|
||||
|
||||
case ETH_P_PPP_SES:
|
||||
if(ADDR_TYPE_PPPOE_SES == expect_layer_type){
|
||||
break;
|
||||
}else{
|
||||
skip_len = ppp_jump_to_layer(next_layer_data, ADDR_TYPE_PPPOE_SES, expect_layer_type);
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x88A8: /* MAC_IN_MAC */
|
||||
skip_len = eth_jump_to_layer(next_layer_data, ADDR_TYPE_MAC, expect_layer_type);
|
||||
break;
|
||||
|
||||
case 0x8847: /* MPLS, ETH_P_MPLS_UC */
|
||||
skip_len = mpls_jump_to_layer(next_layer_data, ADDR_TYPE_MPLS, expect_layer_type);
|
||||
break;
|
||||
|
||||
default:
|
||||
skip_len = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
return skip_len;
|
||||
}
|
||||
|
||||
static int vlan8021q_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type)
|
||||
{
|
||||
int skip_len = 0;
|
||||
const struct mesa_vlan_hdr *vlan_hdr = (const struct mesa_vlan_hdr *)raw_data;
|
||||
const char *next_layer_data = raw_data + sizeof(struct mesa_vlan_hdr);
|
||||
|
||||
if(raw_layer_type == expect_layer_type){
|
||||
return 0;
|
||||
}
|
||||
|
||||
switch(ntohs(vlan_hdr->type)){
|
||||
case ETH_P_ARP:
|
||||
if(ADDR_TYPE_ARP == expect_layer_type){
|
||||
break;
|
||||
}else{
|
||||
skip_len = arp_jump_to_layer(next_layer_data, ADDR_TYPE_ARP, expect_layer_type);
|
||||
}
|
||||
break;
|
||||
|
||||
case ETH_P_IP:
|
||||
if(__ADDR_TYPE_IP_PAIR_V4 == expect_layer_type){
|
||||
break;
|
||||
}else{
|
||||
skip_len = ipv4_jump_to_layer(next_layer_data, __ADDR_TYPE_IP_PAIR_V4, expect_layer_type);
|
||||
}
|
||||
break;
|
||||
|
||||
case ETH_P_IPV6:
|
||||
if(__ADDR_TYPE_IP_PAIR_V6 == expect_layer_type){
|
||||
break;
|
||||
}else{
|
||||
skip_len = ipv6_jump_to_layer(next_layer_data, __ADDR_TYPE_IP_PAIR_V6, expect_layer_type);
|
||||
}
|
||||
break;
|
||||
|
||||
case ETH_P_PPP_SES:
|
||||
if(ADDR_TYPE_PPPOE_SES == expect_layer_type){
|
||||
break;
|
||||
}else{
|
||||
skip_len = ppp_jump_to_layer(next_layer_data, ADDR_TYPE_PPPOE_SES, expect_layer_type);
|
||||
}
|
||||
break;
|
||||
|
||||
case ETH_P_PPP_DISC: /* pppoe<6F><65><EFBFBD>ֽ<D6BD> */
|
||||
skip_len = -1;
|
||||
break;
|
||||
|
||||
case 0x8200: /* XinJing<6E><67><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD><EFBFBD>, δ֪ʲô<CAB2><C3B4> */
|
||||
skip_len = -1;
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("TODO: jmp unsupport type in vlan8021q, 0x%x!\n", ntohs(vlan_hdr->type));
|
||||
skip_len = -1;
|
||||
}
|
||||
|
||||
if(skip_len < 0){
|
||||
return -1;
|
||||
}
|
||||
|
||||
return skip_len + sizeof(struct mesa_vlan_hdr);
|
||||
}
|
||||
|
||||
static int eth_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type)
|
||||
{
|
||||
struct ethhdr *p_eth_hdr = (struct ethhdr *)raw_data;
|
||||
unsigned short eth_type = ntohs(p_eth_hdr->h_proto);
|
||||
//int skip_len = -1;
|
||||
const char *next_layer_data = raw_data + sizeof(struct ethhdr);
|
||||
int layer_skip_len;
|
||||
|
||||
if(raw_layer_type == expect_layer_type){
|
||||
return 0;
|
||||
}
|
||||
|
||||
layer_skip_len = __common_eth_type_dispatch(eth_type, next_layer_data, raw_layer_type, expect_layer_type);
|
||||
if(layer_skip_len < 0){
|
||||
return -1;
|
||||
}
|
||||
|
||||
return layer_skip_len + sizeof(struct ethhdr);
|
||||
}
|
||||
|
||||
|
||||
static int mac_in_mac_jump_to_layer(const char *raw_data, int raw_layer_type, int expect_layer_type)
|
||||
{
|
||||
struct ethhdr *inner_eth_hdr = (struct ethhdr *)(raw_data + sizeof(struct ethhdr ));
|
||||
unsigned short inner_eth_type = ntohs(inner_eth_hdr->h_proto);
|
||||
//int skip_len = -1;
|
||||
const char *next_layer_data = raw_data + sizeof(struct ethhdr);
|
||||
int layer_skip_len;
|
||||
|
||||
if(raw_layer_type == expect_layer_type){
|
||||
return 0;
|
||||
}
|
||||
|
||||
layer_skip_len = __common_eth_type_dispatch(inner_eth_type, next_layer_data, raw_layer_type, expect_layer_type);
|
||||
if(layer_skip_len < 0){
|
||||
return -1;
|
||||
}
|
||||
|
||||
return layer_skip_len + sizeof(struct ethhdr) * 2;
|
||||
}
|
||||
|
||||
/*
|
||||
return value:
|
||||
Non-NULL: the pointer to expect layer;
|
||||
NULL: not found expect layer.
|
||||
*/
|
||||
const void *MESA_net_jump_to_layer(const void *raw_data, int raw_layer_type, int expect_layer_type)
|
||||
{
|
||||
int ret;
|
||||
|
||||
if(raw_layer_type <= __ADDR_TYPE_INIT || raw_layer_type >= __ADDR_TYPE_MAX){
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if(expect_layer_type <= __ADDR_TYPE_INIT || expect_layer_type >= __ADDR_TYPE_MAX){
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if(ADDR_TYPE_IPV4 == expect_layer_type){
|
||||
/* ת<>ɴ<EFBFBD>IPv4<76><34>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD> */
|
||||
expect_layer_type = __ADDR_TYPE_IP_PAIR_V4;
|
||||
}
|
||||
|
||||
if(ADDR_TYPE_IPV6 == expect_layer_type){
|
||||
/* ת<>ɴ<EFBFBD>IPv6<76><36>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD> */
|
||||
expect_layer_type = __ADDR_TYPE_IP_PAIR_V6;
|
||||
}
|
||||
|
||||
if(raw_layer_type == expect_layer_type){
|
||||
return raw_data;
|
||||
}
|
||||
|
||||
switch(raw_layer_type){
|
||||
case ADDR_TYPE_MAC:
|
||||
ret = eth_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
|
||||
break;
|
||||
|
||||
case ADDR_TYPE_ARP:
|
||||
ret = arp_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
|
||||
break;
|
||||
case ADDR_TYPE_VLAN:
|
||||
if(treat_vlan_as_mac_in_mac_sw){
|
||||
ret = mac_in_mac_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
|
||||
}else{
|
||||
ret = vlan8021q_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
|
||||
}
|
||||
break;
|
||||
|
||||
case __ADDR_TYPE_IP_PAIR_V4:
|
||||
ret = ipv4_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
|
||||
break;
|
||||
|
||||
case __ADDR_TYPE_IP_PAIR_V6:
|
||||
ret = ipv6_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
|
||||
break;
|
||||
|
||||
case ADDR_TYPE_MAC_IN_MAC:
|
||||
ret = mac_in_mac_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
|
||||
break;
|
||||
|
||||
case ADDR_TYPE_UDP:
|
||||
ret = udp_jump_to_layer((const char *)raw_data, raw_layer_type, expect_layer_type);
|
||||
break;
|
||||
|
||||
case ADDR_TYPE_PPPOE_SES:
|
||||
case ADDR_TYPE_MPLS:
|
||||
case ADDR_TYPE_GRE:
|
||||
default:
|
||||
printf("TODO: jmp unsupport raw_layer_type:%d in MESA_net_jump_to_layer()!\n", raw_layer_type);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if(ret < 0){
|
||||
return NULL;
|
||||
}
|
||||
|
||||
return ((const char *)raw_data + ret);
|
||||
}
|
||||
|
||||
/*
|
||||
<09><>MESA_net_jump_to_layer()<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
|
||||
MESA_net_jump_to_layer()<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㿪ʼ, <20>ҵ<EFBFBD><D2B5><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD>˳<EFBFBD>;
|
||||
MESA_net_jump_to_layer_greedy()<29><>һֱ<D2BB><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڲ<EFBFBD>Э<EFBFBD><D0AD>ͷ, <20>ʺ<EFBFBD><CABA><EFBFBD><EFBFBD><EFBFBD>ģʽ.
|
||||
|
||||
return value:
|
||||
Non-NULL: the pointer to expect layer;
|
||||
NULL: not found expect layer.
|
||||
*/
|
||||
const void *MESA_net_jump_to_layer_greedy(const void *raw_data, int raw_layer_type, int expect_layer_type)
|
||||
{
|
||||
const void *expect_layer;
|
||||
const void *success_layer = NULL; /* <20><><EFBFBD><EFBFBD>һ<EFBFBD>γɹ<CEB3><C9B9>ҵ<EFBFBD><D2B5>IJ<EFBFBD> */
|
||||
int new_raw_layer_type = raw_layer_type; /* <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD>ܻ<EFBFBD><DCBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD>Ϣ */
|
||||
const char *new_next_layer_data = (char *)raw_data;
|
||||
|
||||
expect_layer = MESA_net_jump_to_layer(new_next_layer_data, new_raw_layer_type, expect_layer_type);
|
||||
while(expect_layer){
|
||||
success_layer = expect_layer;
|
||||
|
||||
switch(expect_layer_type){
|
||||
case __ADDR_TYPE_IP_PAIR_V4:
|
||||
{
|
||||
const struct mesa_ip4_hdr *ip4hdr = (const struct mesa_ip4_hdr *)expect_layer;
|
||||
if(IPPROTO_UDP == ip4hdr->ip_p){
|
||||
new_next_layer_data = (char *)expect_layer + ip4hdr->ip_hl * 4;
|
||||
new_raw_layer_type = ADDR_TYPE_UDP; /* IP<49><50><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƫ<EFBFBD><C6AB>, ֻ֧<D6BB><D6A7>UDP, IPIP, GRE, L2TPv3. */
|
||||
}else{
|
||||
//TODO 2, GRE, IPIP, L2TPv3
|
||||
goto done;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case __ADDR_TYPE_IP_PAIR_V6:
|
||||
{
|
||||
//TODO2,
|
||||
//sapp_runtime_log(20, "MESA_net_jump_to_layer_greedy() not support IPv6 layer yet\n");
|
||||
goto done;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
//sapp_runtime_log(20, "MESA_net_jump_to_layer_greedy() not support layer type:%d\n", expect_layer_type);
|
||||
goto done;
|
||||
}
|
||||
|
||||
expect_layer = MESA_net_jump_to_layer(new_next_layer_data, new_raw_layer_type, expect_layer_type);
|
||||
}
|
||||
|
||||
done:
|
||||
return success_layer;
|
||||
}
|
||||
|
||||
UINT8 net_layer_to_ipv4_protocol(int addr_type)
|
||||
{
|
||||
UINT8 proto = 0;
|
||||
|
||||
switch(addr_type){
|
||||
case __ADDR_TYPE_IP_PAIR_V4:
|
||||
proto = IPPROTO_IPIP;
|
||||
break;
|
||||
|
||||
case __ADDR_TYPE_IP_PAIR_V6:
|
||||
proto = IPPROTO_IPV6;
|
||||
break;
|
||||
|
||||
case ADDR_TYPE_TCP:
|
||||
proto = IPPROTO_TCP;
|
||||
break;
|
||||
|
||||
case ADDR_TYPE_UDP:
|
||||
proto = IPPROTO_UDP;
|
||||
break;
|
||||
|
||||
case ADDR_TYPE_GRE:
|
||||
proto = IPPROTO_GRE;
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("unknown ip4 protocolr:%d\n", addr_type);
|
||||
proto = 0xFF;
|
||||
break;
|
||||
}
|
||||
|
||||
return proto;
|
||||
}
|
||||
|
||||
UINT8 net_layer_to_ipv6_protocol(int addr_type)
|
||||
{
|
||||
UINT8 proto = 0;
|
||||
|
||||
switch(addr_type){
|
||||
case ADDR_TYPE_TCP:
|
||||
proto = 6; //NEXTHDR_TCP;
|
||||
break;
|
||||
|
||||
case ADDR_TYPE_UDP:
|
||||
proto = 17;///NEXTHDR_UDP;
|
||||
break;
|
||||
|
||||
case __ADDR_TYPE_IP_PAIR_V4:
|
||||
proto = 4;///NEXTHDR_IPIP;
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("unknown ip6 next-hdr:%d\n", addr_type);
|
||||
return 0xFF;
|
||||
break;
|
||||
}
|
||||
|
||||
return proto;
|
||||
}
|
||||
|
||||
/*
|
||||
<09><>MESA<53><41>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA>Ethernet<65><74><EFBFBD><EFBFBD>;
|
||||
|
||||
return value:
|
||||
ethernet type, host order.
|
||||
*/
|
||||
UINT16 net_layer_to_ethernet_protocol(int addr_type)
|
||||
{
|
||||
UINT16 ether_type = 0;
|
||||
|
||||
switch(addr_type){
|
||||
case __ADDR_TYPE_IP_PAIR_V4:
|
||||
ether_type = ETH_P_IP;
|
||||
break;
|
||||
|
||||
case __ADDR_TYPE_IP_PAIR_V6:
|
||||
ether_type = ETH_P_IPV6;
|
||||
break;
|
||||
|
||||
case ADDR_TYPE_VLAN:
|
||||
ether_type = ETHERTYPE_VLAN;
|
||||
break;
|
||||
|
||||
case ADDR_TYPE_TCP:
|
||||
case ADDR_TYPE_UDP:
|
||||
//sapp_runtime_log(RLOG_LV_FATAL, "%s:%d: Ethernet can't carry addr type:%d directly!\n", __FILE__, __LINE__,addr_type);
|
||||
//assert(0);
|
||||
ether_type = -1;
|
||||
break;
|
||||
|
||||
case ADDR_TYPE_PPPOE_SES:
|
||||
ether_type = ETH_P_PPP_SES;
|
||||
break;
|
||||
|
||||
case ADDR_TYPE_MPLS:
|
||||
ether_type = ETH_P_MPLS_UC;
|
||||
break;
|
||||
|
||||
case ADDR_TYPE_ARP:
|
||||
ether_type = ETH_P_ARP;
|
||||
break;
|
||||
|
||||
default:
|
||||
/* to do, unknown type */
|
||||
///sapp_runtime_log(RLOG_LV_FATAL, "%s:%d: Ethernet addr type:%d!\n", __FILE__, __LINE__, addr_type);
|
||||
ether_type = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
return ether_type;
|
||||
}
|
||||
|
||||
|
||||
/* ɾ<><C9BE>ĩβ<C4A9>Ļ<EFBFBD><C4BB>з<EFBFBD>"\r\n" */
|
||||
void del_last_rn(char *data, int max_len)
|
||||
{
|
||||
int i;
|
||||
for(i = 0; i < max_len; i++){
|
||||
if(('\r' == data[i]) || ('\n' == data[i])){
|
||||
data[i] = '\0';
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
<09><><EFBFBD><EFBFBD>ֵ:
|
||||
ethernet<65><74><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD>ethͷ<68><CDB7>.
|
||||
*/
|
||||
int get_pkt_len_from_eth_hdr(const struct mesa_ethernet_hdr *ehdr)
|
||||
{
|
||||
int raw_pkt_len = -1;
|
||||
|
||||
switch(ntohs(ehdr->ether_type)){
|
||||
case ETHERTYPE_IP:
|
||||
{
|
||||
struct mesa_ip4_hdr *ip4hdr = (struct mesa_ip4_hdr *)((char *)ehdr + sizeof(struct mesa_ethernet_hdr));
|
||||
raw_pkt_len = ntohs(ip4hdr->ip_len) + sizeof(struct mesa_ethernet_hdr);
|
||||
}
|
||||
break;
|
||||
|
||||
case ETHERTYPE_IPv6:
|
||||
{
|
||||
struct mesa_ip6_hdr *ip6hdr = (struct mesa_ip6_hdr *)((char *)ehdr + sizeof(struct mesa_ethernet_hdr));
|
||||
raw_pkt_len = ntohs(ip6hdr->ip6_payload_len) + sizeof(struct mesa_ip6_hdr) + sizeof(struct mesa_ethernet_hdr);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return raw_pkt_len;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user