Our first packet arrived!

.gitignore

@@ -6,3 +6,4 @@ build-*
 bin/
 obj/
 retired/
+test/

Makefile.djgpp

@@ -55,7 +55,7 @@ ALL_LDLIBS		:= -lc
 
 
 # Source, Binaries, Dependencies
-SRC			:= $(shell find $(SRCDIR) -type f -name '*.c' | grep -v '/linux/' | grep -v '/server/' | grep -v '/primes/' | grep -v '/font/' | grep -v '/retired/')
+SRC			:= $(shell find $(SRCDIR) -type f -name '*.c' | grep -v '/linux/' | grep -v '/server/' | grep -v '/primes/' | grep -v '/font/' | grep -v '/retired/' | grep -v '/test/')
 OBJ			:= $(patsubst $(SRCDIR)/%,$(OBJDIR)/%,$(SRC:.c=.o))
 DEP			:= $(OBJ:.o=.d)
 BIN			:= $(BINDIR)/$(TARGET)

client/src/main.c

@@ -46,7 +46,6 @@
 #include "timer.h"
 #include "gui.h"
 #include "config.h"
-#include "util.h"
 
 #include "welcome.h"
 
@@ -55,6 +54,9 @@
 #endif
 
 
+PacketThreadDataT *__packetThreadData = NULL;  // Exported in os.h
+
+
 static void taskGuiEventLoop(void *data);
 
 
@@ -150,6 +152,8 @@ int main(int argc, char *argv[]) {
 	guiStartup();
 	taskStartup();
 
+	__packetThreadData = packetThreadDataCreate();
+
 #ifdef TESTING
 	taskCreate(comPortScanTest, NULL);
 #else
@@ -159,6 +163,22 @@ int main(int argc, char *argv[]) {
 
 	taskRun();
 
+/*
+	char *testPacket = "Hello Server!";
+	PacketEncodeDataT encoded;
+	encoded.control    = PACKET_CONTROL_DAT;
+	encoded.packetType = PACKET_TYPE_TEST;
+	encoded.channel    = 1;
+	encoded.encrypt    = 0;
+	packetEncode(__packetThreadData, &encoded, testPacket, strlen(testPacket));
+	logWrite("Encoded %d bytes (raw length %d)\n", encoded.length, strlen(testPacket));
+	PacketDecodeDataT decoded;
+	packetDecode(__packetThreadData, &decoded, encoded.dataPointer, encoded.length);
+	logWrite("Decoded %d bytes (raw length %d)\n", decoded.length, encoded.length);
+*/
+
+	packetThreadDataDestroy(&__packetThreadData);
+
 	taskShutdown();
 	guiShutdown();
 	timerShutdown();

client/src/system/os.h

@@ -66,11 +66,12 @@ long biostime(int cmd, long newtime);
 // Now our headers.
 #include "log.h"
 #include "util.h"
+#include "packet.h"
 
 
 // Allocation helpers.
 #define NEW(t,v) (v)=(t*)malloc(sizeof(t))
-#define DEL(v)   free(v); v=NULL
+#define DEL(v)   {free(v); v=NULL;}
 
 // Some helper defines.
 #define DIVISIBLE_BY_EIGHT(x)  ((((x) >> 3) << 3) == (x))
@@ -78,4 +79,10 @@ long biostime(int cmd, long newtime);
 #define LOW_BYTE(b)            ((uint8_t)((b) & 0x00FF))
 
 
+typedef struct PacketThreadDataS PacketThreadDataT;
+
+
+extern PacketThreadDataT *__packetThreadData;  // Declared in main.c
+
+
 #endif // OS_H

client/src/welcome.c

@@ -132,6 +132,16 @@ static void taskConnectClick(void *data) {
 	// Connected!  Show icon and negotiate session.
 	widgetVisibleSet(W(_picConnect), 1);
 	taskYield();
+
+	char *testPacket = "Hello Server!";
+	PacketEncodeDataT encoded;
+	encoded.control    = PACKET_CONTROL_DAT;
+	encoded.packetType = PACKET_TYPE_TEST;
+	encoded.channel    = 1;
+	encoded.encrypt    = 0;
+	packetEncode(__packetThreadData, &encoded, testPacket, strlen(testPacket));
+	comWrite(__configData.serialCom - 1, encoded.dataPointer, encoded.length);
+	logWrite("Sent %d bytes (raw length %d)\n", encoded.length, strlen(testPacket));
 }
 
 

server/server.pro

@@ -44,11 +44,13 @@ HEADERS = \
 	$$SHARED/memory.h \
 	$$SHARED/util.h \
 	$$SHARED/primes.h \
+	$$SHARED/packet.h \
 	src/client.h \
 	src/console.h \
 	src/database.h \
 	src/network.h \
-	src/os.h
+	src/os.h \
+	src/server.h
 
 SOURCES = \
 	$$SHARED/thirdparty/memwatch/memwatch.c \
@@ -58,11 +60,13 @@ SOURCES = \
 	$$SHARED/log.c \
 	$$SHARED/memory.c \
 	$$SHARED/util.c \
+	$$SHARED/packet.c \
 	src/client.c \
 	src/console.c \
 	src/database.c \
 	src/main.c \
-	src/network.c
+	src/network.c \
+	src/server.c
 
 LIBS = \
 	-L/usr/lib/x86_64-linux-gnu/ \

server/src/client.c

@@ -18,29 +18,101 @@
  */
 
 
-#include <pthread.h>
-
 #include "client.h"
+#include "array.h"
 #include "network.h"
 #include "console.h"
+#include "packet.h"
+
+
+static uint8_t clientDequeuePacket(ClientThreadT *client);
+static void    clientProcessPacket(ClientThreadT *client, PacketDecodeDataT *data);
+
+
+static uint8_t clientDequeuePacket(ClientThreadT *client) {
+	uint16_t           length  = 0;
+	char              *data    = NULL;
+	PacketDecodeDataT  decode  = { 0 };
+	ClientRawPacketT  *packet  = NULL;
+
+	// Is there new data to process?
+	pthread_mutex_lock(&client->packetQueueMutex);
+	if (arrlenu(client->packetQueue) > 0) {
+		packet = client->packetQueue[0];
+		length = packet->length;
+		data   = packet->data;
+		arrdel(client->packetQueue, 0);
+	}
+	pthread_mutex_unlock(&client->packetQueueMutex);
+
+	// New data or not, process anything in the queue.
+	if (packetDecode(client->packetThreadData, &decode, data, length)) {
+		clientProcessPacket(client, &decode);
+		DEL(decode.data);
+		if (packet) {
+			DEL(packet->data);
+			DEL(packet);
+		}
+		return 1;
+	}
+
+	// No packet.
+	return 0;
+}
+
+
+static void clientProcessPacket(ClientThreadT *client, PacketDecodeDataT *data) {
+	char     temp[PACKET_MAX + 1] = { 0 };
+	uint16_t x                    = 0;
+
+	for (x=0; x<data->length; x++) temp[x] = data->data[x];
+	temp[x] = 0;
+
+	consoleMessageQueue("%ld: Channel %d Packet %d [%s]\n", client->threadIndex, data->channel, data->packetType, temp);
+}
+
+
+void clientQueuePacket(ClientThreadT *client, uint8_t *data, uint32_t length) {
+	ClientRawPacketT *packet = NULL;
+
+	packet = (ClientRawPacketT *)malloc(sizeof(ClientRawPacketT));
+	if (packet) {
+		packet->data = (char *)malloc(length);
+		if (packet->data) {
+			memcpy(packet->data, data, length);
+			packet->length = length;
+		}
+
+		pthread_mutex_lock(&client->packetQueueMutex);
+		arrput(client->packetQueue, packet);
+		pthread_mutex_unlock(&client->packetQueueMutex);
+	}
+}
 
 
 void *clientThread(void *data) {
 
-	ENetPeer      *peer   = (ENetPeer *)data;
+	ENetPeer        *peer      = (ENetPeer *)data;
-	ClientThreadT *client = (ClientThreadT *)peer->data;
+	ClientThreadT   *client    = (ClientThreadT *)peer->data;
-	ENetPacket    *packet = NULL;
+	ENetPacket      *packet    = NULL;
-	int32_t        r;
+	struct timespec  remaining = { 0, 0 };
+	struct timespec  sleepTime = { 0, 1000000000/100 };  // 1/100th second.
 
 	// Send service banner.
 	packet = enet_packet_create("KPMPGSMKII\r", 11, ENET_PACKET_FLAG_RELIABLE);
-	r = enet_peer_send(peer, 0, packet);
+	enet_peer_send(peer, 0, packet);
-
-	consoleMessageQueue("Packet:  %p %d\n", packet, r);
 
+	// Process packets until we're done.
 	while (client->running) {
-
+		if (!clientDequeuePacket(client)) {
+			// Don't eat all the CPU.
+			nanosleep(&remaining, &sleepTime);
+		}
 	}
 
+	// Clean up client data on the way out.
+	arrfree(client->packetQueue);
+	pthread_mutex_destroy(&client->packetQueueMutex);
+
 	pthread_exit(NULL);
 }

server/src/client.h

@@ -23,17 +23,26 @@
 
 
 #include "os.h"
-#include "network.h"
+#include "packet.h"
 
 
+typedef struct ClientRawPacketS {
+	char     *data;
+	uint32_t  length;
+} ClientRawPacketT;
+
 typedef struct ClientThreadS {
-	uint64_t        threadIndex;
+	uint64_t            threadIndex;
-	pthread_t       threadHandle;
+	pthread_t           threadHandle;
-	pthread_attr_t  threadAttributes;
+	pthread_attr_t      threadAttributes;
-	uint8_t         running;
+	uint8_t             running;
+	PacketThreadDataT  *packetThreadData;
+	ClientRawPacketT  **packetQueue;
+	pthread_mutex_t     packetQueueMutex;
 } ClientThreadT;
 
 
+void  clientQueuePacket(ClientThreadT *client, uint8_t *data, uint32_t length);
 void *clientThread(void *data);
 
 

server/src/console.c

@@ -19,7 +19,6 @@
 
 
 #include <termios.h>
-#include <pthread.h>
 #include <sys/select.h>
 
 #include "console.h"
@@ -27,7 +26,6 @@
 #include "util.h"
 
 
-static        uint8_t           _running             = 1;
 static struct termios           _termios             = { 0 };
 static        char            **_consoleMessageQueue = NULL;
 static        pthread_mutex_t   _messageQueueMutex   = PTHREAD_MUTEX_INITIALIZER;
@@ -65,7 +63,7 @@ void consoleRun(void) {
 	char            c                      = 0;
 	uint8_t         commandOk              = 0;
 	struct timespec remaining              = { 0, 0 };
-	struct timespec sleepTime              = { 0, 1000000000/4 };
+	struct timespec sleepTime              = { 0, 1000000000/4 };  // 1/4th second
 	size_t          i                      = 0;
 
 	if (pthread_mutex_init(&_messageQueueMutex, NULL)) utilDie("Unable to create console message queue mutex.\n");

server/src/main.c

@@ -19,17 +19,17 @@
 
 
 #include "os.h"
-#include "util.h"
-#include "array.h"
-#include "client.h"
 #include "console.h"
-#include "network.h"
 #include "database.h"
 #include "stddclmr.h"
+#include "server.h"
 
 #include "thirdparty/ini/src/ini.h"
 
 
+uint8_t _running = 1;  // Exported in os.h
+
+
 // "Config" items come from the INI file.  "Settings" are from the database.
 
 static        char     *_configServer    = NULL;
@@ -38,12 +38,9 @@ static        char     *_configDatabase  = NULL;
 static        char     *_configUser      = NULL;
 static        char     *_configPassword  = NULL;
 
-static        uint8_t   _running         = 1;
-
 
 static void     configRead(char *file);
 static void     configWrite(char *file);
-static void    *serverThread(void *data);
 
 
 static void configRead(char *file) {
@@ -98,82 +95,12 @@ static void configWrite(char *file) {
 }
 
 
-static void *serverThread(void *data) {
-	ENetHost       *server       = (ENetHost *)data;
-	ENetEvent       event        = { 0 };
-	ClientThreadT  *client       = NULL;
-	uint64_t        nextIndex    = 0;
-	size_t          i            = 0;
-	void           *status       = NULL;
-	char            buffer[2048] = { 0 };
-
-	while (_running) {
-		while (enet_host_service(server, &event, 1) > 0) {
-			switch (event.type) {
-				case ENET_EVENT_TYPE_NONE:
-					break;
-
-				case ENET_EVENT_TYPE_CONNECT:
-					// Create new client.
-					NEW(ClientThreadT, client);
-					if (!client) utilDie("Unable to allocate new client.\n");
-					client->threadIndex  = nextIndex++;
-					client->running      = 1;
-					// Keep our client in the peer data for later.
-					event.peer->data     = (void *)client;
-					// Make new thread for this client.
-					if (pthread_attr_init(&client->threadAttributes) != 0) utilDie("Unable to create client thread attributes.\n");
-					pthread_attr_setdetachstate(&client->threadAttributes, PTHREAD_CREATE_JOINABLE);
-					if (pthread_create(&client->threadHandle, &client->threadAttributes, clientThread, (void *)event.peer) != 0) utilDie("Unable to start client thread.\n");
-					// Tell the console.
-					enet_address_get_host_ip(&event.peer->address, buffer, 2047);
-					consoleMessageQueue("%ld: [%s] connected.\n", client->threadIndex, buffer);
-					break;
-
-				case ENET_EVENT_TYPE_RECEIVE:
-					enet_packet_destroy(event.packet);
-					break;
-
-				case ENET_EVENT_TYPE_DISCONNECT:
-				case ENET_EVENT_TYPE_DISCONNECT_TIMEOUT:
-					// Get our client.
-					client = (ClientThreadT *)event.peer->data;
-					// Stop client processing.
-					client->running = 0;
-					pthread_join(client->threadHandle, &status);
-					// Tell the console.
-					enet_address_get_host_ip(&event.peer->address, buffer, 2047);
-					consoleMessageQueue("%ld: [%s] disconnected.\n", client->threadIndex, buffer);
-					break;
-			}
-		}
-	}
-
-	// Anyone still running, be rude.  Nuke 'em.
-	for (i=0; i<server->peerCount; i++) {
-		client = (ClientThreadT *)server->peers[i].data;
-		// Stop client processing.
-		client->running = 0;
-		pthread_join(client->threadHandle, &status);
-		// Hang up.
-		enet_peer_reset(&server->peers[i]);
-	}
-
-	pthread_exit(NULL);
-}
-
-
 int main(int argc, char *argv[]) {
 
 	char           *configFile             = NULL;
 	uint32_t        settingsMaxClients     = 0;
 	uint32_t        settingsPortNumber     = 0;
 	uint32_t        settingsClientVersion  = 0;
-	ENetHost       *server                 = NULL;
-	ENetAddress     address                = { 0 };
-	pthread_t       serverThreadHandle     = { 0 };
-	pthread_attr_t  serverThreadAttributes = { 0 };
-	void           *serverThreadStatus     = NULL;
 
 	(void)argc;
 
@@ -193,22 +120,7 @@ int main(int argc, char *argv[]) {
 	if (!dbSettingsValueGet("portNumber",    (int32_t *)&settingsPortNumber))    utilDie("Unable to load portNumber.\n");
 	if (!dbSettingsValueGet("clientVersion", (int32_t *)&settingsClientVersion)) utilDie("Unable to load clientVersion.\n");
 
-	// Set up listening socket.
+	serverStartup(settingsPortNumber, settingsMaxClients);
-	address.host = ENET_HOST_ANY;
-	address.port = settingsPortNumber;
-	server = enet_host_create(&address,              /* the address to bind the server host to */
-								settingsMaxClients,   /* allow up to XX clients and/or outgoing connections */
-								1,                    /* allow up to 2 channels to be used, 0 and 1 */
-								0,                    /* assume any amount of incoming bandwidth */
-								0                     /* assume any amount of outgoing bandwidth */
-	);
-	if (server == NULL) utilDie("Unable to open server listening port.\n");
-
-	// Start server thread.
-	if (pthread_attr_init(&serverThreadAttributes) != 0) utilDie("Unable to create server thread attributes.\n");
-	pthread_attr_setdetachstate(&serverThreadAttributes, PTHREAD_CREATE_JOINABLE);
-	if (pthread_create(&serverThreadHandle, &serverThreadAttributes, serverThread, (void *)server) != 0) utilDie("Unable to start server thread.\n");
-
 	logWrite("Server online.\n");
 
 	// Run Console.
@@ -219,10 +131,9 @@ int main(int argc, char *argv[]) {
 
 	// Wait for all running threads to shut down.
 	logWrite("Shutting down.\n");
-	pthread_join(serverThreadHandle, &serverThreadStatus);
+	serverShutdown();
 
 	// Shut down.
-	enet_host_destroy(server);
 	dbDisconnect();
 	configWrite(configFile);
 	DEL(configFile);

server/src/os.h

@@ -32,18 +32,23 @@
 #include <pthread.h>
 
 // Should be after system headers in this file.
-#define MEMORY_CHECK_ENABLED
+//#define MEMORY_CHECK_ENABLED
 #include  "memory.h"
 
 // Now our headers.
 #include "log.h"
+#include "util.h"
+
 
 // Allocation helpers.
 #define NEW(t,v) (v)=(t*)malloc(sizeof(t))
-#define DEL(v)   free(v); v=NULL
+#define DEL(v)   {free(v); v=NULL;}
 
 #define SUCCESS 1
 #define FAIL    0
 
 
+extern uint8_t _running;  // Declared in main.c
+
+
 #endif // OS_H

server/src/server.c

@@ -0,0 +1,135 @@
+/*
+ * Kangaroo Punch MultiPlayer Game Server Mark II
+ * Copyright (C) 2020-2021  Scott Duensing
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+
+#include "server.h"
+#include "console.h"
+#include "network.h"
+#include "client.h"
+#include "packet.h"
+
+
+ENetHost   *_server                 = NULL;
+pthread_t   _serverThreadHandle     = { 0 };
+void       *_serverThreadStatus     = NULL;
+
+
+void serverShutdown(void) {
+	pthread_join(_serverThreadHandle, &_serverThreadStatus);
+	enet_host_destroy(_server);
+}
+
+
+void serverStartup(uint32_t port, uint32_t maxClients) {
+	ENetAddress     address                = { 0 };
+	pthread_attr_t  serverThreadAttributes = { 0 };
+
+	// Set up listening socket.
+	address.host = ENET_HOST_ANY;
+	address.port = port;
+	_server = enet_host_create(&address,      /* the address to bind the server host to */
+								maxClients,  /* allow up to XX clients and/or outgoing connections */
+								1,           /* allow up to 2 channels to be used, 0 and 1 */
+								0,           /* assume any amount of incoming bandwidth */
+								0            /* assume any amount of outgoing bandwidth */
+	);
+	if (_server == NULL) utilDie("Unable to open server listening port.\n");
+
+	// Start server thread.
+	if (pthread_attr_init(&serverThreadAttributes) != 0) utilDie("Unable to create server thread attributes.\n");
+	pthread_attr_setdetachstate(&serverThreadAttributes, PTHREAD_CREATE_JOINABLE);
+	if (pthread_create(&_serverThreadHandle, &serverThreadAttributes, serverThread, (void *)_server) != 0) utilDie("Unable to start server thread.\n");
+}
+
+
+void *serverThread(void *data) {
+	ENetHost         *server                 = (ENetHost *)data;
+	ENetEvent         event                  = { 0 };
+	ClientThreadT    *client                 = NULL;
+	uint64_t          nextIndex              = 0;
+	size_t            i                      = 0;
+	void             *status                 = NULL;
+	char              buffer[2048]           = { 0 };
+
+	while (_running) {
+		while (enet_host_service(server, &event, 1) > 0) {
+			switch (event.type) {
+				case ENET_EVENT_TYPE_NONE:
+					break;
+
+				case ENET_EVENT_TYPE_CONNECT:
+					// Create new client.
+					NEW(ClientThreadT, client);
+					if (!client) utilDie("Unable to allocate new client.\n");
+					client->packetThreadData = packetThreadDataCreate();
+					if (!client->packetThreadData) {
+						utilDie("Unable to allocate packetThreadData for new client.\n");
+						break;  // This break silences an invalid clang warning.
+					}
+					client->threadIndex      = nextIndex++;
+					client->running          = 1;
+					client->packetQueue      = NULL;
+					memset(&client->packetQueueMutex, 1, sizeof(pthread_mutex_t));
+					pthread_mutex_init(&client->packetQueueMutex, NULL);
+					// Keep our client in the peer data for later.
+					event.peer->data = (void *)client;
+					// Make new thread for this client.
+					if (pthread_attr_init(&client->threadAttributes) != 0) utilDie("Unable to create client thread attributes.\n");
+					pthread_attr_setdetachstate(&client->threadAttributes, PTHREAD_CREATE_JOINABLE);
+					if (pthread_create(&client->threadHandle, &client->threadAttributes, clientThread, (void *)event.peer) != 0) utilDie("Unable to start client thread.\n");
+					// Tell the console.
+					enet_address_get_host_ip(&event.peer->address, buffer, 2047);
+					consoleMessageQueue("%ld: [%s] connected.\n", client->threadIndex, buffer);
+					break;
+
+				case ENET_EVENT_TYPE_RECEIVE:
+					// Send the raw packet to the client thread for processing.
+					clientQueuePacket((ClientThreadT *)event.peer->data, event.packet->data, event.packet->dataLength);
+					enet_packet_destroy(event.packet);
+					break;
+
+				case ENET_EVENT_TYPE_DISCONNECT:
+				case ENET_EVENT_TYPE_DISCONNECT_TIMEOUT:
+					// Get our client.
+					client = (ClientThreadT *)event.peer->data;
+					// Stop client processing.
+					client->running = 0;
+					pthread_join(client->threadHandle, &status);
+					// Tell the console.
+					enet_address_get_host_ip(&event.peer->address, buffer, 2047);
+					consoleMessageQueue("%ld: [%s] disconnected.\n", client->threadIndex, buffer);
+					break;
+			}
+		}
+	}
+
+	// Anyone still running, be rude.  Nuke 'em.
+	for (i=0; i<server->peerCount; i++) {
+		client = (ClientThreadT *)server->peers[i].data;
+		if (client) {
+			// Stop client processing.
+			client->running = 0;
+			pthread_join(client->threadHandle, &status);
+			// Hang up.
+			enet_peer_reset(&server->peers[i]);
+		}
+	}
+
+	pthread_exit(NULL);
+}

server/src/server.h

@@ -0,0 +1,33 @@
+/*
+ * Kangaroo Punch MultiPlayer Game Server Mark II
+ * Copyright (C) 2020-2021  Scott Duensing
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+
+#ifndef SERVER_H
+#define SERVER_H
+
+
+#include "os.h"
+
+
+void  serverShutdown(void);
+void  serverStartup(uint32_t port, uint32_t maxClients);
+void *serverThread(void *data);
+
+
+#endif // SERVER_H

shared/packet.c

@@ -77,7 +77,7 @@ uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, cha
 				threadData->newPacket = 1;
 
 				// Check CRC.
-				if (threadData->decodeBuffer[data->length - 1] != packetCRC(threadData->decodeBuffer, data->length - 1, 0)) continue;
+				if ((uint8_t)threadData->decodeBuffer[data->length - 1] != packetCRC(threadData->decodeBuffer, data->length - 1, 0)) continue;
 
 				// Fill decoded data fields.
 				data->packetType = threadData->decodeBuffer[1];
@@ -92,6 +92,8 @@ uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, cha
 				data->data = (char *)malloc(data->length - 4);  // 4 for 3 byte header and 1 byte CRC.
 				if (!data) continue;
 				memcpy(data->data, &threadData->decodeBuffer[3], data->length - 4);  // Skip header and CRC.
+				// Fix length to remove header and checksum.
+				data->length -= 4;
 				break;
 			}
 		} else {
@@ -116,6 +118,16 @@ uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, cha
 }
 
 
+void packetDecodeDataDestroy(PacketDecodeDataT **packet) {
+	PacketDecodeDataT *d = *packet;
+
+	free(d->data);
+	free(d);
+	d = NULL;
+	*packet = d;
+}
+
+
 uint8_t packetEncode(PacketThreadDataT *threadData, PacketEncodeDataT *data, char *input, uint16_t length) {
 	uint8_t  crc     = 0;
 	uint8_t  control = 0;
@@ -167,6 +179,8 @@ uint8_t packetEncode(PacketThreadDataT *threadData, PacketEncodeDataT *data, cha
 	threadData->encodeBuffer[data->length++] = PACKET_FRAME;
 	threadData->encodeBuffer[data->length++] = 0;
 
+	data->dataPointer = threadData->encodeBuffer;
+
 	return 1;
 }
 

shared/packet.h

@@ -52,6 +52,7 @@
 #define PACKET_CONTROL_NAK      2  // Negative acknowledge.
 
 #define PACKET_TYPE_NONE        0  // No packet.
+#define PACKET_TYPE_TEST        1
 
 
 typedef struct PacketDecodeDataS {
@@ -89,6 +90,7 @@ typedef struct PacketThreadDataS {
 
 
 uint8_t            packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, char *input, uint16_t length);
+void               packetDecodeDataDestroy(PacketDecodeDataT **packet);
 uint8_t            packetEncode(PacketThreadDataT *threadData, PacketEncodeDataT *data, char *input, uint16_t length);
 PacketThreadDataT *packetThreadDataCreate(void);
 void               packetThreadDataDestroy(PacketThreadDataT **data);