From c4760438ef0b0fb059b2a31f3cb9278aa1de1b4b Mon Sep 17 00:00:00 2001
From: Ivan Mahonin <bh@icystar.com>
Date: Oct 24 2021 14:56:50 +0000
Subject: tunnel


---

diff --git a/.gitignore b/.gitignore
index 3159f7f..620e5b9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,3 @@
 *.kdev4
+/icetunnel2-debug
+/icetunnel2
diff --git a/build.sh b/build.sh
new file mode 100755
index 0000000..0cf839d
--- /dev/null
+++ b/build.sh
@@ -0,0 +1,12 @@
+#!/bin/bash
+
+set -e
+
+if [ "$1" = "debug" ]; then
+	c++ -Wall -g -O0 *.cpp -o icetunnel2-debug
+else
+	c++ -Wall -DNDEBUG -O3 *.cpp -o icetunnel2
+fi
+
+
+
diff --git a/common.cpp b/common.cpp
index b4cd4e2..8719d8b 100644
--- a/common.cpp
+++ b/common.cpp
@@ -1,4 +1,6 @@
 
+#include <cstdio>
+
 #include "common.h"
 
 
diff --git a/common.h b/common.h
index 0e1763e..cdfa08d 100644
--- a/common.h
+++ b/common.h
@@ -6,16 +6,19 @@ enum {
 	PACKET_SIZE     = 1024,
 	PACKETS_COUNT   = 8*PACKET_SIZE,
 	TCP_BUFFER_SIZE = PACKET_SIZE*PACKETS_COUNT,
+	CRYPT_BLOCK     = 64,
 };
 
+
 enum : unsigned short {
 	CMD_SHIFT   = 12,
 	CMD_MASK    = (unsigned short)(0xffff << CMD_SHIFT),
 	SIZE_MASK   = (unsigned short)(~CMD_MASK),
 	
-	CMD_DATA    = 0,
-	CMD_TERM    = 1,
-	CMD_CONFIRM = 2,
+	CMD_DATA      = 0,
+	CMD_CONFIRM   = 1,
+	CMD_TERM      = 2,
+	CMDMAX        = 3,
 };
 
 
@@ -75,7 +78,6 @@ struct __attribute__((__packed__)) Packet {
 	unsigned short cmdSize;
 	unsigned char payload[PACKET_SIZE];
 	
-	enum { HEADER_SIZE = ((Packet*)nullptr)->payload - nullptr; };
 	
 	inline Packet():
 		connId(), index(), cmdSize(), payload() { }
@@ -98,4 +100,12 @@ struct __attribute__((__packed__)) Packet {
 };
 
 
+enum {
+	FULL_PACKET_SIZE  = sizeof(Packet),
+	HEADER_SIZE       = FULL_PACKET_SIZE - PACKET_SIZE,
+	CRYPT_PACKET_SIZE = ((FULL_PACKET_SIZE - 1)/CRYPT_BLOCK + 1)*CRYPT_BLOCK,
+};
+
+
+
 #endif
diff --git a/connection.cpp b/connection.cpp
index 30d1f80..a580cc0 100644
--- a/connection.cpp
+++ b/connection.cpp
@@ -3,16 +3,36 @@
 #include "connection.h"
 
 
-Connection::Connection(Tunnel &tunnel, const ConnId &id, int tcpSockId):
-	tunnel(tunnel),
-	id(id),
-	tcpSockId(tcpSockId),
+
+Connection::Connection(const Args &args):
+	tunnel(args.tunnel),
+	id(args.id),
+	tcpSockId(args.tcpSockId),
 	udpActive(true),
+	lastUdpInputTime(tunnel.time),
+	lastUdpOutputTime(tunnel.time),
 	tcpSendQueue(*this),
 	tcpRecvQueue(*this),
 	udpSendQueue(*this),
 	udpRecvQueue(*this)
 	{ }
 
+
 Connection::~Connection() { }
 
+
+bool Connection::wantReadFromTcp() const
+	{ return tcpSockId >= 0 && udpActive && tcpRecvQueue.freeSize() > 0; }
+
+
+bool Connection::wantWriteToTcp() const
+	{ return tcpSockId >= 0 && (tcpSendQueue.busySize() > 0 || udpRecvQueue.canSentToTcp()); }
+
+
+Time Connection::whenWriteToUdp() const {
+	Time t = lastUdpOutputTime + tunnel.udpKeepAliveDuration;
+	udpSendQueue.whenWriteToUdp(t);
+	udpRecvQueue.whenWriteToUdp(t);
+	return t;
+}
+
diff --git a/connection.h b/connection.h
index 4707952..daf81ef 100644
--- a/connection.h
+++ b/connection.h
@@ -12,17 +12,28 @@ class Tunnel;
 
 class Connection {
 public:
+	struct Args {
+		Tunnel &tunnel;
+		const ConnId &id;
+		int tcpSockId;
+		inline Args(Tunnel &tunnel, const ConnId &id, int tcpSockId):
+			tunnel(tunnel), id(id), tcpSockId(tcpSockId) { }
+	};
+	
 	Tunnel &tunnel;
 	const ConnId id;
 	int tcpSockId;
 	bool udpActive;
 	
+	Time lastUdpInputTime;
+	Time lastUdpOutputTime;
+	
 	TcpQueue tcpSendQueue;
 	TcpQueue tcpRecvQueue;
 	UdpSendQueue udpSendQueue;
 	UdpRecvQueue udpRecvQueue;
 
-	Connection(Tunnel &tunnel, const ConnId &id, int tcpSockId);
+	explicit Connection(const Args &args);
 	~Connection();
 	
 	bool wantReadFromTcp() const;
diff --git a/crypt.cpp b/crypt.cpp
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/crypt.cpp
@@ -0,0 +1 @@
+
diff --git a/crypt.h b/crypt.h
new file mode 100644
index 0000000..a2d71e6
--- /dev/null
+++ b/crypt.h
@@ -0,0 +1,30 @@
+#ifndef CRYPT_H
+#define CRYPT_H
+
+
+#include <vector>
+
+#include "common.h"
+
+
+
+class Crypt {
+private:
+	Time time;
+	std::vector<unsigned char> keyData;
+	
+public:
+	Crypt();
+	~Crypt();
+	
+	bool setKey(const char *key);
+	void resetKey();
+	
+	void setTime(Time time);
+	
+	bool encrypt(const void *src, int srcSize, void *dst, int &dstSize);
+	bool decrypt(const void *src, int srcSize, void *dst, int &dstSize);
+};
+
+
+#endif
diff --git a/socket.h b/socket.h
index 920afde..fb815d2 100644
--- a/socket.h
+++ b/socket.h
@@ -50,9 +50,9 @@ public:
 	static int tcpSendAsync(int soskId, const void *data, int size);
 	static int tcpRecvAsync(int soskId, void *data, int size);
 	
-	static int udpBind(int soskId, const Address &address);
+	static int udpBind(const Address &address);
 	static int udpSend(int soskId, const Address &address, const void *data, int size);
-	static int udpRecvAsync(Address *address, void *data, int size);
+	static int udpRecvAsync(int soskId, Address &address, void *data, int size);
 	
 	static void close(int soskId);
 };
diff --git a/tunnel.cpp b/tunnel.cpp
index 949938e..7323dbb 100644
--- a/tunnel.cpp
+++ b/tunnel.cpp
@@ -12,51 +12,52 @@ Tunnel::Tunnel():
 	udpPartialSendDuration(500000),
 	udpResendDuration(udpSendDuration * PACKETS_COUNT),
 	udpConfirmDuration(udpResendDuration/8),
+	udpSilenceDuration(300000000),
+	udpKeepAliveDuration(udpSilenceDuration/32),
 	pollDuration(5000000),
 	udpSockId(-1),
 	tcpSockId(-1),
 	nextTermSendTime(time + udpConfirmDuration)
 	{ }
 
+
 Tunnel::~Tunnel()
 	{ close(); }
 
 
-bool Tunnel::initUdpServer(const Address &address, const Address &remoteTcpAddress) {
+bool Tunnel::initUdpServer(const Address &address, const Address &remoteTcpAddress, const char *key) {
 	closeUdpServer();
 	printf("Init UDP server at address: "); address.print();
 	if (remoteTcpAddress.port)
 		{ printf(", with remote TCP address: "); remoteTcpAddress.print(); }
 	printf("\n");
 	
+	if (!crypt.setKey(key))
+		{ printf("bad key, cancelled"); return false; }
+	
 	udpSockId = Socket::udpBind(address);
 	if (udpSockId < 0)
-		{ printf("cannot bind, cancelled"); return false; }
+		{ crypt.resetKey(); printf("cannot bind, cancelled"); return false; }
 	
 	printf("success");
 	this->remoteTcpAddress = remoteTcpAddress;
 	return true;
 }
 
-bool Tunnel::initTcpServer(const Address &address) {
+
+bool Tunnel::initTcpServer(const Address &address, const Address &remoteUdpAddress) {
 	closeTcpServer();
 	printf("Init TCP server at address: "); address.print(); printf("\n");
 	
 	if (udpSockId < 0)
 		{ printf("UDP server was not initialized, cancelled\n"); return false; }
 	
-	int sockId = ::socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
-	struct sockaddr_in addr = {};
-	addr.sin_family = PF_INET;
-	addr.sin_addr.s_addr = address.ipUInt;
-	addr.sin_port = address.port;
-	if (0 != ::bind(sockId, (struct sockaddr*)&addr, sizeof(addr)))
-		{ ::close(sockId); printf("cannot bind, cancelled\n"); return false; }
-	if (0 != ::listen(sockId, 32))
-		{ ::close(sockId); printf("cannot listen, cancelled\n"); return false; }
-
-	tcpSockId = sockId;
+	tcpSockId = Socket::tcpListen(address);
+	if (tcpSockId < 0)
+		{ printf("cannot listen, cancelled"); return false; }
+	
 	printf("success");
+	this->remoteUdpAddress = remoteUdpAddress;
 	return true;
 }
 
@@ -73,6 +74,7 @@ void Tunnel::closeUdpServer() {
 	Socket::close(udpSockId);
 	udpSockId = -1;
 	remoteTcpAddress = Address();
+	crypt.resetKey();
 }
 
 
@@ -89,87 +91,75 @@ void Tunnel::close()
 	{ closeUdpServer(); }
 
 
-void Tunnel::readUdp() {
-	// read and parse UDP input
-	Packet packet;
-	const int minPacketSize = offsetof(Packet, payload) - offsetof(Packet, connId);
-	while(true) {
-		struct sockaddr_in addr = {};
-		socklen_t addrlen = sizeof(addr);
-		int res = ::recvfrom(
-			udpSockId,
-			&packet.connId,
-			MSG_DONTWAIT,
-			sizeof(packet) - offsetof(Packet, connId),
-			(struct sockaddr*)&addr,
-			&addrlen );
-		if (res <= 0)
-			break;
-		if (res < minPacketSize)
-			continue;
-		
-		int cmd = packet.getCommand();
-		int size = packet.getSize();
-		if (res != minPacketSize + packet.getSize())
-			continue;
-		if (cmd != CMD_TERM && cmd != CMD_CONFIRM && cmd != CMD_DATA)
-			continue;
-		
-		ConnId id;
-		id.address.ipUInt = addr.sin_addr.s_addr;
-		id.address.port = addr.sin_port;
-		id.id = packet.connId;
-		int cmd = packet.getCommand();
-		ConnMap::iterator i = connections.find(id);
-		Connection *conn = i == connections.end() ? nullptr : &i->second;
-		
-		if (!conn && remoteTcpAddress.port && cmd != CMD_TERM) {
-			printf("Open incoming connection from UDP: ");
-			id.address.print();
-			printf(", id: %u\n", id.id);
-			
-			int sockId = ::socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
-			struct sockaddr_in addr = {};
-			addr.sin_family = PF_INET;
-			addr.sin_addr.s_addr = id.address.ipUInt;
-			addr.sin_port = id.address.port;
-			if (0 != ::connect(sockId, (struct sockaddr*)&addr, sizeof(addr))) {
-				::close(sockId);
-				printf("cannot init TCP connection, cancelled");
-				continue;
-			}
-			
-			conn = &connections[id];
-			conn->id = id;
-			conn->tcpSockId = sockId;
-			conn->udpActive = true;
-		}
-		
-		if (!conn) {
-			if (cmd != CMD_TERM)
-				rejectedConnections.insert(id);
-			continue;
-		}
-		
-		if (cmd == CMD_TERM) {
-			conn->udpActive = false;
-			continue;
-		}
-		
-		if (cmd == CMD_CONFIRM) {
-			unsigned int index = packet.index;
-			for(int i = 0; i < size; ++i)
-				for(int j = 0; j < 8; ++j, ++index)
-					if (packet.payload[i] & (1 << j))
-						if (PacketInfo *pi = conn->toUdpQueue[index])
-							if (pi->loaded)
-								pi->confirmed = true;
-			while(PacketInfo *pi = conn->toUdpQueue.first())
-				if (pi->loaded && pi->confirmed)
-					conn->toUdpQueue.pop(); else break;
-			continue;
+bool Tunnel::recvUdpPacket() {
+	ConnId id;
+	unsigned char raw[CRYPT_PACKET_SIZE] = {};
+	int rawSize = Socket::udpRecvAsync(udpSockId, id.address, raw, sizeof(raw));
+	if (rawSize <= 0)
+		return false;
+	
+	unsigned char data[CRYPT_PACKET_SIZE] = {};
+	int dataSize = CRYPT_PACKET_SIZE;
+	if (!crypt.decrypt(raw, rawSize, data, dataSize))
+		return true;
+	
+	void *ptr = data;
+	Packet &packet = *(Packet*)ptr;
+	
+	int cmd = packet.getCommand();
+	if (cmd < 0 || cmd > CMDMAX)
+		return true;
+
+	int size = packet.getSize();
+	if (size + HEADER_SIZE > dataSize)
+		return true;
+	
+	id.id = packet.connId;
+	ConnMap::iterator i = connections.find(id);
+	if (i == connections.end() && cmd == CMD_DATA && remoteTcpAddress.port && !closedConnections.count(id)) {
+		printf("Open incoming connection from UDP: ");
+		id.address.print();
+		printf(", id: %u\n", id.id);
+		int sockId = Socket::tcpConnect(remoteTcpAddress);
+		if (sockId < 0) {
+			printf("cannot init TCP connection, cancelled\n");
+		} else {
+			i = connections.emplace(id, Connection::Args(*this, id, sockId)).first;
 		}
 	}
+	
+	if (i == connections.end()) {
+		termQueue.insert(id);
+		return true;
+	}
+	
+	Connection &conn = i->second;
+	if (!conn.udpActive) {
+		if (cmd != CMD_TERM) termQueue.insert(id);
+		return true;
+	}
+	
+	if (cmd == CMD_DATA) {
+		conn.lastUdpInputTime = time;
+		conn.udpRecvQueue.recvUdp(packet.index, packet.payload, size);
+	} else
+	if (cmd == CMD_CONFIRM) {
+		conn.lastUdpInputTime = time;
+		conn.udpSendQueue.confirm(packet.index, packet.payload, size*8);
+	} else
+	if (cmd == CMD_TERM) {
+		conn.udpActive = false;
+	}
+	
+	return true;
+}
+
+
+void Tunnel::sendUdpPacket(const Address &address, const Packet &packet) {
+	unsigned char raw[CRYPT_PACKET_SIZE] = {};
+	int rawSize = CRYPT_PACKET_SIZE;
+	if (crypt.encrypt(&packet, HEADER_SIZE + packet.getSize(), raw, rawSize))
+		Socket::udpSend(udpSockId, address, raw, rawSize);
 }
 
 
@@ -212,6 +202,7 @@ bool Tunnel::iteration() {
 	// wait for events
 	poll.wait(pollTime);
 	time = monotonicTime();
+	crypt.setTime(time);
 	
 	if (poll.list[0].closed || (tcpSockId >= 0 && poll.list[1].closed)) {
 		poll.list.clear();
@@ -230,7 +221,7 @@ bool Tunnel::iteration() {
 			ConnId id;
 			id.address = remoteUdpAddress;
 			id.id = ConnId::generateId();
-			Connection &conn = connections.emplace(id, *this, id, tcpSockId).first->second;
+			Connection &conn = connections.emplace(id, Connection::Args(*this, id, tcpSockId)).first->second;
 			conn.tcpRecvQueue.readFromTcp();
 		}
 	}
@@ -240,8 +231,11 @@ bool Tunnel::iteration() {
 	for(Poll::List::const_iterator i = poll.list.begin() + first; i != poll.list.end(); ++i) {
 		Connection &conn = *i->connection;
 		conn.udpRecvQueue.sendTcp();
-		if (i->canWrite)
-			conn.tcpSendQueue.writeToTcp();
+		if (i->canWrite) while(true) {
+			int size = conn.tcpSendQueue.writeToTcp();
+			if (size <= 0) break;
+			conn.udpRecvQueue.sendTcp();
+		}
 		if (i->canRead && conn.udpActive)
 			conn.tcpRecvQueue.readFromTcp();
 		if (i->closed || (!conn.udpActive && !conn.tcpSendQueue.busySize())) {
@@ -251,15 +245,47 @@ bool Tunnel::iteration() {
 	}
 	
 	// write to udp
+	for(ConnMap::iterator i = connections.begin(); i != connections.end(); ++i) {
+		Connection &conn = i->second;
+		if (conn.udpActive) {
+			bool sent = false;
+			if (conn.udpSendQueue.sendUdp()) sent = true;
+			if (conn.udpRecvQueue.sendUdpConfirm()) sent = true;
+			if (sent) conn.lastUdpOutputTime = time;
+			if (timeLequal(conn.lastUdpOutputTime + udpKeepAliveDuration, time)) {
+				conn.udpRecvQueue.sendUdpConfirm(true);
+				conn.lastUdpOutputTime = time;
+			}
+		}
+	}
+	
+	// close connections	
 	for(ConnMap::iterator i = connections.begin(); i != connections.end();) {
 		ConnMap::iterator j = i++;
 		Connection &conn = j->second;
-		if (conn.udpActive) {
-			conn.udpSendQueue.sendUdp();
-			conn.udpRecvQueue.sendUdpConfirm();
-		} else
-		if (conn.tcpSockId < 0) {
-			connections.erase(j);
+		
+		if (conn.udpActive && timeLess(conn.lastUdpInputTime + udpSilenceDuration, time))
+			conn.udpActive = false;
+		
+		if (!conn.udpActive && !conn.wantWriteToTcp()) {
+			Socket::close(conn.tcpSockId);
+			conn.tcpSockId = -1;
+			closedConnections[conn.id] = time + 2*udpSilenceDuration;
+			connections.erase(i);
+			continue;
+		}
+		
+		if (conn.tcpSockId < 0 && !conn.udpSendQueue.canSentToUdp()) {
+			conn.udpActive = false;
+			closedConnections[conn.id] = time + 2*udpSilenceDuration;
+			connections.erase(i);
+			continue;
+		}
+		
+		if (!conn.udpActive && conn.tcpSockId < 0) {
+			closedConnections[conn.id] = time + 2*udpSilenceDuration;
+			connections.erase(i);
+			continue;
 		}
 	}
 	
diff --git a/tunnel.h b/tunnel.h
index 15ae065..2e36a7c 100644
--- a/tunnel.h
+++ b/tunnel.h
@@ -2,6 +2,7 @@
 #define TUNNEL_H
 
 
+#include "crypt.h"
 #include "socket.h"
 #include "connection.h"
 
@@ -13,6 +14,7 @@
 class Tunnel {
 public:
 	typedef std::map<ConnId, Connection> ConnMap;
+	typedef std::map<ConnId, Time> ClosedConnMap;
 	typedef std::set<ConnId> ConnSet;
 	
 public:
@@ -21,12 +23,16 @@ public:
 	Time udpPartialSendDuration;
 	Time udpResendDuration;
 	Time udpConfirmDuration;
+	Time udpSilenceDuration;
+	Time udpKeepAliveDuration;
 	Time pollDuration;
 	
 	ConnMap connections;
+	ClosedConnMap closedConnections;
 	ConnSet termQueue;
 	
 private:
+	Crypt crypt;
 	Poll poll;
 	int udpSockId;
 	int tcpSockId;
@@ -38,7 +44,7 @@ public:
 	Tunnel();
 	~Tunnel();
 	
-	bool initUdpServer(const Address &address, const Address &remoteTcpAddress);
+	bool initUdpServer(const Address &address, const Address &remoteTcpAddress, const char *key);
 	bool initTcpServer(const Address &address, const Address &remoteUdpAddress);
 	
 	void closeTcpServer();
diff --git a/udprecvqueue.cpp b/udprecvqueue.cpp
index 0300428..d4050f7 100644
--- a/udprecvqueue.cpp
+++ b/udprecvqueue.cpp
@@ -49,9 +49,9 @@ bool UdpRecvQueue::recvUdp(unsigned int index, const void *data, int size) {
 }
 
 
-bool UdpRecvQueue::sendUdpConfirm() {
+bool UdpRecvQueue::sendUdpConfirm(bool force) {
 	Time time = connection.tunnel.time;
-	if (!confirmRequired || timeLess(time, nextConfirmSendTime))
+	if (!force && (!confirmRequired || timeLess(time, nextConfirmSendTime)))
 		return false;
 	
 	unsigned int count = endIndex - currentIndex;
@@ -59,10 +59,10 @@ bool UdpRecvQueue::sendUdpConfirm() {
 	
 	Packet packet;
 	packet.init(connection.id.id, currentIndex, CMD_CONFIRM, bytesCount);
-	for(int i = 0; i < count; ++i)
+	for(int i = 0; i < (int)count; ++i)
 		if (entries[(current + i)%PACKETS_COUNT].received)
 			packet.payload[i/8] |= (1 << (i%8));
-	connection.tunnel.sendUdpPacket(packet);
+	connection.tunnel.sendUdpPacket(connection.id.address, packet);
 	
 	confirmRequired = false;
 	nextConfirmSendTime = time + connection.tunnel.udpConfirmDuration;
@@ -70,6 +70,10 @@ bool UdpRecvQueue::sendUdpConfirm() {
 }
 
 
+bool UdpRecvQueue::canSentToTcp() const
+	{ return cycleLess(currentIndex, endIndex) && entries[current].received && entries[current].size; }
+
+
 int UdpRecvQueue::sendTcp() {
 	int sent = 0;
 	while(cycleLess(currentIndex, endIndex)) {
@@ -93,3 +97,9 @@ int UdpRecvQueue::sendTcp() {
 	return sent;
 }
 
+
+void UdpRecvQueue::whenWriteToUdp(Time &t) const {
+	if (confirmRequired && timeLess(nextConfirmSendTime, t))
+		t = nextConfirmSendTime;
+}
+
diff --git a/udprecvqueue.h b/udprecvqueue.h
index f244b8b..2b98488 100644
--- a/udprecvqueue.h
+++ b/udprecvqueue.h
@@ -34,8 +34,10 @@ public:
 	~UdpRecvQueue();
 	
 	bool recvUdp(unsigned int index, const void *data, int size);
-	bool sendUdpConfirm();
+	bool sendUdpConfirm(bool force = false);
+	bool canSentToTcp() const;
 	int sendTcp();
+	void whenWriteToUdp(Time &t) const;
 };
 
 
diff --git a/udpsendqueue.cpp b/udpsendqueue.cpp
index dd1de09..84c88b0 100644
--- a/udpsendqueue.cpp
+++ b/udpsendqueue.cpp
@@ -32,14 +32,24 @@ UdpSendQueue::Entry* UdpSendQueue::allocEntry() {
 	Entry *e = freeFirst;
 	if (!e) return nullptr;
 	freeFirst = e->next;
-	e->prev = busyLast;
-	e->next = nullptr;
-	(e->prev ? e->prev->next : busyFirst) = e;
+	e->prev = nullptr;
+	e->next = busyFirst;
+	(e->next ? e->next->prev : busyLast) = e;
 	busyLast = e;
 	return e;
 }
 
 
+void UdpSendQueue::moveToEnd(Entry *e) {
+	if (!e->next) return;
+	(e->prev ? e->prev->next : busyFirst) = e->next;
+	(e->next ? e->next->prev : busyLast ) = e->prev;
+	e->next = nullptr;
+	e->prev = busyLast;
+	(e->prev ? e->prev->next : busyFirst) = e;
+}
+
+
 void UdpSendQueue::freeEntry(Entry *e) {
 	(e->prev ? e->prev->next : busyFirst) = e->next;
 	(e->next ? e->next->prev : busyLast ) = e->prev;
@@ -76,11 +86,8 @@ bool UdpSendQueue::sendUdpSingle() {
 		return false;
 	
 	Entry *e = busyFirst;
-	while(e) {
-		if (timeLequal(e->resendTime, time))
-			break;
-		e = e->next;
-	}
+	if (!timeLequal(e->resendTime, time))
+		e = nullptr;
 	
 	if (!e && !freeFirst)
 		return false;
@@ -110,11 +117,12 @@ bool UdpSendQueue::sendUdpSingle() {
 	Packet packet;
 	packet.init(connection.id.id, e->index, CMD_DATA, e->size);
 	if (e->size) memcpy(packet.payload, e->data, e->size);
-	connection.tunnel.sendUdpPacket(packet);
+	connection.tunnel.sendUdpPacket(connection.id.address, packet);
 	
 	e->resendTime = time + connection.tunnel.udpResendDuration;
 	nextSendTime += connection.tunnel.udpPartialSendDuration;
 	nextPartialSendTime = time + connection.tunnel.udpPartialSendDuration;
+	moveToEnd(e);
 	return true;
 }
 
@@ -132,3 +140,21 @@ int UdpSendQueue::sendUdp() {
 	return sent;
 }
 
+
+bool UdpSendQueue::canSentToUdp() const
+	{ return connection.udpActive && (busyFirst || connection.tcpRecvQueue.busySize()); }
+
+
+void UdpSendQueue::whenWriteToUdp(Time &t) const {
+	int size = connection.tcpRecvQueue.busySize();
+	if (size >= PACKET_SIZE) {
+		if (timeLess(nextSendTime, t)) t = nextSendTime;
+		return;
+	}
+
+	Time tt = t;
+	if (size > 0 && timeLess(nextPartialSendTime, tt)) tt = nextPartialSendTime;
+	if (timeLess(busyFirst->resendTime, tt)) tt = busyFirst->resendTime;
+	if (timeLess(tt, nextSendTime)) tt = nextSendTime;
+	if (timeLess(t, tt)) t = tt;
+}
diff --git a/udpsendqueue.h b/udpsendqueue.h
index 25ef443..a5458ec 100644
--- a/udpsendqueue.h
+++ b/udpsendqueue.h
@@ -32,6 +32,7 @@ private:
 	Entry entries[PACKETS_COUNT];
 	
 	Entry* allocEntry();
+	void moveToEnd(Entry *e);
 	void freeEntry(Entry *e);
 	
 public:
@@ -41,6 +42,8 @@ public:
 	void confirm(unsigned int index, const unsigned char *bits, unsigned int bitsCount);
 	bool sendUdpSingle();
 	int sendUdp();
+	bool canSentToUdp() const;
+	void whenWriteToUdp(Time &t) const;
 };