pi_serial_bus_manager/src/p_serial_packet.cpp

#include "p_serial_packet.h"
#include "p_util.h"
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

ps_packet::ps_packet(uint8_t* _msg, size_t _msg_len)
{
	memset(data, 0, MAX_MESSAGE_LEN);
	memcpy(data, _msg, _msg_len);
	src_addr = &data[1];
	dest_addr = &data[2];
	frame_data_len = &data[3];
	frame_data = &data[4];
	msg_len = _msg_len;
	checksum = nullptr;
}

ps_packet::ps_packet(uint8_t _src_addr, uint8_t _dest_addr,
					 uint8_t* _frame_data, uint8_t _frame_data_len)
{
	data[0] = 0x7E;
	src_addr = &data[1];
	dest_addr = &data[2];
	frame_data_len = &data[3];
	frame_data = &data[4];
	msg_len = 5 + _frame_data_len;
	checksum = nullptr;

	*src_addr = _src_addr;
	*dest_addr = _dest_addr;
	*frame_data_len = _frame_data_len;

	memset(frame_data, 0, MAX_FRAME_DATA_LEN);
	memcpy(frame_data, _frame_data, _frame_data_len);
}
ps_packet::~ps_packet()
{}

// takes members and prepares the buffer for transmission
bool ps_packet::encode()
{
	bool ret = true;
	do
	{
		uint8_t csum = calc_checksum();
		if (!stuff_bytes())
		{
			ret = false;
			break;
		}
		checksum = &data[msg_len - 1];
		*checksum = csum;
	} while (0);
	return ret;
}

bool ps_packet::decode()
{
	bool ret = true;
	do
	{
		if (!destuff_bytes())
		{
			ret = false;
			break;
		}
		checksum = &data[msg_len - 1];
		if (!validate())
		{
			ret = false;
			break;
		}

	} while (0);
	return ret;
}

// done after receiving a packet and destuffing it
bool ps_packet::validate()
{
	uint8_t csum = calc_checksum();
	if (csum != *checksum)
	{
		printf("Bad checksum check\nCalculated: 0x%02x\nActual: 0x%02x\n", csum,
			   *checksum);
		return false;
	}
	return true;
}

uint8_t ps_packet::calc_checksum()
{
	uint32_t ret = 0;
	for (size_t ind = 0; ind < *frame_data_len; ind++)
	{
		ret += frame_data[ind];
	}
	ret &= 0xFF;
	ret = 0xFF - ret;
	return (uint8_t)ret;
}

bool ps_packet::stuff_bytes()
{
	int old_ind = 0;
	int new_ind = 0;
	uint8_t temp_buffer[MAX_FRAME_DATA_LEN] = {0};
	memcpy(temp_buffer, frame_data, MAX_FRAME_DATA_LEN);
	msg_len = *frame_data_len + 5;
	for (; old_ind < *frame_data_len; old_ind++)
	{
		if (temp_buffer[old_ind] == 0x7E || temp_buffer[old_ind] == 0x7D)
		{
			msg_len++;
			frame_data[new_ind++] = 0x7D;
			frame_data[new_ind++] = temp_buffer[old_ind] ^ 0x20;
		}
		else
		{
			frame_data[new_ind++] = temp_buffer[old_ind];
		}
	}
	if (msg_len > MAX_MESSAGE_LEN)
	{
		return false;
	}
	return true;
}

bool ps_packet::destuff_bytes()
{
	bool ret = true;

	uint8_t _checksum = data[msg_len - 1];
	int old_ind = 0;
	int new_ind = 0;
	uint8_t temp_buffer[MAX_FRAME_DATA_LEN] = {0};
	memcpy(temp_buffer, frame_data, MAX_FRAME_DATA_LEN);
	memset(frame_data, 0, MAX_FRAME_DATA_LEN);
	for (; new_ind < *frame_data_len; new_ind++)
	{
		if (temp_buffer[old_ind] == 0x7D)
		{
			msg_len--;
			frame_data[new_ind] = temp_buffer[++old_ind] ^ 0x20;
		}
		else
		{
			frame_data[new_ind] = temp_buffer[old_ind];
		}
		old_ind++;
	}
	checksum = &data[msg_len - 1];
	*checksum = _checksum;
	return ret;
}

ps_packet ps_packet::from_xy(int32_t x, int32_t y)
{
	uint8_t frame_data[MAX_FRAME_DATA_LEN];
	memset(frame_data, 0, MAX_FRAME_DATA_LEN);
	// op command
	// 0x01 = set xy
	frame_data[0] = 0x01;		   // + 1
	memcpy(&frame_data[1], &x, 4); // + sizeof(x)
	memcpy(&frame_data[5], &y, 4); // + sizeof(y)

	return ps_packet(MASTER_ADDR, 0x02, frame_data, 1 + sizeof(x) + sizeof(y));
}

ps_packet ps_packet::test_packet_send()
{
	uint8_t frame_data[16] = {0xFF, 0x01, 0x02, 0x03, 0x04, 0x7D, 0x7D, 0x07,
							  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0F, 0x7E};

	return ps_packet(MASTER_ADDR, 0xFF, frame_data, 16);
}

ps_packet ps_packet::test_packet_recv()
{
	uint8_t frame_data[24] = {0x7E, 0xFF, MASTER_ADDR, 0x10, 0xFF, 0x01,
							  0x02, 0x03, 0x04,		   0x7D, 0x5D, 0x7D,
							  0x5D, 0x07, 0x08,		   0x09, 0x0A, 0x0B,
							  0x0C, 0x0D, 0x0F,		   0x7D, 0x5E, 0x29};

	return ps_packet(frame_data, 24);
}

#ifdef DEBUG
void ps_packet::show_packet() const
{
	printf("Start Byte:\t\t0x%02x\n", data[0]);
	printf("Source Address:\t\t0x%02x\n", *src_addr);
	printf("Dest Address:\t\t0x%02x\n", *dest_addr);
	printf("Frame Data Len:\t\t0x%02x\n", *frame_data_len);
	printf("Frame Data:\n");
	for (size_t ind = 0; ind < msg_len - 4; ind++)
	{
		printf("\t\t[%2lu]: 0x%02x\n", ind, frame_data[(int)ind]);
	}
	printf("\n");
	printf("Message Length: %lu\n", msg_len);
	printf("Checksum: 0x%02x\n", *checksum);
}
#endif