Diff: xbus/xbusparser.c

Revision:

3:abc451d82b88

Child:

9:e4dc986e59f6

diff -r b3e402dc11ca -r abc451d82b88 xbus/xbusparser.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/xbus/xbusparser.c	Wed May 13 12:00:28 2015 +0200
@@ -0,0 +1,167 @@
+/*!
+ * \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 <stdlib.h>
+
+#define XBUS_PREAMBLE (0xFA)
+#define XBUS_NO_PAYLOAD (0x00)
+#define XBUS_EXTENDED_LENGTH (0xFF)
+
+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;
+	uint8_t* messageBuffer;
+	uint16_t currentMessageLength;
+	uint16_t payloadReceived;
+	uint8_t currentMessageId;
+	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 void prepareForPayload(struct XbusParser* parser)
+{
+	parser->messageBuffer = parser->callbacks.allocateBuffer(parser->currentMessageLength);
+	parser->payloadReceived = 0;
+}
+
+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->currentMessageId = byte;
+			parser->state = XBPS_Length;
+			break;
+
+		case XBPS_Length:
+			parser->checksum += byte;
+			if (byte == XBUS_NO_PAYLOAD)
+			{
+				parser->state = XBPS_Checksum;
+			}
+			else if (byte < XBUS_EXTENDED_LENGTH)
+			{
+				parser->currentMessageLength = byte;
+				prepareForPayload(parser);
+				parser->state = XBPS_Payload;
+			}
+			else
+			{
+				parser->state = XBPS_ExtendedLengthMsb;
+			}
+			break;
+
+		case XBPS_ExtendedLengthMsb:
+			parser->checksum += byte;
+			parser->currentMessageLength = ((uint16_t)byte) << 8;
+			parser->state = XBPS_ExtendedLengthLsb;
+			break;
+
+		case XBPS_ExtendedLengthLsb:
+			parser->checksum += byte;
+			parser->currentMessageLength |= byte;
+			prepareForPayload(parser);
+			parser->state = XBPS_Payload;
+			break;
+
+		case XBPS_Payload:
+			parser->checksum += byte;
+			if (parser->messageBuffer)
+			{
+				parser->messageBuffer[parser->payloadReceived] = byte;
+			}
+			if (++parser->payloadReceived == parser->currentMessageLength)
+			{
+				parser->state = XBPS_Checksum;
+			}
+			break;
+
+		case XBPS_Checksum:
+			parser->checksum += byte;
+			if ((parser->checksum == 0) && parser->messageBuffer)
+			{
+				parser->callbacks.handleMessage(parser->currentMessageId,
+						parser->currentMessageLength,
+						parser->messageBuffer);
+			}
+			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]);
+	}
+}
+