Tom N
/
LidarSpi
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
MLX_BaseSPI.c
- Committer:
- TNU
- Date:
- 2016-02-25
- Revision:
- 0:dfe498e03679
File content as of revision 0:dfe498e03679:
// ***** ***********************************************************************
/*!
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;
}