1 files changed, 172 insertions, 21 deletions

diff --git a/node/Cluster.cpp b/node/Cluster.cpp
index bfa39d22..5b76d1f0 100644
--- a/node/Cluster.cpp
+++ b/node/Cluster.cpp
@@ -31,10 +31,13 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <math.h>
 
 #include <algorithm>
 #include <utility>
 
+#include "../version.h"
+
 #include "Cluster.hpp"
 #include "RuntimeEnvironment.hpp"
 #include "MulticastGroup.hpp"
@@ -42,22 +45,44 @@
 #include "Salsa20.hpp"
 #include "Poly1305.hpp"
 #include "Packet.hpp"
+#include "Identity.hpp"
 #include "Peer.hpp"
 #include "Switch.hpp"
 #include "Node.hpp"
 
 namespace ZeroTier {
 
-Cluster::Cluster(const RuntimeEnvironment *renv,uint16_t id,DistanceAlgorithm da,int32_t x,int32_t y,int32_t z,void (*sendFunction)(void *,uint16_t,const void *,unsigned int),void *arg) :
+static inline double _dist3d(int x1,int y1,int z1,int x2,int y2,int z2)
+	throw()
+{
+	double dx = ((double)x2 - (double)x1);
+	double dy = ((double)y2 - (double)y1);
+	double dz = ((double)z2 - (double)z1);
+	return sqrt((dx * dx) + (dy * dy) + (dz * dz));
+}
+
+Cluster::Cluster(
+	const RuntimeEnvironment *renv,
+	uint16_t id,
+	const std::vector<InetAddress> &zeroTierPhysicalEndpoints,
+	int32_t x,
+	int32_t y,
+	int32_t z,
+	void (*sendFunction)(void *,unsigned int,const void *,unsigned int),
+	void *sendFunctionArg,
+	int (*addressToLocationFunction)(void *,const struct sockaddr_storage *,int *,int *,int *),
+	void *addressToLocationFunctionArg) :
 	RR(renv),
 	_sendFunction(sendFunction),
-	_arg(arg),
+	_sendFunctionArg(sendFunctionArg),
+	_addressToLocationFunction(addressToLocationFunction),
+	_addressToLocationFunctionArg(addressToLocationFunctionArg),
 	_x(x),
 	_y(y),
 	_z(z),
-	_da(da),
 	_id(id),
-	_members(new _Member[65536])
+	_zeroTierPhysicalEndpoints(zeroTierPhysicalEndpoints),
+	_members(new _Member[ZT_CLUSTER_MAX_MEMBERS])
 {
 	uint16_t stmp[ZT_SHA512_DIGEST_LEN / sizeof(uint16_t)];
 
@@ -114,16 +139,20 @@ void Cluster::handleIncomingStateMessage(const void *msg,unsigned int len)
 		s20.decrypt12(reinterpret_cast<const char *>(msg) + 24,const_cast<void *>(dmsg.data()),dmsg.size());
 	}
 
-	if (dmsg.size() < 2)
+	if (dmsg.size() < 4)
 		return;
 	const uint16_t fromMemberId = dmsg.at<uint16_t>(0);
 	unsigned int ptr = 2;
+	if (fromMemberId == _id)
+		return;
+	const uint16_t toMemberId = dmsg.at<uint16_t>(ptr);
+	ptr += 2;
+	if (toMemberId != _id)
+		return;
 
 	_Member &m = _members[fromMemberId];
 	Mutex::Lock mlck(m.lock);
 
-	m.lastReceivedFrom = RR->node->now();
-
 	try {
 		while (ptr < dmsg.size()) {
 			const unsigned int mlen = dmsg.at<uint16_t>(ptr); ptr += 2;
@@ -143,11 +172,13 @@ void Cluster::handleIncomingStateMessage(const void *msg,unsigned int len)
 						ptr += 8; // skip local clock, not used
 						m.load = dmsg.at<uint64_t>(ptr); ptr += 8;
 						ptr += 8; // skip flags, unused
-						m.physicalAddressCount = dmsg[ptr++];
-						if (m.physicalAddressCount > ZT_CLUSTER_MEMBER_MAX_PHYSICAL_ADDRS)
-							m.physicalAddressCount = ZT_CLUSTER_MEMBER_MAX_PHYSICAL_ADDRS;
-						for(unsigned int i=0;i<m.physicalAddressCount;++i)
-							ptr += m.physicalAddresses[i].deserialize(dmsg,ptr);
+						unsigned int physicalAddressCount = dmsg[ptr++];
+						for(unsigned int i=0;i<physicalAddressCount;++i) {
+							m.zeroTierPhysicalEndpoints.push_back(InetAddress());
+							ptr += m.zeroTierPhysicalEndpoints.back().deserialize(dmsg,ptr);
+							if (!(m.zeroTierPhysicalEndpoints.back()))
+								m.zeroTierPhysicalEndpoints.pop_back();
+						}
 						m.lastReceivedAliveAnnouncement = RR->node->now();
 					}	break;
 
@@ -298,7 +329,7 @@ void Cluster::handleIncomingStateMessage(const void *msg,unsigned int len)
 	}
 }
 
-void Cluster::replicateHavePeer(const Address &peerAddress)
+void Cluster::replicateHavePeer(const Identity &peerId)
 {
 }
 
@@ -312,23 +343,59 @@ void Cluster::replicateCertificateOfNetworkMembership(const CertificateOfMembers
 
 void Cluster::doPeriodicTasks()
 {
-	// Go ahead and flush whenever possible right now
+	const uint64_t now = RR->node->now();
+
 	{
 		Mutex::Lock _l(_memberIds_m);
 		for(std::vector<uint16_t>::const_iterator mid(_memberIds.begin());mid!=_memberIds.end();++mid) {
 			Mutex::Lock _l2(_members[*mid].lock);
-			_flush(*mid);
+
+			if ((now - _members[*mid].lastAnnouncedAliveTo) >= ((ZT_CLUSTER_TIMEOUT / 2) - 1000)) {
+				Buffer<2048> alive;
+				alive.append((uint16_t)ZEROTIER_ONE_VERSION_MAJOR);
+				alive.append((uint16_t)ZEROTIER_ONE_VERSION_MINOR);
+				alive.append((uint16_t)ZEROTIER_ONE_VERSION_REVISION);
+				alive.append((uint8_t)ZT_PROTO_VERSION);
+				if (_addressToLocationFunction) {
+					alive.append((int32_t)_x);
+					alive.append((int32_t)_y);
+					alive.append((int32_t)_z);
+				} else {
+					alive.append((int32_t)0);
+					alive.append((int32_t)0);
+					alive.append((int32_t)0);
+				}
+				alive.append((uint64_t)now);
+				alive.append((uint64_t)0); // TODO: compute and send load average
+				alive.append((uint64_t)0); // unused/reserved flags
+				alive.append((uint8_t)_zeroTierPhysicalEndpoints.size());
+				for(std::vector<InetAddress>::const_iterator pe(_zeroTierPhysicalEndpoints.begin());pe!=_zeroTierPhysicalEndpoints.end();++pe)
+					pe->serialize(alive);
+				_send(*mid,alive.data(),alive.size());
+				_members[*mid].lastAnnouncedAliveTo = now;
+			}
+
+			_flush(*mid); // does nothing if nothing to flush
 		}
 	}
 }
 
 void Cluster::addMember(uint16_t memberId)
 {
+	if (memberId >= ZT_CLUSTER_MAX_MEMBERS)
+		return;
+
 	Mutex::Lock _l2(_members[memberId].lock);
 
-	Mutex::Lock _l(_memberIds_m);
-	_memberIds.push_back(memberId);
-	std::sort(_memberIds.begin(),_memberIds.end());
+	{
+		Mutex::Lock _l(_memberIds_m);
+		if (std::find(_memberIds.begin(),_memberIds.end(),memberId) != _memberIds.end())
+			return;
+		_memberIds.push_back(memberId);
+		std::sort(_memberIds.begin(),_memberIds.end());
+	}
+
+	_members[memberId].clear();
 
 	// Generate this member's message key from the master and its ID
 	uint16_t stmp[ZT_SHA512_DIGEST_LEN / sizeof(uint16_t)];
@@ -346,6 +413,89 @@ void Cluster::addMember(uint16_t memberId)
 	_members[memberId].q.append(iv,16);
 	_members[memberId].q.addSize(8); // room for MAC
 	_members[memberId].q.append((uint16_t)_id);
+	_members[memberId].q.append((uint16_t)memberId);
+}
+
+void Cluster::removeMember(uint16_t memberId)
+{
+	Mutex::Lock _l(_memberIds_m);
+	std::vector<uint16_t> newMemberIds;
+	for(std::vector<uint16_t>::const_iterator mid(_memberIds.begin());mid!=_memberIds.end();++mid) {
+		if (*mid != memberId)
+			newMemberIds.push_back(*mid);
+	}
+	_memberIds = newMemberIds;
+}
+
+bool Cluster::redirectPeer(const SharedPtr<Peer> &peer,const InetAddress &peerPhysicalAddress,bool offload)
+{
+	if (!peerPhysicalAddress) // sanity check
+		return false;
+	if (_addressToLocationFunction) {
+		// Pick based on location if it can be determined
+		int px = 0,py = 0,pz = 0;
+		if (_addressToLocationFunction(_addressToLocationFunctionArg,reinterpret_cast<const struct sockaddr_storage *>(&peerPhysicalAddress),&px,&py,&pz) == 0) {
+			// No geo-info so no change
+			return false;
+		}
+
+		// Find member closest to this peer
+		const uint64_t now = RR->node->now();
+		std::vector<InetAddress> best; // initial "best" is for peer to stay put
+		const double currentDistance = _dist3d(_x,_y,_z,px,py,pz);
+		double bestDistance = (offload ? 2147483648.0 : currentDistance);
+		unsigned int bestMember = _id;
+		{
+			Mutex::Lock _l(_memberIds_m);
+			for(std::vector<uint16_t>::const_iterator mid(_memberIds.begin());mid!=_memberIds.end();++mid) {
+				_Member &m = _members[*mid];
+				Mutex::Lock _ml(m.lock);
+
+				// Consider member if it's alive and has sent us a location and one or more physical endpoints to send peers to
+				if ( ((now - m.lastReceivedAliveAnnouncement) < ZT_CLUSTER_TIMEOUT) && ((m.x != 0)||(m.y != 0)||(m.z != 0)) && (m.zeroTierPhysicalEndpoints.size() > 0) ) {
+					double mdist = _dist3d(m.x,m.y,m.z,px,py,pz);
+					if (mdist < bestDistance) {
+						bestMember = *mid;
+						best = m.zeroTierPhysicalEndpoints;
+					}
+				}
+			}
+		}
+
+		if (best.size() > 0) {
+			TRACE("peer %s is at [%d,%d,%d], distance to us is %f, sending to %u instead for better distance %f",peer->address().toString().c_str(),px,py,pz,currentDistance,bestMember,bestDistance);
+
+			/* if (peer->remoteVersionProtocol() >= 5) {
+				// If it's a newer peer send VERB_PUSH_DIRECT_PATHS which is more idiomatic
+			} else { */
+				// Otherwise send VERB_RENDEZVOUS for ourselves, which will trick peers into trying other endpoints for us even if they're too old for PUSH_DIRECT_PATHS
+				for(std::vector<InetAddress>::const_iterator a(best.begin());a!=best.end();++a) {
+					if ((a->ss_family == AF_INET)||(a->ss_family == AF_INET6)) {
+						Packet outp(peer->address(),RR->identity.address(),Packet::VERB_RENDEZVOUS);
+						outp.append((uint8_t)0); // no flags
+						RR->identity.address().appendTo(outp); // HACK: rendezvous with ourselves! with really old peers this will only work if I'm a root server!
+						outp.append((uint16_t)a->port());
+						if (a->ss_family == AF_INET) {
+							outp.append((uint8_t)4);
+							outp.append(a->rawIpData(),4);
+						} else {
+							outp.append((uint8_t)16);
+							outp.append(a->rawIpData(),16);
+						}
+						RR->sw->send(outp,true,0);
+					}
+				}
+			//}
+
+			return true;
+		} else {
+			TRACE("peer %s is at [%d,%d,%d], distance to us is %f and this seems to be the best",peer->address().toString().c_str(),px,py,pz,currentDistance);
+			return false;
+		}
+	} else {
+		// TODO: pick based on load if no location info?
+		return false;
+	}
 }
 
 void Cluster::_send(uint16_t memberId,const void *msg,unsigned int len)
@@ -366,7 +516,7 @@ void Cluster::_flush(uint16_t memberId)
 {
 	_Member &m = _members[memberId];
 	// assumes m.lock is locked!
-	if (m.q.size() > 26) { // 16-byte IV + 8-byte MAC + 2-byte cluster member ID (latter two bytes are inside crypto envelope)
+	if (m.q.size() > (24 + 2 + 2)) { // 16-byte IV + 8-byte MAC + 2 byte from-member-ID + 2 byte to-member-ID
 		// Create key from member's key and IV
 		char keytmp[32];
 		memcpy(keytmp,m.key,32);
@@ -389,7 +539,7 @@ void Cluster::_flush(uint16_t memberId)
 		memcpy(m.q.field(16,8),mac,8);
 
 		// Send!
-		_sendFunction(_arg,memberId,m.q.data(),m.q.size());
+		_sendFunction(_sendFunctionArg,memberId,m.q.data(),m.q.size());
 
 		// Prepare for more
 		m.q.clear();
@@ -397,7 +547,8 @@ void Cluster::_flush(uint16_t memberId)
 		Utils::getSecureRandom(iv,16);
 		m.q.append(iv,16);
 		m.q.addSize(8); // room for MAC
-		m.q.append((uint16_t)_id);
+		m.q.append((uint16_t)_id); // from member ID
+		m.q.append((uint16_t)memberId); // to member ID
 	}
 }