rust_rsm/net_ext/
ethernet_pkt.rs

1#![allow(non_snake_case)]
2#![allow(non_upper_case_globals)]
3#![allow(non_camel_case_types)]
4use super::*;
5use super::mac_addr::mac_addr_t;
6use std::net::{IpAddr};
7use std::fmt;
8use crate::common::errcode;
9
10
11///ethernet packet parse result
12#[derive(Copy, Clone, Eq, PartialEq)]
13pub struct ethernet_packet_info_t {
14    l3_offset: u16, //l3 header offset
15    l4_offset: u16, //l4 header offset
16    payload_offset:u16,//user payload offset
17    total_len:u16,
18    pub dst_mac: mac_addr_t,
19    pub src_mac: mac_addr_t,
20    pub vlan_layers: u8, //0 no vlan header,1-vlan，2-QinQ
21    pub vlans: [u16; 2],
22    pub ether_type: u16,
23    pub mpls_label: u32,
24    pub arp_op:u16,
25    pub arp_sha: mac_addr_t,
26    pub arp_tha: mac_addr_t,
27    pub ip_hdr_len: u16,
28    pub ip_payload_len: u16,
29    pub ip_ttl: u8,
30    pub src_ip: IpAddr,
31    pub dst_ip: IpAddr,
32    pub ip_proto: u8,
33    pub tp_src: u16, //transport layer source port
34    pub tp_dst: u16, //transport layer destination port
35}
36
37
38impl ethernet_packet_info_t {
39    pub fn new_zero() -> ethernet_packet_info_t {
40        let info:ethernet_packet_info_t=unsafe { std::mem::zeroed::<ethernet_packet_info_t>() };
41        return info;
42    }
43
44    //解析报文的传输层头部
45    fn parse_transport(pkt: &[u8], ip_proto: u8, info: &mut ethernet_packet_info_t) {
46        if pkt.len()<UDP_HDR_SIZE {
47            return;
48        }
49        unsafe {
50        match ip_proto {
51            IpProtos::Ip_Proto_TCP => {
52                info.tp_src = ((*pkt.get_unchecked(0) as u16) << 8) + *pkt.get_unchecked(1) as u16;
53                info.tp_dst = ((*pkt.get_unchecked(2) as u16) << 8) + *pkt.get_unchecked(3) as u16;
54                let tcp_hdr_len=((pkt[12] & 0xF0)>>4)*4;
55                info.payload_offset=info.l4_offset+tcp_hdr_len as u16;
56            }
57            IpProtos::Ip_Proto_UDP => {
58                info.tp_src = ((*pkt.get_unchecked(0) as u16) << 8) + *pkt.get_unchecked(1) as u16;
59                info.tp_dst = ((*pkt.get_unchecked(2) as u16) << 8) + *pkt.get_unchecked(3) as u16;
60                info.payload_offset=info.l4_offset+8;
61            },
62            IpProtos::Ip_Proto_SCTP => {
63                info.tp_src = ((*pkt.get_unchecked(0) as u16) << 8) + *pkt.get_unchecked(1) as u16;
64                info.tp_dst = ((*pkt.get_unchecked(2) as u16) << 8) + *pkt.get_unchecked(3) as u16;
65                info.payload_offset=info.l4_offset+12;
66            }
67            _ => (),
68        }
69    }
70    }
71    fn parse_ipv4(ipv4: &[u8], info: &mut ethernet_packet_info_t) {
72        if ipv4.len()<IPV4_HDR_SIZE {
73            return;
74        }
75        let mut cur_idx = 0;
76        unsafe {
77        info.ip_hdr_len = ((*ipv4.get_unchecked(0) & 0x0F) * 4) as u16;
78        
79        info.ip_payload_len = ((*ipv4.get_unchecked(2) as u16) << 8) + *ipv4.get_unchecked(3) as u16;
80        if info.ip_hdr_len<IPV4_HDR_SIZE as u16 || info.ip_payload_len>ipv4.len() as u16 {
81            return
82        }
83        info.ip_ttl = *ipv4.get_unchecked(8);
84        info.ip_proto = *ipv4.get_unchecked(9);
85        cur_idx += 12;
86
87        info.src_ip = IpAddr::from([
88            *ipv4.get_unchecked(cur_idx),
89            *ipv4.get_unchecked(cur_idx + 1),
90            *ipv4.get_unchecked(cur_idx + 2),
91            *ipv4.get_unchecked(cur_idx + 3),
92        ]);
93        info.dst_ip = IpAddr::from([
94            *ipv4.get_unchecked(cur_idx + 4),
95            *ipv4.get_unchecked(cur_idx + 5),
96            *ipv4.get_unchecked(cur_idx + 6),
97            *ipv4.get_unchecked(cur_idx + 7),
98        ]);
99        info.l4_offset = info.l3_offset+info.ip_hdr_len;
100        info.payload_offset=info.l4_offset;
101        if ipv4.len()>=info.ip_hdr_len as usize + UDP_HDR_SIZE {
102            Self::parse_transport(&ipv4[info.ip_hdr_len as usize..], info.ip_proto, info);
103        }
104
105    }
106        
107    }
108
109    fn parse_ipv6(ipv6: &[u8], info: &mut ethernet_packet_info_t) {
110        if ipv6.len()<IPV6_HDR_SIZE {
111            return;
112        }
113        info.ip_hdr_len = IPV6_HDR_SIZE as u16;
114        info.ip_payload_len = unsafe {  ((*ipv6.get_unchecked(4) as u16) << 8) + *ipv6.get_unchecked(5) as u16};
115        if info.ip_payload_len>ipv6.len() as u16 {
116            return
117        }
118        info.ip_proto = ipv6[6];
119        info.ip_ttl = ipv6[7];
120        let mut ipaddr: [u8; 16] = [0; 16];
121        ipaddr.copy_from_slice(&ipv6[8..24]);
122
123        info.src_ip = IpAddr::from(ipaddr);
124        ipaddr.copy_from_slice(&ipv6[24..40]);
125        info.dst_ip = IpAddr::from(ipaddr);
126        info.l4_offset = info.l3_offset+info.ip_hdr_len;
127        info.payload_offset=info.l4_offset;
128        Self::parse_transport(&ipv6[info.ip_hdr_len as usize..], info.ip_proto, info);
129    }
130
131    fn parse_arp(arp: &[u8], info: &mut ethernet_packet_info_t) {
132        if arp.len()<ARP_PACKET_SIZE {
133            return;
134        }
135        info.arp_sha = mac_addr_t::new(arp[8], arp[9], arp[10], arp[11], arp[12], arp[13]);
136        info.arp_tha = mac_addr_t::new(arp[18], arp[19], arp[20], arp[21], arp[22], arp[23]);
137        info.src_ip = IpAddr::from([arp[14],arp[15],arp[16],arp[17]]);
138        info.dst_ip = IpAddr::from([arp[24],arp[25],arp[26],arp[27]]);
139        info.arp_op = ((arp[6] as u16) <<8 )+arp[7] as u16;
140    }
141
142    //从原始报文中获取信息
143    pub fn from_ethernet_packet(packet: &[u8]) -> Result<Self,errcode::RESULT> {
144        let pkt_len = packet.len();
145        if packet.len()<ETHERNET_HDR_SIZE {
146            return Err(errcode::ERROR_INVALID_MSG);
147        }
148        let mut pkt_info = Self::new_zero();
149        pkt_info.total_len = pkt_len as u16;
150        pkt_info.dst_mac = mac_addr_t::from_slice(packet);
151        pkt_info.src_mac = mac_addr_t::from_slice(&packet[MAC_ADDR_SIZE..]);
152        
153        let ether_type: u16 = unsafe { ((*packet.get_unchecked(12) as u16) << 8) + *packet.get_unchecked(13) as u16 };
154        let mut cur_ptr: usize = ETHERNET_HDR_SIZE;
155        unsafe {
156        pkt_info.ether_type = match ether_type {
157            EthernetTypes::Ethernet_Vlan => {
158                if pkt_len<ETHERNET_HDR_SIZE+4 {
159                    return Err(errcode::ERROR_INVALID_MSG);
160                }
161                pkt_info.vlan_layers = 1;
162                pkt_info.vlans[0] = ((*packet.get_unchecked(cur_ptr) as u16) << 8) + *packet.get_unchecked(cur_ptr + 1) as u16;
163                cur_ptr += 4;
164                ((*packet.get_unchecked(cur_ptr - 2) as u16) << 8) + *packet.get_unchecked(cur_ptr - 1) as u16
165            }
166            EthernetTypes::Ethernet_SVlan => {
167                if pkt_len<ETHERNET_HDR_SIZE+4*2 {
168                    return Err(errcode::ERROR_INVALID_MSG);
169                }
170                pkt_info.vlan_layers = 2;
171                pkt_info.vlans[0] = ((*packet.get_unchecked(cur_ptr) as u16) << 8) + *packet.get_unchecked(cur_ptr + 1) as u16;
172                pkt_info.vlans[1] = ((*packet.get_unchecked(cur_ptr + 4) as u16) << 8) + *packet.get_unchecked(cur_ptr + 5) as u16;
173                cur_ptr += 8;
174                ((*packet.get_unchecked(cur_ptr - 2) as u16) << 8) + *packet.get_unchecked(cur_ptr - 1) as u16
175            }
176            _ => ether_type,
177        };
178        pkt_info.l3_offset = cur_ptr as u16;
179        pkt_info.payload_offset=pkt_info.l3_offset;
180        if cur_ptr>=pkt_len {
181            return Err(errcode::ERROR_INVALID_MSG);
182        }
183    }
184        match pkt_info.ether_type {
185            EthernetTypes::Ethernet_Ipv4 => Self::parse_ipv4(&packet[cur_ptr..], &mut pkt_info),
186            EthernetTypes::Ethernet_Ipv6 => Self::parse_ipv6(&packet[cur_ptr..], &mut pkt_info),
187            EthernetTypes::Ethernet_ARP => Self::parse_arp(&packet[cur_ptr..], &mut pkt_info),
188            _ => (),
189        }
190
191        return Ok(pkt_info);
192    }
193
194    pub fn get_payload_offset(&self)->Result<u16,errcode::RESULT> {
195        if self.payload_offset>0 {
196            return Ok(self.payload_offset)
197        }
198        Err(errcode::ERROR_INVALID_MSG)
199    }
200    pub fn get_l3_hdr_offset(&self)->Result<u16,errcode::RESULT> {
201        return Ok(self.l3_offset)
202    }
203
204    pub fn get_l4_hdr_offset(&self)->Result<u16,errcode::RESULT> {
205        if self.ether_type==EthernetTypes::Ethernet_Ipv4 || self.ether_type==EthernetTypes::Ethernet_Ipv6 {
206            return Ok(self.l4_offset)
207        }
208        Err(errcode::ERROR_INVALID_MSG)
209    }
210
211    pub fn to_string(&self)->String {
212        format!("total_len={},ether_type={:#x},src_mac={},dst_mac={},vlan={},
213        ip_hdr_len={},src_ip={},dst_ip={},ip_proto={},ip_payload_len={},tp_src={},tp_dst={}",
214        self.total_len,self.ether_type,self.src_mac,self.dst_mac,self.vlans[0],
215        self.ip_hdr_len,self.src_ip, self.dst_ip,self.ip_proto,self.ip_payload_len,
216        self.tp_src,self.tp_dst)
217    }
218
219}
220
221impl fmt::Display for ethernet_packet_info_t {
222    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
223        write!(f,"total_len={},ether_type={:#x},src_mac={},dst_mac={},vlan={},
224        ip_hdr_len={},src_ip={},dst_ip={},ip_proto={},ip_payload_len={},tp_src={},tp_dst={}",
225        self.total_len,self.ether_type,self.src_mac,self.dst_mac,self.vlans[0],
226        self.ip_hdr_len,self.src_ip, self.dst_ip,self.ip_proto,self.ip_payload_len,
227        self.tp_src,self.tp_dst)
228    }
229}
rust_rsm/net_ext/ethernet_pkt.rs

rust_rsm/net_ext/
ethernet_pkt.rs
