Add ping support

9 files changed, 464 insertions, 96 deletions

diff --git a/src/benc.c b/src/benc.c
index eef8469..1e85e06 100644
--- a/src/benc.c
+++ b/src/benc.c
@@ -49,19 +49,20 @@ void indent(int depth) {
 	}
 }
 
-void benc_print(const struct benc_node* stream, size_t stream_len, int* depth) {
+void benc_print(const struct benc_node* stream, size_t stream_len) {
+	int depth;
 	const struct benc_node* cursor = stream;
 	for(; cursor < stream + stream_len; cursor++) {
 		switch(cursor->type) {
 			case BNT_INT:
-				indent(*depth);
+				indent(depth);
 				printf("INT %.*s\n", cursor->size, cursor->loc);
 				break;
 			case BNT_STRING:
-				indent(*depth);
+				indent(depth);
 				bool allprint = true;
 				for (const char* c = cursor->loc; c < cursor->loc + cursor->size; c++) {
-					if(!isalnum(*c)) {
+					if(!isalnum(*c) && *c != '_') {
 						allprint = false;
 						break;
 					}
@@ -77,18 +78,18 @@ void benc_print(const struct benc_node* stream, size_t stream_len, int* depth) {
 				printf("\n");
 				break;
 			case BNT_LIST:
-				indent(*depth);
+				indent(depth);
 				printf("LIST\n");
-				*depth = cursor->depth + 1;
+				depth = cursor->depth + 1;
 				break;
 			case BNT_DICT:
-				indent(*depth);
+				indent(depth);
 				printf("DICT\n");
-				*depth = cursor->depth + 1;
+				depth = cursor->depth + 1;
 				break;
 			case BNT_END:
-				*depth = cursor->depth;
-				indent(*depth);
+				depth = cursor->depth;
+				indent(depth);
 				printf("END\n");
 				break;
 		}
@@ -105,16 +106,16 @@ int64_t benc_decode(const char** cursor, const char* end, int* depth, struct ben
 			node->type = BNT_INT;
 			(*cursor)++;
 			if(*cursor >= end) {
-				return -cursor_out;
+				return -BENC_EBADP;
 			}
 			node->loc = *cursor;
 			while(**cursor != 'e') {
 				if(**cursor != '-' && !digit(**cursor)) {
-					return -cursor_out;
+					return -BENC_EBADP;
 				}
 				(*cursor)++;
 				if(*cursor >= end) {
-					return -cursor_out;
+					return -BENC_EBADP;
 				}
 			}
 			node->size = *cursor - node->loc;
@@ -126,7 +127,7 @@ int64_t benc_decode(const char** cursor, const char* end, int* depth, struct ben
 			node->loc = *cursor;
 			(*cursor)++;
 			if(*cursor >= end) {
-				return -cursor_out;
+				return -BENC_EBADP;
 			}
 		} else if(**cursor == 'd') {
 			node->type = BNT_DICT;
@@ -135,7 +136,7 @@ int64_t benc_decode(const char** cursor, const char* end, int* depth, struct ben
 			node->loc = *cursor;
 			(*cursor)++;
 			if(*cursor >= end) {
-				return -cursor_out;
+				return -BENC_EBADP;
 			}
 		} else if(**cursor == 'e') {
 			node->type = BNT_END;
@@ -149,23 +150,23 @@ int64_t benc_decode(const char** cursor, const char* end, int* depth, struct ben
 			bool rc = readint(cursor, &val);
 			node->size = val;
 			if(*cursor >= end) {
-				return -cursor_out;
+				return -BENC_EBADP;
 			}
 			assert(rc);
 			if(**cursor != ':') {
-				return -cursor_out;
+				return -BENC_EBADP;
 			}
 			(*cursor)++;
 			if(*cursor >= end) {
-				return -cursor_out;
+				return -BENC_EBADP;
 			}
 			node->loc = *cursor;
 			(*cursor) += node->size;
 			if(*cursor > end) {
-				return -cursor_out;
+				return -BENC_EBADP;
 			}
 		} else {
-			return -cursor_out;
+			return -BENC_EBADP;
 			dbg("Failing on char \"%c\"", **cursor);
 			assert(false);
 		}
@@ -175,12 +176,24 @@ int64_t benc_decode(const char** cursor, const char* end, int* depth, struct ben
 	return cursor_out;
 }
 
-int bcur_fill(struct bcursor* cursor) {
+int bcur_fill(struct bcursor* cursor, size_t ignoring) {
 	if(cursor->source == cursor->source_end) {
 		return EOF;
 	}
 
-	int read = benc_decode(&cursor->source, cursor->source_end, &cursor->source_depth, cursor->base, cursor->base_len);
+	int read;
+	while(true) {
+		read = benc_decode(&cursor->source, cursor->source_end, &cursor->source_depth, cursor->base, cursor->base_len);
+		if(read < 0) {
+			return EINVAL;
+		}
+		if(read > ignoring) {
+			break;
+		}
+		ignoring -= read;
+	}
+
+	cursor->readhead = cursor->base + ignoring;
 	cursor->end = cursor->base + read;
 
 	return 0;
@@ -195,16 +208,16 @@ int bcur_open(struct bcursor* cursor, const char* source, const char* source_end
 	*((size_t*)&cursor->base_len) = buffer_len;
 	cursor->readhead = buffer;
 
-	return bcur_fill(cursor);
+	return bcur_fill(cursor, 0);
 }
 
 int bcur_next(struct bcursor* cursor, uint32_t steps) {
-	cursor->readhead+=steps;
+	cursor->readhead += steps;
 
-	// Check if we need to read more
 	if(cursor->readhead >= cursor->end) {
-		return bcur_fill(cursor);
+		return bcur_fill(cursor, cursor->readhead - cursor->end);
 	}
+
 	return 0;
 }
 
diff --git a/src/benc.h b/src/benc.h
index be06d5e..e3ca026 100644
--- a/src/benc.h
+++ b/src/benc.h
@@ -5,6 +5,8 @@
 #include <stdbool.h>
 #include <unistd.h>
 
+#define BENC_EBADP 1
+
 enum benc_nodetype {
 	BNT_INT,
 	BNT_STRING,
@@ -24,7 +26,7 @@ struct benc_node {
 
 bool readint(const char** loc, int64_t* val);
 
-void benc_print(const struct benc_node* stream, size_t stream_len, int* depth);
+void benc_print(const struct benc_node* stream, size_t stream_len);
 int64_t benc_decode(const char** cursor, const char* end, int* depth, struct benc_node* stream, size_t stream_len);
 
 struct bcursor {
diff --git a/src/log.h b/src/log.h
index 12135af..874e1bc 100644
--- a/src/log.h
+++ b/src/log.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <stdio.h>
+#include <stdlib.h>
 
 #define dbg(format, ...) \
 	dbgl(format "\n", ##  __VA_ARGS__)
diff --git a/src/main.c b/src/main.c
index 5de849a..297716e 100644
--- a/src/main.c
+++ b/src/main.c
@@ -1,5 +1,6 @@
 #include "routing.h"
 #include "benc.h"
+#include "query.h"
 #include "log.h"
 
 #include <errno.h>
@@ -108,7 +109,21 @@ bool find_req(uint32_t transId, uint16_t* reqId) {
 
 void getclient_response(struct nodeid* self, char* packet, size_t packet_len, int socket, struct sockaddr_in* remote, socklen_t remote_len);
 
+uint8_t rand_byte() {
+	int limit = RAND_MAX - (RAND_MAX % UINT8_MAX);
+	int val;
+	while((val = rand()) > limit);
+
+	return val;
+}
+
 int send_ping(struct nodeid* self, const int sfd, const struct sockaddr* dest_addr, socklen_t dest_len) {
+	// Generate a random target
+	struct nodeid target;
+	for(uint8_t *target_byte = (uint8_t*)&target; target_byte < ((uint8_t*)&target)+sizeof(target); target_byte++) {
+		*target_byte = rand_byte();
+	}
+
 	uint16_t reqId;
 	if(!alloc_req(&reqId)) {
 		return ENOBUFS;
@@ -121,19 +136,25 @@ int send_ping(struct nodeid* self, const int sfd, const struct sockaddr* dest_ad
 	requestdata[reqId].addr = *dest_addr;
 
 	int rc = snprintf(buff+i, 128-i, "d1:ad2:id20:");
+	if(rc < 0)
+		return EPERM;
 	i += rc;
 	memcpy(buff+i, self, sizeof(struct nodeid));
 	i += sizeof(struct nodeid);
-	rc = snprintf(buff+i, 128-i, "6:target20:mnopqrstuvwxyz123456e1:q9:find_node1:t%d:%d1:y1:qe", (reqId/10)+1, reqId);
+	rc = snprintf(buff+i, 128-i, "6:target20:");
+	if(rc < 0)
+		return EPERM;
+	i += rc;
+	memcpy(buff+i, &target, sizeof(struct nodeid));
+	i += sizeof(struct nodeid);
+	rc = snprintf(buff+i, 128-i, "e1:q9:find_node1:t%d:%d1:y1:qe", (reqId/10)+1, reqId);
+	if(rc < 0)
+		return EPERM;
 	i += rc;
 
 	//now reply the client with the same data
 	rc = sendto(sfd, buff, i, 0, dest_addr, dest_len);
 	if (rc == -1) {
-		switch(errno) {
-			case EWOULDBLOCK: case EBADF:
-				return rc;
-		}
 		return EPERM; // Operation not permitted is used as the default "generic" error
 	}
 
@@ -142,11 +163,10 @@ int send_ping(struct nodeid* self, const int sfd, const struct sockaddr* dest_ad
 
 void getclient_response(struct nodeid* self, char* packet, size_t packet_len, int socket, struct sockaddr_in* remote, socklen_t remote_len) {
 	struct benc_node stream[256];
-	const char* cursor = packet;
-	int depth = 0;
-	int len = benc_decode(&cursor, packet+packet_len, &depth, stream, 256);
+	struct bcursor bcursor;
+	bcur_open(&bcursor, packet, packet+packet_len, stream, 256);
 
-	if(len <= 0) {
+	if(bcursor.end - bcursor.readhead <= 0) {
 		err("Reponse too short");
 		exit(EXIT_FAILURE);
 	}
@@ -161,73 +181,71 @@ void getclient_response(struct nodeid* self, char* packet, size_t packet_len, in
 	// Read the payload
 	{
 		// Check that we have a dict
-		struct benc_node* cursor = stream;
-		if(cursor->type != BNT_DICT) {
+		if(bcursor.readhead->type != BNT_DICT) {
 			err("Response is not a dict");
 			exit(EXIT_FAILURE);
 		}
-		cursor++;
+		bcur_next(&bcursor, 1);
 
-		skip_to_key((const struct benc_node**)&cursor, stream+len, (const enum benc_nodetype[]){BNT_STRING}, (const char*[]){"r"}, (const size_t[]){1}, 1);
+		bcur_find_key(&bcursor, (const enum benc_nodetype[]){BNT_STRING}, (const char*[]){"r"}, (const size_t[]){1}, 1);
 		// Skip the key
-		cursor++;
+		bcur_next(&bcursor, 1);
 
-		if(cursor->type != BNT_DICT) {
+		if(bcursor.readhead->type != BNT_DICT) {
 			err("Wrong value type for response");
 			exit(EXIT_FAILURE);
 		}
 
 		// Skip the dict element
-		cursor++;
+		bcur_next(&bcursor, 1);
 
-		while(cursor->type != BNT_END) {
-			switch(skip_to_key((const struct benc_node**)&cursor, stream+len, (const enum benc_nodetype[]){BNT_STRING, BNT_STRING}, (const char*[]){"nodes", "id"}, (const size_t[]){5, 2}, 2)) {
+		while(bcursor.readhead->type != BNT_END) {
+			switch(bcur_find_key(&bcursor, (const enum benc_nodetype[]){BNT_STRING, BNT_STRING}, (const char*[]){"nodes", "id"}, (const size_t[]){5, 2}, 2)) {
 				case 0:
 					// Skip the key
-					cursor++;
+					bcur_next(&bcursor, 1);
 
-					if(cursor->type != BNT_STRING) {
+					if(bcursor.readhead->type != BNT_STRING) {
 						err("Wrong value type for response");
 						exit(EXIT_FAILURE);
 					}
 
-					if((cursor->size % 26) != 0) {
+					if((bcursor.readhead->size % 26) != 0) {
 						err("get_nodes call returned an incorrect nodes array");
 						exit(EXIT_FAILURE);
 					}
 
-					nodes_len = MIN(cursor->size/26, 8);
-					dbg("We have %d (%d/26) nodes", nodes_len, cursor->size);
+					nodes_len = MIN(bcursor.readhead->size/26, 8);
+					dbg("We have %d (%d/26) nodes", nodes_len, bcursor.readhead->size);
 					for(int i = 0; i < nodes_len; i++) {
-						memcpy(nodes+i, cursor->loc+(26*i), 20);
-						memcpy(ips+i, cursor->loc+(26*i)+20, 4);
-						memcpy(ports+i, cursor->loc+(26*i)+24, 2);
+						memcpy(nodes+i, bcursor.readhead->loc+(26*i), 20);
+						memcpy(ips+i, bcursor.readhead->loc+(26*i)+20, 4);
+						memcpy(ports+i, bcursor.readhead->loc+(26*i)+24, 2);
 					}
 
 					// Skip the value
-					cursor++;
+					bcur_next(&bcursor, 1);
 					break;
 				case 1:
 					// Skip the key
-					cursor++;
+					bcur_next(&bcursor, 1);
 
-					if(cursor->type != BNT_STRING) {
+					if(bcursor.readhead->type != BNT_STRING) {
 						err("Wrong value type for response");
 						exit(EXIT_FAILURE);
 					}
 
-					if(cursor->size != 20) {
+					if(bcursor.readhead->size != 20) {
 						err("remote node id was not 20 bytes long");
 						exit(EXIT_FAILURE);
 					}
 
-					memcpy(&id, cursor->loc, 20);
+					memcpy(&id, bcursor.readhead->loc, 20);
 
 					// Skip the value
-					cursor++;
+					bcur_next(&bcursor, 1);
 					break;
 			}
-			assert(cursor < stream+len);
 		}
 	}
 
@@ -329,53 +347,128 @@ int main(int argc, char** argv) {
 
 		printf("Received packet from %s:%d\n", inet_ntoa(remote.sin_addr), ntohs(remote.sin_port));
 		
+		struct bcursor bcursor;
 		struct benc_node stream[256];
-		const char* cursor = buff;
-		int depth = 0;
-		int len = benc_decode(&cursor, buff+recv_len, &depth, stream, 256);
-		benc_print(stream, len, &depth);
+		bcur_open(&bcursor, buff, buff+recv_len, stream, 256);
+		benc_print(bcursor.readhead, bcursor.end - bcursor.readhead);
 
-		struct benc_node* stream_cursor = stream;
-		if(stream_cursor->type != BNT_DICT) {
+		if(bcursor.readhead->type != BNT_DICT) {
 			fatal("First value is not a dict");
 		}
-		stream_cursor++;
+		bcur_next(&bcursor, 1);
+
+		bool discard = false;
+		bool response;
+		bool transaction_set = false;
+		char transaction[64];
+		size_t transaction_len;
+		bool query_set = false;
+		char query[64];
+		size_t query_len;
+		while(bcursor.readhead->type != BNT_END) {
+			switch(bcur_find_key(&bcursor, (const enum benc_nodetype[]){BNT_STRING, BNT_STRING, BNT_STRING}, (const char*[]){"y", "t", "e"}, (const size_t[]){1, 1, 1}, 3)) {
+				case 0:
+					// Skip the key
+					bcur_next(&bcursor, 1);
+					response = *bcursor.readhead->loc=='r';
+					// Skip the value
+					bcur_next(&bcursor, 1);
+					break;
+				case 1: {
+					// Skip the key
+					bcur_next(&bcursor, 1);
+					if(bcursor.readhead->size > 64-1)
+						fatal("Transaction string too long");
+
+					transaction_set = true;
+					transaction_len = bcursor.readhead->size;
+					memcpy(transaction, bcursor.readhead->loc, transaction_len);
+					transaction[transaction_len] = '\0';
 
-		if(skip_to_key((const struct benc_node**)&stream_cursor, stream+len, (const enum benc_nodetype[]){BNT_STRING}, (const char*[]){"t"}, (const size_t[]){1}, 1) == -1) {
-			fatal("No t key in packet");
+					// Skip the value
+					bcur_next(&bcursor, 1);
+					break;
+				}
+				case 3: {
+					bcur_next(&bcursor, 1);
+					query_set = true;
+					query_len = bcursor.readhead->size;
+					memcpy(query, bcursor.readhead->loc, query_len);
+					query[query_len] = '\0';
+					bcur_next(&bcursor, 1);
+				}
+			}
 		}
-		stream_cursor++;
 
-		uint32_t transaction;
-		{
+		if(discard)
+			continue;
+
+		if(response) {
+			uint32_t transaction_number;
+
+			if(!transaction_set)
+				fatal("No transaction in response");
+
 			// Temporary null terminate the string to parse the number without a copy
-			char char_buffer = stream_cursor->loc[stream_cursor->size];
-			((char*)stream_cursor->loc)[stream_cursor->size] = '\0';
 			char* end;
-			transaction = strtol(stream_cursor->loc, &end, 10);
-			((char*)stream_cursor->loc)[stream_cursor->size] = char_buffer;
+			transaction_number = strtol(transaction, &end, 10);
 
-			if(end != stream_cursor->loc+stream_cursor->size) {
-				dbg("DISCARD: Transaction id is not a number %.*s", stream_cursor->size, stream->loc);
-				continue;
+			if(end != transaction+transaction_len) {
+				fatal("DISCARD: Transaction id is not a number %.*s", transaction_len, transaction);
 			}
-		}
 
-		uint16_t reqId;
-		if(!find_req(transaction, &reqId)) {
-			dbg("DISCARD: unknown transaction id %d", transaction);
-			continue;
-		}
-		dbg("Transaction id matches request %d", reqId);
+			uint16_t reqId;
+			if(!find_req(transaction_number, &reqId)) {
+				dbg("DISCARD: unknown transaction id %d", transaction);
+				continue;
+			}
+			dbg("Transaction id matches request %d", reqId);
 
-		if(sockaddr_cmp(&requestdata[reqId].addr, (struct sockaddr*)&remote) != 0) {
-			err("Unexpected IP for valid transaction");
-			exit(EXIT_FAILURE);
-		}
+			if(sockaddr_cmp(&requestdata[reqId].addr, (struct sockaddr*)&remote) != 0) {
+				fatal("Unexpected IP for valid transaction");
+			}
 
-		requestdata[reqId].fun(&self, buff, recv_len, sfd, &remote, remote_len);
+			requestdata[reqId].fun(&self, buff, recv_len, sfd, &remote, remote_len);
+
+			reqalloc[reqId] = false;
+		} else { // Must be a query
+			if(!query_set)
+				fatal("No query function in query request");
+			if(!transaction_set)
+				fatal("No transaction in request");
+
+			assert(strlen(query) == query_len);
+
+			char response[1024];
+			char* end = response+sizeof(response)-1;
+			char* cursor = response;
+
+			int rc = snprintf(cursor, end-cursor , "d1:t%ld:", transaction_len);
+			if(rc < 0)
+				return EPERM;
+			cursor += rc;
+			memcpy(cursor, transaction, transaction_len);
+			cursor += transaction_len;
+			rc = snprintf(cursor, end-cursor, "1:y1:r1:r");
+			if(rc < 0)
+				return EPERM;
+			cursor += rc;
+
+			rc = handle_request(&self, query, buff, recv_len, &cursor, end-cursor-1);
+			if(rc != 0) fatal("Error handling request");
+
+			rc = snprintf(cursor, end-cursor, "e");
+			if(rc < 0)
+				return EPERM;
+			cursor += rc;
+
+			//now reply the client with the same data
+			rc = sendto(sfd, buff, cursor-response, 0, (const struct sockaddr*)&remote, remote_len);
+			if (rc == -1) {
+				return EPERM; // Operation not permitted is used as the default "generic" error
+			}
 
-		reqalloc[reqId] = false;
+		}
 	}
 
 	close(sfd);
diff --git a/src/query.c b/src/query.c
new file mode 100644
index 0000000..5645271
--- /dev/null
+++ b/src/query.c
@@ -0,0 +1,84 @@
+#include "query.h"
+#include "benc.h"
+#include "log.h"
+
+#include <string.h>
+#include <errno.h>
+
+int handle_request(struct nodeid* self, const char* method, const char* packet, size_t packet_len, char** response, size_t response_len) {
+	if(strcmp(method, "ping") == 0) {
+		struct bcursor bcursor;
+		struct benc_node stream[256];
+		bcur_open(&bcursor, packet, packet+packet_len, stream, 256);
+
+		if(bcursor.readhead->type != BNT_DICT) {
+			err("Bad query: Packet is not a dict");
+			return QUERY_EBADQ;
+		}
+		if(bcur_next(&bcursor, 1) < 0) {
+			err("Bad query: No token after outer dict start");
+			return QUERY_EBADQ;
+		}
+
+		if(bcur_find_key(&bcursor, (const enum benc_nodetype[]){BNT_STRING}, (const char*[]){"a"}, (const size_t[]){1}, 1) != 0) {
+			err("Bad query: No arguments to request");
+			return QUERY_EBADQ;
+		}
+		bcur_next(&bcursor, 1);
+		if(bcursor.readhead->type != BNT_DICT) {
+			err("Bad query: Wrong value type for request");
+			return QUERY_EBADQ;
+		}
+		// Skip the dict element
+		bcur_next(&bcursor, 1);
+		bool source_set = false;
+		struct nodeid source_id;
+		while(bcursor.readhead->type != BNT_END) {
+			switch(bcur_find_key(&bcursor, (const enum benc_nodetype[]){BNT_STRING}, (const char*[]){"id"}, (const size_t[]){6}, 1)) {
+				case 0:
+					// Skip the key
+					bcur_next(&bcursor, 1);
+
+					if(bcursor.readhead->type != BNT_STRING) {
+						err("Bad query: Wrong value type for id");
+						return QUERY_EBADQ;
+					}
+
+					if(bcursor.readhead->size != 20) {
+						err("Bad query: Incorrect id length");
+						return QUERY_EBADQ;
+					}
+
+					source_set = true;
+					memcpy(&source_id, bcursor.readhead->loc, 20);
+
+					// Skip the value
+					bcur_next(&bcursor, 1);
+					break;
+			}
+		}
+
+		if(!source_set) {
+			err("Id argument not set");
+			return QUERY_EBADQ;
+		}
+
+		char* end = (*response) + response_len;
+
+		int rc = snprintf(*response, end-*response, "d2:id20:");
+		if(rc < 0)
+			return QUERY_EBADQ;
+		*response += rc;
+		memcpy(*response, self, sizeof(struct nodeid));
+		*response += sizeof(struct nodeid);
+		rc = snprintf(*response, end-*response, "ee");
+		if(rc < 0)
+			return QUERY_EBADQ;
+		*response += rc;
+	} else {
+		return QUERY_EUNK;
+	}
+
+	return 0;
+}
+
diff --git a/src/query.h b/src/query.h
new file mode 100644
index 0000000..c1977b3
--- /dev/null
+++ b/src/query.h
@@ -0,0 +1,10 @@
+#pragma once
+
+#include "routing.h"
+
+#include <stdint.h>
+
+#define QUERY_EBADQ 1
+#define QUERY_EUNK  2
+
+int handle_request(struct nodeid* self, const char* method, const char* packet, size_t packet_len, char** response, size_t response_len);
diff --git a/src/routing.h b/src/routing.h
index 540f3f6..5c98f13 100644
--- a/src/routing.h
+++ b/src/routing.h
@@ -12,7 +12,10 @@ struct addr {
 };
 
 struct nodeid {
-	uint32_t inner[5];
+	union {
+		uint32_t inner[5];
+		char inner_b[20];
+	};
 };
 
 struct entry {
diff --git a/test/benc.c b/test/benc.c
index d734907..a9d6ddb 100644
--- a/test/benc.c
+++ b/test/benc.c
@@ -26,7 +26,7 @@ void test_unfinished_string() {
 	int depth = 0;
 	int64_t len = benc_decode(&cursor, cursor + strlen(packet), &depth, stream, 1);
 
-	TEST_ASSERT_EQUAL(0, len);
+	TEST_ASSERT_EQUAL(-BENC_EBADP, len);
 	TEST_ASSERT_EQUAL(BNT_STRING, stream[0].type);
 	TEST_ASSERT_EQUAL(4, stream[0].size);
 	TEST_ASSERT_EQUAL_PTR(packet + 2, stream[0].loc);
@@ -70,7 +70,7 @@ void test_unfinished_int() {
 	int depth = 0;
 	int64_t len = benc_decode(&cursor, cursor + strlen(packet), &depth, stream, 1);
 
-	TEST_ASSERT_EQUAL(0, len);
+	TEST_ASSERT_EQUAL(-BENC_EBADP, len);
 	TEST_ASSERT_EQUAL(BNT_INT, stream[0].type);
 	TEST_ASSERT_EQUAL_PTR(packet + 1, stream[0].loc);
 }
@@ -98,7 +98,7 @@ void test_incorrect_int_char() {
 	int64_t len = benc_decode(&cursor, cursor + strlen(packet), &depth, stream, 1);
 
 	TEST_ASSERT_EQUAL(packet + 2, cursor);
-	TEST_ASSERT_EQUAL(0, len);
+	TEST_ASSERT_EQUAL(-BENC_EBADP, len);
 	TEST_ASSERT_EQUAL(BNT_INT, stream[0].type);
 	TEST_ASSERT_EQUAL_PTR(packet + 1, stream[0].loc);
 }
@@ -149,6 +149,17 @@ void test_dict() {
 	nodeCursor++;
 }
 
+void test_dict_without_end() {
+	struct benc_node stream[4];
+	char* packet = "d";
+
+	const char* cursor = packet;
+	int depth = 0;
+	int64_t len = benc_decode(&cursor, cursor + strlen(packet), &depth, stream, 4);
+
+	TEST_ASSERT_EQUAL(-1, len);
+}
+
 void test_nested_list() {
 	struct benc_node stream[5];
 	char* packet = "lli1eee";
@@ -343,3 +354,21 @@ void test_stop_at_end() {
 	TEST_ASSERT_EQUAL_MESSAGE(-1, found, "Found something");
 	TEST_ASSERT_EQUAL_PTR_MESSAGE(stream+4, bcursor.readhead, "Didn't stop at dict end");
 }
+
+void test_find_key_with_small_token_buffer() {
+	struct benc_node stream[1];
+	char* packet = "d1:a1:a1:b1:b1:c1:ce";
+
+	struct bcursor bcursor;
+	bcur_open(&bcursor, packet, packet+strlen(packet), stream, 1);
+	bcur_next(&bcursor, 1); // Skip the dict token
+
+	ssize_t found = bcur_find_key(&bcursor, (const enum benc_nodetype[]){BNT_STRING}, (const char*[]){"c"}, (const size_t[]){1}, 1);
+
+	TEST_ASSERT_EQUAL(0, found);
+	TEST_ASSERT_EQUAL(BNT_STRING, bcursor.readhead->type);
+	TEST_ASSERT_EQUAL(1, bcursor.readhead->size);
+	TEST_ASSERT_EQUAL_STRING_LEN("c", bcursor.readhead->loc, 1);
+	bcur_next(&bcursor, 2);
+	TEST_ASSERT_EQUAL(BNT_END, bcursor.readhead->type);
+}
diff --git a/test/query.c b/test/query.c
new file mode 100644
index 0000000..cccb639
--- /dev/null
+++ b/test/query.c
@@ -0,0 +1,133 @@
+#include <unity.h>
+
+#include "query.h"
+
+#include <string.h>
+
+void test_malformed_empty() {
+	struct nodeid self = {.inner={0x0034048f, 0x08000020, 0x00888880, 0x02008460, 0x0ab00521}};
+	char* packet = "";
+	size_t packet_len = 0;
+
+	char response[1024];
+	char* response_cursor = response;
+	char* response_end = response + sizeof(response);
+
+	int rc = handle_request(&self, "ping", packet, packet_len, &response_cursor, response_end-response_cursor);
+
+	TEST_ASSERT_EQUAL(QUERY_EBADQ, rc);
+}
+
+void test_malformed_only_dict_start() {
+	struct nodeid self = {.inner={0x0034048f, 0x08000020, 0x00888880, 0x02008460, 0x0ab00521}};
+	char* packet = "d";
+	size_t packet_len = 1;
+
+	char response[1024];
+	char* response_cursor = response;
+	char* response_end = response + sizeof(response);
+
+	int rc = handle_request(&self, "ping", packet, packet_len, &response_cursor, response_end-response_cursor);
+
+	TEST_ASSERT_EQUAL(QUERY_EBADQ, rc);
+}
+
+void test_malformed_empty_args_key() {
+	struct nodeid self = {.inner={0x0034048f, 0x08000020, 0x00888880, 0x02008460, 0x0ab00521}};
+	char* packet = "d1:ae";
+	size_t packet_len = strlen(packet);
+
+	char response[1024];
+	char* response_cursor = response;
+	char* response_end = response + sizeof(response);
+
+	int rc = handle_request(&self, "ping", packet, packet_len, &response_cursor, response_end-response_cursor);
+
+	TEST_ASSERT_EQUAL(QUERY_EBADQ, rc);
+}
+
+void test_malformed_wrong_args_type() {
+	struct nodeid self = {.inner={0x0034048f, 0x08000020, 0x00888880, 0x02008460, 0x0ab00521}};
+	char* packet = "d1:a1:re";
+	size_t packet_len = strlen(packet);
+
+	char response[1024];
+	char* response_cursor = response;
+	char* response_end = response + sizeof(response);
+
+	int rc = handle_request(&self, "ping", packet, packet_len, &response_cursor, response_end-response_cursor);
+
+	TEST_ASSERT_EQUAL(QUERY_EBADQ, rc);
+}
+
+void test_malformed_empty_args() {
+	struct nodeid self = {.inner={0x0034048f, 0x08000020, 0x00888880, 0x02008460, 0x0ab00521}};
+	char* packet = "d1:adee";
+	size_t packet_len = strlen(packet);
+
+	char response[1024];
+	char* response_cursor = response;
+	char* response_end = response + sizeof(response);
+
+	int rc = handle_request(&self, "ping", packet, packet_len, &response_cursor, response_end-response_cursor);
+
+	TEST_ASSERT_EQUAL(QUERY_EBADQ, rc);
+}
+
+void test_malformed_wrong_id_arg_type() {
+	struct nodeid self = {.inner={0x0034048f, 0x08000020, 0x00888880, 0x02008460, 0x0ab00521}};
+	char* packet = "d1:ad2:idi1eee";
+	size_t packet_len = strlen(packet);
+
+	char response[1024];
+	char* response_cursor = response;
+	char* response_end = response + sizeof(response);
+
+	int rc = handle_request(&self, "ping", packet, packet_len, &response_cursor, response_end-response_cursor);
+
+	TEST_ASSERT_EQUAL(QUERY_EBADQ, rc);
+}
+
+void test_malformed_wrong_id_length() {
+	struct nodeid self = {.inner={0x0034048f, 0x08000020, 0x00888880, 0x02008460, 0x0ab00521}};
+	char* packet = "d1:ad2:id19:aaaaaaaaaaaaaaaaaaaee";
+	size_t packet_len = strlen(packet);
+
+	char response[1024];
+	char* response_cursor = response;
+	char* response_end = response + sizeof(response);
+
+	int rc = handle_request(&self, "ping", packet, packet_len, &response_cursor, response_end-response_cursor);
+
+	TEST_ASSERT_EQUAL(QUERY_EBADQ, rc);
+}
+
+void test_ping() {
+	struct nodeid self = {.inner_b={"aaaaaaaaaaaaaaaaaaab"}};
+	char* packet = "d1:ad2:id20:aaaaaaaaaaaaaaaaaaaaee";
+	size_t packet_len = strlen(packet);
+
+	char response[1024];
+	char* response_cursor = response;
+	char* response_end = response + sizeof(response);
+
+	int rc = handle_request(&self, "ping", packet, packet_len, &response_cursor, response_end-response_cursor);
+
+	TEST_ASSERT_EQUAL(0, rc);
+	TEST_ASSERT_EQUAL(30, response_cursor - response);
+	TEST_ASSERT_EQUAL_CHAR_ARRAY("d2:id20:aaaaaaaaaaaaaaaaaaabee", response, 30);
+}
+
+void test_bad_method() {
+	struct nodeid self = {.inner_b={"aaaaaaaaaaaaaaaaaaab"}};
+	char* packet = "de";
+	size_t packet_len = strlen(packet);
+
+	char response[1024];
+	char* response_cursor = response;
+	char* response_end = response + sizeof(response);
+
+	int rc = handle_request(&self, "someWrongMethod", packet, packet_len, &response_cursor, response_end-response_cursor);
+
+	TEST_ASSERT_EQUAL(QUERY_EUNK, rc);
+}