diff --git a/crates/core/src/bc_protocol/connection/udpconn/mod.rs b/crates/core/src/bc_protocol/connection/udpconn/mod.rs index f781cd7..b3f33b6 100644 --- a/crates/core/src/bc_protocol/connection/udpconn/mod.rs +++ b/crates/core/src/bc_protocol/connection/udpconn/mod.rs @@ -140,13 +140,11 @@ impl UdpSource { // Poll Write std::thread::spawn(move || { - let mut outgoing_history = Default::default(); while !thread_aborter.is_aborted() { if let Err(err) = thread_transmit.poll_write( &thread_socket, &(*thread_discovery_result), &from_outgoing, - &mut outgoing_history, ) { if !thread_aborter.is_aborted() { match err { diff --git a/crates/core/src/bc_protocol/connection/udpconn/transmit.rs b/crates/core/src/bc_protocol/connection/udpconn/transmit.rs index d11665f..f3e49a4 100644 --- a/crates/core/src/bc_protocol/connection/udpconn/transmit.rs +++ b/crates/core/src/bc_protocol/connection/udpconn/transmit.rs @@ -13,7 +13,10 @@ use log::*; use std::collections::HashMap; use std::io::{Error as IoError, ErrorKind}; use std::net::UdpSocket; -use std::sync::atomic::{AtomicU32, Ordering}; +use std::sync::{ + atomic::{AtomicU32, Ordering}, + Mutex, +}; use time::OffsetDateTime; #[derive(Debug, Error)] @@ -54,16 +57,159 @@ pub enum TransmitError { type Result = std::result::Result; pub struct UdpTransmit { - client_wants: AtomicU32, - camera_acknowledged: AtomicU32, - client_sent: AtomicU32, + client_recieved: ClientRecieved, + client_sent: ClientSent, +} + +#[derive(Default)] +struct ClientSent { + buffer: Mutex>>, + last_sent: Mutex>, +} + +impl ClientSent { + fn get_next_packet_id(&self) -> u32 { + let mut lock = self.last_sent.lock().unwrap(); + if let Some(current) = lock.as_ref() { + *lock = Some(current + 1); + } else { + *lock = Some(0); + } + lock.unwrap() + } + + fn register_payload(&self, packet_id: u32, payload: Vec) { + self.buffer + .lock() + .unwrap() + .entry(packet_id) + .or_insert(payload); + } + + // fn acknoledge_to(&self, packet_id: u32) { + // self.buffer.lock().unwrap().retain(|&k, _| k > packet_id); + // } + // + // fn acknoledge_these(&self, packet_ids: Vec) { + // let locked = self.buffer.lock().unwrap(); + // for packet_id in packet_ids { + // let _ = locked.remove(&packet_id); + // } + // } + + fn acknoledge_from_ack_data(&self, start: u32, payload: Vec) { + let mut locked = self.buffer.lock().unwrap(); + locked.retain(|&k, _| k > start); + + for (idx, &value) in payload.iter().enumerate() { + let packet_id = start + idx as u32; + if value > 0 { + locked.remove(&packet_id); + } + } + } + + fn lock(&self) -> ClientSentLocked { + ClientSentLocked::new(self) + } +} + +struct ClientSentLocked<'a> { + locked: std::sync::MutexGuard<'a, HashMap>>, +} + +impl<'a> ClientSentLocked<'a> { + fn new(source: &'a ClientSent) -> Self { + let locked = source.buffer.lock().unwrap(); + Self { locked } + } + + fn needs_resend(&self) -> ResendIter { + ResendIter { + iter: self.locked.iter(), + } + } +} + +struct ResendIter<'a> { + iter: std::collections::hash_map::Iter<'a, u32, Vec>, +} + +impl<'a> std::iter::Iterator for ResendIter<'a> { + type Item = (u32, &'a Vec); + + fn next(&mut self) -> Option { + let (k, v) = self.iter.next()?; + Some((*k, v)) + } +} + +#[derive(Default)] +struct ClientRecieved { + buffer: Mutex>>, + consumed: Mutex>, +} + +impl ClientRecieved { + // Consume all contigious packets + fn consume(&self) -> Vec> { + let mut locked = self.buffer.lock().unwrap(); + let mut results = vec![]; + if locked.keys().max().is_some() { + // Just check there is some data + let mut packet_id_loc = self.consumed.lock().unwrap(); + let mut next_packet_id = packet_id_loc.map(|v| v + 1).unwrap_or(0); + while let Some(payload) = locked.remove(&next_packet_id) { + *packet_id_loc = Some(next_packet_id); + results.push(payload); + next_packet_id = packet_id_loc.unwrap() + 1; + } + } + results + } + + // Revieve a new packet only update if entry is new + fn receive(&self, packet_id: u32, payload: Vec) { + let mut locked = self.buffer.lock().unwrap(); + let _ = locked.entry(packet_id).or_insert(payload); + } + + fn get_needed_packets(&self) -> Option<(u32, Vec)> { + let locked = self.buffer.lock().unwrap(); + let packet_id_loc = self.consumed.lock().unwrap(); + + // If this is none we have recieved no packets so we cannot + // ack anything anyways. Just return None + let mut start = packet_id_loc.as_ref().copied()?; + + // Find the last contigous packet + while locked.contains_key(&(start + 1)) { + start += 1; + } + + // Find last packet in buffer + let vec = if let Some(end) = locked.keys().max() { + let mut vec = vec![]; + for i in (start + 1)..(end + 1) { + if locked.contains_key(&i) { + vec.push(0) + } else { + vec.push(1) + } + } + vec + } else { + vec![] + }; + + Some((start, vec)) + } } impl UdpTransmit { pub fn new() -> Self { Self { - client_wants: Default::default(), - camera_acknowledged: Default::default(), + client_recieved: Default::default(), client_sent: Default::default(), } } @@ -91,7 +237,7 @@ impl UdpTransmit { }) if cid == discovery_result.client_id && did == discovery_result.camera_id => { - // Reply with out C2D_Disc and end + // Reply with C2D_Disc and end let bcudp_msg = BcUdp::Discovery(UdpDiscovery { tid, payload: UdpXml { @@ -116,10 +262,10 @@ impl UdpTransmit { BcUdp::Ack(UdpAck { connection_id, packet_id, - .. + payload, }) if connection_id == discovery_result.client_id => { - self.camera_acknowledged - .fetch_max(packet_id, Ordering::Acquire); + self.client_sent + .acknoledge_from_ack_data(packet_id, payload); } BcUdp::Ack(UdpAck { .. }) => { debug!("Got a UdpAck for another client id"); @@ -131,18 +277,10 @@ impl UdpTransmit { }) if cid == discovery_result.client_id => { // Seperated as let/if due to clippy recommend trace!("Recieving UdpData packet {}", packet_id); - let updated = self - .client_wants - .fetch_update(Ordering::SeqCst, Ordering::SeqCst, |v| { - if v == packet_id { - Some(v + 1) - } else { - None - } - }) - .is_ok(); - if updated { - send_to_incoming.send(payload)?; + self.client_recieved.receive(packet_id, payload); + + for next_payload in self.client_recieved.consume().drain(..) { + send_to_incoming.send(next_payload)?; } } BcUdp::Data(UdpData { .. }) => { @@ -166,28 +304,19 @@ impl UdpTransmit { socket: &UdpSocket, discovery_result: &UdpDiscover, get_from_outgoing: &Receiver>, - outgoing_history: &mut HashMap>, ) -> Result<()> { // Handle Retransmit - let camera_has = self.camera_acknowledged.load(Ordering::Relaxed); - let client_sent = self.client_sent.load(Ordering::Relaxed); - if camera_has < client_sent { - debug!("Resending {:?}", (camera_has + 1)..(client_sent + 1)); - for packet_id in (camera_has + 1)..(client_sent + 1) { - if let Some(payload) = &outgoing_history.get(&packet_id) { - debug!(" - Sending {}", packet_id); - socket - .send(payload) - .map_err(|e| TransmitError::SocketSend { err: e })?; - } - std::thread::sleep(SOCKET_WAIT_TIME); - } + for (_, payload) in self.client_sent.lock().needs_resend() { + socket + .send(payload) + .map_err(|e| TransmitError::SocketSend { err: e })?; + std::thread::sleep(SOCKET_WAIT_TIME); } // Handle recv from `send_to_incoming` and send to `socket` match get_from_outgoing.try_recv() { Ok(payload) => { - let packet_id = self.client_sent.fetch_add(1, Ordering::Relaxed); + let packet_id = self.client_sent.get_next_packet_id(); let bcudp_msg = BcUdp::Data(UdpData { connection_id: discovery_result.camera_id, packet_id, @@ -199,18 +328,19 @@ impl UdpTransmit { socket .send(&buf) .map_err(|e| TransmitError::SocketSend { err: e })?; - outgoing_history.insert(packet_id, buf); + self.client_sent.register_payload(packet_id, buf); } Err(TryRecvError::Empty) => {} Err(e) => return Err(e.into()), } // Handle acknoledgment - let client_wants = self.client_wants.load(Ordering::Relaxed); - if client_wants > 0 { + let client_wants = self.client_recieved.get_needed_packets(); + if let Some((client_wants, also_wants)) = client_wants { let bcack_msg = BcUdp::Ack(UdpAck { connection_id: discovery_result.camera_id, packet_id: client_wants, + payload: also_wants, }); let mut buf = vec![]; @@ -219,6 +349,8 @@ impl UdpTransmit { .send(&buf) .map_err(|e| TransmitError::SocketSend { err: e })?; } + + std::thread::sleep(SOCKET_WAIT_TIME); Ok(()) } } diff --git a/crates/core/src/bcudp/de.rs b/crates/core/src/bcudp/de.rs index 9a52c08..d4c2802 100644 --- a/crates/core/src/bcudp/de.rs +++ b/crates/core/src/bcudp/de.rs @@ -139,15 +139,21 @@ fn udp_ack(buf: &[u8]) -> IResult<&[u8], UdpAck> { // packets to let (buf, _unknown) = le_u32(buf)?; let (buf, payload_size) = le_u32(buf)?; - let (buf, _payload) = take!(buf, payload_size)?; // It is a binary payload of - // `00 01 01 01 01 00 01` - // Probably a truth map of missing packets - // since last contigious packet_id up - // to the last packet we sent and it recieved + let (buf, payload) = if payload_size > 0 { + let (buf, t_payload) = take!(buf, payload_size)?; // It is a binary payload of + // `00 01 01 01 01 00 01` + // Probably a truth map of missing packets + // since last contigious packet_id up + // to the last packet we sent and it recieved + (buf, t_payload.to_vec()) + } else { + (buf, vec![]) + }; let data = UdpAck { connection_id, packet_id, + payload, }; Ok((buf, data)) } @@ -303,6 +309,7 @@ mod tests { Ok(BcUdp::Ack(UdpAck { connection_id: 80, packet_id: 2439, + .. })) ); } diff --git a/crates/core/src/bcudp/model.rs b/crates/core/src/bcudp/model.rs index 39de3be..712475a 100644 --- a/crates/core/src/bcudp/model.rs +++ b/crates/core/src/bcudp/model.rs @@ -56,7 +56,11 @@ pub struct UdpAck { // 2 Bytes Unknown: Observed values `00000000`, `d6010000`, `d7160000` `09e00000` // Unknown but seems to change randomly every second // 2 Bytes size of a payload - // Payload of `00 01 01 01 01` where `01` is added after every repeat + // + /// Payload of `00 01 01 01 01` where `01` is added after every repeat + /// + /// This is a truth table of packets after `packet_id` that have not been recieved + pub payload: Vec, } /// Magic for the UDP Data packet diff --git a/crates/core/src/bcudp/ser.rs b/crates/core/src/bcudp/ser.rs index b64d2fe..e9071f7 100644 --- a/crates/core/src/bcudp/ser.rs +++ b/crates/core/src/bcudp/ser.rs @@ -22,7 +22,10 @@ impl BcUdp { let xml_payload = encrypt(payload.tid, &payload.payload.serialize(vec![]).unwrap()); gen(bcudp_disc(payload, &xml_payload), buf)? } - BcUdp::Ack(payload) => gen(bcudp_ack(payload), buf)?, + BcUdp::Ack(payload) => { + let binary_payload = &payload.payload; + gen(bcudp_ack(payload, binary_payload), buf)? + } BcUdp::Data(payload) => { let binary_payload = &payload.payload; gen(bcudp_data(payload, binary_payload), buf)? @@ -48,7 +51,10 @@ fn bcudp_disc<'a, W: 'a + Write>( )) } -fn bcudp_ack<'a, W: 'a + Write>(payload: &'a UdpAck) -> impl SerializeFn + 'a { +fn bcudp_ack<'a, W: 'a + Write>( + payload: &'a UdpAck, + binary_payload: &'a [u8], +) -> impl SerializeFn + 'a { tuple(( le_u32(MAGIC_HEADER_UDP_ACK), le_i32(payload.connection_id), @@ -56,7 +62,8 @@ fn bcudp_ack<'a, W: 'a + Write>(payload: &'a UdpAck) -> impl SerializeFn + 'a le_u32(0), le_u32(payload.packet_id), le_u32(0), - le_u32(0), + le_u32(binary_payload.len() as u32), + slice(binary_payload), )) }