Tom N / LidarSpi

This is a class which contains function to interface with the MLX75320

Dependents:   MLX75320_API

MLX_BaseSPI.c@13:ccf4ab73c33d, 2016-05-17 (annotated)

Committer:
TNU
Date:
Tue May 17 18:54:04 2016 +0000
Revision:
13:ccf4ab73c33d
Parent:
0:dfe498e03679 
Added documentation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TNU 0:dfe498e03679 1 // ***** ***********************************************************************
TNU 0:dfe498e03679 2 /*!
TNU 0:dfe498e03679 3 Module: Melexis Base SPI
TNU 0:dfe498e03679 4
TNU 0:dfe498e03679 5 Platform: Win32 - Win64 - embedded
TNU 0:dfe498e03679 6
TNU 0:dfe498e03679 7 \file MLX_BaseSPI.h
TNU 0:dfe498e03679 8
TNU 0:dfe498e03679 9 \brief Base control module the ASIC SPI, Melexis protocol.
TNU 0:dfe498e03679 10
TNU 0:dfe498e03679 11 \author Jean-F. Bernier
TNU 0:dfe498e03679 12
TNU 0:dfe498e03679 13 \since 2015-04-23
TNU 0:dfe498e03679 14 */
TNU 0:dfe498e03679 15 // ***************************************************************************
TNU 0:dfe498e03679 16
TNU 0:dfe498e03679 17 //*****************************************************************************
TNU 0:dfe498e03679 18 //*************** Header includes *********************************************
TNU 0:dfe498e03679 19 //*****************************************************************************
TNU 0:dfe498e03679 20 //#include "Tools.h"
TNU 0:dfe498e03679 21 #include <string.h>
TNU 0:dfe498e03679 22 #include "MLX_BaseSPI.h"
TNU 0:dfe498e03679 23
TNU 0:dfe498e03679 24
TNU 0:dfe498e03679 25 // Bit field widths in a packet
TNU 0:dfe498e03679 26 #define WID_TYPE 4 /// Packet type
TNU 0:dfe498e03679 27 #define WID_SIZE 8 /// Size
TNU 0:dfe498e03679 28 #define WID_SEQ 4 /// Sequence number
TNU 0:dfe498e03679 29 #define WID_DATA 16 /// Additional data
TNU 0:dfe498e03679 30 #define WID_ADDR 16 /// Address
TNU 0:dfe498e03679 31 #define WID_RES_DATA 32 /// Response data
TNU 0:dfe498e03679 32 #define WID_ISERR 1 /// Is error or not
TNU 0:dfe498e03679 33 #define WID_ERROR 15 /// Error qualifier
TNU 0:dfe498e03679 34
TNU 0:dfe498e03679 35 // Bit position offsets in a short packet
TNU 0:dfe498e03679 36 #ifdef RIGHT_TO_LEFT
TNU 0:dfe498e03679 37 #define OFF_TYPE (MLX_SHORT_SZ - WID_TYPE)
TNU 0:dfe498e03679 38 #define OFF_SIZE (OFF_TYPE - WID_SIZE)
TNU 0:dfe498e03679 39 #define OFF_SEQ (OFF_SIZE - WID_SEQ)
TNU 0:dfe498e03679 40 #define OFF_ADDR (OFF_SEQ - WID_ADDR)
TNU 0:dfe498e03679 41 #define OFF_DATA (OFF_ADDR - WID_DATA)
TNU 0:dfe498e03679 42 #define OFF_RES_DATA (OFF_SEQ - WID_RES_DATA)
TNU 0:dfe498e03679 43 #define OFF_ISERR (MLX_SHORT_SZ - 16 - WID_ISERR)
TNU 0:dfe498e03679 44 #define OFF_ERROR (OFF_ISERR - WID_ERROR)
TNU 0:dfe498e03679 45
TNU 0:dfe498e03679 46 #define BIT_ORDER 0
TNU 0:dfe498e03679 47 #else
TNU 0:dfe498e03679 48 #define OFF_TYPE (WID_TYPE - 1)
TNU 0:dfe498e03679 49 #define OFF_SIZE (OFF_TYPE + WID_SIZE)
TNU 0:dfe498e03679 50 #define OFF_SEQ (OFF_SIZE + WID_SEQ)
TNU 0:dfe498e03679 51 #define OFF_ADDR (OFF_SEQ + WID_ADDR)
TNU 0:dfe498e03679 52 #define OFF_DATA (OFF_ADDR + WID_DATA)
TNU 0:dfe498e03679 53 #define OFF_RES_DATA (OFF_SEQ + WID_RES_DATA)
TNU 0:dfe498e03679 54 #define OFF_ISERR 16
TNU 0:dfe498e03679 55 #define OFF_ERROR (OFF_ISERR+ WID_ERROR)
TNU 0:dfe498e03679 56
TNU 0:dfe498e03679 57 #define BIT_ORDER 1
TNU 0:dfe498e03679 58 #endif
TNU 0:dfe498e03679 59
TNU 0:dfe498e03679 60 //*****************************************************************************
TNU 0:dfe498e03679 61 //*************** Private Function Declarations *******************************
TNU 0:dfe498e03679 62 //*****************************************************************************
TNU 0:dfe498e03679 63 static uint16 CRC16_Compute ( const uint8 *data, uint16 n);
TNU 0:dfe498e03679 64 static BOOL CRC16_IsValid ( const uint8 *data, uint n, uint nbCrc);
TNU 0:dfe498e03679 65 static void FillCRC_short ( PACK_SHORT *pack);
TNU 0:dfe498e03679 66 static void FillCRC_long1 ( PACK_LONG1 *pack);
TNU 0:dfe498e03679 67 static void FillCRC_long2 ( PACK_LONG2 *pack);
TNU 0:dfe498e03679 68
TNU 0:dfe498e03679 69
TNU 0:dfe498e03679 70 static BOOL ValidCRC_short( const PACK_SHORT *pack);
TNU 0:dfe498e03679 71 static BOOL ValidCRC_long1 ( const PACK_LONG1 *pack);
TNU 0:dfe498e03679 72 static BOOL ValidCRC_long2 ( const PACK_LONG2 *pack);
TNU 0:dfe498e03679 73
TNU 0:dfe498e03679 74
TNU 0:dfe498e03679 75 static int EncodeS ( PACK_SHORT *pack, const MLX_PackNfo *nfo);
TNU 0:dfe498e03679 76 static int DecodeS ( const PACK_SHORT *pack, MLX_PackNfo *nfo);
TNU 0:dfe498e03679 77
TNU 0:dfe498e03679 78 static int EncodeL1(PACK_LONG1 *pack, const MLX_PackNfo *nfo);
TNU 0:dfe498e03679 79 static int DecodeL1(const PACK_LONG1 *pack, MLX_PackNfo *nfo);
TNU 0:dfe498e03679 80
TNU 0:dfe498e03679 81 static int EncodeL2(PACK_LONG2 *pack, const MLX_PackNfo *nfo);
TNU 0:dfe498e03679 82 static int DecodeL2(const PACK_LONG2 *pack, MLX_PackNfo *nfo);
TNU 0:dfe498e03679 83
TNU 0:dfe498e03679 84 //*****************************************************************************
TNU 0:dfe498e03679 85 //*************** Function Definitions ****************************************
TNU 0:dfe498e03679 86 //*****************************************************************************
TNU 0:dfe498e03679 87
TNU 0:dfe498e03679 88 // ********************************
TNU 0:dfe498e03679 89 // ***** MASTER SIDE FUNCTIONS ****
TNU 0:dfe498e03679 90 // ********************************
TNU 0:dfe498e03679 91
TNU 0:dfe498e03679 92 /// ****************************************************************************
TNU 0:dfe498e03679 93 /// \fn MLX_ReqReadReg
TNU 0:dfe498e03679 94 ///
TNU 0:dfe498e03679 95 /// \brief Creates a read register request.
TNU 0:dfe498e03679 96 ///
TNU 0:dfe498e03679 97 /// \param[out] *pack: Packet to encode as a read register
TNU 0:dfe498e03679 98 /// \param[in] reg: Register address to read
TNU 0:dfe498e03679 99 ///
TNU 0:dfe498e03679 100 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 101 ///
TNU 0:dfe498e03679 102 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 103 /// \since 2015-04-01
TNU 0:dfe498e03679 104 /// ****************************************************************************
TNU 0:dfe498e03679 105 int MLX_ReqReadReg( PACK_SHORT *pack, uint16 reg)
TNU 0:dfe498e03679 106 {
TNU 0:dfe498e03679 107 int res;
TNU 0:dfe498e03679 108 MLX_PackNfo nfo;
TNU 0:dfe498e03679 109
TNU 0:dfe498e03679 110 // Reset packet!
TNU 0:dfe498e03679 111 memset( pack, 0, sizeof(*pack));
TNU 0:dfe498e03679 112
TNU 0:dfe498e03679 113 nfo.pktype = PACK_RREG;
TNU 0:dfe498e03679 114 nfo.size = 1;
TNU 0:dfe498e03679 115 nfo.seq = 0;
TNU 0:dfe498e03679 116 nfo.addr = reg;
TNU 0:dfe498e03679 117
TNU 0:dfe498e03679 118 res = EncodeS( pack, &nfo);
TNU 0:dfe498e03679 119 if (res < 0)
TNU 0:dfe498e03679 120 return -1;
TNU 0:dfe498e03679 121
TNU 0:dfe498e03679 122 return 0;
TNU 0:dfe498e03679 123 }
TNU 0:dfe498e03679 124
TNU 0:dfe498e03679 125
TNU 0:dfe498e03679 126 /// ****************************************************************************
TNU 0:dfe498e03679 127 /// \fn MLX_ReqWriteReg
TNU 0:dfe498e03679 128 ///
TNU 0:dfe498e03679 129 /// \brief Creates a write register request.
TNU 0:dfe498e03679 130 ///
TNU 0:dfe498e03679 131 /// \param[out] *pack: Packet to encode as a write register
TNU 0:dfe498e03679 132 /// \param[in] reg: Register address to write
TNU 0:dfe498e03679 133 /// \param[in] val: Value to write
TNU 0:dfe498e03679 134 ///
TNU 0:dfe498e03679 135 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 136 ///
TNU 0:dfe498e03679 137 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 138 /// \since 2015-04-07
TNU 0:dfe498e03679 139 /// ****************************************************************************
TNU 0:dfe498e03679 140 int MLX_ReqWriteReg( PACK_SHORT *pack, uint16 reg, uint16 val)
TNU 0:dfe498e03679 141 {
TNU 0:dfe498e03679 142 int res;
TNU 0:dfe498e03679 143 MLX_PackNfo nfo;
TNU 0:dfe498e03679 144
TNU 0:dfe498e03679 145 // Reset packet!
TNU 0:dfe498e03679 146 memset( pack, 0, sizeof(*pack));
TNU 0:dfe498e03679 147
TNU 0:dfe498e03679 148 nfo.pktype = PACK_WREG;
TNU 0:dfe498e03679 149 nfo.size = 1;
TNU 0:dfe498e03679 150 nfo.seq = 0;
TNU 0:dfe498e03679 151 nfo.addr = reg;
TNU 0:dfe498e03679 152 nfo.data = val;
TNU 0:dfe498e03679 153
TNU 0:dfe498e03679 154 res = EncodeS( pack, &nfo);
TNU 0:dfe498e03679 155 if (res < 0)
TNU 0:dfe498e03679 156 return -1;
TNU 0:dfe498e03679 157
TNU 0:dfe498e03679 158 return 0;
TNU 0:dfe498e03679 159 }
TNU 0:dfe498e03679 160
TNU 0:dfe498e03679 161
TNU 0:dfe498e03679 162 /// ****************************************************************************
TNU 0:dfe498e03679 163 /// \fn MLX_ReqReadTrc
TNU 0:dfe498e03679 164 ///
TNU 0:dfe498e03679 165 /// \brief Creates a read raw data request.
TNU 0:dfe498e03679 166 ///
TNU 0:dfe498e03679 167 /// \param[out] *pack: Packet to encode
TNU 0:dfe498e03679 168 ///
TNU 0:dfe498e03679 169 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 170 ///
TNU 0:dfe498e03679 171 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 172 /// \since 2015-05-07
TNU 0:dfe498e03679 173 /// ****************************************************************************
TNU 0:dfe498e03679 174 int MLX_ReqReadTrc( PACK_SHORT *pack)
TNU 0:dfe498e03679 175 {
TNU 0:dfe498e03679 176 int res;
TNU 0:dfe498e03679 177 MLX_PackNfo nfo;
TNU 0:dfe498e03679 178
TNU 0:dfe498e03679 179 nfo.pktype = PACK_RFIRM;
TNU 0:dfe498e03679 180 nfo.size = 0;
TNU 0:dfe498e03679 181 nfo.seq = 0;
TNU 0:dfe498e03679 182 nfo.fwtype = FW_RAW;
TNU 0:dfe498e03679 183
TNU 0:dfe498e03679 184 res = EncodeS( pack, &nfo);
TNU 0:dfe498e03679 185 if (res < 0)
TNU 0:dfe498e03679 186 return -1;
TNU 0:dfe498e03679 187
TNU 0:dfe498e03679 188 return 0;
TNU 0:dfe498e03679 189 }
TNU 0:dfe498e03679 190
TNU 0:dfe498e03679 191
TNU 0:dfe498e03679 192 /// ****************************************************************************
TNU 0:dfe498e03679 193 /// \fn MLX_ReqReadEch
TNU 0:dfe498e03679 194 ///
TNU 0:dfe498e03679 195 /// \brief Creates a read echoes request.
TNU 0:dfe498e03679 196 ///
TNU 0:dfe498e03679 197 /// \param[out] *pack: Packet to encode
TNU 0:dfe498e03679 198 ///
TNU 0:dfe498e03679 199 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 200 ///
TNU 0:dfe498e03679 201 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 202 /// \since 2015-05-28
TNU 0:dfe498e03679 203 /// ****************************************************************************
TNU 0:dfe498e03679 204 int MLX_ReqReadEch( PACK_SHORT *pack)
TNU 0:dfe498e03679 205 {
TNU 0:dfe498e03679 206 int res;
TNU 0:dfe498e03679 207 MLX_PackNfo nfo;
TNU 0:dfe498e03679 208
TNU 0:dfe498e03679 209 nfo.pktype = PACK_RFIRM;
TNU 0:dfe498e03679 210 nfo.size = 0;
TNU 0:dfe498e03679 211 nfo.seq = 0;
TNU 0:dfe498e03679 212 nfo.fwtype = FW_PROCESSED;
TNU 0:dfe498e03679 213
TNU 0:dfe498e03679 214 res = EncodeS( pack, &nfo);
TNU 0:dfe498e03679 215 if (res < 0)
TNU 0:dfe498e03679 216 return -1;
TNU 0:dfe498e03679 217
TNU 0:dfe498e03679 218 return 0;
TNU 0:dfe498e03679 219 }
TNU 0:dfe498e03679 220
TNU 0:dfe498e03679 221
TNU 0:dfe498e03679 222 /// ****************************************************************************
TNU 0:dfe498e03679 223 /// \fn MLX_ReqWriteFW
TNU 0:dfe498e03679 224 ///
TNU 0:dfe498e03679 225 /// \brief Creates a write firmware request.
TNU 0:dfe498e03679 226 ///
TNU 0:dfe498e03679 227 /// \param[out] *pack: Packet to encode
TNU 0:dfe498e03679 228 /// \param[in] type: Firmware type to write
TNU 0:dfe498e03679 229 /// \param[in] size: Words (2 bytes) to write. 0 means 256. If more than 1, the
TNU 0:dfe498e03679 230 /// data will be located in following long packets. The size field
TNU 0:dfe498e03679 231 /// of these long packets will be the size for this long pack only.
TNU 0:dfe498e03679 232 /// \param[in] data: Data to be written, used only if size == 1
TNU 0:dfe498e03679 233 ///
TNU 0:dfe498e03679 234 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 235 ///
TNU 0:dfe498e03679 236 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 237 /// \since 2015-05-07
TNU 0:dfe498e03679 238 /// ****************************************************************************
TNU 0:dfe498e03679 239 int MLX_ReqWriteFW( PACK_SHORT *pack, uint size, uint16 data)
TNU 0:dfe498e03679 240 {
TNU 0:dfe498e03679 241 int res;
TNU 0:dfe498e03679 242 MLX_PackNfo nfo;
TNU 0:dfe498e03679 243
TNU 0:dfe498e03679 244 nfo.pktype = PACK_WFIRM;
TNU 0:dfe498e03679 245 nfo.fwtype = FW_PATCH;
TNU 0:dfe498e03679 246 nfo.size = size;
TNU 0:dfe498e03679 247 nfo.seq = 0;
TNU 0:dfe498e03679 248 nfo.data = data;
TNU 0:dfe498e03679 249
TNU 0:dfe498e03679 250 res = EncodeS( pack, &nfo);
TNU 0:dfe498e03679 251 if (res < 0)
TNU 0:dfe498e03679 252 return -1;
TNU 0:dfe498e03679 253
TNU 0:dfe498e03679 254 return 0;
TNU 0:dfe498e03679 255 }
TNU 0:dfe498e03679 256
TNU 0:dfe498e03679 257
TNU 0:dfe498e03679 258 /// ****************************************************************************
TNU 0:dfe498e03679 259 /// \fn MLX_WriteDataL
TNU 0:dfe498e03679 260 ///
TNU 0:dfe498e03679 261 /// \brief Creates a long write packet.
TNU 0:dfe498e03679 262 ///
TNU 0:dfe498e03679 263 /// \param[out] *pack: Packet to encode
TNU 0:dfe498e03679 264 /// \param[in] size: Words (2 bytes) to take from data. Max = MLX_LONG_DATA_SZ
TNU 0:dfe498e03679 265 /// \param[in] seq: Serial sequence number stamping the packets
TNU 0:dfe498e03679 266 /// \param[in] data: Data to be written
TNU 0:dfe498e03679 267 ///
TNU 0:dfe498e03679 268 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 269 ///
TNU 0:dfe498e03679 270 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 271 /// \since 2015-05-07
TNU 0:dfe498e03679 272 /// ****************************************************************************
TNU 0:dfe498e03679 273 int MLX_WriteDataL2( PACK_LONG2 *pack, uint size, uint seq, const uint8 *data)
TNU 0:dfe498e03679 274 {
TNU 0:dfe498e03679 275 int res;
TNU 0:dfe498e03679 276 MLX_PackNfo nfo;
TNU 0:dfe498e03679 277
TNU 0:dfe498e03679 278 if (size > MLX_LONG2_DATA_SZ)
TNU 0:dfe498e03679 279 return -1;
TNU 0:dfe498e03679 280
TNU 0:dfe498e03679 281 nfo.pktype = PACK_WDATA_L;
TNU 0:dfe498e03679 282 nfo.size = size;
TNU 0:dfe498e03679 283 nfo.seq = seq;
TNU 0:dfe498e03679 284 nfo.dataP = data;
TNU 0:dfe498e03679 285
TNU 0:dfe498e03679 286 res = EncodeL2( pack, &nfo);
TNU 0:dfe498e03679 287 if (res < 0)
TNU 0:dfe498e03679 288 return -1;
TNU 0:dfe498e03679 289
TNU 0:dfe498e03679 290 return 0;
TNU 0:dfe498e03679 291 }
TNU 0:dfe498e03679 292
TNU 0:dfe498e03679 293
TNU 0:dfe498e03679 294 /// ****************************************************************************
TNU 0:dfe498e03679 295 /// \fn MLX_DecodeResS / L
TNU 0:dfe498e03679 296 ///
TNU 0:dfe498e03679 297 /// \brief Decodes a read or write response from slave.
TNU 0:dfe498e03679 298 ///
TNU 0:dfe498e03679 299 /// \param[in] *pack: Packet to decode
TNU 0:dfe498e03679 300 /// \param[out] *val: Data value extracted from response
TNU 0:dfe498e03679 301 ///
TNU 0:dfe498e03679 302 /// \return 0 on success, negative on error.
TNU 0:dfe498e03679 303 ///
TNU 0:dfe498e03679 304 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 305 /// \since 2015-04-24
TNU 0:dfe498e03679 306 /// ****************************************************************************
TNU 0:dfe498e03679 307 int MLX_DecodeResS( const PACK_SHORT *pack, uint32 *val)
TNU 0:dfe498e03679 308 {
TNU 0:dfe498e03679 309 int res;
TNU 0:dfe498e03679 310 MLX_PackNfo nfo;
TNU 0:dfe498e03679 311
TNU 0:dfe498e03679 312 if (!ValidCRC_short(pack)){
TNU 0:dfe498e03679 313 return -5;
TNU 0:dfe498e03679 314 }
TNU 0:dfe498e03679 315
TNU 0:dfe498e03679 316 //SWAP( rx+off, 1);
TNU 0:dfe498e03679 317
TNU 0:dfe498e03679 318 res = DecodeS( pack, &nfo);
TNU 0:dfe498e03679 319 if (res < 0 || nfo.pktype != PACK_RDATA_RESP_S)
TNU 0:dfe498e03679 320 if (nfo.pktype == PACK_STATUS_S) return -6;
TNU 0:dfe498e03679 321 else return -20;
TNU 0:dfe498e03679 322
TNU 0:dfe498e03679 323 *val = nfo.data;
TNU 0:dfe498e03679 324 return 0;
TNU 0:dfe498e03679 325 }
TNU 0:dfe498e03679 326
TNU 0:dfe498e03679 327 int MLX_DecodeResL1(const PACK_LONG1 *pack)
TNU 0:dfe498e03679 328 {
TNU 0:dfe498e03679 329 int res;
TNU 0:dfe498e03679 330 MLX_PackNfo nfo;
TNU 0:dfe498e03679 331
TNU 0:dfe498e03679 332 if (!ValidCRC_long1(pack)){
TNU 0:dfe498e03679 333 return -9;
TNU 0:dfe498e03679 334 }
TNU 0:dfe498e03679 335
TNU 0:dfe498e03679 336 res = DecodeL1(pack, &nfo);
TNU 0:dfe498e03679 337 if (res < 0 || nfo.pktype != PACK_RDATA_RESP_L)
TNU 0:dfe498e03679 338 return -10;
TNU 0:dfe498e03679 339
TNU 0:dfe498e03679 340 return 0;
TNU 0:dfe498e03679 341 }
TNU 0:dfe498e03679 342
TNU 0:dfe498e03679 343 int MLX_DecodeResL2(const PACK_LONG2 *pack)
TNU 0:dfe498e03679 344 {
TNU 0:dfe498e03679 345 int res;
TNU 0:dfe498e03679 346 MLX_PackNfo nfo;
TNU 0:dfe498e03679 347
TNU 0:dfe498e03679 348 if (!ValidCRC_long2(pack)){
TNU 0:dfe498e03679 349 return -9;
TNU 0:dfe498e03679 350 }
TNU 0:dfe498e03679 351
TNU 0:dfe498e03679 352 res = DecodeL2(pack, &nfo);
TNU 0:dfe498e03679 353 if (res < 0 || ((nfo.pktype != PACK_RDATA_RESP_L) && (nfo.pktype != PACK_STATUS_L)))
TNU 0:dfe498e03679 354 return -10;
TNU 0:dfe498e03679 355
TNU 0:dfe498e03679 356 return 0;
TNU 0:dfe498e03679 357 }
TNU 0:dfe498e03679 358
TNU 0:dfe498e03679 359
TNU 0:dfe498e03679 360 // ********************************
TNU 0:dfe498e03679 361 // ***** SLAVE SIDE FUNCTIONS *****
TNU 0:dfe498e03679 362 // ********************************
TNU 0:dfe498e03679 363
TNU 0:dfe498e03679 364 /// ****************************************************************************
TNU 0:dfe498e03679 365 /// \fn MLX_ResReadReg
TNU 0:dfe498e03679 366 ///
TNU 0:dfe498e03679 367 /// \brief Creates a read register response.
TNU 0:dfe498e03679 368 ///
TNU 0:dfe498e03679 369 /// \param[out] *pack: Packet to encode as a response
TNU 0:dfe498e03679 370 /// \param[in] val: Register value
TNU 0:dfe498e03679 371 ///
TNU 0:dfe498e03679 372 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 373 ///
TNU 0:dfe498e03679 374 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 375 /// \since 2015-04-01
TNU 0:dfe498e03679 376 /// ****************************************************************************
TNU 0:dfe498e03679 377 int MLX_ResReadReg( PACK_SHORT *pack, uint16 val)
TNU 0:dfe498e03679 378 {
TNU 0:dfe498e03679 379 int res;
TNU 0:dfe498e03679 380 MLX_PackNfo nfo;
TNU 0:dfe498e03679 381
TNU 0:dfe498e03679 382 // Reset packet!
TNU 0:dfe498e03679 383 memset( pack, 0, sizeof(*pack));
TNU 0:dfe498e03679 384
TNU 0:dfe498e03679 385 nfo.pktype = PACK_RDATA_RESP_S;
TNU 0:dfe498e03679 386 nfo.size = 1;
TNU 0:dfe498e03679 387 nfo.seq = 0;
TNU 0:dfe498e03679 388 nfo.data = val;
TNU 0:dfe498e03679 389
TNU 0:dfe498e03679 390 res = EncodeS( pack, &nfo);
TNU 0:dfe498e03679 391 if (res < 0)
TNU 0:dfe498e03679 392 return -1;
TNU 0:dfe498e03679 393
TNU 0:dfe498e03679 394 return 0;
TNU 0:dfe498e03679 395 }
TNU 0:dfe498e03679 396
TNU 0:dfe498e03679 397 /// ****************************************************************************
TNU 0:dfe498e03679 398 /// \fn MLX_DecodeReqS
TNU 0:dfe498e03679 399 ///
TNU 0:dfe498e03679 400 /// \brief Decodes a request from master.
TNU 0:dfe498e03679 401 ///
TNU 0:dfe498e03679 402 /// \param[in] *pack: Packet to decode
TNU 0:dfe498e03679 403 /// \param[out] *nfo: Filled info struct
TNU 0:dfe498e03679 404 ///
TNU 0:dfe498e03679 405 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 406 ///
TNU 0:dfe498e03679 407 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 408 /// \since 2015-04-28
TNU 0:dfe498e03679 409 /// ****************************************************************************
TNU 0:dfe498e03679 410 int MLX_DecodeReqS( const PACK_SHORT *pack, MLX_PackNfo *nfo)
TNU 0:dfe498e03679 411 {
TNU 0:dfe498e03679 412 int res;
TNU 0:dfe498e03679 413
TNU 0:dfe498e03679 414 if (!ValidCRC_short(pack)){
TNU 0:dfe498e03679 415 return -1;
TNU 0:dfe498e03679 416 }
TNU 0:dfe498e03679 417
TNU 0:dfe498e03679 418 res = DecodeS( pack, nfo);
TNU 0:dfe498e03679 419 if (res < 0)
TNU 0:dfe498e03679 420 return -1;
TNU 0:dfe498e03679 421
TNU 0:dfe498e03679 422 return 0;
TNU 0:dfe498e03679 423 }
TNU 0:dfe498e03679 424
TNU 0:dfe498e03679 425
TNU 0:dfe498e03679 426 /// ****************************************************************************
TNU 0:dfe498e03679 427 /// \fn MLX_ResReadData
TNU 0:dfe498e03679 428 ///
TNU 0:dfe498e03679 429 /// \brief Encodes a long read response.
TNU 0:dfe498e03679 430 ///
TNU 0:dfe498e03679 431 /// \param[in] *pack: Packet to decode
TNU 0:dfe498e03679 432 /// \param[out] *nfo: Filled info struct
TNU 0:dfe498e03679 433 ///
TNU 0:dfe498e03679 434 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 435 ///
TNU 0:dfe498e03679 436 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 437 /// \since 2015-05-07
TNU 0:dfe498e03679 438 /// ****************************************************************************
TNU 0:dfe498e03679 439 int MLX_ResReadData( PACK_LONG1 *pack, uint size, uint seq, const uint8 *data)
TNU 0:dfe498e03679 440 {
TNU 0:dfe498e03679 441 int res;
TNU 0:dfe498e03679 442 MLX_PackNfo nfo;
TNU 0:dfe498e03679 443
TNU 0:dfe498e03679 444 if (size > MLX_LONG1_DATA_SZ)
TNU 0:dfe498e03679 445 return -1;
TNU 0:dfe498e03679 446
TNU 0:dfe498e03679 447 nfo.pktype = PACK_RDATA_RESP_L;
TNU 0:dfe498e03679 448 nfo.size = size;
TNU 0:dfe498e03679 449 nfo.seq = seq;
TNU 0:dfe498e03679 450 nfo.dataP = data;
TNU 0:dfe498e03679 451
TNU 0:dfe498e03679 452 res = EncodeL1( pack, &nfo);
TNU 0:dfe498e03679 453 if (res < 0)
TNU 0:dfe498e03679 454 return -1;
TNU 0:dfe498e03679 455
TNU 0:dfe498e03679 456 return 0;
TNU 0:dfe498e03679 457 }
TNU 0:dfe498e03679 458
TNU 0:dfe498e03679 459 /// ****************************************************************************
TNU 0:dfe498e03679 460 /// \fn MLX_ResReadDataE
TNU 0:dfe498e03679 461 ///
TNU 0:dfe498e03679 462 /// \brief Encodes an echo read response.
TNU 0:dfe498e03679 463 ///
TNU 0:dfe498e03679 464 /// \param[in] *pack: Packet to decode
TNU 0:dfe498e03679 465 /// \param[out] *nfo: Filled info struct
TNU 0:dfe498e03679 466 ///
TNU 0:dfe498e03679 467 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 468 ///
TNU 0:dfe498e03679 469 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 470 /// \since 2015-05-07
TNU 0:dfe498e03679 471 /// ****************************************************************************
TNU 0:dfe498e03679 472 int MLX_ResReadDataL2(PACK_LONG2 *pack, uint size, uint seq, const uint8 *data)
TNU 0:dfe498e03679 473 {
TNU 0:dfe498e03679 474 int res;
TNU 0:dfe498e03679 475 MLX_PackNfo nfo;
TNU 0:dfe498e03679 476
TNU 0:dfe498e03679 477 if (size > MLX_LONG2_DATA_SZ)
TNU 0:dfe498e03679 478 return -1;
TNU 0:dfe498e03679 479
TNU 0:dfe498e03679 480 nfo.pktype = PACK_RDATA_RESP_L;
TNU 0:dfe498e03679 481 nfo.size = size;
TNU 0:dfe498e03679 482 nfo.seq = seq;
TNU 0:dfe498e03679 483 nfo.dataP = data;
TNU 0:dfe498e03679 484
TNU 0:dfe498e03679 485 res = EncodeL2(pack, &nfo);
TNU 0:dfe498e03679 486 if (res < 0)
TNU 0:dfe498e03679 487 return -1;
TNU 0:dfe498e03679 488
TNU 0:dfe498e03679 489 return 0;
TNU 0:dfe498e03679 490 }
TNU 0:dfe498e03679 491
TNU 0:dfe498e03679 492
TNU 0:dfe498e03679 493 // ********************************
TNU 0:dfe498e03679 494 // ****** COMMON FUNCTIONS ********
TNU 0:dfe498e03679 495 // ********************************
TNU 0:dfe498e03679 496
TNU 0:dfe498e03679 497 /// ****************************************************************************
TNU 0:dfe498e03679 498 /// \fn FillCRC_short / long
TNU 0:dfe498e03679 499 ///
TNU 0:dfe498e03679 500 /// \brief Computes and sets the CRC field in a packet.
TNU 0:dfe498e03679 501 ///
TNU 0:dfe498e03679 502 /// \param[in/out] *pack: Packet on which to set the CRC
TNU 0:dfe498e03679 503 ///
TNU 0:dfe498e03679 504 /// \return Nothing.
TNU 0:dfe498e03679 505 ///
TNU 0:dfe498e03679 506 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 507 /// \since 2015-04-24
TNU 0:dfe498e03679 508 /// ****************************************************************************
TNU 0:dfe498e03679 509 void FillCRC_short( PACK_SHORT *pack)
TNU 0:dfe498e03679 510 {
TNU 0:dfe498e03679 511 pack->crc = CRC16_Compute( (uint8*)pack, sizeof(*pack)-MLX_CRC_SZ);
TNU 0:dfe498e03679 512 //SWAP(&pack->crc,2);
TNU 0:dfe498e03679 513 }
TNU 0:dfe498e03679 514
TNU 0:dfe498e03679 515 void FillCRC_long1(PACK_LONG1 *pack)
TNU 0:dfe498e03679 516 {
TNU 0:dfe498e03679 517 pack->crc = CRC16_Compute((uint8*)pack, sizeof(*pack) - MLX_CRC_SZ);
TNU 0:dfe498e03679 518 //SWAP(&pack->crc,2);
TNU 0:dfe498e03679 519 }
TNU 0:dfe498e03679 520
TNU 0:dfe498e03679 521 void FillCRC_long2(PACK_LONG2 *pack)
TNU 0:dfe498e03679 522 {
TNU 0:dfe498e03679 523 pack->crc = CRC16_Compute((uint8*)pack, sizeof(*pack) - MLX_CRC_SZ);
TNU 0:dfe498e03679 524 //SWAP(&pack->crc,2);
TNU 0:dfe498e03679 525 }
TNU 0:dfe498e03679 526
TNU 0:dfe498e03679 527 /// ****************************************************************************
TNU 0:dfe498e03679 528 /// \fn ValidCRC_short / long
TNU 0:dfe498e03679 529 ///
TNU 0:dfe498e03679 530 /// \brief Validates the CRC of a packet.
TNU 0:dfe498e03679 531 ///
TNU 0:dfe498e03679 532 /// \param[in/out] pack: Packet on which to validate the CRC
TNU 0:dfe498e03679 533 ///
TNU 0:dfe498e03679 534 /// \return Validation status. TRUE if valid, FALSE if invalid.
TNU 0:dfe498e03679 535 ///
TNU 0:dfe498e03679 536 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 537 /// \since 2015-04-24
TNU 0:dfe498e03679 538 /// ****************************************************************************
TNU 0:dfe498e03679 539 BOOL ValidCRC_short( const PACK_SHORT *pack)
TNU 0:dfe498e03679 540 {
TNU 0:dfe498e03679 541 return CRC16_IsValid( (uint8*)pack, sizeof(*pack), MLX_CRC_SZ);
TNU 0:dfe498e03679 542 }
TNU 0:dfe498e03679 543
TNU 0:dfe498e03679 544 BOOL ValidCRC_long1( const PACK_LONG1 *pack)
TNU 0:dfe498e03679 545 {
TNU 0:dfe498e03679 546 return CRC16_IsValid( (uint8*)pack, sizeof(*pack), MLX_CRC_SZ);
TNU 0:dfe498e03679 547 }
TNU 0:dfe498e03679 548
TNU 0:dfe498e03679 549 BOOL ValidCRC_long2(const PACK_LONG2 *pack)
TNU 0:dfe498e03679 550 {
TNU 0:dfe498e03679 551 return CRC16_IsValid((uint8*)pack, sizeof(*pack), MLX_CRC_SZ);
TNU 0:dfe498e03679 552 }
TNU 0:dfe498e03679 553
TNU 0:dfe498e03679 554 /// ****************************************************************************
TNU 0:dfe498e03679 555 /// \fn CRC16_Compute
TNU 0:dfe498e03679 556 ///
TNU 0:dfe498e03679 557 /// \brief Computes the CRC16 of a data vector.
TNU 0:dfe498e03679 558 /// This is the CCITT CRC 16 polynomial X^16 + X^12 + X^5 + 1.
TNU 0:dfe498e03679 559 /// This works out to be 0x1021, but the way the algorithm works
TNU 0:dfe498e03679 560 /// lets us use 0x8408 (the reverse of the bit pattern). The high
TNU 0:dfe498e03679 561 /// bit is always assumed to be set, thus we only use 16 bits to
TNU 0:dfe498e03679 562 /// represent the 17 bit value.
TNU 0:dfe498e03679 563 ///
TNU 0:dfe498e03679 564 /// Reference: stjarnhimlen.se/snippets/crc-16.c
TNU 0:dfe498e03679 565 ///
TNU 0:dfe498e03679 566 /// \param[in] *data: Data vector for which to compute the CRC
TNU 0:dfe498e03679 567 /// \param[in] n: Number of bytes in data
TNU 0:dfe498e03679 568 ///
TNU 0:dfe498e03679 569 /// \return The CRC
TNU 0:dfe498e03679 570 ///
TNU 0:dfe498e03679 571 /// \authors Jean-F. Bernier
TNU 0:dfe498e03679 572 /// \since 2015-03-31
TNU 0:dfe498e03679 573 /// ****************************************************************************
TNU 0:dfe498e03679 574 /* uint16 CRC16_Compute( const uint8 *data, uint16 n)
TNU 0:dfe498e03679 575 {
TNU 0:dfe498e03679 576 uint8 a;
TNU 0:dfe498e03679 577 uint tmp;
TNU 0:dfe498e03679 578 uint crc = 0x0000;
TNU 0:dfe498e03679 579
TNU 0:dfe498e03679 580 const uint POLY = 0x8408;
TNU 0:dfe498e03679 581
TNU 0:dfe498e03679 582 if (n == 0)
TNU 0:dfe498e03679 583 return (~crc);
TNU 0:dfe498e03679 584
TNU 0:dfe498e03679 585 do
TNU 0:dfe498e03679 586 {
TNU 0:dfe498e03679 587 tmp = (uint)0xFF & *data++;
TNU 0:dfe498e03679 588
TNU 0:dfe498e03679 589 for (a = 0 ; a < 8; ++a)
TNU 0:dfe498e03679 590 {
TNU 0:dfe498e03679 591 tmp >>= 1;
TNU 0:dfe498e03679 592 if ((crc & 0x0001) ^ (tmp & 0x0001))
TNU 0:dfe498e03679 593 crc = (crc >> 1) ^ POLY;
TNU 0:dfe498e03679 594 else
TNU 0:dfe498e03679 595 crc >>= 1;
TNU 0:dfe498e03679 596 }
TNU 0:dfe498e03679 597 } while (--n);
TNU 0:dfe498e03679 598
TNU 0:dfe498e03679 599 crc = ~crc;
TNU 0:dfe498e03679 600 tmp = crc;
TNU 0:dfe498e03679 601 crc = (crc << 8) | (tmp >> 8 & 0xFF);
TNU 0:dfe498e03679 602
TNU 0:dfe498e03679 603 return crc;
TNU 0:dfe498e03679 604 }
TNU 0:dfe498e03679 605 */
TNU 0:dfe498e03679 606
TNU 0:dfe498e03679 607 /*
TNU 0:dfe498e03679 608 * Copyright 2001-2010 Georges Menie (www.menie.org)
TNU 0:dfe498e03679 609 * All rights reserved.
TNU 0:dfe498e03679 610 * Redistribution and use in source and binary forms, with or without
TNU 0:dfe498e03679 611 * modification, are permitted provided that the following conditions are met:
TNU 0:dfe498e03679 612 *
TNU 0:dfe498e03679 613 * * Redistributions of source code must retain the above copyright
TNU 0:dfe498e03679 614 * notice, this list of conditions and the following disclaimer.
TNU 0:dfe498e03679 615 * * Redistributions in binary form must reproduce the above copyright
TNU 0:dfe498e03679 616 * notice, this list of conditions and the following disclaimer in the
TNU 0:dfe498e03679 617 * documentation and/or other materials provided with the distribution.
TNU 0:dfe498e03679 618 * * Neither the name of the University of California, Berkeley nor the
TNU 0:dfe498e03679 619 * names of its contributors may be used to endorse or promote products
TNU 0:dfe498e03679 620 * derived from this software without specific prior written permission.
TNU 0:dfe498e03679 621 *
TNU 0:dfe498e03679 622 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
TNU 0:dfe498e03679 623 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
TNU 0:dfe498e03679 624 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
TNU 0:dfe498e03679 625 * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
TNU 0:dfe498e03679 626 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
TNU 0:dfe498e03679 627 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
TNU 0:dfe498e03679 628 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
TNU 0:dfe498e03679 629 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
TNU 0:dfe498e03679 630 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
TNU 0:dfe498e03679 631 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
TNU 0:dfe498e03679 632 */
TNU 0:dfe498e03679 633
TNU 0:dfe498e03679 634 /* CRC16 implementation acording to CCITT standards */
TNU 0:dfe498e03679 635
TNU 0:dfe498e03679 636 static const uint16 crc16tab[256] = {
TNU 0:dfe498e03679 637 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
TNU 0:dfe498e03679 638 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
TNU 0:dfe498e03679 639 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
TNU 0:dfe498e03679 640 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
TNU 0:dfe498e03679 641 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
TNU 0:dfe498e03679 642 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
TNU 0:dfe498e03679 643 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
TNU 0:dfe498e03679 644 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
TNU 0:dfe498e03679 645 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
TNU 0:dfe498e03679 646 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
TNU 0:dfe498e03679 647 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
TNU 0:dfe498e03679 648 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
TNU 0:dfe498e03679 649 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
TNU 0:dfe498e03679 650 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
TNU 0:dfe498e03679 651 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
TNU 0:dfe498e03679 652 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
TNU 0:dfe498e03679 653 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
TNU 0:dfe498e03679 654 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
TNU 0:dfe498e03679 655 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
TNU 0:dfe498e03679 656 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
TNU 0:dfe498e03679 657 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
TNU 0:dfe498e03679 658 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
TNU 0:dfe498e03679 659 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
TNU 0:dfe498e03679 660 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
TNU 0:dfe498e03679 661 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
TNU 0:dfe498e03679 662 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
TNU 0:dfe498e03679 663 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
TNU 0:dfe498e03679 664 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
TNU 0:dfe498e03679 665 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
TNU 0:dfe498e03679 666 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
TNU 0:dfe498e03679 667 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
TNU 0:dfe498e03679 668 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
TNU 0:dfe498e03679 669 };
TNU 0:dfe498e03679 670
TNU 0:dfe498e03679 671 uint16 CRC16_Compute(const uint8 *buf, uint16 len)
TNU 0:dfe498e03679 672 {
TNU 0:dfe498e03679 673 uint16 tmp;
TNU 0:dfe498e03679 674 int counter;
TNU 0:dfe498e03679 675 uint16 crc = 0;
TNU 0:dfe498e03679 676 for (counter = 0; counter < len; counter++)
TNU 0:dfe498e03679 677 crc = (crc << 8) ^ crc16tab[((crc >> 8) ^ *(char *)buf++) & 0x00FF];
TNU 0:dfe498e03679 678
TNU 0:dfe498e03679 679 tmp = crc;
TNU 0:dfe498e03679 680 crc = (crc << 8) | (tmp >> 8 & 0xFF);
TNU 0:dfe498e03679 681
TNU 0:dfe498e03679 682 return crc;
TNU 0:dfe498e03679 683 }
TNU 0:dfe498e03679 684
TNU 0:dfe498e03679 685
TNU 0:dfe498e03679 686 /// ****************************************************************************
TNU 0:dfe498e03679 687 /// \fn CRC16_IsValid
TNU 0:dfe498e03679 688 ///
TNU 0:dfe498e03679 689 /// \brief Validates the CRC in a byte vector. Computes CRC on all bytes except
TNU 0:dfe498e03679 690 /// the X lasts CRC bytes and compares it to the one found in these X lasts.
TNU 0:dfe498e03679 691 ///
TNU 0:dfe498e03679 692 /// \param[in] *data: Data vector for which to validate the CRC
TNU 0:dfe498e03679 693 /// \param[in] n: Total bytes in data
TNU 0:dfe498e03679 694 /// \param[in] nbCrc: Nb of bytes at the end of data for the CRC
TNU 0:dfe498e03679 695 ///
TNU 0:dfe498e03679 696 /// \return TRUE if valid, FALSE otherwise.
TNU 0:dfe498e03679 697 ///
TNU 0:dfe498e03679 698 /// \authors Jean-F. Bernier
TNU 0:dfe498e03679 699 /// \since 2014-10-21
TNU 0:dfe498e03679 700 /// ****************************************************************************
TNU 0:dfe498e03679 701 BOOL CRC16_IsValid( const uint8 *data, uint n, uint nbCrc)
TNU 0:dfe498e03679 702 {
TNU 0:dfe498e03679 703 if (n <= 1){
TNU 0:dfe498e03679 704 return FALSE;
TNU 0:dfe498e03679 705 }
TNU 0:dfe498e03679 706
TNU 0:dfe498e03679 707 uint16 test = *(uint16*)(data+n-nbCrc);
TNU 0:dfe498e03679 708 //SWAP( &test, 1);
TNU 0:dfe498e03679 709 uint16 crc = CRC16_Compute( data, n-nbCrc);
TNU 0:dfe498e03679 710 return (crc == test);
TNU 0:dfe498e03679 711 }
TNU 0:dfe498e03679 712
TNU 0:dfe498e03679 713
TNU 0:dfe498e03679 714
TNU 0:dfe498e03679 715 /// ****************************************************************************
TNU 0:dfe498e03679 716 /// \fn SetBitVector
TNU 0:dfe498e03679 717 ///
TNU 0:dfe498e03679 718 /// \brief Sets a value in a vector of bits at chosen offset and bit width.
TNU 0:dfe498e03679 719 /// Input bits all pre-supposed at zero in buffer.
TNU 0:dfe498e03679 720 ///
TNU 0:dfe498e03679 721 /// \param[in] *buf: Target vector of bits to be modified. The first byte will contain
TNU 0:dfe498e03679 722 /// the lowest bits (at lowest offset).
TNU 0:dfe498e03679 723 /// \param[in] off: Position offset in bits where to put val, 0-based
TNU 0:dfe498e03679 724 /// \param[in] wid: Number of bits to take from val [0 32]
TNU 0:dfe498e03679 725 /// \param[in] val: Value to set in buf. Its low bit is always set first at chosen offset.
TNU 0:dfe498e03679 726 /// \param[in] inv: Inverse bit ordering inside a byte. Usually, bit#0 is the least significant to the right.
TNU 0:dfe498e03679 727 /// With this, bit#0 becomes the most significant to the left.
TNU 0:dfe498e03679 728 ///
TNU 0:dfe498e03679 729 /// \return Error code: 0 on success, negative otherwise.
TNU 0:dfe498e03679 730 ///
TNU 0:dfe498e03679 731 /// \authors Jean-F. Bernier
TNU 0:dfe498e03679 732 /// \since 2015-04-01
TNU 0:dfe498e03679 733 /// ****************************************************************************
TNU 0:dfe498e03679 734 int SetBitVector( uint8 *buf, uint32 off, uchar wid, uint32 val, uchar inv)
TNU 0:dfe498e03679 735 {
TNU 0:dfe498e03679 736 if (wid <= 0 || wid > 32 || buf == NULL)
TNU 0:dfe498e03679 737 return -1;
TNU 0:dfe498e03679 738
TNU 0:dfe498e03679 739 if (inv && wid > off+1)
TNU 0:dfe498e03679 740 return -2;
TNU 0:dfe498e03679 741
TNU 0:dfe498e03679 742 buf += (off >> 3); // Byte count to move
TNU 0:dfe498e03679 743 uint8 rem = off % 8; // Remaining bits to move
TNU 0:dfe498e03679 744
TNU 0:dfe498e03679 745 // While there is a remaining bit to set...
TNU 0:dfe498e03679 746 while (wid >= 1)
TNU 0:dfe498e03679 747 {
TNU 0:dfe498e03679 748 // Check the bit value. If zero, nothing to do.
TNU 0:dfe498e03679 749 if (val & 1)
TNU 0:dfe498e03679 750 {
TNU 0:dfe498e03679 751 if (!inv)
TNU 0:dfe498e03679 752 BIT_SET( *buf, rem);
TNU 0:dfe498e03679 753 else
TNU 0:dfe498e03679 754 BIT_SET( *buf, 7-rem);
TNU 0:dfe498e03679 755 }
TNU 0:dfe498e03679 756
TNU 0:dfe498e03679 757 if (!inv)
TNU 0:dfe498e03679 758 {
TNU 0:dfe498e03679 759 // Wrap around. Goto next byte when setting bit index > 7
TNU 0:dfe498e03679 760 ++rem;
TNU 0:dfe498e03679 761 if (rem >= 8)
TNU 0:dfe498e03679 762 {
TNU 0:dfe498e03679 763 rem = 0;
TNU 0:dfe498e03679 764 ++buf;
TNU 0:dfe498e03679 765 }
TNU 0:dfe498e03679 766 }
TNU 0:dfe498e03679 767 else
TNU 0:dfe498e03679 768 {
TNU 0:dfe498e03679 769 // Wrap around. Goto previous byte when setting "negative" bit index
TNU 0:dfe498e03679 770 --rem;
TNU 0:dfe498e03679 771 if (rem >= 8)
TNU 0:dfe498e03679 772 {
TNU 0:dfe498e03679 773 rem = 7;
TNU 0:dfe498e03679 774 --buf;
TNU 0:dfe498e03679 775 }
TNU 0:dfe498e03679 776 }
TNU 0:dfe498e03679 777
TNU 0:dfe498e03679 778 // Activate the next bit
TNU 0:dfe498e03679 779 val >>= 1;
TNU 0:dfe498e03679 780 --wid;
TNU 0:dfe498e03679 781 }
TNU 0:dfe498e03679 782
TNU 0:dfe498e03679 783 return 0;
TNU 0:dfe498e03679 784 }
TNU 0:dfe498e03679 785
TNU 0:dfe498e03679 786
TNU 0:dfe498e03679 787 /// ****************************************************************************
TNU 0:dfe498e03679 788 /// \fn GetBitVector
TNU 0:dfe498e03679 789 ///
TNU 0:dfe498e03679 790 /// \brief Gets a sub-array of bits inside a bigger one.
TNU 0:dfe498e03679 791 /// Selects the bits to obtain using offset and bit width.
TNU 0:dfe498e03679 792 ///
TNU 0:dfe498e03679 793 /// \param[in] *buf: Target vector of bits to be read. The first byte will contain
TNU 0:dfe498e03679 794 /// the lowest bits (at lowest offset).
TNU 0:dfe498e03679 795 /// \param[in] off: Position offset in bits where to start reading the low bit, 0-based
TNU 0:dfe498e03679 796 /// \param[in] wid: Number of bits to read into val [0 32]
TNU 0:dfe498e03679 797 /// \param[out] *val: Value obtained from buf.
TNU 0:dfe498e03679 798 /// \param[in] inv: Inverse bit ordering inside a byte. Usually, bit#0 is the least significant to the right.
TNU 0:dfe498e03679 799 /// With this, bit#0 becomes the most significant to the left.
TNU 0:dfe498e03679 800 ///
TNU 0:dfe498e03679 801 /// \return Error code: 0 on success, negative otherwise.
TNU 0:dfe498e03679 802 ///
TNU 0:dfe498e03679 803 /// \authors Jean-F. Bernier
TNU 0:dfe498e03679 804 /// \since 2015-04-28
TNU 0:dfe498e03679 805 /// ****************************************************************************
TNU 0:dfe498e03679 806 int GetBitVector( const uint8 *buf, uint32 off, uchar wid, uint32 *val, uchar inv)
TNU 0:dfe498e03679 807 {
TNU 0:dfe498e03679 808 if (wid <= 0 || wid > 32 || buf == NULL)
TNU 0:dfe498e03679 809 return -1;
TNU 0:dfe498e03679 810
TNU 0:dfe498e03679 811 if (inv && wid > off+1)
TNU 0:dfe498e03679 812 return -2;
TNU 0:dfe498e03679 813
TNU 0:dfe498e03679 814 buf += (off >> 3); // Byte count to move
TNU 0:dfe498e03679 815 uint8 rem = off % 8; // Remaining bits to move
TNU 0:dfe498e03679 816 uchar bit, b = 0;
TNU 0:dfe498e03679 817 *val = 0;
TNU 0:dfe498e03679 818
TNU 0:dfe498e03679 819 // While there is a remaining bit to set...
TNU 0:dfe498e03679 820 while (wid >= 1)
TNU 0:dfe498e03679 821 {
TNU 0:dfe498e03679 822 if (!inv)
TNU 0:dfe498e03679 823 bit = BIT_GET( *buf, rem);
TNU 0:dfe498e03679 824 else
TNU 0:dfe498e03679 825 bit = BIT_GET( *buf, 7-rem);
TNU 0:dfe498e03679 826
TNU 0:dfe498e03679 827 if (bit)
TNU 0:dfe498e03679 828 BIT_SET( *val, b);
TNU 0:dfe498e03679 829
TNU 0:dfe498e03679 830 if (!inv)
TNU 0:dfe498e03679 831 {
TNU 0:dfe498e03679 832 // Wrap around. Goto next byte when setting bit index > 7
TNU 0:dfe498e03679 833 ++rem;
TNU 0:dfe498e03679 834 if (rem >= 8)
TNU 0:dfe498e03679 835 {
TNU 0:dfe498e03679 836 rem = 0;
TNU 0:dfe498e03679 837 ++buf;
TNU 0:dfe498e03679 838 }
TNU 0:dfe498e03679 839 }
TNU 0:dfe498e03679 840 else
TNU 0:dfe498e03679 841 {
TNU 0:dfe498e03679 842 // Wrap around. Goto previous byte when setting "negative" bit index
TNU 0:dfe498e03679 843 --rem;
TNU 0:dfe498e03679 844 if (rem >= 8)
TNU 0:dfe498e03679 845 {
TNU 0:dfe498e03679 846 rem = 7;
TNU 0:dfe498e03679 847 --buf;
TNU 0:dfe498e03679 848 }
TNU 0:dfe498e03679 849 }
TNU 0:dfe498e03679 850
TNU 0:dfe498e03679 851 // Activate the next bit
TNU 0:dfe498e03679 852 --wid;
TNU 0:dfe498e03679 853 ++b;
TNU 0:dfe498e03679 854 }
TNU 0:dfe498e03679 855
TNU 0:dfe498e03679 856 return 0;
TNU 0:dfe498e03679 857 }
TNU 0:dfe498e03679 858
TNU 0:dfe498e03679 859 /// ****************************************************************************
TNU 0:dfe498e03679 860 /// \fn ReverseBits
TNU 0:dfe498e03679 861 ///
TNU 0:dfe498e03679 862 /// \brief Reverses a bit pattern like a mirror. First bits becomes last.
TNU 0:dfe498e03679 863 /// Ex.: 011001 <-> 100110 (v = 25, n = 6)
TNU 0:dfe498e03679 864 ///
TNU 0:dfe498e03679 865 /// \param[in] bits: Value to use as bit pattern
TNU 0:dfe498e03679 866 /// \param[in] nb: Number of bits to take from v. Starts at low bits.
TNU 0:dfe498e03679 867 ///
TNU 0:dfe498e03679 868 /// \return The value with a reversed bit pattern.
TNU 0:dfe498e03679 869 ///
TNU 0:dfe498e03679 870 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 871 /// \since 2015-04-30
TNU 0:dfe498e03679 872 /// ****************************************************************************
TNU 0:dfe498e03679 873 /*uint32 ReverseBits( uint32 bits, char nb)
TNU 0:dfe498e03679 874 {
TNU 0:dfe498e03679 875 // If n == 0, no bits to use. So result will be zero.
TNU 0:dfe498e03679 876 uint32 res = 0;
TNU 0:dfe498e03679 877
TNU 0:dfe498e03679 878 while ((signed char)--nb >= 0)
TNU 0:dfe498e03679 879 {
TNU 0:dfe498e03679 880 res <<= 1; // Shift current result by 1
TNU 0:dfe498e03679 881 res += (bits & 1); // Take the current bit into result
TNU 0:dfe498e03679 882 bits >>= 1; // Prepare for next bit
TNU 0:dfe498e03679 883 }
TNU 0:dfe498e03679 884
TNU 0:dfe498e03679 885 return res;
TNU 0:dfe498e03679 886 }
TNU 0:dfe498e03679 887 */
TNU 0:dfe498e03679 888 /// ****************************************************************************
TNU 0:dfe498e03679 889 /// \fn EncodeS
TNU 0:dfe498e03679 890 ///
TNU 0:dfe498e03679 891 /// \brief Generic short packet encoder.
TNU 0:dfe498e03679 892 ///
TNU 0:dfe498e03679 893 /// \param[out] *pack: Resulting packet encoded with nfo
TNU 0:dfe498e03679 894 /// \param[in] *nfo: Info used to encode packet
TNU 0:dfe498e03679 895 ///
TNU 0:dfe498e03679 896 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 897 ///
TNU 0:dfe498e03679 898 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 899 /// \since 2015-04-27
TNU 0:dfe498e03679 900 /// ****************************************************************************
TNU 0:dfe498e03679 901 int EncodeS( PACK_SHORT *pack, const MLX_PackNfo *nfo)
TNU 0:dfe498e03679 902 {
TNU 0:dfe498e03679 903 int res;
TNU 0:dfe498e03679 904
TNU 0:dfe498e03679 905 // Reject long packets
TNU 0:dfe498e03679 906 if (nfo->pktype == PACK_STATUS_L || nfo->pktype == PACK_RDATA_RESP_L || nfo->pktype == PACK_WDATA_L)
TNU 0:dfe498e03679 907 {
TNU 0:dfe498e03679 908 return -1;
TNU 0:dfe498e03679 909 }
TNU 0:dfe498e03679 910
TNU 0:dfe498e03679 911 if (nfo->pktype == PACK_STATUS_S)
TNU 0:dfe498e03679 912 {
TNU 0:dfe498e03679 913 res = SetBitVector( pack->buf, OFF_TYPE, WID_TYPE, nfo->pktype, BIT_ORDER);
TNU 0:dfe498e03679 914 if (res < 0)
TNU 0:dfe498e03679 915 return -1;
TNU 0:dfe498e03679 916
TNU 0:dfe498e03679 917 // Is error field
TNU 0:dfe498e03679 918 res = SetBitVector( pack->buf, OFF_ISERR, WID_ISERR, nfo->iserr, BIT_ORDER);
TNU 0:dfe498e03679 919 if (res < 0)
TNU 0:dfe498e03679 920 return -1;
TNU 0:dfe498e03679 921
TNU 0:dfe498e03679 922 // Error qualifier field
TNU 0:dfe498e03679 923 res = SetBitVector( pack->buf, OFF_ERROR, WID_ERROR, nfo->error, BIT_ORDER);
TNU 0:dfe498e03679 924 if (res < 0)
TNU 0:dfe498e03679 925 return -1;
TNU 0:dfe498e03679 926
TNU 0:dfe498e03679 927 goto END;
TNU 0:dfe498e03679 928 }
TNU 0:dfe498e03679 929
TNU 0:dfe498e03679 930 // Packet type field
TNU 0:dfe498e03679 931 res = SetBitVector( pack->buf, OFF_TYPE, WID_TYPE, nfo->pktype, BIT_ORDER);
TNU 0:dfe498e03679 932 if (res < 0)
TNU 0:dfe498e03679 933 return -1;
TNU 0:dfe498e03679 934
TNU 0:dfe498e03679 935 // Size field
TNU 0:dfe498e03679 936 res = SetBitVector( pack->buf, OFF_SIZE, WID_SIZE, nfo->size, BIT_ORDER);
TNU 0:dfe498e03679 937 if (res < 0)
TNU 0:dfe498e03679 938 return -1;
TNU 0:dfe498e03679 939
TNU 0:dfe498e03679 940 // Sequence field
TNU 0:dfe498e03679 941 res = SetBitVector( pack->buf, OFF_SEQ, WID_SEQ, nfo->seq, BIT_ORDER);
TNU 0:dfe498e03679 942 if (res < 0)
TNU 0:dfe498e03679 943 return -1;
TNU 0:dfe498e03679 944
TNU 0:dfe498e03679 945 // Address field
TNU 0:dfe498e03679 946 if (nfo->pktype == PACK_RREG || nfo->pktype == PACK_WREG)
TNU 0:dfe498e03679 947 {
TNU 0:dfe498e03679 948 res = SetBitVector( pack->buf, OFF_ADDR, WID_ADDR, nfo->addr, BIT_ORDER);
TNU 0:dfe498e03679 949 }
TNU 0:dfe498e03679 950 else if (nfo->pktype == PACK_RFIRM)
TNU 0:dfe498e03679 951 {
TNU 0:dfe498e03679 952 res = SetBitVector( pack->buf, OFF_ADDR, WID_ADDR, nfo->fwtype, BIT_ORDER);
TNU 0:dfe498e03679 953 }
TNU 0:dfe498e03679 954 else if (nfo->pktype == PACK_WFIRM)
TNU 0:dfe498e03679 955 {
TNU 0:dfe498e03679 956 res = SetBitVector(pack->buf, OFF_ADDR, WID_ADDR, nfo->fwtype, BIT_ORDER);
TNU 0:dfe498e03679 957 }
TNU 0:dfe498e03679 958
TNU 0:dfe498e03679 959 if (res < 0)
TNU 0:dfe498e03679 960 return -1;
TNU 0:dfe498e03679 961
TNU 0:dfe498e03679 962 // Data field
TNU 0:dfe498e03679 963 if (nfo->pktype == PACK_RDATA_RESP_S)
TNU 0:dfe498e03679 964 {
TNU 0:dfe498e03679 965 res = SetBitVector( pack->buf, OFF_RES_DATA, WID_RES_DATA, nfo->data, BIT_ORDER);
TNU 0:dfe498e03679 966 }
TNU 0:dfe498e03679 967 else if (nfo->pktype == PACK_WREG)
TNU 0:dfe498e03679 968 {
TNU 0:dfe498e03679 969 res = SetBitVector( pack->buf, OFF_DATA, WID_DATA, nfo->data, BIT_ORDER);
TNU 0:dfe498e03679 970 }
TNU 0:dfe498e03679 971 else if (nfo->pktype == PACK_WFIRM)
TNU 0:dfe498e03679 972 {
TNU 0:dfe498e03679 973 res = SetBitVector(pack->buf, OFF_DATA, WID_DATA, nfo->data, BIT_ORDER);
TNU 0:dfe498e03679 974 }
TNU 0:dfe498e03679 975
TNU 0:dfe498e03679 976 if (res < 0)
TNU 0:dfe498e03679 977 return -1;
TNU 0:dfe498e03679 978
TNU 0:dfe498e03679 979 //SWAP(&pack,sizeof(*pack));
TNU 0:dfe498e03679 980
TNU 0:dfe498e03679 981 END:
TNU 0:dfe498e03679 982 FillCRC_short(pack);
TNU 0:dfe498e03679 983 //SWAP( &pack->crc, 1);
TNU 0:dfe498e03679 984
TNU 0:dfe498e03679 985 return 0;
TNU 0:dfe498e03679 986 }
TNU 0:dfe498e03679 987
TNU 0:dfe498e03679 988 /// ****************************************************************************
TNU 0:dfe498e03679 989 /// \fn DecodeS
TNU 0:dfe498e03679 990 ///
TNU 0:dfe498e03679 991 /// \brief Generic short packet decoder.
TNU 0:dfe498e03679 992 ///
TNU 0:dfe498e03679 993 /// \param[in] *pack: Packet to be decoded into info
TNU 0:dfe498e03679 994 /// \param[out] *nfo: Filled packet info from pack data
TNU 0:dfe498e03679 995 ///
TNU 0:dfe498e03679 996 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 997 ///
TNU 0:dfe498e03679 998 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 999 /// \since 2015-04-27
TNU 0:dfe498e03679 1000 /// ****************************************************************************
TNU 0:dfe498e03679 1001 int DecodeS( const PACK_SHORT *pack, MLX_PackNfo *nfo)
TNU 0:dfe498e03679 1002 {
TNU 0:dfe498e03679 1003 uint32 v;
TNU 0:dfe498e03679 1004 int res;
TNU 0:dfe498e03679 1005
TNU 0:dfe498e03679 1006 //SWAP(&pack, sizeof(*pack));
TNU 0:dfe498e03679 1007
TNU 0:dfe498e03679 1008 res = GetBitVector( pack->buf, OFF_TYPE, WID_TYPE, &v, BIT_ORDER);
TNU 0:dfe498e03679 1009 if (res < 0)
TNU 0:dfe498e03679 1010 return -1;
TNU 0:dfe498e03679 1011 nfo->pktype = (PackType)v;
TNU 0:dfe498e03679 1012
TNU 0:dfe498e03679 1013 res = GetBitVector( pack->buf, OFF_SIZE, WID_SIZE, &v, BIT_ORDER);
TNU 0:dfe498e03679 1014 if (res < 0)
TNU 0:dfe498e03679 1015 return -1;
TNU 0:dfe498e03679 1016 nfo->size = v;
TNU 0:dfe498e03679 1017
TNU 0:dfe498e03679 1018 res = GetBitVector( pack->buf, OFF_SEQ, WID_SEQ, &v, BIT_ORDER);
TNU 0:dfe498e03679 1019 if (res < 0)
TNU 0:dfe498e03679 1020 return -1;
TNU 0:dfe498e03679 1021 nfo->seq = v;
TNU 0:dfe498e03679 1022
TNU 0:dfe498e03679 1023 if (nfo->pktype == PACK_STATUS_S)
TNU 0:dfe498e03679 1024 {
TNU 0:dfe498e03679 1025 res = GetBitVector( pack->buf, OFF_ISERR, WID_ISERR, &v, BIT_ORDER);
TNU 0:dfe498e03679 1026 if (res < 0)
TNU 0:dfe498e03679 1027 return -1;
TNU 0:dfe498e03679 1028
TNU 0:dfe498e03679 1029 nfo->iserr = v;
TNU 0:dfe498e03679 1030
TNU 0:dfe498e03679 1031 res = GetBitVector( pack->buf, OFF_ERROR, WID_ERROR, &v, BIT_ORDER);
TNU 0:dfe498e03679 1032 if (res < 0)
TNU 0:dfe498e03679 1033 return -1;
TNU 0:dfe498e03679 1034
TNU 0:dfe498e03679 1035 nfo->error = v;
TNU 0:dfe498e03679 1036 return 0;
TNU 0:dfe498e03679 1037 }
TNU 0:dfe498e03679 1038
TNU 0:dfe498e03679 1039
TNU 0:dfe498e03679 1040 if (nfo->pktype == PACK_RREG || nfo->pktype == PACK_WREG)
TNU 0:dfe498e03679 1041 {
TNU 0:dfe498e03679 1042 res = GetBitVector( pack->buf, OFF_ADDR, WID_ADDR, &v, BIT_ORDER);
TNU 0:dfe498e03679 1043 if (res < 0)
TNU 0:dfe498e03679 1044 return -1;
TNU 0:dfe498e03679 1045
TNU 0:dfe498e03679 1046 nfo->addr = v;
TNU 0:dfe498e03679 1047 }
TNU 0:dfe498e03679 1048
TNU 0:dfe498e03679 1049
TNU 0:dfe498e03679 1050 v = 0;
TNU 0:dfe498e03679 1051 if (nfo->pktype == PACK_RDATA_RESP_S)
TNU 0:dfe498e03679 1052 {
TNU 0:dfe498e03679 1053 res = GetBitVector( pack->buf, OFF_RES_DATA, WID_RES_DATA, &v, BIT_ORDER);
TNU 0:dfe498e03679 1054 }
TNU 0:dfe498e03679 1055 else if (nfo->pktype == PACK_WREG || nfo->pktype == PACK_WFIRM)
TNU 0:dfe498e03679 1056 {
TNU 0:dfe498e03679 1057 res = GetBitVector( pack->buf, OFF_DATA, WID_DATA, &v, BIT_ORDER);
TNU 0:dfe498e03679 1058 }
TNU 0:dfe498e03679 1059
TNU 0:dfe498e03679 1060 if (res < 0)
TNU 0:dfe498e03679 1061 return -1;
TNU 0:dfe498e03679 1062
TNU 0:dfe498e03679 1063 nfo->data = v;
TNU 0:dfe498e03679 1064
TNU 0:dfe498e03679 1065 return 0;
TNU 0:dfe498e03679 1066 }
TNU 0:dfe498e03679 1067
TNU 0:dfe498e03679 1068 /// ****************************************************************************
TNU 0:dfe498e03679 1069 /// \fn EncodeL
TNU 0:dfe498e03679 1070 ///
TNU 0:dfe498e03679 1071 /// \brief Generic long packet encoder.
TNU 0:dfe498e03679 1072 ///
TNU 0:dfe498e03679 1073 /// \param[out] *pack: Resulting packet encoded with nfo
TNU 0:dfe498e03679 1074 /// \param[in] *nfo: Info used to encode packet
TNU 0:dfe498e03679 1075 ///
TNU 0:dfe498e03679 1076 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 1077 ///
TNU 0:dfe498e03679 1078 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 1079 /// \since 2015-04-27
TNU 0:dfe498e03679 1080 /// ****************************************************************************
TNU 0:dfe498e03679 1081 int EncodeL1( PACK_LONG1 *pack, const MLX_PackNfo *nfo)
TNU 0:dfe498e03679 1082 {
TNU 0:dfe498e03679 1083 int res;
TNU 0:dfe498e03679 1084
TNU 0:dfe498e03679 1085 // Reset packet!
TNU 0:dfe498e03679 1086 memset( pack, 0, sizeof(*pack));
TNU 0:dfe498e03679 1087
TNU 0:dfe498e03679 1088 // Reject short packets
TNU 0:dfe498e03679 1089 if (nfo->pktype != PACK_STATUS_L && nfo->pktype != PACK_RDATA_RESP_L && nfo->pktype != PACK_WDATA_L)
TNU 0:dfe498e03679 1090 {
TNU 0:dfe498e03679 1091 return -1;
TNU 0:dfe498e03679 1092 }
TNU 0:dfe498e03679 1093
TNU 0:dfe498e03679 1094 if (nfo->pktype == PACK_STATUS_L)
TNU 0:dfe498e03679 1095 {
TNU 0:dfe498e03679 1096 res = SetBitVector( pack->buf, OFF_TYPE, WID_TYPE, nfo->pktype, BIT_ORDER);
TNU 0:dfe498e03679 1097 if (res < 0)
TNU 0:dfe498e03679 1098 return -1;
TNU 0:dfe498e03679 1099
TNU 0:dfe498e03679 1100 // Is error field
TNU 0:dfe498e03679 1101 res = SetBitVector( pack->buf, OFF_ISERR, WID_ISERR, nfo->iserr, BIT_ORDER);
TNU 0:dfe498e03679 1102 if (res < 0)
TNU 0:dfe498e03679 1103 return -1;
TNU 0:dfe498e03679 1104
TNU 0:dfe498e03679 1105 // Error qualifier field
TNU 0:dfe498e03679 1106 res = SetBitVector( pack->buf, OFF_ERROR, WID_ERROR, nfo->error, BIT_ORDER);
TNU 0:dfe498e03679 1107 if (res < 0)
TNU 0:dfe498e03679 1108 return -1;
TNU 0:dfe498e03679 1109
TNU 0:dfe498e03679 1110 goto END;
TNU 0:dfe498e03679 1111 }
TNU 0:dfe498e03679 1112
TNU 0:dfe498e03679 1113 // Packet type field
TNU 0:dfe498e03679 1114 res = SetBitVector( pack->buf, OFF_TYPE, WID_TYPE, nfo->pktype, BIT_ORDER);
TNU 0:dfe498e03679 1115 if (res < 0)
TNU 0:dfe498e03679 1116 return -1;
TNU 0:dfe498e03679 1117
TNU 0:dfe498e03679 1118 // Size field
TNU 0:dfe498e03679 1119 res = SetBitVector( pack->buf, OFF_SIZE, WID_SIZE, nfo->size, BIT_ORDER);
TNU 0:dfe498e03679 1120 if (res < 0)
TNU 0:dfe498e03679 1121 return -1;
TNU 0:dfe498e03679 1122
TNU 0:dfe498e03679 1123 // Sequence field
TNU 0:dfe498e03679 1124 res = SetBitVector( pack->buf, OFF_SEQ, WID_SEQ, nfo->seq, BIT_ORDER);
TNU 0:dfe498e03679 1125 if (res < 0)
TNU 0:dfe498e03679 1126 return -1;
TNU 0:dfe498e03679 1127
TNU 0:dfe498e03679 1128 // Data field
TNU 0:dfe498e03679 1129 memcpy( pack->data, nfo->dataP, nfo->size);
TNU 0:dfe498e03679 1130 if (res < 0)
TNU 0:dfe498e03679 1131 return -1;
TNU 0:dfe498e03679 1132
TNU 0:dfe498e03679 1133 //SWAP(&pack,sizeof(*pack));
TNU 0:dfe498e03679 1134
TNU 0:dfe498e03679 1135 END:
TNU 0:dfe498e03679 1136 FillCRC_long1(pack);
TNU 0:dfe498e03679 1137 //SWAP( &pack->crc, 1);
TNU 0:dfe498e03679 1138
TNU 0:dfe498e03679 1139 return 0;
TNU 0:dfe498e03679 1140 }
TNU 0:dfe498e03679 1141
TNU 0:dfe498e03679 1142
TNU 0:dfe498e03679 1143
TNU 0:dfe498e03679 1144 /// ****************************************************************************
TNU 0:dfe498e03679 1145 /// \fn DecodeL
TNU 0:dfe498e03679 1146 ///
TNU 0:dfe498e03679 1147 /// \brief Generic long packet decoder.
TNU 0:dfe498e03679 1148 ///
TNU 0:dfe498e03679 1149 /// \param[in] *pack: Packet to be decoded into info
TNU 0:dfe498e03679 1150 /// \param[out] *nfo: Filled packet info from pack data
TNU 0:dfe498e03679 1151 ///
TNU 0:dfe498e03679 1152 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 1153 ///
TNU 0:dfe498e03679 1154 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 1155 /// \since 2015-04-27
TNU 0:dfe498e03679 1156 /// ****************************************************************************
TNU 0:dfe498e03679 1157 int DecodeL1( const PACK_LONG1 *pack, MLX_PackNfo *nfo)
TNU 0:dfe498e03679 1158 {
TNU 0:dfe498e03679 1159 uint32 v;
TNU 0:dfe498e03679 1160 int res;
TNU 0:dfe498e03679 1161
TNU 0:dfe498e03679 1162 //SWAP(&pack,sizeof(*pack));
TNU 0:dfe498e03679 1163
TNU 0:dfe498e03679 1164 res = GetBitVector( pack->buf, OFF_TYPE, WID_TYPE, &v, BIT_ORDER);
TNU 0:dfe498e03679 1165 if (res < 0)
TNU 0:dfe498e03679 1166 return -1;
TNU 0:dfe498e03679 1167 nfo->pktype = (PackType)v;
TNU 0:dfe498e03679 1168
TNU 0:dfe498e03679 1169 res = GetBitVector( pack->buf, OFF_SIZE, WID_SIZE, &v, BIT_ORDER);
TNU 0:dfe498e03679 1170 if (res < 0)
TNU 0:dfe498e03679 1171 return -1;
TNU 0:dfe498e03679 1172 nfo->size = v;
TNU 0:dfe498e03679 1173
TNU 0:dfe498e03679 1174 res = GetBitVector( pack->buf, OFF_SEQ, WID_SEQ, &v, BIT_ORDER);
TNU 0:dfe498e03679 1175 if (res < 0)
TNU 0:dfe498e03679 1176 return -1;
TNU 0:dfe498e03679 1177 nfo->seq = v;
TNU 0:dfe498e03679 1178
TNU 0:dfe498e03679 1179 if (nfo->pktype == PACK_STATUS_L)
TNU 0:dfe498e03679 1180 {
TNU 0:dfe498e03679 1181 res = GetBitVector( pack->buf, OFF_ISERR, WID_ISERR, &v, BIT_ORDER);
TNU 0:dfe498e03679 1182 if (res < 0)
TNU 0:dfe498e03679 1183 return -1;
TNU 0:dfe498e03679 1184
TNU 0:dfe498e03679 1185 nfo->iserr = v;
TNU 0:dfe498e03679 1186
TNU 0:dfe498e03679 1187 res = GetBitVector( pack->buf, OFF_ERROR, WID_ERROR, &v, BIT_ORDER);
TNU 0:dfe498e03679 1188 if (res < 0)
TNU 0:dfe498e03679 1189 return -1;
TNU 0:dfe498e03679 1190
TNU 0:dfe498e03679 1191 nfo->error = v;
TNU 0:dfe498e03679 1192 return 0;
TNU 0:dfe498e03679 1193 }
TNU 0:dfe498e03679 1194
TNU 0:dfe498e03679 1195 return 0;
TNU 0:dfe498e03679 1196 }
TNU 0:dfe498e03679 1197
TNU 0:dfe498e03679 1198 /// ****************************************************************************
TNU 0:dfe498e03679 1199 /// \fn EncodeL2
TNU 0:dfe498e03679 1200 ///
TNU 0:dfe498e03679 1201 /// \brief Generic long packet encoder.
TNU 0:dfe498e03679 1202 ///
TNU 0:dfe498e03679 1203 /// \param[out] *pack: Resulting packet encoded with nfo
TNU 0:dfe498e03679 1204 /// \param[in] *nfo: Info used to encode packet
TNU 0:dfe498e03679 1205 ///
TNU 0:dfe498e03679 1206 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 1207 ///
TNU 0:dfe498e03679 1208 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 1209 /// \since 2015-04-27
TNU 0:dfe498e03679 1210 /// ****************************************************************************
TNU 0:dfe498e03679 1211 int EncodeL2(PACK_LONG2 *pack, const MLX_PackNfo *nfo)
TNU 0:dfe498e03679 1212 {
TNU 0:dfe498e03679 1213 int res;
TNU 0:dfe498e03679 1214
TNU 0:dfe498e03679 1215 // Reset packet!
TNU 0:dfe498e03679 1216 memset(pack, 0, sizeof(*pack));
TNU 0:dfe498e03679 1217
TNU 0:dfe498e03679 1218 // Reject short packets
TNU 0:dfe498e03679 1219 if (nfo->pktype != PACK_STATUS_L && nfo->pktype != PACK_RDATA_RESP_L && nfo->pktype != PACK_WDATA_L)
TNU 0:dfe498e03679 1220 {
TNU 0:dfe498e03679 1221 return -1;
TNU 0:dfe498e03679 1222 }
TNU 0:dfe498e03679 1223
TNU 0:dfe498e03679 1224 if (nfo->pktype == PACK_STATUS_L)
TNU 0:dfe498e03679 1225 {
TNU 0:dfe498e03679 1226 res = SetBitVector(pack->buf, OFF_TYPE, WID_TYPE, nfo->pktype, BIT_ORDER);
TNU 0:dfe498e03679 1227 if (res < 0)
TNU 0:dfe498e03679 1228 return -1;
TNU 0:dfe498e03679 1229
TNU 0:dfe498e03679 1230 // Is error field
TNU 0:dfe498e03679 1231 res = SetBitVector(pack->buf, OFF_ISERR, WID_ISERR, nfo->iserr, BIT_ORDER);
TNU 0:dfe498e03679 1232 if (res < 0)
TNU 0:dfe498e03679 1233 return -1;
TNU 0:dfe498e03679 1234
TNU 0:dfe498e03679 1235 // Error qualifier field
TNU 0:dfe498e03679 1236 res = SetBitVector(pack->buf, OFF_ERROR, WID_ERROR, nfo->error, BIT_ORDER);
TNU 0:dfe498e03679 1237 if (res < 0)
TNU 0:dfe498e03679 1238 return -1;
TNU 0:dfe498e03679 1239
TNU 0:dfe498e03679 1240 goto END;
TNU 0:dfe498e03679 1241 }
TNU 0:dfe498e03679 1242
TNU 0:dfe498e03679 1243 // Packet type field
TNU 0:dfe498e03679 1244 res = SetBitVector(pack->buf, OFF_TYPE, WID_TYPE, nfo->pktype, BIT_ORDER);
TNU 0:dfe498e03679 1245 if (res < 0)
TNU 0:dfe498e03679 1246 return -1;
TNU 0:dfe498e03679 1247
TNU 0:dfe498e03679 1248 // Size field
TNU 0:dfe498e03679 1249 res = SetBitVector(pack->buf, OFF_SIZE, WID_SIZE, nfo->size, BIT_ORDER);
TNU 0:dfe498e03679 1250 if (res < 0)
TNU 0:dfe498e03679 1251 return -1;
TNU 0:dfe498e03679 1252
TNU 0:dfe498e03679 1253 // Sequence field
TNU 0:dfe498e03679 1254 res = SetBitVector(pack->buf, OFF_SEQ, WID_SEQ, nfo->seq, BIT_ORDER);
TNU 0:dfe498e03679 1255 if (res < 0)
TNU 0:dfe498e03679 1256 return -1;
TNU 0:dfe498e03679 1257
TNU 0:dfe498e03679 1258 // Data field
TNU 0:dfe498e03679 1259 memcpy(pack->data, nfo->dataP, nfo->size*2);
TNU 0:dfe498e03679 1260
TNU 0:dfe498e03679 1261 if (res < 0)
TNU 0:dfe498e03679 1262 return -1;
TNU 0:dfe498e03679 1263
TNU 0:dfe498e03679 1264 //SWAP(&pack,sizeof(*pack));
TNU 0:dfe498e03679 1265
TNU 0:dfe498e03679 1266 END:
TNU 0:dfe498e03679 1267 FillCRC_long2(pack);
TNU 0:dfe498e03679 1268 //SWAP( &pack->crc, 1);
TNU 0:dfe498e03679 1269
TNU 0:dfe498e03679 1270 return 0;
TNU 0:dfe498e03679 1271 }
TNU 0:dfe498e03679 1272
TNU 0:dfe498e03679 1273
TNU 0:dfe498e03679 1274
TNU 0:dfe498e03679 1275 /// ****************************************************************************
TNU 0:dfe498e03679 1276 /// \fn DecodeE
TNU 0:dfe498e03679 1277 ///
TNU 0:dfe498e03679 1278 /// \brief Generic long packet decoder.
TNU 0:dfe498e03679 1279 ///
TNU 0:dfe498e03679 1280 /// \param[in] *pack: Packet to be decoded into info
TNU 0:dfe498e03679 1281 /// \param[out] *nfo: Filled packet info from pack data
TNU 0:dfe498e03679 1282 ///
TNU 0:dfe498e03679 1283 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 1284 ///
TNU 0:dfe498e03679 1285 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 1286 /// \since 2015-04-27
TNU 0:dfe498e03679 1287 /// ****************************************************************************
TNU 0:dfe498e03679 1288 int DecodeL2(const PACK_LONG2 *pack, MLX_PackNfo *nfo)
TNU 0:dfe498e03679 1289 {
TNU 0:dfe498e03679 1290 uint32 v;
TNU 0:dfe498e03679 1291 int res;
TNU 0:dfe498e03679 1292
TNU 0:dfe498e03679 1293 //SWAP(&pack,sizeof(*pack));
TNU 0:dfe498e03679 1294
TNU 0:dfe498e03679 1295 res = GetBitVector(pack->buf, OFF_TYPE, WID_TYPE, &v, BIT_ORDER);
TNU 0:dfe498e03679 1296 if (res < 0)
TNU 0:dfe498e03679 1297 return -1;
TNU 0:dfe498e03679 1298 nfo->pktype = (PackType)v;
TNU 0:dfe498e03679 1299
TNU 0:dfe498e03679 1300 res = GetBitVector(pack->buf, OFF_SIZE, WID_SIZE, &v, BIT_ORDER);
TNU 0:dfe498e03679 1301 if (res < 0)
TNU 0:dfe498e03679 1302 return -1;
TNU 0:dfe498e03679 1303 nfo->size = v;
TNU 0:dfe498e03679 1304
TNU 0:dfe498e03679 1305 res = GetBitVector(pack->buf, OFF_SEQ, WID_SEQ, &v, BIT_ORDER);
TNU 0:dfe498e03679 1306 if (res < 0)
TNU 0:dfe498e03679 1307 return -1;
TNU 0:dfe498e03679 1308 nfo->seq = v;
TNU 0:dfe498e03679 1309
TNU 0:dfe498e03679 1310 if (nfo->pktype == PACK_STATUS_L)
TNU 0:dfe498e03679 1311 {
TNU 0:dfe498e03679 1312 res = GetBitVector(pack->buf, OFF_ISERR, WID_ISERR, &v, BIT_ORDER);
TNU 0:dfe498e03679 1313 if (res < 0)
TNU 0:dfe498e03679 1314 return -1;
TNU 0:dfe498e03679 1315
TNU 0:dfe498e03679 1316 nfo->iserr = v;
TNU 0:dfe498e03679 1317
TNU 0:dfe498e03679 1318 res = GetBitVector(pack->buf, OFF_ERROR, WID_ERROR, &v, BIT_ORDER);
TNU 0:dfe498e03679 1319 if (res < 0)
TNU 0:dfe498e03679 1320 return -1;
TNU 0:dfe498e03679 1321
TNU 0:dfe498e03679 1322 nfo->error = v;
TNU 0:dfe498e03679 1323 return 0;
TNU 0:dfe498e03679 1324 }
TNU 0:dfe498e03679 1325
TNU 0:dfe498e03679 1326 return 0;
TNU 0:dfe498e03679 1327 }
TNU 0:dfe498e03679 1328
TNU 0:dfe498e03679 1329
TNU 0:dfe498e03679 1330
TNU 0:dfe498e03679 1331
TNU 0:dfe498e03679 1332 /// ****************************************************************************
TNU 0:dfe498e03679 1333 /// \fn MLX_EncodeStatusS
TNU 0:dfe498e03679 1334 ///
TNU 0:dfe498e03679 1335 /// \brief Creates a short status packet.
TNU 0:dfe498e03679 1336 ///
TNU 0:dfe498e03679 1337 /// \param[out] *pack: Resulting encoded packet
TNU 0:dfe498e03679 1338 /// \param[in] iserr: Flag telling if it is an error or not
TNU 0:dfe498e03679 1339 /// \param[in] err: Error qualifier data
TNU 0:dfe498e03679 1340 ///
TNU 0:dfe498e03679 1341 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 1342 ///
TNU 0:dfe498e03679 1343 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 1344 /// \since 2015-04-28
TNU 0:dfe498e03679 1345 /// ****************************************************************************
TNU 0:dfe498e03679 1346 int MLX_EncodeStatusS( PACK_SHORT *pack, uint8 iserr, uint16 err)
TNU 0:dfe498e03679 1347 {
TNU 0:dfe498e03679 1348 int res;
TNU 0:dfe498e03679 1349 MLX_PackNfo nfo;
TNU 0:dfe498e03679 1350
TNU 0:dfe498e03679 1351 // Reset packet!
TNU 0:dfe498e03679 1352 memset( pack, 0, sizeof(*pack));
TNU 0:dfe498e03679 1353
TNU 0:dfe498e03679 1354 nfo.pktype = PACK_STATUS_S;
TNU 0:dfe498e03679 1355 nfo.iserr = iserr;
TNU 0:dfe498e03679 1356 nfo.error = err;
TNU 0:dfe498e03679 1357
TNU 0:dfe498e03679 1358 res = EncodeS( pack, &nfo);
TNU 0:dfe498e03679 1359 if (res < 0)
TNU 0:dfe498e03679 1360 return -1;
TNU 0:dfe498e03679 1361
TNU 0:dfe498e03679 1362 return 0;
TNU 0:dfe498e03679 1363 }
TNU 0:dfe498e03679 1364
TNU 0:dfe498e03679 1365 /// ****************************************************************************
TNU 0:dfe498e03679 1366 /// \fn MLX_DecodeStatusS
TNU 0:dfe498e03679 1367 ///
TNU 0:dfe498e03679 1368 /// \brief Attempts to decode a packet as a status packet.
TNU 0:dfe498e03679 1369 ///
TNU 0:dfe498e03679 1370 /// \param[in] *pack: Packet to decode
TNU 0:dfe498e03679 1371 /// \param[out] *iserr: Flag telling if it is an error or not
TNU 0:dfe498e03679 1372 /// \param[out] *err: Error qualifier data
TNU 0:dfe498e03679 1373 ///
TNU 0:dfe498e03679 1374 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 1375 ///
TNU 0:dfe498e03679 1376 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 1377 /// \since 2015-05-01
TNU 0:dfe498e03679 1378 /// ****************************************************************************
TNU 0:dfe498e03679 1379 int MLX_DecodeStatusS( const PACK_SHORT *pack, uint8 *iserr, uint16 *err)
TNU 0:dfe498e03679 1380 {
TNU 0:dfe498e03679 1381 int res;
TNU 0:dfe498e03679 1382 MLX_PackNfo nfo;
TNU 0:dfe498e03679 1383
TNU 0:dfe498e03679 1384 if (!ValidCRC_short(pack)){
TNU 0:dfe498e03679 1385 return -1;
TNU 0:dfe498e03679 1386 }
TNU 0:dfe498e03679 1387
TNU 0:dfe498e03679 1388 res = DecodeS( pack, &nfo);
TNU 0:dfe498e03679 1389 if (res < 0 || nfo.pktype != PACK_STATUS_S)
TNU 0:dfe498e03679 1390 return -1;
TNU 0:dfe498e03679 1391
TNU 0:dfe498e03679 1392 *iserr = nfo.iserr;
TNU 0:dfe498e03679 1393 *err = nfo.error;
TNU 0:dfe498e03679 1394
TNU 0:dfe498e03679 1395 return 0;
TNU 0:dfe498e03679 1396 }
TNU 0:dfe498e03679 1397
TNU 0:dfe498e03679 1398
TNU 0:dfe498e03679 1399 /// ****************************************************************************
TNU 0:dfe498e03679 1400 /// \fn MLX_EncodeStatusL
TNU 0:dfe498e03679 1401 ///
TNU 0:dfe498e03679 1402 /// \brief Creates a long status packet.
TNU 0:dfe498e03679 1403 ///
TNU 0:dfe498e03679 1404 /// \param[out] *pack: Resulting encoded packet
TNU 0:dfe498e03679 1405 /// \param[in] iserr: Flag telling if it is an error or not
TNU 0:dfe498e03679 1406 /// \param[in] err: Error qualifier data
TNU 0:dfe498e03679 1407 ///
TNU 0:dfe498e03679 1408 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 1409 ///
TNU 0:dfe498e03679 1410 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 1411 /// \since 2015-05-19
TNU 0:dfe498e03679 1412 /// ****************************************************************************
TNU 0:dfe498e03679 1413 int MLX_EncodeStatusL1( PACK_LONG1 *pack, uint8 iserr, uint16 err)
TNU 0:dfe498e03679 1414 {
TNU 0:dfe498e03679 1415 int res;
TNU 0:dfe498e03679 1416 MLX_PackNfo nfo;
TNU 0:dfe498e03679 1417
TNU 0:dfe498e03679 1418 // Reset packet!
TNU 0:dfe498e03679 1419 memset(pack, 0, sizeof(*pack));
TNU 0:dfe498e03679 1420
TNU 0:dfe498e03679 1421 nfo.pktype = PACK_STATUS_L;
TNU 0:dfe498e03679 1422 nfo.iserr = iserr;
TNU 0:dfe498e03679 1423 nfo.error = err;
TNU 0:dfe498e03679 1424
TNU 0:dfe498e03679 1425 res = EncodeL1( pack, &nfo);
TNU 0:dfe498e03679 1426 if (res < 0)
TNU 0:dfe498e03679 1427 return -1;
TNU 0:dfe498e03679 1428
TNU 0:dfe498e03679 1429 return 0;
TNU 0:dfe498e03679 1430 }
TNU 0:dfe498e03679 1431
TNU 0:dfe498e03679 1432 /// ****************************************************************************
TNU 0:dfe498e03679 1433 /// \fn MLX_DecodeStatusL
TNU 0:dfe498e03679 1434 ///
TNU 0:dfe498e03679 1435 /// \brief Attempts to decode a packet as a status packet.
TNU 0:dfe498e03679 1436 ///
TNU 0:dfe498e03679 1437 /// \param[in] *pack: Packet to decode
TNU 0:dfe498e03679 1438 /// \param[out] *iserr: Flag telling if it is an error or not
TNU 0:dfe498e03679 1439 /// \param[out] *err: Error qualifier data
TNU 0:dfe498e03679 1440 ///
TNU 0:dfe498e03679 1441 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 1442 ///
TNU 0:dfe498e03679 1443 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 1444 /// \since 2015-05-19
TNU 0:dfe498e03679 1445 /// ****************************************************************************
TNU 0:dfe498e03679 1446 int MLX_DecodeStatusL1( const PACK_LONG1 *pack, uint8 *iserr, uint16 *err)
TNU 0:dfe498e03679 1447 {
TNU 0:dfe498e03679 1448 int res;
TNU 0:dfe498e03679 1449 MLX_PackNfo nfo;
TNU 0:dfe498e03679 1450
TNU 0:dfe498e03679 1451 if (!ValidCRC_long1(pack))
TNU 0:dfe498e03679 1452 return -1;
TNU 0:dfe498e03679 1453
TNU 0:dfe498e03679 1454 res = DecodeL1( pack, &nfo);
TNU 0:dfe498e03679 1455 if (res < 0 || nfo.pktype != PACK_STATUS_L)
TNU 0:dfe498e03679 1456 return -1;
TNU 0:dfe498e03679 1457
TNU 0:dfe498e03679 1458 *iserr = nfo.iserr;
TNU 0:dfe498e03679 1459 *err = nfo.error;
TNU 0:dfe498e03679 1460
TNU 0:dfe498e03679 1461 return 0;
TNU 0:dfe498e03679 1462 }
TNU 0:dfe498e03679 1463
TNU 0:dfe498e03679 1464 /// ****************************************************************************
TNU 0:dfe498e03679 1465 /// \fn MLX_EncodeStatusL2
TNU 0:dfe498e03679 1466 ///
TNU 0:dfe498e03679 1467 /// \brief Creates a echo status packet.
TNU 0:dfe498e03679 1468 ///
TNU 0:dfe498e03679 1469 /// \param[out] *pack: Resulting encoded packet
TNU 0:dfe498e03679 1470 /// \param[in] iserr: Flag telling if it is an error or not
TNU 0:dfe498e03679 1471 /// \param[in] err: Error qualifier data
TNU 0:dfe498e03679 1472 ///
TNU 0:dfe498e03679 1473 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 1474 ///
TNU 0:dfe498e03679 1475 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 1476 /// \since 2015-05-19
TNU 0:dfe498e03679 1477 /// ****************************************************************************
TNU 0:dfe498e03679 1478 int MLX_EncodeStatusL2(PACK_LONG2 *pack, uint8 iserr, uint16 err)
TNU 0:dfe498e03679 1479 {
TNU 0:dfe498e03679 1480 int res;
TNU 0:dfe498e03679 1481 MLX_PackNfo nfo;
TNU 0:dfe498e03679 1482
TNU 0:dfe498e03679 1483 // Reset packet!
TNU 0:dfe498e03679 1484 memset(pack, 0, sizeof(*pack));
TNU 0:dfe498e03679 1485
TNU 0:dfe498e03679 1486 nfo.pktype = PACK_STATUS_L;
TNU 0:dfe498e03679 1487 nfo.iserr = iserr;
TNU 0:dfe498e03679 1488 nfo.error = err;
TNU 0:dfe498e03679 1489
TNU 0:dfe498e03679 1490 res = EncodeL2(pack, &nfo);
TNU 0:dfe498e03679 1491 if (res < 0)
TNU 0:dfe498e03679 1492 return -1;
TNU 0:dfe498e03679 1493
TNU 0:dfe498e03679 1494 return 0;
TNU 0:dfe498e03679 1495 }
TNU 0:dfe498e03679 1496
TNU 0:dfe498e03679 1497 /// ****************************************************************************
TNU 0:dfe498e03679 1498 /// \fn MLX_DecodeStatusL2
TNU 0:dfe498e03679 1499 ///
TNU 0:dfe498e03679 1500 /// \brief Attempts to decode a packet as a status packet.
TNU 0:dfe498e03679 1501 ///
TNU 0:dfe498e03679 1502 /// \param[in] *pack: Packet to decode
TNU 0:dfe498e03679 1503 /// \param[out] *iserr: Flag telling if it is an error or not
TNU 0:dfe498e03679 1504 /// \param[out] *err: Error qualifier data
TNU 0:dfe498e03679 1505 ///
TNU 0:dfe498e03679 1506 /// \return Error code: 0 on success or negative on error.
TNU 0:dfe498e03679 1507 ///
TNU 0:dfe498e03679 1508 /// \authors Jean-Fran�ois Bernier
TNU 0:dfe498e03679 1509 /// \since 2015-05-19
TNU 0:dfe498e03679 1510 /// ****************************************************************************
TNU 0:dfe498e03679 1511 int MLX_DecodeStatusL2(const PACK_LONG2 *pack, uint8 *iserr, uint16 *err)
TNU 0:dfe498e03679 1512 {
TNU 0:dfe498e03679 1513 int res;
TNU 0:dfe498e03679 1514 MLX_PackNfo nfo;
TNU 0:dfe498e03679 1515
TNU 0:dfe498e03679 1516 if (!ValidCRC_long2(pack))
TNU 0:dfe498e03679 1517 return -1;
TNU 0:dfe498e03679 1518
TNU 0:dfe498e03679 1519 res = DecodeL2(pack, &nfo);
TNU 0:dfe498e03679 1520 if (res < 0 || nfo.pktype != PACK_STATUS_L)
TNU 0:dfe498e03679 1521 return -1;
TNU 0:dfe498e03679 1522
TNU 0:dfe498e03679 1523 *iserr = nfo.iserr;
TNU 0:dfe498e03679 1524 *err = nfo.error;
TNU 0:dfe498e03679 1525
TNU 0:dfe498e03679 1526 return 0;
TNU 0:dfe498e03679 1527 }
TNU 0:dfe498e03679 1528
TNU 0:dfe498e03679 1529