communication.c

#include "communication.h"
#include "commands.h"
#include <string.h>
#include <ctype.h>

static int msgNum = 0;

tokenList_t tokenize(char* cmd) {
	int maxTokens = 16;
	tokenList_t ret;
	ret.numTokens = 0;
	ret.tokens = malloc(sizeof(char*) * maxTokens);
	ret.tokens[0] = NULL;

	int i = 0;
	char* token = strtok(cmd, " ");
	while (token != NULL && i < maxTokens - 1) {
		ret.tokens[i++] = token;
		ret.tokens[i] = NULL;
		ret.numTokens++;
		token = strtok(NULL, " ");
	}

	return ret;
}

int checkFloat(char *floatString, float *value) {
	char *tmp;
	*value = strtod(floatString, &tmp);
	if(!(isspace(*tmp) || *tmp == 0)) {
		fprintf(stderr, "%s is not a valid floating-point number\n", floatString);
		return 0;
	}
	return 1;
}

int checkInt(char *intString, int *value) {
	char *tmp;
	long temp_long;
	temp_long = strtol(intString, &tmp, 0); // base 10 number inputted
	if(temp_long < INT_MIN || temp_long > INT_MAX ||  !(isspace(*tmp) || *tmp == 0)) {
		fprintf(stderr, "%s is not a valid integer number\n", intString);
		return 0;
	}
	printf("temp: %ld\n\n", temp_long);
	*value = (int) temp_long;
	return 1;
}

//--------------------------------
// Ground Station
//--------------------------------

// Formatting commands from ground station CLI
int formatCommand(char *command, unsigned char **formattedCommand) {
	//fprintf(stderr, "length = %li , received '%s'\n", strlen(command), command);
	char cmd[strlen(command)];
	strncpy(cmd, command, strlen(command));
	cmd[strlen(command)] = '\0';

	tokenList_t tokens = tokenize(cmd);
	float floatValue = 0.0;
	int intValue = 0;
	int valid;
	metadata_t metadata;
	
	// ----------------------------------------------
	if(tokens.numTokens > 0) {
		for(int type = 0; type < MAX_TYPE; type++)
		{
			for(int subtype = 0; subtype < MAX_SUBTYPE; subtype++)
			{
				if(strcmp(tokens.tokens[0], MessageTypes[type].subtypes[subtype].cmdText) == 0)
				{
					printf("Sending\n\ttype: %d, \n\tsubtype: %d\n\tcommand: %s\n", type, subtype, MessageTypes[type].subtypes[subtype].cmdText);
					
					// Make sure the second token is the right type
					switch (MessageTypes[type].subtypes[subtype].cmdDataType)
					{
						// Validate the float input
						case floatType:
							metadata.begin_char = (char) BEGIN_CHAR;
							metadata.msg_type = MessageTypes[type].ID;
							metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;

							if(MessageTypes[type].ID == 0x01) {
								valid = checkFloat(tokens.tokens[1], &floatValue);
								if(!valid) {
									return -1;
								}
								metadata.data_len = sizeof(floatValue);
							} else {
								metadata.data_len = 0;
							}
							
							metadata.msg_id = msgNum++;
							formatPacket(&metadata, &floatValue, formattedCommand);
							break;
						
						// Validate the integer input
						case intType:
							metadata.begin_char = (char) BEGIN_CHAR;
							metadata.msg_type = MessageTypes[type].ID;
							metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
							
							if(MessageTypes[type].ID == 0x01) {
								valid = checkInt(tokens.tokens[1], &intValue);
								if(!valid) {
									return -1;
								}
								metadata.data_len = sizeof(intValue);
							} else {
								metadata.data_len = 0;
							}
							
							metadata.msg_id = msgNum++;
							formatPacket(&metadata, &intValue, formattedCommand);
							
							break;
						
						// Validate the string input (doesn't need to happen)
						case stringType:
							metadata.begin_char = (char) BEGIN_CHAR;
							metadata.msg_type = MessageTypes[type].ID;
							metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
							metadata.msg_id = msgNum++;
							metadata.data_len = strlen(tokens.tokens[1]);
							
							formatPacket(&metadata, &tokens.tokens[1], formattedCommand);
							
							break;
						default:
							return -1;
					}
					return 0;
				}
			}
		}
	}

	// Only gets here if the command does not exist
	return -1;	
}
// QUAD & Ground Station
// Format the log data from log_message
//int formatData(unsigned char *log_msg, unsigned char *formattedCommand)
int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand)
{
	*formattedCommand = malloc(sizeof(unsigned char) * metadata->data_len + 8);
	//----------------------------------------------------------------------------------------------
	//	   index||	   0	|	  1	   |	  2		 |	3 & 4 |		 5 & 6		 |	7+	|	end	   |
	//---------------------------------------------------------------------------------------------|
	// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
	//-------------------------------------------------------------------------------------------- |
	//	   bytes||	   1	|	  1	   |	  1		 |	  2	  |		   2		 | var	|	 1	   |
	//----------------------------------------------------------------------------------------------
	
	// Begin Char:
	(*formattedCommand)[0] = metadata->begin_char;

	// Msg type:
	(*formattedCommand)[1] = metadata->msg_type;
	
	// Msg subtype
	(*formattedCommand)[2] = metadata->msg_subtype;
	
	//Msg id (msgNum is 2 bytes)
	(*formattedCommand)[3] = (metadata->msg_id & 0x000000ff);
	(*formattedCommand)[4] = ((metadata->msg_id >> 8) & 0x000000ff);
	
	// Data length and data - bytes 5&6 for len, 7+ for data
	(*formattedCommand)[5] = metadata->data_len & 0x000000ff;
	(*formattedCommand)[6] = (metadata->data_len >> 8) & 0x000000ff;
	
	memcpy(&((*formattedCommand)[7]), data, metadata->data_len);
	
	// Checksum
	// receive data and calculate checksum
	int i;
	char data_checksum;
	for(i = 0; i < 7 + metadata->data_len; i++)
	{
		data_checksum ^= (*formattedCommand)[i];
	}
	
	(*formattedCommand)[7 + metadata->data_len] = data_checksum;
	
	return 0;
}

// returns the length of the data in bytes (datalen from packet) and fills data
// and metadata with the packet information
// use as follows:
//
//		packet is the entire packet message (formatted) 
//		data is an unallocated (char *) (pass it to this function as &data) 
//		meta_data is a pointer to an instance of metadata_t
//
int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data)
{
	//----------------------------------------------------------------------------------------------
	//     index||     0    |     1    |      2      |  3 & 4 |      5 & 6       |  7+  |   end    |
	//---------------------------------------------------------------------------------------------|
	// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
	//-------------------------------------------------------------------------------------------- |
	//     bytes||     1    |     1    |      1      |    2   |        2         | var  |    1     |
	//----------------------------------------------------------------------------------------------	
	
	// first byte must be the begin char
	if(packet[0] != 0xBE)
		return -1;

	// receive metadata
	meta_data->begin_char = packet[0];
	meta_data->msg_type = packet[1];
	meta_data->msg_subtype = packet[2];
	meta_data->msg_id = (packet[4] << 8) | (packet[3]);
	meta_data->data_len = (packet[6] << 8) | (packet[5]);
	unsigned char packet_checksum = packet[7+meta_data->data_len];
	//fprintf(stderr, "datalen: %d\n", meta_data->data_len);
	
	int i;
	
	// receive data
	*data = malloc(meta_data->data_len);
	for(i = 0; i < meta_data->data_len; i++)
	{
		(*data)[i] = packet[7+i];
	}

	// calculate checksum
	unsigned char calculated_checksum = 0;
	for(i = 0; i < meta_data->data_len + 7; i++)
	{
		calculated_checksum ^= packet[i];
	}

	// compare checksum
	if(packet_checksum != calculated_checksum)
		fprintf(stderr, "Checksums did not match (Parse Packet): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);

	return 0;
}

// QUAD & Ground Station
// Process the command received
int processCommand(unsigned char *packet, modular_structs_t *structs) {
	int validPacket;
	unsigned char *data;
	metadata_t metadata;
	
	// Validate the message is correctly formatted
	validPacket = parse_packet(packet, &data, &metadata);
	if(validPacket != 0) {
		return -1;
	}
	
	if(metadata.data_len >= 0) {
		/* Null check*/
		if (MessageTypes[(unsigned char)metadata.msg_type].subtypes[
				(unsigned char) metadata.msg_subtype].functionPtr) {
			// Call the appropriate subtype function
			(* (MessageTypes[(unsigned char)metadata.msg_type].subtypes[(unsigned char)metadata.msg_subtype].functionPtr))(data, metadata.data_len, structs);
		}
			
		return 0;
	}
	
	// Only gets here if there is an error
	return -1;
}