handle incoming handshake init packets

4 files changed, 86 insertions, 28 deletions

diff --git a/src/interface/peer_server.rs b/src/interface/peer_server.rs
index 0c6c130..b88235e 100644
--- a/src/interface/peer_server.rs
+++ b/src/interface/peer_server.rs
@@ -1,14 +1,16 @@
 use super::{SharedState, SharedPeer, debug_packet};
 use consts::{REKEY_AFTER_TIME, KEEPALIVE_TIMEOUT};
+use protocol::Session;
 
 use std::io;
 use std::net::SocketAddr;
 use std::time::Duration;
 
+use base64;
 use byteorder::{ByteOrder, BigEndian, LittleEndian};
 use futures::{Async, Future, Stream, Sink, Poll, future, unsync, sync, stream};
 use pnet::packet::ipv4::Ipv4Packet;
-use snow::NoiseBuilder;
+use snow::{self, NoiseBuilder};
 use tokio_core::net::{UdpSocket, UdpCodec, UdpFramed};
 use tokio_core::reactor::Handle;
 use tokio_io::codec::Framed;
@@ -82,12 +84,57 @@ impl PeerServer {
         self.udp_tx.clone()
     }
 
-    fn handle_incoming_packet(&mut self, _addr: SocketAddr, packet: Vec<u8>) {
+    fn handle_incoming_packet(&mut self, addr: SocketAddr, packet: Vec<u8>) {
         debug!("got a UDP packet of length {}, packet type {}", packet.len(), packet[0]);
-        let state = self.shared_state.borrow_mut();
+        let mut state = self.shared_state.borrow_mut();
         match packet[0] {
             1 => {
-                warn!("got handshake initialization, can't handle yet.");
+                let their_index = LittleEndian::read_u32(&packet[4..]);
+
+                let mut noise = NoiseBuilder::new("Noise_IKpsk2_25519_ChaChaPoly_BLAKE2s".parse().unwrap())
+                    .local_private_key(&state.interface_info.private_key.expect("no private key!"))
+                    .prologue("WireGuard v1 zx2c4 Jason@zx2c4.com".as_bytes())
+                    .build_responder().unwrap();
+
+                let mut timestamp = [0u8; 116];
+                if let Err(_) = noise.read_message(&packet[8..116], &mut timestamp) {
+                    warn!("failed to parse incoming handshake");
+                    return;
+                }
+
+                let peer_ref = {
+                    let their_pubkey = match noise {
+                        snow::Session::Handshake(ref mut handshake_state) => {
+                            handshake_state.get_remote_static().expect("should have remote static key")
+                        },
+                        _ => unreachable!()
+                    };
+
+                    info!("their_pubkey: {}", base64::encode(&their_pubkey[..]));
+                    let peer_ref = state.pubkey_map.get(&their_pubkey[..]);
+                    if peer_ref.is_none() {
+                        warn!("unknown public key received");
+                        return;
+                    }
+                    peer_ref.unwrap().clone()
+                };
+
+                let mut peer = peer_ref.borrow_mut();
+
+                match noise {
+                    snow::Session::Handshake(ref mut handshake_state) => {
+                        handshake_state.set_psk(2, &peer.info.psk.expect("no psk!"));
+                    },
+                    _ => unreachable!()
+                }
+
+                peer.set_next_session(Session::with_their_index(noise, their_index));
+                let _ = state.index_map.insert(peer.our_next_index().unwrap(), peer_ref.clone());
+
+                let response_packet = peer.get_response_packet();
+
+                self.handle.spawn(self.udp_tx.clone().send((addr.clone(), response_packet)).then(|_| Ok(())));
+                info!("sent handshake response");
             },
             2 => {
                 let their_index = LittleEndian::read_u32(&packet[4..]);
@@ -212,15 +259,18 @@ impl PeerServer {
         if let Some((_, _, peer)) = state.ip4_map.longest_match(destination) {
             let mut peer = peer.borrow_mut();
             out_packet[0] = 4;
-            let their_index = peer.their_current_index().expect("no current index for them");
-            let endpoint = peer.info.endpoint.unwrap();
-            peer.tx_bytes += packet.len() as u64;
-            let noise = peer.current_noise().expect("current noise session");
-            LittleEndian::write_u32(&mut out_packet[4..], their_index);
-            LittleEndian::write_u64(&mut out_packet[8..], noise.sending_nonce().unwrap());
-            let len = noise.write_message(&packet, &mut out_packet[16..]).expect("failed to encrypt outgoing UDP packet");
-            out_packet.truncate(16+len);
-            self.handle.spawn(self.udp_tx.clone().send((endpoint, out_packet)).then(|_| Ok(())));
+            if let Some(their_index) = peer.their_current_index() {
+                let endpoint = peer.info.endpoint.unwrap();
+                peer.tx_bytes += packet.len() as u64;
+                let noise = peer.current_noise().expect("current noise session");
+                LittleEndian::write_u32(&mut out_packet[4..], their_index);
+                LittleEndian::write_u64(&mut out_packet[8..], noise.sending_nonce().unwrap());
+                let len = noise.write_message(&packet, &mut out_packet[16..]).expect("failed to encrypt outgoing UDP packet");
+                out_packet.truncate(16 + len);
+                self.handle.spawn(self.udp_tx.clone().send((endpoint, out_packet)).then(|_| Ok(())));
+            } else {
+                info!("got outgoing packet with no current session");
+            }
         } else {
             warn!("got packet with no available outgoing route");
         }
diff --git a/src/protocol/mod.rs b/src/protocol/mod.rs
index bae5340..55e5ed7 100644
--- a/src/protocol/mod.rs
+++ b/src/protocol/mod.rs
@@ -1,3 +1,4 @@
 mod peer;
 
 pub use self::peer::Peer;
+pub use self::peer::Session;
diff --git a/src/protocol/peer.rs b/src/protocol/peer.rs
index 90def84..ed1e6de 100644
--- a/src/protocol/peer.rs
+++ b/src/protocol/peer.rs
@@ -65,6 +65,7 @@ impl From<snow::Session> for Session {
     }
 }
 
+#[derive(Default)]
 pub struct Sessions {
     pub past: Option<Session>,
     pub current: Option<Session>,
@@ -89,17 +90,9 @@ fn memcpy(out: &mut [u8], data: &[u8]) {
 
 impl Peer {
     pub fn new(info: PeerInfo) -> Peer {
-        Peer {
-            info,
-            sessions: Sessions {
-                past: None,
-                current: None,
-                next: None
-            },
-            tx_bytes: 0,
-            rx_bytes: 0,
-            last_handshake: None,
-        }
+        let mut peer = Peer::default();
+        peer.info = info;
+        peer
     }
 
     pub fn set_next_session(&mut self, session: Session) {
@@ -172,9 +165,6 @@ impl Peer {
         BigEndian::write_i32(&mut tai64n[8..], now.nsec);
         let mut initiation_packet = vec![0; 148];
         initiation_packet[0] = 1; /* Type: Initiation */
-        initiation_packet[1] = 0; /* Reserved */
-        initiation_packet[2] = 0; /* Reserved */
-        initiation_packet[3] = 0; /* Reserved */
         LittleEndian::write_u32(&mut initiation_packet[4..], self.our_next_index().unwrap());
         self.sessions.next.as_mut().unwrap().noise.write_message(&tai64n, &mut initiation_packet[8..]).unwrap();
         let mut mac_key_input = [0; 40];
@@ -187,6 +177,23 @@ impl Peer {
         initiation_packet
     }
 
+    pub fn get_response_packet(&mut self) -> Vec<u8> {
+        let mut packet = vec![0; 76];
+        packet[0] = 2; /* Type: Response */
+        let session = self.sessions.next.as_mut().unwrap();
+        LittleEndian::write_u32(&mut packet[4..], session.our_index);
+        LittleEndian::write_u32(&mut packet[8..], session.their_index);
+        session.noise.write_message(&[], &mut packet[12..]).unwrap();
+        let mut mac_key_input = [0; 40];
+        memcpy(&mut mac_key_input, b"mac1----");
+        memcpy(&mut mac_key_input[8..], &self.info.pub_key);
+        let mac_key = blake2s(32, &[], &mac_key_input);
+        let mac = blake2s(16, mac_key.as_bytes(), &packet[0..44]);
+        memcpy(&mut packet[44..], mac.as_bytes());
+
+        packet
+    }
+
     pub fn to_config_string(&self) -> String {
         let mut s = format!("public_key={}\n", hex::encode(&self.info.pub_key));
         if let Some(ref psk) = self.info.psk {
diff --git a/src/types.rs b/src/types.rs
index 29b02bb..10f6c17 100644
--- a/src/types.rs
+++ b/src/types.rs
@@ -4,7 +4,7 @@ use byteorder::{BigEndian, LittleEndian, WriteBytesExt, ReadBytesExt};
 use std::fmt::{self, Display, Formatter};
 use std::net::{Ipv4Addr, IpAddr, SocketAddr};
 
-#[derive(Clone, Debug)]
+#[derive(Clone, Debug, Default)]
 pub struct PeerInfo {
     pub pub_key: [u8; 32],
     pub psk: Option<[u8; 32]>,