recv 6in4 traffic and write to tun

3 files changed, 140 insertions, 4 deletions

diff --git a/Cargo.lock b/Cargo.lock
index dce4d9f..d97a8fc 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -362,9 +362,9 @@ checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646"
 
 [[package]]
 name = "libc"
-version = "0.2.142"
+version = "0.2.143"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6a987beff54b60ffa6d51982e1aa1146bc42f19bd26be28b0586f252fccf5317"
+checksum = "edc207893e85c5d6be840e969b496b53d94cec8be2d501b214f50daa97fa8024"
 
 [[package]]
 name = "lock_api"
@@ -657,12 +657,15 @@ dependencies = [
 name = "rsdsl_6in4"
 version = "0.1.0"
 dependencies = [
+ "byteorder",
  "ipnet",
+ "libc",
  "notify",
  "rsdsl_ip_config",
  "rsdsl_netlinkd",
  "serde",
  "serde_json",
+ "socket2 0.5.2",
  "tun-tap",
 ]
 
@@ -818,6 +821,16 @@ dependencies = [
 ]
 
 [[package]]
+name = "socket2"
+version = "0.5.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6d283f86695ae989d1e18440a943880967156325ba025f05049946bff47bcc2b"
+dependencies = [
+ "libc",
+ "windows-sys 0.48.0",
+]
+
+[[package]]
 name = "static_assertions"
 version = "1.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
@@ -890,7 +903,7 @@ dependencies = [
  "mio 0.8.6",
  "num_cpus",
  "pin-project-lite",
- "socket2",
+ "socket2 0.4.9",
  "windows-sys 0.48.0",
 ]
 
diff --git a/Cargo.toml b/Cargo.toml
index 65e7dae..ce9021d 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -6,10 +6,13 @@ edition = "2021"
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
+byteorder = "1.4.3"
 ipnet = "2.7.2"
+libc = "0.2.143"
 notify = "5.1.0"
 rsdsl_ip_config = { git = "https://github.com/rsdsl/ip_config.git", version = "0.1.0" }
 rsdsl_netlinkd = { git = "https://github.com/rsdsl/netlinkd.git", version = "0.1.1" }
 serde = { version = "1.0", features = ["derive"] }
 serde_json = "1.0"
+socket2 = { version = "0.5.2", features = ["all"] }
 tun-tap = "0.1.3"
diff --git a/src/main.rs b/src/main.rs
index 9abd562..ba14b17 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,15 +1,19 @@
 use std::fs::File;
+use std::io::{Read, Write};
 use std::net::{Ipv4Addr, Ipv6Addr};
 use std::path::Path;
+use std::sync::Arc;
 use std::thread;
 use std::time::Duration;
 
+use byteorder::{ByteOrder, NetworkEndian as NE};
 use ipnet::Ipv6Net;
 use notify::event::{CreateKind, ModifyKind};
 use notify::{Event, EventKind, RecursiveMode, Watcher};
 use rsdsl_netlinkd::error::Result;
 use rsdsl_netlinkd::{addr, link, route};
 use serde::{Deserialize, Serialize};
+use socket2::{Socket, Type};
 use tun_tap::{Iface, Mode};
 
 #[derive(Clone, Debug, Serialize, Deserialize)]
@@ -39,6 +43,111 @@ impl From<Config> for UsableConfig {
     }
 }
 
+fn tun2he(tun: Arc<Iface>) -> Result<()> {
+    let local = Ipv4Addr::new(10, 128, 10, 237);
+    let remote = Ipv4Addr::new(10, 128, 10, 185);
+
+    let mut sock = Socket::new(
+        libc::AF_PACKET.into(),
+        Type::DGRAM,
+        Some(((libc::ETH_P_IPV6 as u16).to_be() as i32).into()),
+    )?;
+
+    let mut buf = [0; 4 + 1492];
+    loop {
+        let n = match tun.recv(&mut buf[20..]) {
+            Ok(v) => v,
+            Err(e) => {
+                println!("[6in4] tun2he warning: {}", e);
+                continue;
+            }
+        };
+        let buf = &mut buf[..20 + n];
+
+        let ether_type = NE::read_u16(&buf[22..24]);
+        if ether_type != libc::ETH_P_IPV6 as u16 {
+            println!(
+                "[6in4] drop outbound non-ipv6 pkt, ethertype: 0x{:04x}",
+                ether_type
+            );
+            continue;
+        }
+
+        // Construct IP(v4) header.
+        buf[4] = 0b01000101; // Version = 4, IHL = 5
+        buf[5] = 0; // DSCP / ECP = 0
+        NE::write_u16(&mut buf[6..8], (n - 4 + 20) as u16); // Size = dynamic
+        NE::write_u16(&mut buf[8..10], 0); // ID = 0
+        buf[10] = 0b01000000; // Flags = DF, Fragment Offset = 0...
+        buf[11] = 0; // Fragment Offset = ...0
+        buf[12] = 64; // TTL = 64
+        buf[13] = 41; // Protocol = 41 (6in4)
+        NE::write_u16(&mut buf[14..16], 0); // Checksum = 0 (computed later)
+        buf[16..20].copy_from_slice(&local.octets()); // Source IP Address = dynamic
+        buf[20..24].copy_from_slice(&remote.octets()); // Destination IP Address = HE
+
+        let mut sum = 0i32;
+        for i in 0..10 {
+            let j = 4 + (i * 2);
+            sum += NE::read_u16(&buf[j..2 + j]) as i32;
+        }
+
+        while sum >> 16 > 0 {
+            sum = (sum & 0xffff) + (sum >> 16);
+        }
+
+        NE::write_u16(&mut buf[14..16], !(sum as u16));
+
+        match sock.write(&buf[4..]) {
+            Ok(sent) if sent != buf[4..].len() => println!(
+                "[6in4] tun2he warning: partial transmission ({} < {})",
+                sent,
+                buf[4..].len()
+            ),
+            Ok(_) => {}
+            Err(e) => println!("[6in4] tun2he warning: {}", e),
+        }
+    }
+}
+
+fn he2tun(tun: Arc<Iface>) -> Result<()> {
+    let mut sock = Socket::new(
+        libc::AF_PACKET.into(),
+        Type::DGRAM,
+        Some(((libc::ETH_P_IP as u16).to_be() as i32).into()),
+    )?;
+
+    let mut buf = [0; 4 + 1500];
+    NE::write_u16(&mut buf[2..4], libc::ETH_P_IPV6 as u16);
+
+    loop {
+        let n = match sock.read(&mut buf[4..]) {
+            Ok(v) => v,
+            Err(e) => {
+                println!("[6in4] he2tun warning: {}", e);
+                continue;
+            }
+        };
+        let buf = &mut buf[..4 + n];
+
+        // Only process 6in4.
+        if buf[13] != 41 {
+            continue;
+        }
+
+        NE::write_u16(&mut buf[22..24], libc::ETH_P_IPV6 as u16);
+        match tun.send(&buf[20..]) {
+            Ok(sent) if sent != buf.len() - 20 => println!(
+                "[6in4] he2tun warning: partial transmission ({} < {})",
+                sent,
+                buf.len() - 20
+            ),
+            Ok(_) => {}
+            Err(e) => println!("[6in4] he2tun warning: {}", e),
+        }
+    }
+}
+
 fn main() -> Result<()> {
     let mut file = File::open("/data/he6in4.conf")?;
     let config: Config = serde_json::from_reader(&mut file)?;
@@ -50,10 +159,21 @@ fn main() -> Result<()> {
         thread::sleep(Duration::from_secs(8));
     }
 
-    let tun = Iface::new("he6in4", Mode::Tun)?;
+    let tun = Arc::new(Iface::new("he6in4", Mode::Tun)?);
+    let tun2 = tun.clone();
 
     configure_tunnel(&config);
 
+    thread::spawn(move || match tun2he(tun2) {
+        Ok(_) => {}
+        Err(e) => panic!("tun2he error: {}", e),
+    });
+
+    thread::spawn(move || match he2tun(tun) {
+        Ok(_) => {}
+        Err(e) => panic!("he2tun error: {}", e),
+    });
+
     let mut watcher = notify::recommended_watcher(move |res: notify::Result<Event>| match res {
         Ok(event) => match event.kind {
             EventKind::Create(kind) if kind == CreateKind::File => {