Complete working raw socket forwarding

rust/Cargo.lock

@@ -62,6 +62,7 @@ dependencies = [
  "buddy_system_allocator 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)",
  "isomorphic_drivers 0.1.0 (git+https://github.com/rcore-os/isomorphic_drivers)",
  "smoltcp 0.5.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "treebitmap 0.3.1 (git+https://github.com/jiegec/treebitmap)",
 ]
 
 [[package]]
@@ -80,6 +81,11 @@ name = "spin"
 version = "0.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 
+[[package]]
+name = "treebitmap"
+version = "0.3.1"
+source = "git+https://github.com/jiegec/treebitmap#9c0ddcfaed0c960fb86a66c2b5abb5af70d6f1d1"
+
 [[package]]
 name = "volatile"
 version = "0.2.6"
@@ -96,4 +102,5 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 "checksum managed 0.7.1 (registry+https://github.com/rust-lang/crates.io-index)" = "fdcec5e97041c7f0f1c5b7d93f12e57293c831c646f4cc7a5db59460c7ea8de6"
 "checksum smoltcp 0.5.0 (registry+https://github.com/rust-lang/crates.io-index)" = "fef582369edb298c6c41319a544ca9c4e83622f226055ccfcb35974fbb55ed34"
 "checksum spin 0.5.0 (registry+https://github.com/rust-lang/crates.io-index)" = "44363f6f51401c34e7be73db0db371c04705d35efbe9f7d6082e03a921a32c55"
+"checksum treebitmap 0.3.1 (git+https://github.com/jiegec/treebitmap)" = "<none>"
 "checksum volatile 0.2.6 (registry+https://github.com/rust-lang/crates.io-index)" = "6af0edf5b4faacc31fc51159244d78d65ec580f021afcef7bd53c04aeabc7f29"

rust/Cargo.toml

@@ -8,3 +8,4 @@ edition = "2018"
 buddy_system_allocator = "0.1"
 isomorphic_drivers = { git = "https://github.com/rcore-os/isomorphic_drivers", default-features = false, features = ["log"]}
 smoltcp = { version = "0.5.0", default-features = false, features = ["alloc", "log", "proto-ipv4", "socket-tcp", "socket-raw"] }
+treebitmap = { git = "https://github.com/jiegec/treebitmap" , features = ["alloc"] }

rust/src/bin/raw_socket.rs

@@ -1,12 +1,18 @@
+#![feature(alloc)]
 #![no_std]
 #![no_main]
 
 #[macro_use]
 extern crate rcore_user;
+extern crate alloc;
 
+extern crate treebitmap;
+
+use alloc::format;
 use core::default::Default;
 use core::mem::size_of;
 use rcore_user::syscall::{sys_ioctl, sys_read, sys_sendto, sys_setsockopt, sys_socket};
+use treebitmap::*;
 
 #[repr(C)]
 #[derive(Default)]
@@ -46,16 +52,40 @@ struct ArpReq {
     arp_dev: [u8; 16],
 }
 
+pub fn get_routing_table() -> IpLookupTable<Ipv4Addr, (u32, [u8; 6], &'static str)> {
+    let mut table = IpLookupTable::new();
+    // iface 0 10.0.0.0/24 00:16:31:ff:a4:9f enp0s4f0
+    table.insert(
+        Ipv4Addr::new(10, 0, 0, 0),
+        24,
+        (0, [0x00, 0x16, 0x31, 0xff, 0xa4, 0x9f], "enp0s4f0\0"),
+    );
+    // iface 1 10.0.1.0/24 54:51:9f:71:c0:01 enp0s5f0
+    table.insert(
+        Ipv4Addr::new(10, 0, 1, 0),
+        24,
+        (1, [0x54, 0x51, 0x9f, 0x71, 0xc0, 0x01], "enp0s5f0\0"),
+    );
+
+    table
+}
+
 // IMPORTANT: Must define main() like this
 #[no_mangle]
 pub unsafe fn main() {
     println!("Raw socket test");
+
     // raw socket, icmp
     let capture_fd = sys_socket(2, 3, 1);
     // inet socket, udp
     let arp_fd = sys_socket(2, 2, 0);
     // packet socket, raw
-    let packet = sys_socket(17, 3, 0);
+    let packet_fd = sys_socket(17, 3, 0);
+    // netlink socket, raw
+    let netlink_fd = sys_socket(16, 3, 0);
+
+    let table = get_routing_table();
+
     let mut buffer = [0u8; 2048];
     // set header included
     let included = 1u32;
@@ -78,7 +108,6 @@ pub unsafe fn main() {
     let mut arp: ArpReq = Default::default();
     // inet
     arp.arp_pa.sin_family = 2;
-    arp.arp_dev[0..9].copy_from_slice(&[b'e', b'n', b'p', b'0', b's', b'4', b'f', b'0', 0]);
     loop {
         let len = sys_read(
             capture_fd as usize,
@@ -86,33 +115,79 @@ pub unsafe fn main() {
             buffer.len(),
         ) as usize;
         println!("Got packet of len {}", len);
-        if ETHER_HEADER_LEN + len > 20 && buffer[ETHER_HEADER_LEN + 0] == 0x45 {
+        // check ethertype and ip version ihl
+        if ETHER_HEADER_LEN + len > 20
+            && buffer[12] == 0x08
+            && buffer[13] == 0x00
+            && buffer[ETHER_HEADER_LEN + 0] == 0x45
+        {
             // ipv4
             let ttl = buffer[ETHER_HEADER_LEN + 8];
             if ttl > 1 {
-                println!("ttl {}", ttl);
+                if buffer[ETHER_HEADER_LEN + 19] == 2 {
+                    // to myself
+                    println!("packet to myself");
+                    continue;
+                }
+                let lookup_ip = Ipv4Addr::new(
+                    buffer[ETHER_HEADER_LEN + 16],
+                    buffer[ETHER_HEADER_LEN + 17],
+                    buffer[ETHER_HEADER_LEN + 18],
+                    buffer[ETHER_HEADER_LEN + 19],
+                );
+                let route_match = table.longest_match(lookup_ip);
+                match route_match {
+                    Some((_, _, (out_if, mac, name))) => {
                         let dst_ip = (buffer[ETHER_HEADER_LEN + 19] as u32) << 24
                             | (buffer[ETHER_HEADER_LEN + 18] as u32) << 16
                             | (buffer[ETHER_HEADER_LEN + 17] as u32) << 8
                             | (buffer[ETHER_HEADER_LEN + 16] as u32);
-                println!("dst_ip {:#X}", dst_ip);
                         arp.arp_pa.sin_addr = dst_ip;
+                        arp.arp_dev[0..9].copy_from_slice(name.as_bytes());
                         // SIOCGARP
-                sys_ioctl(arp_fd as usize, 0x8954, &arp as *const ArpReq as usize);
+                        if sys_ioctl(arp_fd as usize, 0x8954, &arp as *const ArpReq as usize) < 0 {
-                println!("dst_mac {:X?}", &arp.arp_ha.sha_data[0..6]);
+                            println!("dst ip not in arp table, skipping");
+                            continue;
+                        }
 
-                // fill src and dst mac
+                        let message = format!("from {}.{}.{}.{} {:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X} to {}.{}.{}.{} {:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X} dev {}:{} with ttl {}", buffer[ETHER_HEADER_LEN + 12],
+                        buffer[ETHER_HEADER_LEN+ 13],
+                        buffer[ETHER_HEADER_LEN+ 14],
+                        buffer[ETHER_HEADER_LEN+ 15],
+                        mac[0],
+                        mac[1],
+                        mac[2],
+                        mac[3],
+                        mac[4],
+                        mac[5],
+                        buffer[ETHER_HEADER_LEN+ 16],
+                        buffer[ETHER_HEADER_LEN+ 17],
+                        buffer[ETHER_HEADER_LEN+ 18],
+                        buffer[ETHER_HEADER_LEN+ 19],
+                        arp.arp_ha.sha_data[0],
+                        arp.arp_ha.sha_data[1],
+                        arp.arp_ha.sha_data[2],
+                        arp.arp_ha.sha_data[3],
+                        arp.arp_ha.sha_data[4],
+                        arp.arp_ha.sha_data[5],
+                        out_if,
+                        name,
+                        ttl,
+                        );
+                        println!("{}", message);
+
+                        // fill dst and src mac
                         // todo: get mac from if instead of hard coding
-                buffer[0..6].copy_from_slice(&[0, 0x16, 0x31, 0xFF, 0xA4, 0x9F]);
+                        buffer[0..6].copy_from_slice(&arp.arp_ha.sha_data[0..6]);
-                buffer[6..12].copy_from_slice(&arp.arp_ha.sha_data[0..6]);
+                        buffer[6..12].copy_from_slice(mac);
                         buffer[ETHER_HEADER_LEN + 8] = ttl - 1;
                         let checksum = (buffer[ETHER_HEADER_LEN + 10] as u16) << 8
                             | (buffer[ETHER_HEADER_LEN + 11] as u16);
                         buffer[ETHER_HEADER_LEN + 10] = ((checksum + 0x0100) >> 8) as u8;
                         buffer[ETHER_HEADER_LEN + 11] = (checksum + 0x0100) as u8;
-                addr.sll_ifindex = 0;
+                        addr.sll_ifindex = *out_if;
                         sys_sendto(
-                    packet as usize,
+                            packet_fd as usize,
                             buffer.as_ptr(),
                             len as usize + ETHER_HEADER_LEN,
                             0,
@@ -120,6 +195,9 @@ pub unsafe fn main() {
                             size_of::<SockAddrLl>(),
                         );
                     }
+                    None => continue,
+                }
+            }
         }
     }
 }

rust/src/io.rs

@@ -194,7 +194,7 @@ impl fmt::Write for StdOut {
 pub const O_RDONLY: usize = 0; // open for reading only
 pub const O_WRONLY: usize = 1; // open for writing only
 pub const O_RDWR: usize = 2; // open for reading and writing
-// then or in any of these:
+                             // then or in any of these:
 pub const O_CREAT: usize = 0x00000004; // create file if it does not exist
 pub const O_EXCL: usize = 0x00000008; // error if O_CREAT and the file exists
 pub const O_TRUNC: usize = 0x00000010; // truncate file upon open

rust/src/lang_items.rs

@@ -1,9 +1,9 @@
 use crate::syscall::sys_exit;
 use crate::ALLOCATOR;
 
+use super::syscall::*;
 use core::alloc::Layout;
 use core::panic::PanicInfo;
-use super::syscall::*;
 
 #[linkage = "weak"]
 #[no_mangle]
@@ -14,7 +14,9 @@ fn main() {
 fn init_heap() {
     const HEAP_SIZE: usize = 0x1000;
     static mut HEAP: [u8; HEAP_SIZE] = [0; HEAP_SIZE];
-    unsafe { ALLOCATOR.lock().init(HEAP.as_ptr() as usize, HEAP_SIZE); }
+    unsafe {
+        ALLOCATOR.lock().init(HEAP.as_ptr() as usize, HEAP_SIZE);
+    }
 }
 
 /// MIPS use __start for entry point instead of _start

rust/src/lib.rs

@@ -10,8 +10,8 @@ extern crate alloc;
 
 #[macro_use]
 pub mod io;
-pub mod syscall;
 pub mod lang_items;
+pub mod syscall;
 
 use buddy_system_allocator::LockedHeap;
 

rust/src/syscall.rs

@@ -2,7 +2,15 @@ use crate::ALLOCATOR;
 use alloc::string::String;
 
 #[inline(always)]
-fn sys_call(syscall_id: SyscallId, arg0: usize, arg1: usize, arg2: usize, arg3: usize, arg4: usize, arg5: usize) -> i32 {
+fn sys_call(
+    syscall_id: SyscallId,
+    arg0: usize,
+    arg1: usize,
+    arg2: usize,
+    arg3: usize,
+    arg4: usize,
+    arg5: usize,
+) -> i32 {
     let id = syscall_id as usize;
     let ret: i32;
 
@@ -44,7 +52,9 @@ fn sys_call(syscall_id: SyscallId, arg0: usize, arg1: usize, arg2: usize, arg3:
 pub fn enlarge_heap() {
     const HEAP_SIZE: usize = 16 * 1024 * 1024;
     let addr = sys_mmap(0, HEAP_SIZE, 0x3, 0x22, 0, 0) as usize;
-    unsafe { ALLOCATOR.lock().init(addr, HEAP_SIZE); }
+    unsafe {
+        ALLOCATOR.lock().init(addr, HEAP_SIZE);
+    }
 }
 
 pub fn sys_exit(code: usize) -> ! {
@@ -52,9 +62,16 @@ pub fn sys_exit(code: usize) -> ! {
     unreachable!()
 }
 
-
 pub fn sys_exec(name: *const u8, argv: *const *const u8, envp: *const *const u8) -> i32 {
-    sys_call(SyscallId::Exec, name as usize, argv as usize, envp as usize, 0, 0, 0)
+    sys_call(
+        SyscallId::Exec,
+        name as usize,
+        argv as usize,
+        envp as usize,
+        0,
+        0,
+        0,
+    )
 }
 
 pub fn sys_write(fd: usize, base: *const u8, len: usize) -> i32 {
@@ -71,7 +88,15 @@ pub fn sys_open(path: &str, flags: usize) -> i32 {
     let end = unsafe { &mut *(path.as_ptr().offset(path.len() as isize) as *mut u8) };
     let backup = replace(end, 0);
     const AT_FDCWD: isize = -100;
-    let ret = sys_call(SyscallId::Openat, AT_FDCWD as usize, path.as_ptr() as usize, flags, 0, 0, 0);
+    let ret = sys_call(
+        SyscallId::Openat,
+        AT_FDCWD as usize,
+        path.as_ptr() as usize,
+        flags,
+        0,
+        0,
+        0,
+    );
     *end = backup;
     ret
 }
@@ -95,7 +120,15 @@ pub fn sys_vfork() -> i32 {
     const CLONE_VFORK: usize = 0x00004000;
     const CLONE_VM: usize = 0x00000100;
     const SIGCHILD: usize = 17;
-    sys_call(SyscallId::Clone, CLONE_VFORK | CLONE_VM | SIGCHILD, sp, 0, 0, 0, 0)
+    sys_call(
+        SyscallId::Clone,
+        CLONE_VFORK | CLONE_VM | SIGCHILD,
+        sp,
+        0,
+        0,
+        0,
+        0,
+    )
 }
 
 /// Wait the process exit.
@@ -128,13 +161,29 @@ pub fn sys_getcwd() -> String {
 /// Change the current working directory
 pub fn sys_chdir(path: &str) {
     let path = String::from(path) + "\0";
-    sys_call(SyscallId::Chdir, path.as_bytes().as_ptr() as usize, 0, 0, 0, 0, 0);
+    sys_call(
+        SyscallId::Chdir,
+        path.as_bytes().as_ptr() as usize,
+        0,
+        0,
+        0,
+        0,
+        0,
+    );
 }
 
 /// Check file accessibility
 pub fn sys_access(path: &str) -> i32 {
     let path = String::from(path) + "\0";
-    sys_call(SyscallId::FAccessAt, -100isize as usize, path.as_bytes().as_ptr() as usize, 0, 0, 0, 0)
+    sys_call(
+        SyscallId::FAccessAt,
+        -100isize as usize,
+        path.as_bytes().as_ptr() as usize,
+        0,
+        0,
+        0,
+        0,
+    )
 }
 
 #[repr(C)]
@@ -147,9 +196,17 @@ pub struct TimeSpec {
 pub fn sys_sleep(time: usize) -> i32 {
     let ts = TimeSpec {
         sec: time as u64,
-        nsec: 0
+        nsec: 0,
     };
-    sys_call(SyscallId::Sleep, &ts as *const TimeSpec as usize, 0, 0, 0, 0, 0)
+    sys_call(
+        SyscallId::Sleep,
+        &ts as *const TimeSpec as usize,
+        0,
+        0,
+        0,
+        0,
+        0,
+    )
 }
 
 pub fn sys_get_time() -> i32 {
@@ -170,10 +227,25 @@ pub fn sys_map_pci_device(vendor: usize, product: usize) -> i32 {
 
 pub fn sys_get_paddr(vaddr: &[u64], paddr: &mut [u64]) -> i32 {
     assert_eq!(vaddr.len(), paddr.len());
-    sys_call(SyscallId::GetPaddr, vaddr.as_ptr() as usize, paddr.as_ptr() as usize, vaddr.len(), 0, 0, 0)
+    sys_call(
+        SyscallId::GetPaddr,
+        vaddr.as_ptr() as usize,
+        paddr.as_ptr() as usize,
+        vaddr.len(),
+        0,
+        0,
+        0,
+    )
 }
 
-pub fn sys_mmap(addr: usize, len: usize, prot: usize, flags: usize, fd: usize, offset: usize) -> i32 {
+pub fn sys_mmap(
+    addr: usize,
+    len: usize,
+    prot: usize,
+    flags: usize,
+    fd: usize,
+    offset: usize,
+) -> i32 {
     sys_call(SyscallId::Mmap, addr, len, prot, flags, fd, offset)
 }
 
@@ -185,8 +257,23 @@ pub fn sys_setsockopt(fd: usize, level: usize, opt: usize, optval: usize, optlen
     sys_call(SyscallId::SetSockOpt, fd, level, opt, optval, optlen, 0)
 }
 
-pub fn sys_sendto(fd: usize, base: *const u8, len: usize, flags: usize, addr: usize, addr_len: usize) -> i32 {
+pub fn sys_sendto(
-    sys_call(SyscallId::SendTo, fd, base as usize, len, flags, addr, addr_len)
+    fd: usize,
+    base: *const u8,
+    len: usize,
+    flags: usize,
+    addr: usize,
+    addr_len: usize,
+) -> i32 {
+    sys_call(
+        SyscallId::SendTo,
+        fd,
+        base as usize,
+        len,
+        flags,
+        addr,
+        addr_len,
+    )
 }
 
 pub fn sys_ioctl(fd: usize, request: usize, arg1: usize) -> i32 {