M1-nw-project/protocol.cpp

/***************************************************************************
 * By Théophile Bastian, 2017
 * M1 Network course project at ENS Cachan, Juliusz Chroboczek.
 * License: WTFPL v2 <http://www.wtfpl.net/>
 **************************************************************************/

#include "protocol.h"
#include <cstring>

using namespace std;

const size_t MAX_MTU = (1 << 16) + 42;

Protocol::Protocol(const SockAddr& listenAddr, u64 selfId) :
	sock(socket(PF_INET6, SOCK_DGRAM, 0)), listenAddr(listenAddr),
	selfId(selfId), terminating(false)
{
	int reuseVal = 1;
	if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseVal, sizeof(reuseVal)))
	{
		perror("Could not set SO_REUSEADDR for socket");
		throw NwError();
	}
	if(bind(sock, (struct sockaddr*)(&listenAddr), sizeof(listenAddr)) != 0) {
		perror("Cannot bind socket");
		throw NwError();
	}

	// Set socket reception timeout
	/*
	struct timeval tv;
	tv.tv_sec = 0;
	tv.tv_usec = 100000;
	if (setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) {
		perror("Cannot set socket timeout");
		throw NwError();
	}
	*/

	startPollNetwork();
}

Protocol::~Protocol() {
	terminating = true;
	pollThread.join();
}

void Protocol::addIdAddr(const SockAddr& addr, u64 id) {
	addrMap.insert({id, addr});
}

bool Protocol::readyRead() const {
	return !availPackets.empty();
}

Bytes Protocol::getPacket(SockAddr* from) {
	if(!readyRead())
		return Bytes();
	AvailPacket pck;
	{
		lock_guard<mutex>(this->availPacketsMutex);
		pck = availPackets.front();
		availPackets.pop();
	}
	if(from != NULL)
		*from = pck.from;
	return pck.data;
}

void Protocol::sendBody(const Bytes& body, const SockAddr& to) {
	Bytes pck;
	pck << csts::MAGIC << csts::VERSION << (u16)body.size() << selfId
		<< body;
	//TODO check size < MTU
	char buffer[MAX_MTU];
	pck.writeToBuffer(buffer, MAX_MTU);
	ssize_t rc =
		sendto(sock, buffer, pck.size(), 0, (struct sockaddr*)&to, sizeof(to));
	if(rc != (ssize_t)pck.size()) {
		//TODO log warning.
	}
	fprintf(stderr, "[INFO] sending packet to port %hu\n", ntohs(to.sin6_port));
}
void Protocol::sendBody(const Bytes& body, const u64& id) {
	return sendBody(body, addrOfId(id));
}

void Protocol::sendEmpty(const SockAddr& to) {
	sendBody(Bytes(), to);
}
void Protocol::sendEmpty(u64 to) {
	sendEmpty(addrOfId(to));
}

void Protocol::sendIHU(const SockAddr& to, u64 id) {
	Bytes pck;
	pck << csts::TLV_IHU << (u8) 8 << id;
	sendBody(pck, to);
}
void Protocol::sendIHU(u64 id) {
	sendIHU(addrOfId(id), id);
}

void Protocol::sendNReq(const SockAddr& to) {
	Bytes pck;
	pck << csts::TLV_NR << (u8) 0;
	sendBody(pck, to);
}
void Protocol::sendNReq(u64 id) {
	sendNReq(addrOfId(id));
}

void Protocol::sendNeighbours(const SockAddr& to,
				const std::vector<Neighbour>& neigh) {
	Bytes pck;
	u8 len = neigh.size() * (8+16+2);
	pck << csts::TLV_NEIGH << len;
	for(const Neighbour& nei : neigh) {
		u8 addr[16];
		memcpy(addr, nei.addr.sin6_addr.s6_addr, 16);
		pck << nei.id;
		for(int b=0; b < 16; b++)
			pck << addr[b];
		pck << (u16)nei.addr.sin6_port;
	}

	sendBody(pck, to);
}
void Protocol::sendNeighbours(u64 id, const std::vector<Neighbour>& neigh) {
	sendNeighbours(addrOfId(id), neigh);
}

void Protocol::sendData(const SockAddr& to, const char* data, size_t len,
		u32 seqno, u64 datId)
{
	Bytes pck;
	if(len + 12 >= 1<<8)
		throw DataTooLongError();
	pck << csts::TLV_DATA << (u8) (12 + len) << seqno << datId
		<< Bytes(data, len);
	sendBody(pck, to);
}
void Protocol::sendData(u64 id, const char* data, size_t len, u32 seqno,
		u64 datId)
{
	sendData(addrOfId(id), data, len, seqno, datId);
}

void Protocol::sendIHave(const SockAddr& to, u64 datId, u32 seqno) {
	Bytes pck;
	pck << csts::TLV_IHAVE << (u8) 12 << seqno << datId;
	sendBody(pck, to);
}
void Protocol::sendIHave(u64 id, u64 datId, u32 seqno) {
	sendIHave(addrOfId(id), datId, seqno);
}

void Protocol::startPollNetwork() {
	pollThread = std::thread([this] { this->pollNetwork(); });
}

void Protocol::pollNetwork() {
	u8 buffer[MAX_MTU];
	while(!terminating) {
		struct sockaddr fromAddr;
		memset(&fromAddr, 0, sizeof(fromAddr));
		socklen_t fromAddrLen=sizeof(fromAddr);
		ssize_t readDat = recvfrom(sock, buffer, MAX_MTU, 0,
				&fromAddr, &fromAddrLen);
		if(readDat < 0) {
			perror("[WARNING] Bad packet");
			continue;
		}
		if(readDat <= 0) {
			fprintf(stderr, "[WARNING] Empty packet.\n");
			continue;
		}
		Bytes data(buffer, readDat);
		u8 magic, version;
		data >> magic >> version;
		if(magic != csts::MAGIC) {
			fprintf(stderr, "[WARNING] Bad magic byte %u\n", magic);
			continue;
		}
		if(version != csts::VERSION) {
			fprintf(stderr, "[WARNING] Bad version %d\n", version);
			continue;
		}
		u16 bodyLen;
		data >> bodyLen;
		if(data.size() < bodyLen + 8u) {
			fprintf(stderr, "[WARNING] Body too short (%lu < %d)\n",
					data.size(), bodyLen+8u);
			continue;
		}
		else if(data.size() != bodyLen + 8u) {
			fprintf(stderr, "[WARNING] Body too long\n");
		}

		SockAddr convFromAddr;
		if(fromAddr.sa_family == AF_INET)
			convFromAddr = addrOfV4(*((struct sockaddr_in*)&fromAddr));
		else if(fromAddr.sa_family == AF_INET6)
			convFromAddr = *(SockAddr*)&fromAddr;
		else {
			fprintf(stderr, "[ERROR] Unknown address family %d.\n",
					fromAddr.sa_family);
			fprintf(stderr, "\t%hu\n", ((SockAddr*)&fromAddr)->sin6_port);
			while(data.size() > 0) {
				u8 c;
				data >> c;
				fprintf(stderr, "%02X|", c);
			}
			fprintf(stderr, "\n");
			continue;
		}

		puts("Received packet");
		AvailPacket pck;
		pck.from = convFromAddr;
		pck.data = data;
		{
			lock_guard<mutex>(this->availPacketsMutex);
			availPackets.push(pck);
		}
	}
}

const SockAddr& Protocol::addrOfId(u64 id) {
	try {
		return addrMap.at(id);
	} catch(std::out_of_range& e) {
		throw UnknownId(id);
	}
}

SockAddr Protocol::addrOfV4(const sockaddr_in& addrv4) {
	SockAddr out;
	memset(&out, 0, sizeof(SockAddr));
	out.sin6_family = AF_INET6;
	out.sin6_port = addrv4.sin_port;
	char addr6[17];
	inet_pton(AF_INET6, "::ffff:0:0", addr6);
	memcpy(addr6 + 12, &addrv4.sin_addr.s_addr, 4);
	return out;
}