kpmpgsmkii/shared/packet.c

/*
 * 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 "packet.h"


static uint8_t packetCRC(char *data, uint16_t length, uint8_t startAt);


static uint8_t packetCRC(char *data, uint16_t length, uint8_t startAt) {
	uint16_t x = 0;

	for (x=0; x<length; x++) {
		startAt += data[x];
	}

	return startAt;
}


uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, char *input, uint16_t length) {
	uint16_t x = 0;
	char     c = 0;

	// input and inputLength are incoming raw data or NULL and 0 to continue processing already received data.
	// Returns 1 on packet ready, 0 on still waiting.

	// Is there input data to add to the queue?
	if (input != NULL && length > 0) {
		for (x=0; x<length; x++) {
			threadData->decodeQueue[threadData->decodeQueueHead++] = input[x];
			if (threadData->decodeQueueHead >= PACKET_INPUT_QUEUE_SIZE) {
				threadData->decodeQueueHead = 0;
			}
		}
	}

	while (1) {
		// Do we have data to process?
		if (threadData->decodeQueueHead == threadData->decodeQueueTail) return 0;

		// Get next byte.
		c = threadData->decodeQueue[threadData->decodeQueueTail++];
		if (threadData->decodeQueueTail >= PACKET_INPUT_QUEUE_SIZE) {
			threadData->decodeQueueTail = 0;
		}

		// New packet?
		if (threadData->newPacket) {
			threadData->newPacket = 0;
			threadData->inEscape  = 0;
			data->length          = 0;
		}

		// Are we escaped?
		if (threadData->inEscape) {
			threadData->inEscape = 0;
			// Is this the end of the packet?
			if (c != PACKET_FRAME) {
				threadData->newPacket = 1;

				// Check CRC.
				if (threadData->decodeBuffer[data->length - 1] != packetCRC(threadData->decodeBuffer, data->length - 1, 0)) continue;

				// Fill decoded data fields.
				data->packetType = threadData->decodeBuffer[1];
				data->channel    = threadData->decodeBuffer[2];

				// ***TODO*** Blowfish Decryption.
				if (threadData->decodeBuffer[0] & 32) {

				}

				// Copy packet data to new buffer.
				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.
				break;
			}
		} else {
			// Are we escaping?
			if (c == PACKET_FRAME) {
				// Yes.  Don't add this byte.
				threadData->inEscape = 1;
				continue;
			}
			// Did we overflow?
			if (data->length >= PACKET_MAX) {
				// Yup.  Dump it.
				threadData->newPacket = 1;
				continue;
			}
			// Add byte to packet.
			threadData->decodeBuffer[data->length++] = c;
		}
	}

	return 1;
}


uint8_t packetEncode(PacketThreadDataT *threadData, PacketEncodeDataT *data, char *input, uint16_t length) {
	uint8_t  crc     = 0;
	uint8_t  control = 0;

	// Returns 1 on success, 0 on failure.

	// Packet too large?
	if (length > PACKET_MAX) return 0;

	data->dataPointer = NULL;
	data->length   = 0;
	data->sequence    = threadData->sequence++;

	// Make needed header bytes.
	control = (data->control << 6) + (data->encrypt << 5) + (data->sequence & 0x1f);

	// Calculate CRC over header bytes and payload.
	crc = packetCRC((char *)&control,          1, crc);
	crc = packetCRC((char *)&data->packetType, 1, crc);
	crc = packetCRC((char *)&data->channel,    1, crc);
	crc = packetCRC(input, length, crc);

	// Add header bytes.
	threadData->encodeBuffer[data->length++] = control;
	if (control == PACKET_FRAME) threadData->encodeBuffer[data->length++] = PACKET_FRAME;
	threadData->encodeBuffer[data->length++] = data->packetType;
	if (data->packetType == PACKET_FRAME) threadData->encodeBuffer[data->length++] = PACKET_FRAME;
	threadData->encodeBuffer[data->length++] = data->channel;
	if (data->channel == PACKET_FRAME) threadData->encodeBuffer[data->length++] = PACKET_FRAME;

	// ***TODO*** Blowfish Encryption.
	if (data->encrypt) {

	}

	// Add payload.
	while (length--) {
		// Is this a frame character?  If so, escape it.
		if (*input == PACKET_FRAME) threadData->encodeBuffer[data->length++] = PACKET_FRAME;
		// Add data.
		threadData->encodeBuffer[data->length++] = *input++;
	}

	// Add CRC.
	threadData->encodeBuffer[data->length++] = crc;
	if (crc == PACKET_FRAME) threadData->encodeBuffer[data->length++] = PACKET_FRAME;

	// Mark end of packet.
	threadData->encodeBuffer[data->length++] = PACKET_FRAME;
	threadData->encodeBuffer[data->length++] = 0;

	return 1;
}


PacketThreadDataT *packetThreadDataCreate(void) {
	PacketThreadDataT *data = NULL;

	data = (PacketThreadDataT *)malloc(sizeof(PacketThreadDataT));
	if (data) {
		data->sequence        = 0;
		data->decodeQueueHead = 0;
		data->decodeQueueTail = 0;
		data->newPacket       = 1;
	}

	return data;
}


void packetThreadDataDestroy(PacketThreadDataT **data) {
	PacketThreadDataT *d = *data;

	free(d);
	d = NULL;
	*data = d;
}