3 files changed, 209 insertions, 27 deletions

diff --git a/core/io/packet_peer_udp.cpp b/core/io/packet_peer_udp.cpp
index ef4fdd689c..d33ba6f855 100644
--- a/core/io/packet_peer_udp.cpp
+++ b/core/io/packet_peer_udp.cpp
@@ -32,8 +32,6 @@
 
 #include "core/io/ip.h"
 
-PacketPeerUDP *(*PacketPeerUDP::_create)() = NULL;
-
 void PacketPeerUDP::set_blocking_mode(bool p_enable) {
 
 	blocking = p_enable;
@@ -59,6 +57,177 @@ Error PacketPeerUDP::_set_dest_address(const String &p_address, int p_port) {
 	return OK;
 }
 
+int PacketPeerUDP::get_available_packet_count() const {
+
+	// TODO we should deprecate this, and expose poll instead!
+	Error err = const_cast<PacketPeerUDP *>(this)->_poll();
+	if (err != OK)
+		return -1;
+
+	return queue_count;
+}
+
+Error PacketPeerUDP::get_packet(const uint8_t **r_buffer, int &r_buffer_size) {
+
+	Error err = _poll();
+	if (err != OK)
+		return err;
+	if (queue_count == 0)
+		return ERR_UNAVAILABLE;
+
+	uint32_t size = 0;
+	uint8_t ipv6[16];
+	rb.read(ipv6, 16, true);
+	packet_ip.set_ipv6(ipv6);
+	rb.read((uint8_t *)&packet_port, 4, true);
+	rb.read((uint8_t *)&size, 4, true);
+	rb.read(packet_buffer, size, true);
+	--queue_count;
+	*r_buffer = packet_buffer;
+	r_buffer_size = size;
+	return OK;
+}
+
+Error PacketPeerUDP::put_packet(const uint8_t *p_buffer, int p_buffer_size) {
+
+	ERR_FAIL_COND_V(!_sock.is_valid(), ERR_UNAVAILABLE);
+	ERR_FAIL_COND_V(!peer_addr.is_valid(), ERR_UNCONFIGURED);
+
+	Error err;
+	int sent = -1;
+
+	if (!_sock->is_open()) {
+		IP::Type ip_type = peer_addr.is_ipv4() ? IP::TYPE_IPV4 : IP::TYPE_IPV6;
+		err = _sock->open(NetSocket::TYPE_UDP, ip_type);
+		ERR_FAIL_COND_V(err != OK, err);
+		_sock->set_blocking_enabled(false);
+	}
+
+	do {
+		err = _sock->sendto(p_buffer, p_buffer_size, sent, peer_addr, peer_port);
+		if (err != OK) {
+			if (err != ERR_BUSY)
+				return FAILED;
+			else if (!blocking)
+				return ERR_BUSY;
+			// Keep trying to send full packet
+			continue;
+		}
+		return OK;
+
+	} while (sent != p_buffer_size);
+
+	return OK;
+}
+
+int PacketPeerUDP::get_max_packet_size() const {
+
+	return 512; // uhm maybe not
+}
+
+Error PacketPeerUDP::listen(int p_port, const IP_Address &p_bind_address, int p_recv_buffer_size) {
+
+	ERR_FAIL_COND_V(!_sock.is_valid(), ERR_UNAVAILABLE);
+	ERR_FAIL_COND_V(_sock->is_open(), ERR_ALREADY_IN_USE);
+	ERR_FAIL_COND_V(!p_bind_address.is_valid() && !p_bind_address.is_wildcard(), ERR_INVALID_PARAMETER);
+
+	Error err;
+	IP::Type ip_type = IP::TYPE_ANY;
+
+	if (p_bind_address.is_valid())
+		ip_type = p_bind_address.is_ipv4() ? IP::TYPE_IPV4 : IP::TYPE_IPV6;
+
+	err = _sock->open(NetSocket::TYPE_UDP, ip_type);
+
+	if (err != OK)
+		return ERR_CANT_CREATE;
+
+	_sock->set_blocking_enabled(false);
+	_sock->set_reuse_address_enabled(true);
+	err = _sock->bind(p_bind_address, p_port);
+
+	if (err != OK) {
+		_sock->close();
+		return err;
+	}
+	rb.resize(nearest_shift(p_recv_buffer_size));
+	return OK;
+}
+
+void PacketPeerUDP::close() {
+
+	if (_sock.is_valid())
+		_sock->close();
+	rb.resize(16);
+	queue_count = 0;
+}
+
+Error PacketPeerUDP::wait() {
+
+	ERR_FAIL_COND_V(!_sock.is_valid(), ERR_UNAVAILABLE);
+	return _sock->poll(NetSocket::POLL_TYPE_IN, -1);
+}
+
+Error PacketPeerUDP::_poll() {
+
+	ERR_FAIL_COND_V(!_sock.is_valid(), ERR_UNAVAILABLE);
+
+	if (!_sock->is_open()) {
+		return FAILED;
+	}
+
+	Error err;
+	int read;
+	IP_Address ip;
+	uint16_t port;
+
+	while (true) {
+		err = _sock->recvfrom(recv_buffer, sizeof(recv_buffer), read, ip, port);
+
+		if (err != OK) {
+			if (err == ERR_BUSY)
+				break;
+			return FAILED;
+		}
+
+		if (rb.space_left() < read + 24) {
+#ifdef TOOLS_ENABLED
+			WARN_PRINTS("Buffer full, dropping packets!");
+#endif
+			continue;
+		}
+
+		uint32_t port32 = port;
+		rb.write(ip.get_ipv6(), 16);
+		rb.write((uint8_t *)&port32, 4);
+		rb.write((uint8_t *)&read, 4);
+		rb.write(recv_buffer, read);
+		++queue_count;
+	}
+
+	return OK;
+}
+bool PacketPeerUDP::is_listening() const {
+
+	return _sock.is_valid() && _sock->is_open();
+}
+
+IP_Address PacketPeerUDP::get_packet_address() const {
+
+	return packet_ip;
+}
+
+int PacketPeerUDP::get_packet_port() const {
+
+	return packet_port;
+}
+
+void PacketPeerUDP::set_dest_address(const IP_Address &p_address, int p_port) {
+
+	peer_addr = p_address;
+	peer_port = p_port;
+}
+
 void PacketPeerUDP::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("listen", "port", "bind_address", "recv_buf_size"), &PacketPeerUDP::listen, DEFVAL("*"), DEFVAL(65536));
@@ -66,26 +235,21 @@ void PacketPeerUDP::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("wait"), &PacketPeerUDP::wait);
 	ClassDB::bind_method(D_METHOD("is_listening"), &PacketPeerUDP::is_listening);
 	ClassDB::bind_method(D_METHOD("get_packet_ip"), &PacketPeerUDP::_get_packet_ip);
-	//ClassDB::bind_method(D_METHOD("get_packet_address"),&PacketPeerUDP::_get_packet_address);
 	ClassDB::bind_method(D_METHOD("get_packet_port"), &PacketPeerUDP::get_packet_port);
 	ClassDB::bind_method(D_METHOD("set_dest_address", "host", "port"), &PacketPeerUDP::_set_dest_address);
 }
 
-Ref<PacketPeerUDP> PacketPeerUDP::create_ref() {
-
-	if (!_create)
-		return Ref<PacketPeerUDP>();
-	return Ref<PacketPeerUDP>(_create());
-}
-
-PacketPeerUDP *PacketPeerUDP::create() {
+PacketPeerUDP::PacketPeerUDP() {
 
-	if (!_create)
-		return NULL;
-	return _create();
+	_sock = Ref<NetSocket>(NetSocket::create());
+	blocking = true;
+	packet_port = 0;
+	queue_count = 0;
+	peer_port = 0;
+	rb.resize(16);
 }
 
-PacketPeerUDP::PacketPeerUDP() {
+PacketPeerUDP::~PacketPeerUDP() {
 
-	blocking = true;
+	close();
 }
diff --git a/core/io/packet_peer_udp.h b/core/io/packet_peer_udp.h
index 2ed53cef7f..4366b0eb82 100644
--- a/core/io/packet_peer_udp.h
+++ b/core/io/packet_peer_udp.h
@@ -32,36 +32,54 @@
 #define PACKET_PEER_UDP_H
 
 #include "core/io/ip.h"
+#include "core/io/net_socket.h"
 #include "core/io/packet_peer.h"
 
 class PacketPeerUDP : public PacketPeer {
 	GDCLASS(PacketPeerUDP, PacketPeer);
 
 protected:
+	enum {
+		PACKET_BUFFER_SIZE = 65536
+	};
+
+	RingBuffer<uint8_t> rb;
+	uint8_t recv_buffer[PACKET_BUFFER_SIZE];
+	uint8_t packet_buffer[PACKET_BUFFER_SIZE];
+	IP_Address packet_ip;
+	int packet_port;
+	int queue_count;
+
+	IP_Address peer_addr;
+	int peer_port;
 	bool blocking;
+	Ref<NetSocket> _sock;
 
-	static PacketPeerUDP *(*_create)();
 	static void _bind_methods();
 
 	String _get_packet_ip() const;
 
 	Error _set_dest_address(const String &p_address, int p_port);
+	Error _poll();
 
 public:
 	void set_blocking_mode(bool p_enable);
 
-	virtual Error listen(int p_port, const IP_Address &p_bind_address = IP_Address("*"), int p_recv_buffer_size = 65536) = 0;
-	virtual void close() = 0;
-	virtual Error wait() = 0;
-	virtual bool is_listening() const = 0;
-	virtual IP_Address get_packet_address() const = 0;
-	virtual int get_packet_port() const = 0;
-	virtual void set_dest_address(const IP_Address &p_address, int p_port) = 0;
+	Error listen(int p_port, const IP_Address &p_bind_address = IP_Address("*"), int p_recv_buffer_size = 65536);
+	void close();
+	Error wait();
+	bool is_listening() const;
+	IP_Address get_packet_address() const;
+	int get_packet_port() const;
+	void set_dest_address(const IP_Address &p_address, int p_port);
 
-	static Ref<PacketPeerUDP> create_ref();
-	static PacketPeerUDP *create();
+	Error put_packet(const uint8_t *p_buffer, int p_buffer_size);
+	Error get_packet(const uint8_t **r_buffer, int &r_buffer_size);
+	int get_available_packet_count() const;
+	int get_max_packet_size() const;
 
 	PacketPeerUDP();
+	~PacketPeerUDP();
 };
 
 #endif // PACKET_PEER_UDP_H
diff --git a/core/register_core_types.cpp b/core/register_core_types.cpp
index d93cad3f94..dcf7ee1de6 100644
--- a/core/register_core_types.cpp
+++ b/core/register_core_types.cpp
@@ -148,7 +148,7 @@ void register_core_types() {
 	ClassDB::register_class<StreamPeerBuffer>();
 	ClassDB::register_custom_instance_class<StreamPeerTCP>();
 	ClassDB::register_custom_instance_class<TCP_Server>();
-	ClassDB::register_custom_instance_class<PacketPeerUDP>();
+	ClassDB::register_class<PacketPeerUDP>();
 	ClassDB::register_custom_instance_class<StreamPeerSSL>();
 	ClassDB::register_virtual_class<IP>();
 	ClassDB::register_virtual_class<PacketPeer>();