3 files changed, 51 insertions, 3 deletions

diff --git a/toxcore/Lossless_UDP.c b/toxcore/Lossless_UDP.c
index 6b2c83a5..fe22f869 100644
--- a/toxcore/Lossless_UDP.c
+++ b/toxcore/Lossless_UDP.c
@@ -185,8 +185,10 @@ int new_connection(Lossless_UDP *ludp, IP_Port ip_port)
 {
     int connection_id = getconnection_id(ludp, ip_port);
 
-    if (connection_id != -1)
+    if (connection_id != -1) {
+        confirm_connection(ludp, connection_id);
         return connection_id;
+    }
 
     tox_array_for_each(&ludp->connections, Connection, tmp) {
         if (tmp->status == 0) {
@@ -486,7 +488,41 @@ int read_packet(Lossless_UDP *ludp, int connection_id, uint8_t *data)
     ++connection->successful_read;
     connection->recvbuffer[index].size = 0;
     return size;
+}
+
+/* Like read_packet() but does leaves the queue as is.
+ *  return 0 if there is no received data in the buffer.
+ *  return length of received packet if successful.
+ */
+int read_packet_silent(Lossless_UDP *ludp, int connection_id, uint8_t *data)
+{
+    if (recvqueue(ludp, connection_id) == 0)
+        return 0;
+
+    Connection *connection = &tox_array_get(&ludp->connections, connection_id, Connection);
+
+    if (connection->status == 0)
+        return 0;
+
+    uint16_t index = connection->successful_read % connection->recvbuffer_length;
+    uint16_t size  = connection->recvbuffer[index].size;
+    memcpy(data, connection->recvbuffer[index].data, size);
+    return size;
+}
+/* Discard the next packet to be read from the queue
+ *  return 0 if success.
+ *  return -1 if failure.
+ */
+int discard_packet(Lossless_UDP *ludp, int connection_id)
+{
+    if (recvqueue(ludp, connection_id) == 0)
+        return -1;
 
+    Connection *connection = &tox_array_get(&ludp->connections, connection_id, Connection);
+    uint16_t index = connection->successful_read % connection->recvbuffer_length;
+    ++connection->successful_read;
+    connection->recvbuffer[index].size = 0;
+    return 0;
 }
 
 /*  return 0 if data could not be put in packet queue.
diff --git a/toxcore/Lossless_UDP.h b/toxcore/Lossless_UDP.h
index aa6e344f..9b5c2406 100644
--- a/toxcore/Lossless_UDP.h
+++ b/toxcore/Lossless_UDP.h
@@ -196,6 +196,18 @@ char id_packet(Lossless_UDP *ludp, int connection_id);
  */
 int read_packet(Lossless_UDP *ludp, int connection_id, uint8_t *data);
 
+/* Like read_packet() but does leaves the queue as is.
+ *  return 0 if there is no received data in the buffer.
+ *  return length of received packet if successful.
+ */
+int read_packet_silent(Lossless_UDP *ludp, int connection_id, uint8_t *data);
+
+/* Discard the next packet to be read from the queue
+ *  return 0 if success.
+ *  return -1 if failure.
+ */
+int discard_packet(Lossless_UDP *ludp, int connection_id);
+
 /*  return 0 if data could not be put in packet queue.
  *  return 1 if data was put into the queue.
  */
diff --git a/toxcore/net_crypto.c b/toxcore/net_crypto.c
index ca23957d..41c8c45c 100644
--- a/toxcore/net_crypto.c
+++ b/toxcore/net_crypto.c
@@ -516,7 +516,7 @@ int crypto_inbound(Net_Crypto *c, uint8_t *public_key, uint8_t *secret_nonce, ui
 
             if (id_packet(c->lossless_udp, c->incoming_connections[i]) == 2) {
                 uint8_t temp_data[MAX_DATA_SIZE];
-                uint16_t len = read_packet(c->lossless_udp, c->incoming_connections[i], temp_data);
+                uint16_t len = read_packet_silent(c->lossless_udp, c->incoming_connections[i], temp_data);
 
                 if (handle_cryptohandshake(c, public_key, secret_nonce, session_key, temp_data, len)) {
                     int connection_id = c->incoming_connections[i];
@@ -570,7 +570,7 @@ int accept_crypto_inbound(Net_Crypto *c, int connection_id, uint8_t *public_key,
 {
     uint32_t i;
 
-    if (connection_id == -1)
+    if (discard_packet(c->lossless_udp, connection_id) == -1)
         return -1;
 
     /*