1 files changed, 126 insertions, 0 deletions
diff --git a/nbtpd/packet.c b/nbtpd/packet.c
new file mode 100644
index 0000000..8302550
--- /dev/null
+++ b/nbtpd/packet.c
@@ -0,0 +1,126 @@
+#include <stdlib.h>
+#include <unistd.h>
+#include <arpa/inet.h>
+#include <string.h>
+
+#include "packet.h"
+
+char *nbd_tftp_error_to_message(nbd_tftp_ecode error) {
+	switch (error) {
+		case 0:
+			return "ERROR";
+		case 1:
+			return "File not Found";
+		case 2:
+			return "Access violation";
+		case 3:
+			return "Disk full or allocation exceeded";
+		case 4:
+			return "Illegal TFTP operation";
+		case 5:
+			return "Unknown Transfer ID";
+		case 6:
+			return "File already exists";
+		case 7:
+			return "No such user";
+	}
+	return "UNKNOWN ERROR";
+}
+
+char *nbd_tftp_ser_data(nbd_tftp_packet_data d) {
+	char *buf = malloc(sizeof(d.opcode) + sizeof(d.block_num) + d.datalen);
+	if (buf != NULL) {
+		uint16_t netopcode = htons(d.opcode);
+		uint16_t netbnum = htons(d.block_num);
+		memcpy(buf, &netopcode, sizeof(netopcode));
+		memcpy((buf + sizeof(netopcode)), &netbnum, sizeof(netbnum));
+		memcpy((buf + sizeof(netopcode) + sizeof(netbnum)), d.data, d.datalen);
+	}
+	return buf;
+}
+
+char *nbd_tftp_ser_data_from_parts(uint16_t blocknum, char *data, size_t datalen) {
+	char *buf = malloc(sizeof(uint16_t) + sizeof(blocknum) + datalen);
+	if (buf != NULL) {
+		uint16_t netopcode = htons(3);
+		uint16_t netbnum = htons(blocknum);
+		memcpy(buf, &netopcode, sizeof(netopcode));
+		memcpy((buf + sizeof(netopcode)), &netbnum, sizeof(netbnum));
+		memcpy((buf + sizeof(netopcode) + sizeof(netbnum)), data, datalen);
+	}
+	return buf;
+}
+
+nbd_tftp_packet_data nbd_tftp_de_data(char *data, size_t len) {
+	nbd_tftp_packet_data ret = { 0, 0, (char *)NULL, (size_t)0 };
+	if ((len >= 4) && (data != NULL)) {
+		ret.opcode = ((uint16_t)data[0] << 8) + data[1];
+		ret.block_num = ((uint16_t)data[2] << 8) + data[3];
+		ret.datalen = len - 4;
+		if (ret.datalen > 0) {
+			if ((ret.data = malloc(ret.datalen)) != NULL) {
+				memcpy(ret.data, (data + 4), ret.datalen);
+			}
+		}
+	}
+	return ret;
+}
+
+char *nbd_tftp_ser_error(nbd_tftp_packet_error e) {
+	char *buf = malloc(sizeof(e.opcode) + sizeof((uint16_t)e.err) + strlen(e.emsg));
+	if (buf != NULL) {
+		uint16_t netopcode = htons(e.opcode);
+		uint16_t netecode = htons((uint16_t)e.err);
+		memcpy(buf, &netopcode, sizeof(netopcode));
+		memcpy((buf + sizeof(netopcode)), &netecode, sizeof(netecode));
+		memcpy((buf + sizeof(netopcode) + sizeof(netecode)), e.emsg, strlen(e.emsg));
+	}
+	return buf;
+}
+
+char *nbd_tftp_ser_error_from_code(nbd_tftp_ecode error) {
+	char *emsg = nbd_tftp_error_to_message(error);
+	char *buf = malloc(sizeof(uint16_t) + sizeof(uint16_t) + strlen(emsg));
+	if (buf != NULL) {
+		uint16_t netopcode = htons(5);
+		uint16_t netecode = htons((uint16_t)error);
+		memcpy(buf, &netopcode, sizeof(netopcode));
+		memcpy((buf + sizeof(netopcode)), &netecode, sizeof(netecode));
+		memcpy((buf + sizeof(netopcode) + sizeof(netecode)), emsg, strlen(emsg));
+	}
+	return buf;
+}
+
+char *nbd_tftp_ser_ack(nbd_tftp_packet_ack ack) {
+	char *buf = malloc(sizeof(ack.opcode) + sizeof(ack.block_num));
+	if (buf != NULL) {
+		uint16_t netopcode = htons(ack.opcode);
+		uint16_t netbnum = htons(ack.block_num);
+		memcpy(buf, &netopcode, sizeof(netopcode));
+		memcpy((buf + sizeof(netopcode)), &netbnum, sizeof(netbnum));
+	}
+	return buf;
+}
+
+char *nbd_tftp_ser_ack_from_block_num(uint16_t block_num) {
+	char *buf = malloc(sizeof(uint16_t) + sizeof(block_num));
+	if (buf != NULL) {
+		uint16_t netopcode = htons(4);
+		uint16_t netbnum = htons(block_num);
+		memcpy(buf, &netopcode, sizeof(netopcode));
+		memcpy((buf + sizeof(netopcode)), &netbnum, sizeof(netbnum));
+	}
+	return buf;
+}
+
+nbd_tftp_packet_ack nbd_tftp_de_ack(char *buf, ssize_t buflen) {
+	nbd_tftp_packet_ack ack = { 0, 0 };
+	if ((buf != NULL) && (buflen == 4)) {
+		uint16_t opcode = ((uint16_t)buf[0] << 8) + buf[1];
+		uint16_t block_num = ((uint16_t)buf[2] << 8) + buf[3];
+		ack.opcode = opcode;
+		ack.block_num = block_num;
+	}
+	return ack;
+}
+