New and improved handshake

6 files changed, 183 insertions, 79 deletions

diff --git a/src/fastd.c b/src/fastd.c
index bcd6bf6..cd1f1df 100644
--- a/src/fastd.c
+++ b/src/fastd.c
@@ -596,7 +596,7 @@ static void handle_tasks(fastd_context *ctx) {
 			if (task->peer->state != STATE_WAIT && task->peer->state != STATE_TEMP)
 				break;
 
-			pr_debug(ctx, "Sending handshake...");
+			pr_debug(ctx, "Sending handshake to %P...", task->peer);
 			fastd_handshake_send(ctx, task->peer);
 
 			if (task->peer->state == STATE_WAIT)
diff --git a/src/handshake.c b/src/handshake.c
index 991d994..e30ab9f 100644
--- a/src/handshake.c
+++ b/src/handshake.c
@@ -33,80 +33,189 @@
 #include <string.h>
 
 
+static const char const *RECORD_TYPES[RECORD_MAX] = {
+	"reply code",
+	"error detail",
+	"flags",
+	"protocol",
+	"method name",
+};
+
+static const char const *REPLY_TYPES[REPLY_MAX] = {
+	"success",
+	"mandatory field missing",
+	"unacceptable value",
+};
+
+
+static inline void handshake_add(fastd_context *ctx, fastd_buffer *buffer, fastd_handshake_record_type type, size_t len, const void *data) {
+	if ((uint8_t*)buffer->data + buffer->len + 2 + len > (uint8_t*)buffer->base + buffer->base_len)
+		exit_bug(ctx, "not enough buffer allocated for handshake");
+
+	uint8_t *dst = (uint8_t*)buffer->data + buffer->len;
+
+	dst[0] = type;
+	dst[1] = len;
+	memcpy(dst+2, data, len);
+
+	buffer->len += 2 + len;
+}
+
 void fastd_handshake_send(fastd_context *ctx, fastd_peer *peer) {
 	size_t method_len = strlen(ctx->conf->method->name);
-	size_t len = sizeof(fastd_packet_request)+method_len;
-	fastd_buffer buffer = fastd_buffer_alloc(len, 0, 0);
-	fastd_packet_request *request = buffer.data;
+	fastd_buffer buffer = fastd_buffer_alloc(sizeof(fastd_packet), 0,
+						 2+1 +           /* protocol */
+						 2+method_len    /* method name */
+						 );
+	fastd_packet *request = buffer.data;
 
 	request->reply = 0;
 	request->cp = 0;
 	request->req_id = ++peer->last_req_id;
 	request->rsv = 0;
-	request->flags = 0;
-	request->proto = ctx->conf->protocol;
-	request->method_len = method_len;
-	strncpy(request->method_name, ctx->conf->method->name, method_len);
+
+	uint8_t protocol = ctx->conf->protocol;
+	handshake_add(ctx, &buffer, RECORD_PROTOCOL, 1, &protocol);
+
+	handshake_add(ctx, &buffer, RECORD_METHOD_NAME, method_len, ctx->conf->method->name);
 
 	fastd_task_put_send_handshake(ctx, peer, buffer);
 }
 
+
+
 void fastd_handshake_handle(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer) {
-	if (buffer.len < sizeof(fastd_packet_any))
+	if (buffer.len < sizeof(fastd_packet)) {
+		pr_warn(ctx, "received an invalid handshake from %P", peer);
 		goto end_free;
+	}
 
 	fastd_packet *packet = buffer.base;
 
-	if (!packet->any.reply && !packet->any.cp) {
-		if (buffer.len < sizeof(fastd_packet_request))
-			goto end_free;
+	size_t lengths[RECORD_MAX];
+	void *records[RECORD_MAX] = { 0 };
 
-		if (buffer.len < sizeof(fastd_packet_request) + packet->request.method_len)
-			goto end_free;
+	uint8_t *ptr = packet->tlv_data;
+	while (true) {
+		if (ptr+2 > (uint8_t*)buffer.data + buffer.len)
+			break;
+
+		uint8_t type = ptr[0];
+		uint8_t len = ptr[1];
 
-		if (packet->request.flags)
-			goto end_free; // TODO
+		if (ptr+2+len > (uint8_t*)buffer.data + buffer.len)
+			break;
 
-		if (packet->request.proto != ctx->conf->protocol)
-			goto end_free; // TODO
+		lengths[type] = len;
+		records[type] = ptr+2;
 
-		if (packet->request.method_len != strlen(ctx->conf->method->name) ||
-		    strncmp(packet->request.method_name, ctx->conf->method->name, packet->request.method_len))
-			goto end_free; // TODO
+		ptr += 2+len;
+	}
 
-		fastd_buffer reply_buffer = fastd_buffer_alloc(sizeof(fastd_packet_reply), 0, 0);
-		fastd_packet_reply *reply = reply_buffer.data;
+	if (!packet->reply) {
+		fastd_buffer reply_buffer;
+		fastd_packet *reply;
+
+		uint8_t reply_code = REPLY_SUCCESS;
+		uint8_t error_detail = 0;
+
+		if (!records[RECORD_PROTOCOL]) {
+			reply_code = REPLY_MANDATORY_MISSING;
+			error_detail = RECORD_PROTOCOL;
+			goto send_reply;
+		}
+
+		if (lengths[RECORD_PROTOCOL] != 1 || *(uint8_t*)records[RECORD_PROTOCOL] != ctx->conf->protocol) {
+			reply_code = REPLY_UNACCEPTABLE_VALUE;
+			error_detail = RECORD_PROTOCOL;
+			goto send_reply;
+		}
+
+		if (!records[RECORD_METHOD_NAME]) {
+			reply_code = REPLY_MANDATORY_MISSING;
+			error_detail = RECORD_METHOD_NAME;
+			goto send_reply;
+		}
+
+		if (lengths[RECORD_METHOD_NAME] != strlen(ctx->conf->method->name)
+		    || strncmp((char*)records[RECORD_METHOD_NAME], ctx->conf->method->name, lengths[RECORD_METHOD_NAME])) {
+			reply_code = REPLY_UNACCEPTABLE_VALUE;
+			error_detail = RECORD_METHOD_NAME;
+			goto send_reply;
+		}
+
+	send_reply:
+		reply_buffer = fastd_buffer_alloc(sizeof(fastd_packet), 0, 6 /* enough space for reply code and error detail */);
+		reply = reply_buffer.data;
 
 		reply->reply = 1;
-		reply->cp = 0;
-		reply->req_id = packet->request.req_id;
+		reply->cp = packet->cp;
+		reply->req_id = packet->req_id;
 		reply->rsv = 0;
-		reply->reply_code = REPLY_SUCCESS;
+
+		handshake_add(ctx, &reply_buffer, RECORD_REPLY_CODE, 1, &reply_code);
+
+		if (reply_code)
+			handshake_add(ctx, &reply_buffer, RECORD_ERROR_DETAIL, 1, &error_detail);
 
 		fastd_task_put_send_handshake(ctx, peer, reply_buffer);
 	}
-	else if (packet->any.reply) {
-		if (buffer.len < sizeof(fastd_packet_reply))
-			goto end_free;
-
-		if (!packet->reply.cp) {
-			if (packet->reply.req_id != peer->last_req_id)
+	else {
+		if (!packet->cp) {
+			if (packet->req_id != peer->last_req_id) {
+				pr_warn(ctx, "received handshake reply with request ID %u from %P while %u was expected", packet->req_id, peer, peer->last_req_id);
 				goto end_free;
+			}
 		}
 		else {
-			goto end_free; // TODO
+			goto end_free; /* TODO */
 		}
 
-		switch (packet->reply.reply_code) {
+		if (!records[RECORD_REPLY_CODE] || lengths[RECORD_REPLY_CODE] != 1) {
+			pr_warn(ctx, "received handshake reply without reply code from %P", peer);
+			goto end_free;
+		}
+
+		uint8_t reply_code = *(uint8_t*)records[RECORD_REPLY_CODE];
+		uint8_t error_detail;
+		const char *error_field_str;
+
+		switch (reply_code) {
 		case REPLY_SUCCESS:
-			pr_info(ctx, "Handshake successful.");
-			pr_info(ctx, "Connection established.");
-			peer->state = STATE_ESTABLISHED;
+			pr_info(ctx, "Handshake with %P successful.", peer);
+			fastd_peer_set_established(ctx, peer);
 			ctx->conf->method->init(ctx, peer);
 			break;
 
 		default:
-			pr_warn(ctx, "Handshake failed with code %i.", packet->reply.reply_code);
+			if (reply_code >= REPLY_MAX) {
+				pr_warn(ctx, "Handshake with %P failed with unknown code %i", peer, reply_code);
+				break;
+			}
+
+			if (!records[RECORD_ERROR_DETAIL] || lengths[RECORD_ERROR_DETAIL] != 1) {
+				pr_warn(ctx, "Handshake with %P failed with code %s", peer, REPLY_TYPES[reply_code]);
+				break;
+			}
+
+			error_detail = *(uint8_t*)records[RECORD_ERROR_DETAIL];
+			if (error_detail >= RECORD_MAX)
+				error_field_str = "<unknown>";
+			else
+				error_field_str = RECORD_TYPES[error_detail];
+
+			switch (reply_code) {
+			case REPLY_MANDATORY_MISSING:
+				pr_warn(ctx, "Handshake with %P failed: mandatory field `%s' missing", peer, error_field_str);
+				break;
+
+			case REPLY_UNACCEPTABLE_VALUE:
+				pr_warn(ctx, "Handshake with %P failed: unacceptable value for field `%s'", peer, error_field_str);
+				break;
+
+			default: /* just to silence the warning */
+				break;
+			}
 		}
 	}
 
diff --git a/src/method_null.c b/src/method_null.c
index 8c144e8..62be501 100644
--- a/src/method_null.c
+++ b/src/method_null.c
@@ -51,20 +51,24 @@ static char* null_peer_str(const fastd_context *ctx, const fastd_peer *peer) {
 	char addr_buf[INET6_ADDRSTRLEN] = "";
 	char *ret;
 
+	const char *temp = fastd_peer_is_temporary(peer) ? " (temporary)" : "";
+
 	switch (peer->address.sa.sa_family) {
 	case AF_UNSPEC:
-		return strdup("<floating>");
+		if (asprintf(&ret, "<floating>%s", temp) > 0)
+			return ret;
+		break;
 
 	case AF_INET:
 		if (inet_ntop(AF_INET, &peer->address.in.sin_addr, addr_buf, sizeof(addr_buf))) {
-			if (asprintf(&ret, "%s:%u", addr_buf, ntohs(peer->address.in.sin_port)) > 0)
+			if (asprintf(&ret, "%s:%u%s", addr_buf, ntohs(peer->address.in.sin_port), temp) > 0)
 				return ret;
 		}
 		break;
 
 	case AF_INET6:
 		if (inet_ntop(AF_INET6, &peer->address.in6.sin6_addr, addr_buf, sizeof(addr_buf))) {
-			if (asprintf(&ret, "[%s]:%u", addr_buf, ntohs(peer->address.in6.sin6_port)) > 0)
+			if (asprintf(&ret, "[%s]:%u%s", addr_buf, ntohs(peer->address.in6.sin6_port), temp) > 0)
 				return ret;
 		}
 		break;
@@ -82,7 +86,6 @@ static void null_init(fastd_context *ctx, fastd_peer *peer) {
 
 static void null_handle_recv(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer) {
 	if (!fastd_peer_is_established(peer)) {
-		pr_info(ctx, "Connection established.");
 		fastd_peer_set_established(ctx, peer);
 	}
 
diff --git a/src/packet.h b/src/packet.h
index 12d15fc..9ae04f7 100644
--- a/src/packet.h
+++ b/src/packet.h
@@ -38,49 +38,38 @@ typedef enum _fastd_packet_type {
 	PACKET_DATA,
 } fastd_packet_type;
 
+typedef enum _fastd_handshake_record_type {
+	RECORD_REPLY_CODE = 0,
+	RECORD_ERROR_DETAIL,
+	RECORD_FLAGS,
+	RECORD_PROTOCOL,
+	RECORD_METHOD_NAME,
+	RECORD_MAX,
+} fastd_handshake_record_type;
+
 typedef enum _fastd_reply_code {
 	REPLY_SUCCESS = 0,
+	REPLY_MANDATORY_MISSING,
+	REPLY_UNACCEPTABLE_VALUE,
+	REPLY_MAX,
 } fastd_reply_code;
 
 
+typedef struct _fastd_packet {
 #if defined(__LITTLE_ENDIAN_BITFIELD)
-#define FASTD_PACKET_COMMON \
-	unsigned req_id : 6; \
-	unsigned cp     : 1; \
-	unsigned reply  : 1; \
-	uint8_t rsv
+	unsigned req_id : 6;
+	unsigned cp     : 1;
+	unsigned reply  : 1;
 #elif defined (__BIG_ENDIAN_BITFIELD)
-#define FASTD_PACKET_COMMON \
-	unsigned reply  : 1; \
-	unsigned cp     : 1; \
-	unsigned req_id : 6; \
-	uint8_t rsv
+	unsigned reply  : 1;
+	unsigned cp     : 1;
+	unsigned req_id : 6;
 #else
 #error "Bitfield endianess not defined."
 #endif
 
-
-typedef struct __attribute__ ((__packed__)) _fastd_packet_any {
-	FASTD_PACKET_COMMON;
-} fastd_packet_any;
-
-typedef struct __attribute__ ((__packed__)) _fastd_packet_request {
-	FASTD_PACKET_COMMON;
-	uint8_t flags;
-	uint8_t proto;
-	uint8_t method_len;
-	char    method_name[];
-} fastd_packet_request;
-
-typedef struct __attribute__ ((__packed__)) _fastd_packet_reply {
-	FASTD_PACKET_COMMON;
-	uint8_t reply_code;
-} fastd_packet_reply;
-
-typedef union _fastd_packet {
-	fastd_packet_any any;
-	fastd_packet_request request;
-	fastd_packet_reply reply;
+	uint16_t rsv;
+	uint8_t tlv_data[];
 } fastd_packet;
 
 #endif /* _FASTD_PACKET_H_ */
diff --git a/src/peer.c b/src/peer.c
index 5be79bb..0e53608 100644
--- a/src/peer.c
+++ b/src/peer.c
@@ -141,7 +141,7 @@ fastd_peer* fastd_peer_add_temp(fastd_context *ctx, const fastd_peer_address *ad
 	peer->state = STATE_TEMP;
 	peer->seen = ctx->now;
 
-	pr_debug(ctx, "adding peer %P (temporary)", peer);
+	pr_debug(ctx, "adding peer %P", peer);
 
 	return peer;
 }
@@ -230,7 +230,7 @@ void fastd_peer_eth_addr_add(fastd_context *ctx, fastd_peer *peer, const fastd_e
 	
 	ctx->eth_addr[min] = (fastd_peer_eth_addr){ *addr, peer, ctx->now };
 
-	pr_debug(ctx, "Learned new MAC address %E", addr);
+	pr_debug(ctx, "Learned new MAC address %E on peer %P", addr, peer);
 }
 
 void fastd_peer_eth_addr_cleanup(fastd_context *ctx) {
diff --git a/src/peer.h b/src/peer.h
index 8a1ca7e..4668243 100644
--- a/src/peer.h
+++ b/src/peer.h
@@ -81,15 +81,15 @@ static inline bool fastd_peer_config_is_floating(const fastd_peer_config *config
 	return (config->address.sa.sa_family == AF_UNSPEC);
 }
 
-static inline bool fastd_peer_is_floating(fastd_peer *peer) {
+static inline bool fastd_peer_is_floating(const fastd_peer *peer) {
 	return (peer->config && fastd_peer_config_is_floating(peer->config));
 }
 
-static inline bool fastd_peer_is_temporary(fastd_peer *peer) {
+static inline bool fastd_peer_is_temporary(const fastd_peer *peer) {
 	return (peer->state == STATE_TEMP || peer->state == STATE_TEMP_ESTABLISHED);
 }
 
-static inline bool fastd_peer_is_established(fastd_peer *peer) {
+static inline bool fastd_peer_is_established(const fastd_peer *peer) {
 	return (peer->state == STATE_ESTABLISHED || peer->state == STATE_TEMP_ESTABLISHED);
 }
 
@@ -105,7 +105,10 @@ static inline void fastd_peer_set_established(fastd_context *ctx, fastd_peer *pe
 
 	default:
 		pr_warn(ctx, "tried to set an already established connection to established");
+		return;
 	}
+
+	pr_info(ctx, "Connection with %P established.", peer);
 }
 
 static inline bool fastd_eth_addr_is_unicast(const fastd_eth_addr *addr) {