Tom N
This is a class which contains function to interface with the MLX75320
Diff: MLX_BaseSPI.c
- Revision:
- 0:dfe498e03679
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MLX_BaseSPI.c Thu Feb 25 08:02:11 2016 +0000
@@ -0,0 +1,1529 @@
+// ***** ***********************************************************************
+/*!
+Module: Melexis Base SPI
+
+Platform: Win32 - Win64 - embedded
+
+\file MLX_BaseSPI.h
+
+\brief Base control module the ASIC SPI, Melexis protocol.
+
+\author Jean-F. Bernier
+
+\since 2015-04-23
+*/
+// ***************************************************************************
+
+//*****************************************************************************
+//*************** Header includes *********************************************
+//*****************************************************************************
+//#include "Tools.h"
+#include <string.h>
+#include "MLX_BaseSPI.h"
+
+
+// Bit field widths in a packet
+#define WID_TYPE 4 /// Packet type
+#define WID_SIZE 8 /// Size
+#define WID_SEQ 4 /// Sequence number
+#define WID_DATA 16 /// Additional data
+#define WID_ADDR 16 /// Address
+#define WID_RES_DATA 32 /// Response data
+#define WID_ISERR 1 /// Is error or not
+#define WID_ERROR 15 /// Error qualifier
+
+// Bit position offsets in a short packet
+#ifdef RIGHT_TO_LEFT
+ #define OFF_TYPE (MLX_SHORT_SZ - WID_TYPE)
+ #define OFF_SIZE (OFF_TYPE - WID_SIZE)
+ #define OFF_SEQ (OFF_SIZE - WID_SEQ)
+ #define OFF_ADDR (OFF_SEQ - WID_ADDR)
+ #define OFF_DATA (OFF_ADDR - WID_DATA)
+ #define OFF_RES_DATA (OFF_SEQ - WID_RES_DATA)
+ #define OFF_ISERR (MLX_SHORT_SZ - 16 - WID_ISERR)
+ #define OFF_ERROR (OFF_ISERR - WID_ERROR)
+
+ #define BIT_ORDER 0
+#else
+ #define OFF_TYPE (WID_TYPE - 1)
+ #define OFF_SIZE (OFF_TYPE + WID_SIZE)
+ #define OFF_SEQ (OFF_SIZE + WID_SEQ)
+ #define OFF_ADDR (OFF_SEQ + WID_ADDR)
+ #define OFF_DATA (OFF_ADDR + WID_DATA)
+ #define OFF_RES_DATA (OFF_SEQ + WID_RES_DATA)
+ #define OFF_ISERR 16
+ #define OFF_ERROR (OFF_ISERR+ WID_ERROR)
+
+ #define BIT_ORDER 1
+#endif
+
+//*****************************************************************************
+//*************** Private Function Declarations *******************************
+//*****************************************************************************
+static uint16 CRC16_Compute ( const uint8 *data, uint16 n);
+static BOOL CRC16_IsValid ( const uint8 *data, uint n, uint nbCrc);
+static void FillCRC_short ( PACK_SHORT *pack);
+static void FillCRC_long1 ( PACK_LONG1 *pack);
+static void FillCRC_long2 ( PACK_LONG2 *pack);
+
+
+static BOOL ValidCRC_short( const PACK_SHORT *pack);
+static BOOL ValidCRC_long1 ( const PACK_LONG1 *pack);
+static BOOL ValidCRC_long2 ( const PACK_LONG2 *pack);
+
+
+static int EncodeS ( PACK_SHORT *pack, const MLX_PackNfo *nfo);
+static int DecodeS ( const PACK_SHORT *pack, MLX_PackNfo *nfo);
+
+static int EncodeL1(PACK_LONG1 *pack, const MLX_PackNfo *nfo);
+static int DecodeL1(const PACK_LONG1 *pack, MLX_PackNfo *nfo);
+
+static int EncodeL2(PACK_LONG2 *pack, const MLX_PackNfo *nfo);
+static int DecodeL2(const PACK_LONG2 *pack, MLX_PackNfo *nfo);
+
+//*****************************************************************************
+//*************** Function Definitions ****************************************
+//*****************************************************************************
+
+// ********************************
+// ***** MASTER SIDE FUNCTIONS ****
+// ********************************
+
+/// ****************************************************************************
+/// \fn MLX_ReqReadReg
+///
+/// \brief Creates a read register request.
+///
+/// \param[out] *pack: Packet to encode as a read register
+/// \param[in] reg: Register address to read
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-01
+/// ****************************************************************************
+int MLX_ReqReadReg( PACK_SHORT *pack, uint16 reg)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ // Reset packet!
+ memset( pack, 0, sizeof(*pack));
+
+ nfo.pktype = PACK_RREG;
+ nfo.size = 1;
+ nfo.seq = 0;
+ nfo.addr = reg;
+
+ res = EncodeS( pack, &nfo);
+ if (res < 0)
+ return -1;
+
+ return 0;
+}
+
+
+/// ****************************************************************************
+/// \fn MLX_ReqWriteReg
+///
+/// \brief Creates a write register request.
+///
+/// \param[out] *pack: Packet to encode as a write register
+/// \param[in] reg: Register address to write
+/// \param[in] val: Value to write
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-07
+/// ****************************************************************************
+int MLX_ReqWriteReg( PACK_SHORT *pack, uint16 reg, uint16 val)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ // Reset packet!
+ memset( pack, 0, sizeof(*pack));
+
+ nfo.pktype = PACK_WREG;
+ nfo.size = 1;
+ nfo.seq = 0;
+ nfo.addr = reg;
+ nfo.data = val;
+
+ res = EncodeS( pack, &nfo);
+ if (res < 0)
+ return -1;
+
+ return 0;
+}
+
+
+/// ****************************************************************************
+/// \fn MLX_ReqReadTrc
+///
+/// \brief Creates a read raw data request.
+///
+/// \param[out] *pack: Packet to encode
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-05-07
+/// ****************************************************************************
+int MLX_ReqReadTrc( PACK_SHORT *pack)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ nfo.pktype = PACK_RFIRM;
+ nfo.size = 0;
+ nfo.seq = 0;
+ nfo.fwtype = FW_RAW;
+
+ res = EncodeS( pack, &nfo);
+ if (res < 0)
+ return -1;
+
+ return 0;
+}
+
+
+/// ****************************************************************************
+/// \fn MLX_ReqReadEch
+///
+/// \brief Creates a read echoes request.
+///
+/// \param[out] *pack: Packet to encode
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-05-28
+/// ****************************************************************************
+int MLX_ReqReadEch( PACK_SHORT *pack)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ nfo.pktype = PACK_RFIRM;
+ nfo.size = 0;
+ nfo.seq = 0;
+ nfo.fwtype = FW_PROCESSED;
+
+ res = EncodeS( pack, &nfo);
+ if (res < 0)
+ return -1;
+
+ return 0;
+}
+
+
+/// ****************************************************************************
+/// \fn MLX_ReqWriteFW
+///
+/// \brief Creates a write firmware request.
+///
+/// \param[out] *pack: Packet to encode
+/// \param[in] type: Firmware type to write
+/// \param[in] size: Words (2 bytes) to write. 0 means 256. If more than 1, the
+/// data will be located in following long packets. The size field
+/// of these long packets will be the size for this long pack only.
+/// \param[in] data: Data to be written, used only if size == 1
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-05-07
+/// ****************************************************************************
+int MLX_ReqWriteFW( PACK_SHORT *pack, uint size, uint16 data)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ nfo.pktype = PACK_WFIRM;
+ nfo.fwtype = FW_PATCH;
+ nfo.size = size;
+ nfo.seq = 0;
+ nfo.data = data;
+
+ res = EncodeS( pack, &nfo);
+ if (res < 0)
+ return -1;
+
+ return 0;
+}
+
+
+/// ****************************************************************************
+/// \fn MLX_WriteDataL
+///
+/// \brief Creates a long write packet.
+///
+/// \param[out] *pack: Packet to encode
+/// \param[in] size: Words (2 bytes) to take from data. Max = MLX_LONG_DATA_SZ
+/// \param[in] seq: Serial sequence number stamping the packets
+/// \param[in] data: Data to be written
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-05-07
+/// ****************************************************************************
+int MLX_WriteDataL2( PACK_LONG2 *pack, uint size, uint seq, const uint8 *data)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ if (size > MLX_LONG2_DATA_SZ)
+ return -1;
+
+ nfo.pktype = PACK_WDATA_L;
+ nfo.size = size;
+ nfo.seq = seq;
+ nfo.dataP = data;
+
+ res = EncodeL2( pack, &nfo);
+ if (res < 0)
+ return -1;
+
+ return 0;
+}
+
+
+/// ****************************************************************************
+/// \fn MLX_DecodeResS / L
+///
+/// \brief Decodes a read or write response from slave.
+///
+/// \param[in] *pack: Packet to decode
+/// \param[out] *val: Data value extracted from response
+///
+/// \return 0 on success, negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-24
+/// ****************************************************************************
+int MLX_DecodeResS( const PACK_SHORT *pack, uint32 *val)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ if (!ValidCRC_short(pack)){
+ return -5;
+ }
+
+ //SWAP( rx+off, 1);
+
+ res = DecodeS( pack, &nfo);
+ if (res < 0 || nfo.pktype != PACK_RDATA_RESP_S)
+ if (nfo.pktype == PACK_STATUS_S) return -6;
+ else return -20;
+
+ *val = nfo.data;
+ return 0;
+}
+
+int MLX_DecodeResL1(const PACK_LONG1 *pack)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ if (!ValidCRC_long1(pack)){
+ return -9;
+ }
+
+ res = DecodeL1(pack, &nfo);
+ if (res < 0 || nfo.pktype != PACK_RDATA_RESP_L)
+ return -10;
+
+ return 0;
+}
+
+int MLX_DecodeResL2(const PACK_LONG2 *pack)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ if (!ValidCRC_long2(pack)){
+ return -9;
+ }
+
+ res = DecodeL2(pack, &nfo);
+ if (res < 0 || ((nfo.pktype != PACK_RDATA_RESP_L) && (nfo.pktype != PACK_STATUS_L)))
+ return -10;
+
+ return 0;
+}
+
+
+// ********************************
+// ***** SLAVE SIDE FUNCTIONS *****
+// ********************************
+
+/// ****************************************************************************
+/// \fn MLX_ResReadReg
+///
+/// \brief Creates a read register response.
+///
+/// \param[out] *pack: Packet to encode as a response
+/// \param[in] val: Register value
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-01
+/// ****************************************************************************
+int MLX_ResReadReg( PACK_SHORT *pack, uint16 val)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ // Reset packet!
+ memset( pack, 0, sizeof(*pack));
+
+ nfo.pktype = PACK_RDATA_RESP_S;
+ nfo.size = 1;
+ nfo.seq = 0;
+ nfo.data = val;
+
+ res = EncodeS( pack, &nfo);
+ if (res < 0)
+ return -1;
+
+ return 0;
+}
+
+/// ****************************************************************************
+/// \fn MLX_DecodeReqS
+///
+/// \brief Decodes a request from master.
+///
+/// \param[in] *pack: Packet to decode
+/// \param[out] *nfo: Filled info struct
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-28
+/// ****************************************************************************
+int MLX_DecodeReqS( const PACK_SHORT *pack, MLX_PackNfo *nfo)
+{
+ int res;
+
+ if (!ValidCRC_short(pack)){
+ return -1;
+ }
+
+ res = DecodeS( pack, nfo);
+ if (res < 0)
+ return -1;
+
+ return 0;
+}
+
+
+/// ****************************************************************************
+/// \fn MLX_ResReadData
+///
+/// \brief Encodes a long read response.
+///
+/// \param[in] *pack: Packet to decode
+/// \param[out] *nfo: Filled info struct
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-05-07
+/// ****************************************************************************
+int MLX_ResReadData( PACK_LONG1 *pack, uint size, uint seq, const uint8 *data)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ if (size > MLX_LONG1_DATA_SZ)
+ return -1;
+
+ nfo.pktype = PACK_RDATA_RESP_L;
+ nfo.size = size;
+ nfo.seq = seq;
+ nfo.dataP = data;
+
+ res = EncodeL1( pack, &nfo);
+ if (res < 0)
+ return -1;
+
+ return 0;
+}
+
+/// ****************************************************************************
+/// \fn MLX_ResReadDataE
+///
+/// \brief Encodes an echo read response.
+///
+/// \param[in] *pack: Packet to decode
+/// \param[out] *nfo: Filled info struct
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-05-07
+/// ****************************************************************************
+int MLX_ResReadDataL2(PACK_LONG2 *pack, uint size, uint seq, const uint8 *data)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ if (size > MLX_LONG2_DATA_SZ)
+ return -1;
+
+ nfo.pktype = PACK_RDATA_RESP_L;
+ nfo.size = size;
+ nfo.seq = seq;
+ nfo.dataP = data;
+
+ res = EncodeL2(pack, &nfo);
+ if (res < 0)
+ return -1;
+
+ return 0;
+}
+
+
+// ********************************
+// ****** COMMON FUNCTIONS ********
+// ********************************
+
+/// ****************************************************************************
+/// \fn FillCRC_short / long
+///
+/// \brief Computes and sets the CRC field in a packet.
+///
+/// \param[in/out] *pack: Packet on which to set the CRC
+///
+/// \return Nothing.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-24
+/// ****************************************************************************
+void FillCRC_short( PACK_SHORT *pack)
+{
+ pack->crc = CRC16_Compute( (uint8*)pack, sizeof(*pack)-MLX_CRC_SZ);
+ //SWAP(&pack->crc,2);
+}
+
+void FillCRC_long1(PACK_LONG1 *pack)
+{
+ pack->crc = CRC16_Compute((uint8*)pack, sizeof(*pack) - MLX_CRC_SZ);
+ //SWAP(&pack->crc,2);
+}
+
+void FillCRC_long2(PACK_LONG2 *pack)
+{
+ pack->crc = CRC16_Compute((uint8*)pack, sizeof(*pack) - MLX_CRC_SZ);
+ //SWAP(&pack->crc,2);
+}
+
+/// ****************************************************************************
+/// \fn ValidCRC_short / long
+///
+/// \brief Validates the CRC of a packet.
+///
+/// \param[in/out] pack: Packet on which to validate the CRC
+///
+/// \return Validation status. TRUE if valid, FALSE if invalid.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-24
+/// ****************************************************************************
+BOOL ValidCRC_short( const PACK_SHORT *pack)
+{
+ return CRC16_IsValid( (uint8*)pack, sizeof(*pack), MLX_CRC_SZ);
+}
+
+BOOL ValidCRC_long1( const PACK_LONG1 *pack)
+{
+ return CRC16_IsValid( (uint8*)pack, sizeof(*pack), MLX_CRC_SZ);
+}
+
+BOOL ValidCRC_long2(const PACK_LONG2 *pack)
+{
+ return CRC16_IsValid((uint8*)pack, sizeof(*pack), MLX_CRC_SZ);
+}
+
+/// ****************************************************************************
+/// \fn CRC16_Compute
+///
+/// \brief Computes the CRC16 of a data vector.
+/// This is the CCITT CRC 16 polynomial X^16 + X^12 + X^5 + 1.
+/// This works out to be 0x1021, but the way the algorithm works
+/// lets us use 0x8408 (the reverse of the bit pattern). The high
+/// bit is always assumed to be set, thus we only use 16 bits to
+/// represent the 17 bit value.
+///
+/// Reference: stjarnhimlen.se/snippets/crc-16.c
+///
+/// \param[in] *data: Data vector for which to compute the CRC
+/// \param[in] n: Number of bytes in data
+///
+/// \return The CRC
+///
+/// \authors Jean-F. Bernier
+/// \since 2015-03-31
+/// ****************************************************************************
+/* uint16 CRC16_Compute( const uint8 *data, uint16 n)
+{
+ uint8 a;
+ uint tmp;
+ uint crc = 0x0000;
+
+ const uint POLY = 0x8408;
+
+ if (n == 0)
+ return (~crc);
+
+ do
+ {
+ tmp = (uint)0xFF & *data++;
+
+ for (a = 0 ; a < 8; ++a)
+ {
+ tmp >>= 1;
+ if ((crc & 0x0001) ^ (tmp & 0x0001))
+ crc = (crc >> 1) ^ POLY;
+ else
+ crc >>= 1;
+ }
+ } while (--n);
+
+ crc = ~crc;
+ tmp = crc;
+ crc = (crc << 8) | (tmp >> 8 & 0xFF);
+
+ return crc;
+}
+*/
+
+/*
+* Copyright 2001-2010 Georges Menie (www.menie.org)
+* All rights reserved.
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in the
+* documentation and/or other materials provided with the distribution.
+* * Neither the name of the University of California, Berkeley nor the
+* names of its contributors may be used to endorse or promote products
+* derived from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+* DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+/* CRC16 implementation acording to CCITT standards */
+
+static const uint16 crc16tab[256] = {
+ 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
+ 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
+ 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
+ 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
+ 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
+ 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
+ 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
+ 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
+ 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
+ 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
+ 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
+ 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
+ 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
+ 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
+ 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
+ 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
+ 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
+ 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
+ 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
+ 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
+ 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
+ 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+ 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
+ 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
+ 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
+ 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
+ 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
+ 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
+ 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
+ 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
+ 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
+ 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
+};
+
+uint16 CRC16_Compute(const uint8 *buf, uint16 len)
+{
+ uint16 tmp;
+ int counter;
+ uint16 crc = 0;
+ for (counter = 0; counter < len; counter++)
+ crc = (crc << 8) ^ crc16tab[((crc >> 8) ^ *(char *)buf++) & 0x00FF];
+
+ tmp = crc;
+ crc = (crc << 8) | (tmp >> 8 & 0xFF);
+
+ return crc;
+}
+
+
+/// ****************************************************************************
+/// \fn CRC16_IsValid
+///
+/// \brief Validates the CRC in a byte vector. Computes CRC on all bytes except
+/// the X lasts CRC bytes and compares it to the one found in these X lasts.
+///
+/// \param[in] *data: Data vector for which to validate the CRC
+/// \param[in] n: Total bytes in data
+/// \param[in] nbCrc: Nb of bytes at the end of data for the CRC
+///
+/// \return TRUE if valid, FALSE otherwise.
+///
+/// \authors Jean-F. Bernier
+/// \since 2014-10-21
+/// ****************************************************************************
+BOOL CRC16_IsValid( const uint8 *data, uint n, uint nbCrc)
+{
+ if (n <= 1){
+ return FALSE;
+ }
+
+ uint16 test = *(uint16*)(data+n-nbCrc);
+ //SWAP( &test, 1);
+ uint16 crc = CRC16_Compute( data, n-nbCrc);
+ return (crc == test);
+}
+
+
+
+/// ****************************************************************************
+/// \fn SetBitVector
+///
+/// \brief Sets a value in a vector of bits at chosen offset and bit width.
+/// Input bits all pre-supposed at zero in buffer.
+///
+/// \param[in] *buf: Target vector of bits to be modified. The first byte will contain
+/// the lowest bits (at lowest offset).
+/// \param[in] off: Position offset in bits where to put val, 0-based
+/// \param[in] wid: Number of bits to take from val [0 32]
+/// \param[in] val: Value to set in buf. Its low bit is always set first at chosen offset.
+/// \param[in] inv: Inverse bit ordering inside a byte. Usually, bit#0 is the least significant to the right.
+/// With this, bit#0 becomes the most significant to the left.
+///
+/// \return Error code: 0 on success, negative otherwise.
+///
+/// \authors Jean-F. Bernier
+/// \since 2015-04-01
+/// ****************************************************************************
+int SetBitVector( uint8 *buf, uint32 off, uchar wid, uint32 val, uchar inv)
+{
+ if (wid <= 0 || wid > 32 || buf == NULL)
+ return -1;
+
+ if (inv && wid > off+1)
+ return -2;
+
+ buf += (off >> 3); // Byte count to move
+ uint8 rem = off % 8; // Remaining bits to move
+
+ // While there is a remaining bit to set...
+ while (wid >= 1)
+ {
+ // Check the bit value. If zero, nothing to do.
+ if (val & 1)
+ {
+ if (!inv)
+ BIT_SET( *buf, rem);
+ else
+ BIT_SET( *buf, 7-rem);
+ }
+
+ if (!inv)
+ {
+ // Wrap around. Goto next byte when setting bit index > 7
+ ++rem;
+ if (rem >= 8)
+ {
+ rem = 0;
+ ++buf;
+ }
+ }
+ else
+ {
+ // Wrap around. Goto previous byte when setting "negative" bit index
+ --rem;
+ if (rem >= 8)
+ {
+ rem = 7;
+ --buf;
+ }
+ }
+
+ // Activate the next bit
+ val >>= 1;
+ --wid;
+ }
+
+ return 0;
+}
+
+
+/// ****************************************************************************
+/// \fn GetBitVector
+///
+/// \brief Gets a sub-array of bits inside a bigger one.
+/// Selects the bits to obtain using offset and bit width.
+///
+/// \param[in] *buf: Target vector of bits to be read. The first byte will contain
+/// the lowest bits (at lowest offset).
+/// \param[in] off: Position offset in bits where to start reading the low bit, 0-based
+/// \param[in] wid: Number of bits to read into val [0 32]
+/// \param[out] *val: Value obtained from buf.
+/// \param[in] inv: Inverse bit ordering inside a byte. Usually, bit#0 is the least significant to the right.
+/// With this, bit#0 becomes the most significant to the left.
+///
+/// \return Error code: 0 on success, negative otherwise.
+///
+/// \authors Jean-F. Bernier
+/// \since 2015-04-28
+/// ****************************************************************************
+int GetBitVector( const uint8 *buf, uint32 off, uchar wid, uint32 *val, uchar inv)
+{
+ if (wid <= 0 || wid > 32 || buf == NULL)
+ return -1;
+
+ if (inv && wid > off+1)
+ return -2;
+
+ buf += (off >> 3); // Byte count to move
+ uint8 rem = off % 8; // Remaining bits to move
+ uchar bit, b = 0;
+ *val = 0;
+
+ // While there is a remaining bit to set...
+ while (wid >= 1)
+ {
+ if (!inv)
+ bit = BIT_GET( *buf, rem);
+ else
+ bit = BIT_GET( *buf, 7-rem);
+
+ if (bit)
+ BIT_SET( *val, b);
+
+ if (!inv)
+ {
+ // Wrap around. Goto next byte when setting bit index > 7
+ ++rem;
+ if (rem >= 8)
+ {
+ rem = 0;
+ ++buf;
+ }
+ }
+ else
+ {
+ // Wrap around. Goto previous byte when setting "negative" bit index
+ --rem;
+ if (rem >= 8)
+ {
+ rem = 7;
+ --buf;
+ }
+ }
+
+ // Activate the next bit
+ --wid;
+ ++b;
+ }
+
+ return 0;
+}
+
+/// ****************************************************************************
+/// \fn ReverseBits
+///
+/// \brief Reverses a bit pattern like a mirror. First bits becomes last.
+/// Ex.: 011001 <-> 100110 (v = 25, n = 6)
+///
+/// \param[in] bits: Value to use as bit pattern
+/// \param[in] nb: Number of bits to take from v. Starts at low bits.
+///
+/// \return The value with a reversed bit pattern.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-30
+/// ****************************************************************************
+/*uint32 ReverseBits( uint32 bits, char nb)
+{
+ // If n == 0, no bits to use. So result will be zero.
+ uint32 res = 0;
+
+ while ((signed char)--nb >= 0)
+ {
+ res <<= 1; // Shift current result by 1
+ res += (bits & 1); // Take the current bit into result
+ bits >>= 1; // Prepare for next bit
+ }
+
+ return res;
+}
+*/
+/// ****************************************************************************
+/// \fn EncodeS
+///
+/// \brief Generic short packet encoder.
+///
+/// \param[out] *pack: Resulting packet encoded with nfo
+/// \param[in] *nfo: Info used to encode packet
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-27
+/// ****************************************************************************
+int EncodeS( PACK_SHORT *pack, const MLX_PackNfo *nfo)
+{
+ int res;
+
+ // Reject long packets
+ if (nfo->pktype == PACK_STATUS_L || nfo->pktype == PACK_RDATA_RESP_L || nfo->pktype == PACK_WDATA_L)
+ {
+ return -1;
+ }
+
+ if (nfo->pktype == PACK_STATUS_S)
+ {
+ res = SetBitVector( pack->buf, OFF_TYPE, WID_TYPE, nfo->pktype, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Is error field
+ res = SetBitVector( pack->buf, OFF_ISERR, WID_ISERR, nfo->iserr, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Error qualifier field
+ res = SetBitVector( pack->buf, OFF_ERROR, WID_ERROR, nfo->error, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ goto END;
+ }
+
+ // Packet type field
+ res = SetBitVector( pack->buf, OFF_TYPE, WID_TYPE, nfo->pktype, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Size field
+ res = SetBitVector( pack->buf, OFF_SIZE, WID_SIZE, nfo->size, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Sequence field
+ res = SetBitVector( pack->buf, OFF_SEQ, WID_SEQ, nfo->seq, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Address field
+ if (nfo->pktype == PACK_RREG || nfo->pktype == PACK_WREG)
+ {
+ res = SetBitVector( pack->buf, OFF_ADDR, WID_ADDR, nfo->addr, BIT_ORDER);
+ }
+ else if (nfo->pktype == PACK_RFIRM)
+ {
+ res = SetBitVector( pack->buf, OFF_ADDR, WID_ADDR, nfo->fwtype, BIT_ORDER);
+ }
+ else if (nfo->pktype == PACK_WFIRM)
+ {
+ res = SetBitVector(pack->buf, OFF_ADDR, WID_ADDR, nfo->fwtype, BIT_ORDER);
+ }
+
+ if (res < 0)
+ return -1;
+
+ // Data field
+ if (nfo->pktype == PACK_RDATA_RESP_S)
+ {
+ res = SetBitVector( pack->buf, OFF_RES_DATA, WID_RES_DATA, nfo->data, BIT_ORDER);
+ }
+ else if (nfo->pktype == PACK_WREG)
+ {
+ res = SetBitVector( pack->buf, OFF_DATA, WID_DATA, nfo->data, BIT_ORDER);
+ }
+ else if (nfo->pktype == PACK_WFIRM)
+ {
+ res = SetBitVector(pack->buf, OFF_DATA, WID_DATA, nfo->data, BIT_ORDER);
+ }
+
+ if (res < 0)
+ return -1;
+
+ //SWAP(&pack,sizeof(*pack));
+
+END:
+ FillCRC_short(pack);
+ //SWAP( &pack->crc, 1);
+
+ return 0;
+}
+
+/// ****************************************************************************
+/// \fn DecodeS
+///
+/// \brief Generic short packet decoder.
+///
+/// \param[in] *pack: Packet to be decoded into info
+/// \param[out] *nfo: Filled packet info from pack data
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-27
+/// ****************************************************************************
+int DecodeS( const PACK_SHORT *pack, MLX_PackNfo *nfo)
+{
+ uint32 v;
+ int res;
+
+ //SWAP(&pack, sizeof(*pack));
+
+ res = GetBitVector( pack->buf, OFF_TYPE, WID_TYPE, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+ nfo->pktype = (PackType)v;
+
+ res = GetBitVector( pack->buf, OFF_SIZE, WID_SIZE, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+ nfo->size = v;
+
+ res = GetBitVector( pack->buf, OFF_SEQ, WID_SEQ, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+ nfo->seq = v;
+
+ if (nfo->pktype == PACK_STATUS_S)
+ {
+ res = GetBitVector( pack->buf, OFF_ISERR, WID_ISERR, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ nfo->iserr = v;
+
+ res = GetBitVector( pack->buf, OFF_ERROR, WID_ERROR, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ nfo->error = v;
+ return 0;
+ }
+
+
+ if (nfo->pktype == PACK_RREG || nfo->pktype == PACK_WREG)
+ {
+ res = GetBitVector( pack->buf, OFF_ADDR, WID_ADDR, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ nfo->addr = v;
+ }
+
+
+ v = 0;
+ if (nfo->pktype == PACK_RDATA_RESP_S)
+ {
+ res = GetBitVector( pack->buf, OFF_RES_DATA, WID_RES_DATA, &v, BIT_ORDER);
+ }
+ else if (nfo->pktype == PACK_WREG || nfo->pktype == PACK_WFIRM)
+ {
+ res = GetBitVector( pack->buf, OFF_DATA, WID_DATA, &v, BIT_ORDER);
+ }
+
+ if (res < 0)
+ return -1;
+
+ nfo->data = v;
+
+ return 0;
+}
+
+/// ****************************************************************************
+/// \fn EncodeL
+///
+/// \brief Generic long packet encoder.
+///
+/// \param[out] *pack: Resulting packet encoded with nfo
+/// \param[in] *nfo: Info used to encode packet
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-27
+/// ****************************************************************************
+int EncodeL1( PACK_LONG1 *pack, const MLX_PackNfo *nfo)
+{
+ int res;
+
+ // Reset packet!
+ memset( pack, 0, sizeof(*pack));
+
+ // Reject short packets
+ if (nfo->pktype != PACK_STATUS_L && nfo->pktype != PACK_RDATA_RESP_L && nfo->pktype != PACK_WDATA_L)
+ {
+ return -1;
+ }
+
+ if (nfo->pktype == PACK_STATUS_L)
+ {
+ res = SetBitVector( pack->buf, OFF_TYPE, WID_TYPE, nfo->pktype, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Is error field
+ res = SetBitVector( pack->buf, OFF_ISERR, WID_ISERR, nfo->iserr, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Error qualifier field
+ res = SetBitVector( pack->buf, OFF_ERROR, WID_ERROR, nfo->error, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ goto END;
+ }
+
+ // Packet type field
+ res = SetBitVector( pack->buf, OFF_TYPE, WID_TYPE, nfo->pktype, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Size field
+ res = SetBitVector( pack->buf, OFF_SIZE, WID_SIZE, nfo->size, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Sequence field
+ res = SetBitVector( pack->buf, OFF_SEQ, WID_SEQ, nfo->seq, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Data field
+ memcpy( pack->data, nfo->dataP, nfo->size);
+ if (res < 0)
+ return -1;
+
+ //SWAP(&pack,sizeof(*pack));
+
+END:
+ FillCRC_long1(pack);
+ //SWAP( &pack->crc, 1);
+
+ return 0;
+}
+
+
+
+/// ****************************************************************************
+/// \fn DecodeL
+///
+/// \brief Generic long packet decoder.
+///
+/// \param[in] *pack: Packet to be decoded into info
+/// \param[out] *nfo: Filled packet info from pack data
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-27
+/// ****************************************************************************
+int DecodeL1( const PACK_LONG1 *pack, MLX_PackNfo *nfo)
+{
+ uint32 v;
+ int res;
+
+ //SWAP(&pack,sizeof(*pack));
+
+ res = GetBitVector( pack->buf, OFF_TYPE, WID_TYPE, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+ nfo->pktype = (PackType)v;
+
+ res = GetBitVector( pack->buf, OFF_SIZE, WID_SIZE, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+ nfo->size = v;
+
+ res = GetBitVector( pack->buf, OFF_SEQ, WID_SEQ, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+ nfo->seq = v;
+
+ if (nfo->pktype == PACK_STATUS_L)
+ {
+ res = GetBitVector( pack->buf, OFF_ISERR, WID_ISERR, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ nfo->iserr = v;
+
+ res = GetBitVector( pack->buf, OFF_ERROR, WID_ERROR, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ nfo->error = v;
+ return 0;
+ }
+
+ return 0;
+}
+
+/// ****************************************************************************
+/// \fn EncodeL2
+///
+/// \brief Generic long packet encoder.
+///
+/// \param[out] *pack: Resulting packet encoded with nfo
+/// \param[in] *nfo: Info used to encode packet
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-27
+/// ****************************************************************************
+int EncodeL2(PACK_LONG2 *pack, const MLX_PackNfo *nfo)
+{
+ int res;
+
+ // Reset packet!
+ memset(pack, 0, sizeof(*pack));
+
+ // Reject short packets
+ if (nfo->pktype != PACK_STATUS_L && nfo->pktype != PACK_RDATA_RESP_L && nfo->pktype != PACK_WDATA_L)
+ {
+ return -1;
+ }
+
+ if (nfo->pktype == PACK_STATUS_L)
+ {
+ res = SetBitVector(pack->buf, OFF_TYPE, WID_TYPE, nfo->pktype, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Is error field
+ res = SetBitVector(pack->buf, OFF_ISERR, WID_ISERR, nfo->iserr, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Error qualifier field
+ res = SetBitVector(pack->buf, OFF_ERROR, WID_ERROR, nfo->error, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ goto END;
+ }
+
+ // Packet type field
+ res = SetBitVector(pack->buf, OFF_TYPE, WID_TYPE, nfo->pktype, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Size field
+ res = SetBitVector(pack->buf, OFF_SIZE, WID_SIZE, nfo->size, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Sequence field
+ res = SetBitVector(pack->buf, OFF_SEQ, WID_SEQ, nfo->seq, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ // Data field
+ memcpy(pack->data, nfo->dataP, nfo->size*2);
+
+ if (res < 0)
+ return -1;
+
+ //SWAP(&pack,sizeof(*pack));
+
+END:
+ FillCRC_long2(pack);
+ //SWAP( &pack->crc, 1);
+
+ return 0;
+}
+
+
+
+/// ****************************************************************************
+/// \fn DecodeE
+///
+/// \brief Generic long packet decoder.
+///
+/// \param[in] *pack: Packet to be decoded into info
+/// \param[out] *nfo: Filled packet info from pack data
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-27
+/// ****************************************************************************
+int DecodeL2(const PACK_LONG2 *pack, MLX_PackNfo *nfo)
+{
+ uint32 v;
+ int res;
+
+ //SWAP(&pack,sizeof(*pack));
+
+ res = GetBitVector(pack->buf, OFF_TYPE, WID_TYPE, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+ nfo->pktype = (PackType)v;
+
+ res = GetBitVector(pack->buf, OFF_SIZE, WID_SIZE, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+ nfo->size = v;
+
+ res = GetBitVector(pack->buf, OFF_SEQ, WID_SEQ, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+ nfo->seq = v;
+
+ if (nfo->pktype == PACK_STATUS_L)
+ {
+ res = GetBitVector(pack->buf, OFF_ISERR, WID_ISERR, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ nfo->iserr = v;
+
+ res = GetBitVector(pack->buf, OFF_ERROR, WID_ERROR, &v, BIT_ORDER);
+ if (res < 0)
+ return -1;
+
+ nfo->error = v;
+ return 0;
+ }
+
+ return 0;
+}
+
+
+
+
+/// ****************************************************************************
+/// \fn MLX_EncodeStatusS
+///
+/// \brief Creates a short status packet.
+///
+/// \param[out] *pack: Resulting encoded packet
+/// \param[in] iserr: Flag telling if it is an error or not
+/// \param[in] err: Error qualifier data
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-04-28
+/// ****************************************************************************
+int MLX_EncodeStatusS( PACK_SHORT *pack, uint8 iserr, uint16 err)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ // Reset packet!
+ memset( pack, 0, sizeof(*pack));
+
+ nfo.pktype = PACK_STATUS_S;
+ nfo.iserr = iserr;
+ nfo.error = err;
+
+ res = EncodeS( pack, &nfo);
+ if (res < 0)
+ return -1;
+
+ return 0;
+}
+
+/// ****************************************************************************
+/// \fn MLX_DecodeStatusS
+///
+/// \brief Attempts to decode a packet as a status packet.
+///
+/// \param[in] *pack: Packet to decode
+/// \param[out] *iserr: Flag telling if it is an error or not
+/// \param[out] *err: Error qualifier data
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-05-01
+/// ****************************************************************************
+int MLX_DecodeStatusS( const PACK_SHORT *pack, uint8 *iserr, uint16 *err)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ if (!ValidCRC_short(pack)){
+ return -1;
+ }
+
+ res = DecodeS( pack, &nfo);
+ if (res < 0 || nfo.pktype != PACK_STATUS_S)
+ return -1;
+
+ *iserr = nfo.iserr;
+ *err = nfo.error;
+
+ return 0;
+}
+
+
+/// ****************************************************************************
+/// \fn MLX_EncodeStatusL
+///
+/// \brief Creates a long status packet.
+///
+/// \param[out] *pack: Resulting encoded packet
+/// \param[in] iserr: Flag telling if it is an error or not
+/// \param[in] err: Error qualifier data
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-05-19
+/// ****************************************************************************
+int MLX_EncodeStatusL1( PACK_LONG1 *pack, uint8 iserr, uint16 err)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ // Reset packet!
+ memset(pack, 0, sizeof(*pack));
+
+ nfo.pktype = PACK_STATUS_L;
+ nfo.iserr = iserr;
+ nfo.error = err;
+
+ res = EncodeL1( pack, &nfo);
+ if (res < 0)
+ return -1;
+
+ return 0;
+}
+
+/// ****************************************************************************
+/// \fn MLX_DecodeStatusL
+///
+/// \brief Attempts to decode a packet as a status packet.
+///
+/// \param[in] *pack: Packet to decode
+/// \param[out] *iserr: Flag telling if it is an error or not
+/// \param[out] *err: Error qualifier data
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-05-19
+/// ****************************************************************************
+int MLX_DecodeStatusL1( const PACK_LONG1 *pack, uint8 *iserr, uint16 *err)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ if (!ValidCRC_long1(pack))
+ return -1;
+
+ res = DecodeL1( pack, &nfo);
+ if (res < 0 || nfo.pktype != PACK_STATUS_L)
+ return -1;
+
+ *iserr = nfo.iserr;
+ *err = nfo.error;
+
+ return 0;
+}
+
+/// ****************************************************************************
+/// \fn MLX_EncodeStatusL2
+///
+/// \brief Creates a echo status packet.
+///
+/// \param[out] *pack: Resulting encoded packet
+/// \param[in] iserr: Flag telling if it is an error or not
+/// \param[in] err: Error qualifier data
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-05-19
+/// ****************************************************************************
+int MLX_EncodeStatusL2(PACK_LONG2 *pack, uint8 iserr, uint16 err)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ // Reset packet!
+ memset(pack, 0, sizeof(*pack));
+
+ nfo.pktype = PACK_STATUS_L;
+ nfo.iserr = iserr;
+ nfo.error = err;
+
+ res = EncodeL2(pack, &nfo);
+ if (res < 0)
+ return -1;
+
+ return 0;
+}
+
+/// ****************************************************************************
+/// \fn MLX_DecodeStatusL2
+///
+/// \brief Attempts to decode a packet as a status packet.
+///
+/// \param[in] *pack: Packet to decode
+/// \param[out] *iserr: Flag telling if it is an error or not
+/// \param[out] *err: Error qualifier data
+///
+/// \return Error code: 0 on success or negative on error.
+///
+/// \authors Jean-Fran�ois Bernier
+/// \since 2015-05-19
+/// ****************************************************************************
+int MLX_DecodeStatusL2(const PACK_LONG2 *pack, uint8 *iserr, uint16 *err)
+{
+ int res;
+ MLX_PackNfo nfo;
+
+ if (!ValidCRC_long2(pack))
+ return -1;
+
+ res = DecodeL2(pack, &nfo);
+ if (res < 0 || nfo.pktype != PACK_STATUS_L)
+ return -1;
+
+ *iserr = nfo.iserr;
+ *err = nfo.error;
+
+ return 0;
+}
+
+