File content as of revision 19:46e88d37ecef:

/*!
 * \file
 * \copyright
 * Copyright (C) Xsens Technologies B.V., 2015.  All rights reserved.
 *
 * This source code is intended for use only by Xsens Technologies BV and
 * those that have explicit written permission to use it from
 * Xsens Technologies BV.
 *
 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
 * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
 * PARTICULAR PURPOSE.
 */

#include "xbusparser.h"
#include "xbusdef.h"
#include "xbusutility.h"
#include <stdlib.h>
#include <string.h>
#include <assert.h>

/*!
 * \brief Max message length for parsed message types.
 * Unparsed types, e.g. MtData2 packets, will have buffers dynamically
 * requested, so are not constrained by this value.
 */
#define XBUS_MAX_MESSAGE_LENGTH (64)

enum XbusParserState
{
	XBPS_Preamble,          /*!< \brief Looking for preamble. */
	XBPS_BusId,             /*!< \brief Waiting for bus ID. */
	XBPS_MessageId,         /*!< \brief Waiting for message ID. */
	XBPS_Length,            /*!< \brief Waiting for length. */
	XBPS_ExtendedLengthMsb, /*!< \brief Waiting for extended length MSB*/
	XBPS_ExtendedLengthLsb, /*!< \brief Waiting for extended length LSB*/
	XBPS_Payload,           /*!< \brief Reading payload. */
	XBPS_Checksum           /*!< \brief Waiting for checksum. */
};

struct XbusParser
{
	struct XbusParserCallback callbacks;
	struct XbusMessage currentMessage;
	uint8_t rxBuffer[XBUS_MAX_MESSAGE_LENGTH];
	uint16_t payloadReceived;
	uint8_t checksum;
	enum XbusParserState state;
};

size_t XbusParser_mem(void)
{
	return sizeof(struct XbusParser);
}

struct XbusParser* XbusParser_create(struct XbusParserCallback const* callback)
{
	void* mem = malloc(XbusParser_mem());
	if (mem)
	{
		return XbusParser_init(mem, callback);
	}
	return NULL;
}

void XbusParser_destroy(struct XbusParser* parser)
{
	free(parser);
}

struct XbusParser* XbusParser_init(void* parserMem, struct XbusParserCallback const* callback)
{
	struct XbusParser* parser = (struct XbusParser*)parserMem;
	parser->state = XBPS_Preamble;
	parser->callbacks.allocateBuffer = callback->allocateBuffer;
	parser->callbacks.handleMessage = callback->handleMessage;
	return parser;
}

static bool canParseMessagePayload(enum XsMessageId mid)
{
	switch (mid)
	{
		case XMID_DeviceId:
			return true;

		default:
			return false;
	}
}

static void parseMessagePayload(struct XbusParser* parser)
{
	switch (parser->currentMessage.mid)
	{
		case XMID_DeviceId:
			{
				uint32_t* deviceId = parser->callbacks.allocateBuffer(sizeof(uint32_t));
				if (deviceId)
				{
					XbusUtility_readU32(deviceId, parser->rxBuffer);
					parser->currentMessage.data = deviceId;
				}
				else
				{
					parser->currentMessage.data = NULL;
				}
			}
			break;

		default:
			assert(!canParseMessagePayload(parser->currentMessage.mid));
			break;
	}
}

void prepareForPayload(struct XbusParser* parser)
{
	parser->currentMessage.data = NULL;
	parser->payloadReceived = 0;

	if (canParseMessagePayload(parser->currentMessage.mid))
	{
		assert(parser->currentMessage.length < XBUS_MAX_MESSAGE_LENGTH);
		if (parser->currentMessage.length < XBUS_MAX_MESSAGE_LENGTH)
		{
			parser->currentMessage.data = parser->rxBuffer;
		}
	}
	else
	{
		parser->currentMessage.data = parser->callbacks.allocateBuffer(parser->currentMessage.length);
	}
}

void XbusParser_parseByte(struct XbusParser* parser, const uint8_t byte)
{
	switch (parser->state)
	{
		case XBPS_Preamble:
			if (byte == XBUS_PREAMBLE)
			{
				parser->checksum = 0;
				parser->state = XBPS_BusId;
			}
			break;

		case XBPS_BusId:
			parser->checksum += byte;
			parser->state = XBPS_MessageId;
			break;

		case XBPS_MessageId:
			parser->checksum += byte;
			parser->currentMessage.mid = (enum XsMessageId)byte;
			parser->state = XBPS_Length;
			break;

		case XBPS_Length:
			parser->checksum += byte;
			if (byte == XBUS_NO_PAYLOAD)
			{
				parser->currentMessage.length = byte;
				parser->currentMessage.data = NULL;
				parser->state = XBPS_Checksum;
			}
			else if (byte < XBUS_EXTENDED_LENGTH)
			{
				parser->currentMessage.length = byte;
				prepareForPayload(parser);
				parser->state = XBPS_Payload;
			}
			else
			{
				parser->state = XBPS_ExtendedLengthMsb;
			}
			break;

		case XBPS_ExtendedLengthMsb:
			parser->checksum += byte;
			parser->currentMessage.length = ((uint16_t)byte) << 8;
			parser->state = XBPS_ExtendedLengthLsb;
			break;

		case XBPS_ExtendedLengthLsb:
			parser->checksum += byte;
			parser->currentMessage.length |= byte;
			prepareForPayload(parser);
			parser->state = XBPS_Payload;
			break;

		case XBPS_Payload:
			parser->checksum += byte;
			if (parser->currentMessage.data)
			{
				((uint8_t*)parser->currentMessage.data)[parser->payloadReceived] = byte;
			}
			if (++parser->payloadReceived == parser->currentMessage.length)
			{
				parser->state = XBPS_Checksum;
			}
			break;

		case XBPS_Checksum:
			parser->checksum += byte;
			if ((parser->checksum == 0) &&
					((parser->currentMessage.length == 0) ||
					 parser->currentMessage.data))
			{
				parseMessagePayload(parser);
				parser->callbacks.handleMessage(&parser->currentMessage);
			}
			parser->state = XBPS_Preamble;
			break;
	}
}

void XbusParser_parseBuffer(struct XbusParser* parser, uint8_t const* buf, size_t bufSize)
{
	for (size_t i = 0; i < bufSize; ++i)
	{
		XbusParser_parseByte(parser, buf[i]);
	}
}