xbusparser.c

00001 /*!
00002  * \file
00003  * \copyright Copyright (C) Xsens Technologies B.V., 2015.
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License"); you may not
00006  * use this file except in compliance with the License. You may obtain a copy
00007  * of the License at
00008  *
00009  * http://www.apache.org/licenses/LICENSE-2.0
00010  *
00011  * Unless required by applicable law or agreed to in writing, software
00012  * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
00013  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
00014  * License for the specific language governing permissions and limitations
00015  * under the License.
00016  */
00017 
00018 #include "xbusparser.h "
00019 #include "xbusdef.h "
00020 #include "xbusutility.h "
00021 #include <stdlib.h>
00022 #include <string.h>
00023 
00024 /*! \brief XbusParser states. */
00025 enum XbusParserState
00026 {
00027     XBPS_Preamble,          /*!< \brief Looking for preamble. */
00028     XBPS_BusId,             /*!< \brief Waiting for bus ID. */
00029     XBPS_MessageId,         /*!< \brief Waiting for message ID. */
00030     XBPS_Length,            /*!< \brief Waiting for length. */
00031     XBPS_ExtendedLengthMsb, /*!< \brief Waiting for extended length MSB*/
00032     XBPS_ExtendedLengthLsb, /*!< \brief Waiting for extended length LSB*/
00033     XBPS_Payload,           /*!< \brief Reading payload. */
00034     XBPS_Checksum           /*!< \brief Waiting for checksum. */
00035 };
00036 
00037 /*!
00038  * \brief Xbus Parser state structure.
00039  */
00040 struct XbusParser
00041 {
00042     /*! \brief Callbacks for memory management, and message handling. */
00043     struct XbusParserCallback callbacks;
00044     /*! \brief Storage for the current message being received. */
00045     struct XbusMessage currentMessage;
00046     /*! \brief The number of bytes of payload received for the current message. */
00047     uint16_t payloadReceived;
00048     /*! \brief The calculated checksum for the current message. */
00049     uint8_t checksum;
00050     /*! \brief The state of the parser. */
00051     enum XbusParserState state;
00052 };
00053 
00054 /*!
00055  * \brief Get the amount of memory needed for the XbusParser structure.
00056  */
00057 size_t XbusParser_mem(void)
00058 {
00059     return sizeof(struct XbusParser);
00060 }
00061 
00062 /*!
00063  * \brief Create a new XbusParser object.
00064  * \param callback Pointer to callback structure containing callback functions
00065  * for memory management and handling received messages.
00066  * \returns Pointer the new XbusParser structure.
00067  *
00068  * Uses malloc to allocate the memory required for the parser.
00069  */
00070 struct XbusParser* XbusParser_create(struct XbusParserCallback const* callback)
00071 {
00072     void* mem = malloc(XbusParser_mem());
00073     if (mem)
00074     {
00075         return XbusParser_init(mem, callback);
00076     }
00077     return NULL;
00078 }
00079 
00080 /*!
00081  * \brief Frees an XbusParser structure allocated by XbusParser_create().
00082  */
00083 void XbusParser_destroy(struct XbusParser* parser)
00084 {
00085     free(parser);
00086 }
00087 
00088 /*!
00089  * \brief Initializes an XbusParser in the passed memory location.
00090  * \param parserMem Pointer to memory to use for storing parser state. Should
00091  * be at least as big as the value returned by XbusParser_mem().
00092  * \param callback Pointer to callback structure containing callback functions
00093  * for memory management and handling received messages.
00094  * \returns Initialized XbusParser structure.
00095  */
00096 struct XbusParser* XbusParser_init(void* parserMem, struct XbusParserCallback const* callback)
00097 {
00098     struct XbusParser* parser = (struct XbusParser*)parserMem;
00099     parser->state = XBPS_Preamble;
00100     parser->callbacks.allocateBuffer = callback->allocateBuffer;
00101     parser->callbacks.deallocateBuffer = callback->deallocateBuffer;
00102     parser->callbacks.handleMessage = callback->handleMessage;
00103     return parser;
00104 }
00105 
00106 /*!
00107  * \brief Parse an XMID_DeviceId message to extract the device ID value.
00108 
00109  * Replaces the raw Xbus message data with the device ID.
00110  */
00111 static void parseDeviceId(struct XbusParser* parser, uint8_t const* rawData)
00112 {
00113     uint32_t* deviceId = parser->callbacks.allocateBuffer(sizeof(uint32_t));
00114     if (deviceId)
00115     {
00116         XbusUtility_readU32(deviceId, rawData);
00117         parser->currentMessage.data = deviceId;
00118         parser->currentMessage.length = 1;
00119     }
00120     else
00121     {
00122         parser->currentMessage.data = NULL;
00123     }
00124 }
00125 
00126 /*!
00127  * \brief Parse an XMID_OutputConfig message.
00128  *
00129  * Replaces the raw Xbus message data with an array of OutputConfiguration
00130  * structures.
00131  */
00132 static void parseOutputConfig(struct XbusParser* parser, uint8_t const* rawData)
00133 {
00134     uint8_t fields = parser->currentMessage.length / 4;
00135     struct OutputConfiguration* conf = parser->callbacks.allocateBuffer(fields * sizeof(struct OutputConfiguration));
00136     if (conf)
00137     {
00138         parser->currentMessage.data = conf;
00139         parser->currentMessage.length = fields;
00140 
00141         for (int i = 0; i < fields; ++i)
00142         {
00143             rawData = XbusUtility_readU16((uint16_t*)&conf->dtype, rawData);
00144             rawData = XbusUtility_readU16(&conf->freq, rawData);
00145             ++conf;
00146         }
00147     }
00148     else
00149     {
00150         parser->currentMessage.data = NULL;
00151     }
00152 }
00153 
00154 /*!
00155  * \brief Converts raw Xbus payload data to native structures if possible.
00156  *
00157  * Raw data payloads are converted to native data structures and the
00158  * message data pointer is changed to point to the native structure.
00159  * The raw data is automatically deallocated.
00160  */
00161 static void parseMessagePayload(struct XbusParser* parser)
00162 {
00163     uint8_t const* const rawData = parser->currentMessage.data;
00164     switch (parser->currentMessage.mid)
00165     {
00166         default:
00167             // Leave parsing and memory management to user code
00168             return;
00169 
00170         case XMID_DeviceId:
00171             parseDeviceId(parser, rawData);
00172             break;
00173 
00174         case XMID_OutputConfig:
00175             parseOutputConfig(parser, rawData);
00176             break;
00177     }
00178 
00179     if (rawData)
00180         parser->callbacks.deallocateBuffer(rawData);
00181 }
00182 
00183 /*!
00184  * \brief Prepare for receiving a message payload.
00185  *
00186  * Requests a memory area to store the received data to using the
00187  * registered callbacks.
00188  */
00189 void prepareForPayload(struct XbusParser* parser)
00190 {
00191     parser->payloadReceived = 0;
00192     parser->currentMessage.data = parser->callbacks.allocateBuffer(parser->currentMessage.length);
00193 }
00194 
00195 /*!
00196  * \brief Parse a byte of data from a motion tracker.
00197  *
00198  * When a complete message is received the user will be notified by a call
00199  * to the handleMessage() callback function.
00200  */
00201 void XbusParser_parseByte(struct XbusParser* parser, const uint8_t byte)
00202 {
00203     switch (parser->state)
00204     {
00205         case XBPS_Preamble:
00206             if (byte == XBUS_PREAMBLE)
00207             {
00208                 parser->checksum = 0;
00209                 parser->state = XBPS_BusId;
00210             }
00211             break;
00212 
00213         case XBPS_BusId:
00214             parser->checksum += byte;
00215             parser->state = XBPS_MessageId;
00216             break;
00217 
00218         case XBPS_MessageId:
00219             parser->checksum += byte;
00220             parser->currentMessage.mid = (enum XsMessageId)byte;
00221             parser->state = XBPS_Length;
00222             break;
00223 
00224         case XBPS_Length:
00225             parser->checksum += byte;
00226             if (byte == XBUS_NO_PAYLOAD)
00227             {
00228                 parser->currentMessage.length = byte;
00229                 parser->currentMessage.data = NULL;
00230                 parser->state = XBPS_Checksum;
00231             }
00232             else if (byte < XBUS_EXTENDED_LENGTH)
00233             {
00234                 parser->currentMessage.length = byte;
00235                 prepareForPayload(parser);
00236                 parser->state = XBPS_Payload;
00237             }
00238             else
00239             {
00240                 parser->state = XBPS_ExtendedLengthMsb;
00241             }
00242             break;
00243 
00244         case XBPS_ExtendedLengthMsb:
00245             parser->checksum += byte;
00246             parser->currentMessage.length = ((uint16_t)byte) << 8;
00247             parser->state = XBPS_ExtendedLengthLsb;
00248             break;
00249 
00250         case XBPS_ExtendedLengthLsb:
00251             parser->checksum += byte;
00252             parser->currentMessage.length |= byte;
00253             prepareForPayload(parser);
00254             parser->state = XBPS_Payload;
00255             break;
00256 
00257         case XBPS_Payload:
00258             parser->checksum += byte;
00259             if (parser->currentMessage.data)
00260             {
00261                 ((uint8_t*)parser->currentMessage.data)[parser->payloadReceived] = byte;
00262             }
00263             if (++parser->payloadReceived == parser->currentMessage.length)
00264             {
00265                 parser->state = XBPS_Checksum;
00266             }
00267             break;
00268 
00269         case XBPS_Checksum:
00270             parser->checksum += byte;
00271             if ((parser->checksum == 0) &&
00272                     ((parser->currentMessage.length == 0) ||
00273                      parser->currentMessage.data))
00274             {
00275                 parseMessagePayload(parser);
00276                 parser->callbacks.handleMessage(&parser->currentMessage);
00277             }
00278             else if (parser->currentMessage.data)
00279             {
00280                 parser->callbacks.deallocateBuffer(parser->currentMessage.data);
00281             }
00282             parser->state = XBPS_Preamble;
00283             break;
00284     }
00285 }
00286 
00287 /*!
00288  * \brief Parse a buffer of data received from a motion tracker.
00289  */
00290 void XbusParser_parseBuffer(struct XbusParser* parser, uint8_t const* buf, size_t bufSize)
00291 {
00292     for (size_t i = 0; i < bufSize; ++i)
00293     {
00294         XbusParser_parseByte(parser, buf[i]);
00295     }
00296 }
00297