sbgEComProtocol.c
Go to the documentation of this file.
1 #include "sbgEComProtocol.h"
2 #include <crc/sbgCrc.h>
3 
4 //----------------------------------------------------------------------//
5 //- Communication protocol operations -//
6 //----------------------------------------------------------------------//
7 
15 {
16  SbgErrorCode errorCode = SBG_NO_ERROR;
17 
18  assert(pHandle);
19  assert(pInterface);
20 
21  //
22  // Initialize the created protocol handle
23  //
24  pHandle->pLinkedInterface = pInterface;
25  pHandle->rxBufferSize = 0;
26 
27  return errorCode;
28 }
29 
36 {
37  assert(pHandle);
38 
39  //
40  // Reset all members to zero
41  //
42  pHandle->pLinkedInterface = NULL;
43  pHandle->rxBufferSize = 0;
44 
45  //
46  // Don't have to do anything
47  //
48  return SBG_NO_ERROR;
49 }
50 
60 SbgErrorCode sbgEComProtocolSend(SbgEComProtocol *pHandle, uint8_t msgClass, uint8_t msg, const void *pData, size_t size)
61 {
62  SbgErrorCode errorCode = SBG_NO_ERROR;
63  uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
64  SbgStreamBuffer outputStream;
65  uint16_t frameCrc;
66 
67  assert(pHandle);
68 
69  if ( (size <= SBG_ECOM_MAX_PAYLOAD_SIZE) && ( ((size > 0) && (pData)) || (size == 0) ) )
70  {
71  //
72  // Create a stream buffer to write the frame
73  //
74  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
75 
76  //
77  // Write the header
78  //
81 
82  //
83  // Write the message ID and class
84  //
85  sbgStreamBufferWriteUint8LE(&outputStream, msg);
86  sbgStreamBufferWriteUint8LE(&outputStream, msgClass);
87 
88  //
89  // Write the length field
90  //
91  sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)size);
92 
93  //
94  // Write the payload part
95  //
96  sbgStreamBufferWriteBuffer(&outputStream, pData, size);
97 
98  //
99  // Compute the CRC, we skip the two sync chars
100  //
101  frameCrc = sbgCrc16Compute(((uint8_t*)sbgStreamBufferGetLinkedBuffer(&outputStream)) + 2, sbgStreamBufferGetLength(&outputStream) - 2);
102 
103  //
104  // Write the CRC
105  //
106  sbgStreamBufferWriteUint16LE(&outputStream, frameCrc);
107 
108  //
109  // Write ETX char
110  //
112 
113  //
114  // The frame has been generated so send it
115  //
116  errorCode = sbgInterfaceWrite(pHandle->pLinkedInterface, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream));
117  }
118  else
119  {
120  //
121  // Invalid input parameters
122  //
123  errorCode = SBG_INVALID_PARAMETER;
124  }
125 
126  return errorCode;
127 }
128 
143 SbgErrorCode sbgEComProtocolReceive(SbgEComProtocol *pHandle, uint8_t *pMsgClass, uint8_t *pMsg, void *pData, size_t *pSize, size_t maxSize)
144 {
145  SbgErrorCode errorCode = SBG_NOT_READY;
146  SbgStreamBuffer inputStream;
147  bool syncFound;
148  size_t payloadSize = 0;
149  uint16_t frameCrc;
150  uint16_t computedCrc;
151  size_t i;
152  size_t numBytesRead;
153  uint8_t receivedMsgClass;
154  uint8_t receivedMsg;
155  size_t payloadOffset;
156 
157  assert(pHandle);
158 
159  //
160  // Set the return size to 0 in order to avoid possible bugs
161  //
162  if (pSize)
163  {
164  *pSize = 0;
165  }
166 
167  //
168  // Check if we can receive some new data (the receive buffer isn't full)
169  //
170  if (pHandle->rxBufferSize < SBG_ECOM_MAX_BUFFER_SIZE)
171  {
172  //
173  // First try to read as much data as we can
174  //
175  if (sbgInterfaceRead(pHandle->pLinkedInterface, pHandle->rxBuffer + pHandle->rxBufferSize, &numBytesRead, SBG_ECOM_MAX_BUFFER_SIZE - pHandle->rxBufferSize) == SBG_NO_ERROR)
176  {
177  //
178  // No error during reading so increment the number of bytes stored in the rx buffer
179  //
180  pHandle->rxBufferSize += numBytesRead;
181  }
182  }
183 
184  //
185  // We have read all available data and stored them into the rx buffer
186  // We will try to process all received data until we have found a valid frame.
187  //
188  while (pHandle->rxBufferSize > 0)
189  {
190  //
191  // For now, we haven't found any start of frame
192  //
193  syncFound = FALSE;
194 
195  //
196  // To find a valid start of frame we need at least 2 bytes in the reception buffer
197  //
198  if (pHandle->rxBufferSize >= 2)
199  {
200  //
201  // Try to find a valid start of frame by looking for SYNC_1 and SYNC_2 chars
202  //
203  for (i = 0; i < pHandle->rxBufferSize-1; i++)
204  {
205  //
206  // A valid start of frame should begin with SYNC and when STX chars
207  //
208  if ( (pHandle->rxBuffer[i] == SBG_ECOM_SYNC_1) && (pHandle->rxBuffer[i+1] == SBG_ECOM_SYNC_2) )
209  {
210  //
211  // We have found the sync char, test if we have dummy bytes at the begining of the frame
212  //
213  if (i > 0)
214  {
215  //
216  // Remove all dumy received bytes before the begining of the frame
217  //
218  memmove(pHandle->rxBuffer, pHandle->rxBuffer+i, pHandle->rxBufferSize-i);
219  pHandle->rxBufferSize = pHandle->rxBufferSize-i;
220  }
221 
222  //
223  // The sync has been found
224  //
225  syncFound = TRUE;
226  break;
227  }
228  }
229  }
230 
231  //
232  // Check if a valid start of frame has been found
233  //
234  if (syncFound)
235  {
236  //
237  // A valid start of frame has been found, try to extract the frame if we have at least a whole frame.
238  //
239  if (pHandle->rxBufferSize < 8)
240  {
241  //
242  // Don't have enough data for a valid frame
243  //
244  return SBG_NOT_READY;
245  }
246 
247  //
248  // Initialize an input stream buffer to parse the received frame
249  //
250  sbgStreamBufferInitForRead(&inputStream, pHandle->rxBuffer, pHandle->rxBufferSize);
251 
252  //
253  // Skip both the Sync 1 and Sync 2 chars
254  //
255  sbgStreamBufferSeek(&inputStream, sizeof(uint8_t)*2, SB_SEEK_CUR_INC);
256 
257  //
258  // Read the command
259  //
260  receivedMsg = sbgStreamBufferReadUint8LE(&inputStream);
261  receivedMsgClass = sbgStreamBufferReadUint8LE(&inputStream);
262 
263  //
264  // Read the payload size
265  //
266  payloadSize = (uint16_t)sbgStreamBufferReadUint16LE(&inputStream);
267 
268  //
269  // Check that the payload size is valid
270  //
271  if (payloadSize <= SBG_ECOM_MAX_PAYLOAD_SIZE)
272  {
273  //
274  // Check if we have received the whole frame
275  //
276  if (pHandle->rxBufferSize < payloadSize+9)
277  {
278  //
279  // Don't have received the whole frame
280  //
281  return SBG_NOT_READY;
282  }
283 
284  //
285  // We should have the whole frame so for now, skip the payload part but before store the current cursor
286  //
287  payloadOffset = sbgStreamBufferTell(&inputStream);
288  sbgStreamBufferSeek(&inputStream, payloadSize, SB_SEEK_CUR_INC);
289 
290  //
291  // Read the frame CRC
292  //
293  frameCrc = sbgStreamBufferReadUint16LE(&inputStream);
294 
295  //
296  // Read and test the frame ETX
297  //
298  if (sbgStreamBufferReadUint8(&inputStream) == SBG_ECOM_ETX)
299  {
300  //
301  // Go back at the beginning of the payload part
302  //
303  sbgStreamBufferSeek(&inputStream, payloadOffset, SB_SEEK_SET);
304 
305  //
306  // We have a frame so return the received command if needed even if the CRC is still not validated
307  //
308  if (pMsg)
309  {
310  *pMsg = receivedMsg;
311  }
312  if (pMsgClass)
313  {
314  *pMsgClass = receivedMsgClass;
315  }
316 
317  //
318  // Compute the CRC of the received frame (Skip SYNC 1 and SYNC 2 chars)
319  //
320  computedCrc = sbgCrc16Compute(((uint8_t*)sbgStreamBufferGetLinkedBuffer(&inputStream)) + 2, payloadSize + 4);
321 
322  //
323  // Check if the received frame has a valid CRC
324  //
325  if (frameCrc == computedCrc)
326  {
327  //
328  // Extract the payload if needed
329  //
330  if (payloadSize > 0)
331  {
332  //
333  // Check if input parameters are valid
334  //
335  if ( (pData) && (pSize) )
336  {
337  //
338  // Check if we have enough space to store the payload
339  //
340  if (payloadSize <= maxSize)
341  {
342  //
343  // Copy the payload and return the payload size
344  //
345  *pSize = payloadSize;
346  memcpy(pData, sbgStreamBufferGetCursor(&inputStream), payloadSize);
347  errorCode = SBG_NO_ERROR;
348  }
349  else
350  {
351  //
352  // Not enough space to store the payload, we will just drop the received data
353  //
354  errorCode = SBG_BUFFER_OVERFLOW;
355  }
356  }
357  else
358  {
359  errorCode = SBG_NULL_POINTER;
360  }
361  }
362  else
363  {
364  //
365  // No payload but the frame has been read successfully
366  //
367  errorCode = SBG_NO_ERROR;
368  }
369  }
370  else
371  {
372  //
373  // We have an invalid frame CRC and we will directly return this error
374  //
375  errorCode = SBG_INVALID_CRC;
376  }
377 
378  //
379  // We have read a whole valid frame so remove it from the buffer
380  // First, test if the reception buffer contains more than just the current frame
381  //
382  if (pHandle->rxBufferSize > payloadSize+9)
383  {
384  //
385  // We remove the read frame but we keep the remaining data
386  //
387  pHandle->rxBufferSize = pHandle->rxBufferSize-(payloadSize+9);
388  memmove(pHandle->rxBuffer, pHandle->rxBuffer+payloadSize+9, pHandle->rxBufferSize);
389  }
390  else
391  {
392  //
393  // We have parsed the whole received buffer so just empty it
394  //
395  pHandle->rxBufferSize = 0;
396  }
397 
398  //
399  // We have at least found a complete frame
400  //
401  return errorCode;
402  }
403  }
404 
405  //
406  // Frame size invalid or the found frame is invalid so we should have incorrectly detected a start of frame.
407  // Remove the SYNC 1 and SYNC 2 chars to retry to find a new frame
408  //
409  pHandle->rxBufferSize -= 2;
410  memmove(pHandle->rxBuffer, pHandle->rxBuffer+2, pHandle->rxBufferSize);
411  }
412  else
413  {
414  //
415  // Unable to find a valid start of frame so check if the last byte is a SYNC char in order to keep it for next time
416  //
417  if (pHandle->rxBuffer[pHandle->rxBufferSize-1] == SBG_ECOM_SYNC_1)
418  {
419  //
420  // Report the SYNC char and discard all other bytes in the buffer
421  //
422  pHandle->rxBuffer[0] = SBG_ECOM_SYNC_1;
423  pHandle->rxBufferSize = 1;
424  }
425  else
426  {
427  //
428  // Discard the whole buffer
429  //
430  pHandle->rxBufferSize = 0;
431  }
432 
433  //
434  // Unable to find a frame
435  //
436  return SBG_NOT_READY;
437  }
438  }
439 
440  //
441  // The whole buffer has been paresed and no valid frame has been found
442  //
443  return SBG_NOT_READY;
444 }
445 
446 //----------------------------------------------------------------------//
447 //- Frame generation to stream buffer -//
448 //----------------------------------------------------------------------//
449 
461 SbgErrorCode sbgEComStartFrameGeneration(SbgStreamBuffer *pOutputStream, uint8_t msgClass, uint8_t msg, size_t *pStreamCursor)
462 {
463  assert(pOutputStream);
464  assert(pStreamCursor);
465 
466  //
467  // Backup the current position in the stream buffer
468  //
469  *pStreamCursor = sbgStreamBufferTell(pOutputStream);
470 
471  //
472  // Write the header
473  //
476 
477  //
478  // Write the message ID and class
479  //
480  sbgStreamBufferWriteUint8LE(pOutputStream, msg);
481  sbgStreamBufferWriteUint8LE(pOutputStream, msgClass);
482 
483  //
484  // For now, we don't know the payload size so skip it
485  //
486  return sbgStreamBufferSeek(pOutputStream, sizeof(uint16_t), SB_SEEK_CUR_INC);
487 }
488 
499 SbgErrorCode sbgEComFinalizeFrameGeneration(SbgStreamBuffer *pOutputStream, size_t streamCursor)
500 {
501  SbgErrorCode errorCode;
502  size_t payloadSize;
503  size_t currentPos;
504  uint16_t frameCrc;
505 
506  assert(pOutputStream);
507 
508  //
509  // Test if any error has occurred on the stream first
510  //
511  errorCode = sbgStreamBufferGetLastError(pOutputStream);
512 
513  //
514  // Is the stream buffer error free ?
515  //
516  if (errorCode == SBG_NO_ERROR)
517  {
518  //
519  // Compute the payload size (written data minus the header)
520  //
521  payloadSize = sbgStreamBufferGetLength(pOutputStream) - streamCursor - 6;
522 
523  //
524  // Test that the payload size is valid
525  //
526  if (payloadSize <= SBG_ECOM_MAX_PAYLOAD_SIZE)
527  {
528  //
529  // Backup the current cursor position
530  //
531  currentPos = sbgStreamBufferTell(pOutputStream);
532 
533  //
534  // Goto the payload size field (4th byte in the frame)
535  //
536  sbgStreamBufferSeek(pOutputStream, streamCursor+4, SB_SEEK_SET);
537 
538  //
539  // Write the payload size
540  //
541  sbgStreamBufferWriteUint16LE(pOutputStream, (uint16_t)payloadSize);
542 
543  //
544  // Go back to the previous position
545  //
546  sbgStreamBufferSeek(pOutputStream, currentPos, SB_SEEK_SET);
547 
548  //
549  // Compute the 16 bits CRC on the whole frame except Sync 1 and Sync 2
550  //
551  frameCrc = sbgCrc16Compute((uint8_t*)sbgStreamBufferGetLinkedBuffer(pOutputStream) + streamCursor + 2, payloadSize + 4);
552 
553  //
554  // Append the CRC
555  //
556  sbgStreamBufferWriteUint16LE(pOutputStream, frameCrc);
557 
558  //
559  // Append the ETX
560  //
561  errorCode = sbgStreamBufferWriteUint8LE(pOutputStream, SBG_ECOM_ETX);
562  }
563  else
564  {
565  //
566  // Invalid payload size
567  //
568  errorCode = SBG_BUFFER_OVERFLOW;
569  SBG_LOG_ERROR(errorCode, "Payload of %u bytes is too big for a valid sbgECom log", payloadSize);
570  }
571  }
572  else
573  {
574  //
575  // Notify error
576  //
577  SBG_LOG_ERROR(errorCode, "Unable to finalize frame because of an error on Stream Buffer");
578  }
579 
580  return errorCode;
581 }
SbgInterface * pLinkedInterface
SBG_INLINE SbgErrorCode sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize)
SbgErrorCode sbgEComProtocolReceive(SbgEComProtocol *pHandle, uint8_t *pMsgClass, uint8_t *pMsg, void *pData, size_t *pSize, size_t maxSize)
SBG_INLINE SbgErrorCode sbgStreamBufferSeek(SbgStreamBuffer *pHandle, size_t offset, SbgSBSeekOrigin origin)
SBG_INLINE uint8_t sbgStreamBufferReadUint8(SbgStreamBuffer *pHandle)
SBG_COMMON_LIB_API uint16_t sbgCrc16Compute(const void *pData, size_t dataSize)
Definition: sbgCrc.c:253
#define SBG_ECOM_ETX
SbgErrorCode sbgEComProtocolSend(SbgEComProtocol *pHandle, uint8_t msgClass, uint8_t msg, const void *pData, size_t size)
#define TRUE
Definition: sbgDefines.h:77
SBG_INLINE SbgErrorCode sbgStreamBufferInitForWrite(SbgStreamBuffer *pHandle, void *pLinkedBuffer, size_t bufferSize)
SbgErrorCode sbgEComProtocolInit(SbgEComProtocol *pHandle, SbgInterface *pInterface)
SBG_INLINE uint16_t sbgStreamBufferReadUint16LE(SbgStreamBuffer *pHandle)
SbgErrorCode sbgEComFinalizeFrameGeneration(SbgStreamBuffer *pOutputStream, size_t streamCursor)
uint8_t rxBuffer[SBG_ECOM_MAX_BUFFER_SIZE]
#define SBG_ECOM_SYNC_1
SBG_INLINE SbgErrorCode sbgInterfaceWrite(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite)
Definition: sbgInterface.h:165
#define sbgStreamBufferWriteUint8LE
SBG_INLINE void * sbgStreamBufferGetLinkedBuffer(SbgStreamBuffer *pHandle)
#define NULL
Definition: sbgDefines.h:81
SbgErrorCode sbgEComStartFrameGeneration(SbgStreamBuffer *pOutputStream, uint8_t msgClass, uint8_t msg, size_t *pStreamCursor)
SBG_INLINE SbgErrorCode sbgStreamBufferWriteBuffer(SbgStreamBuffer *pHandle, const void *pBuffer, size_t numBytesToWrite)
SBG_INLINE size_t sbgStreamBufferTell(SbgStreamBuffer *pHandle)
#define sbgStreamBufferReadUint8LE
This file provides CRC-32 and CRC-16 methods.
#define SBG_LOG_ERROR(format,...)
Definition: sbgDebug.h:62
SBG_INLINE SbgErrorCode sbgInterfaceRead(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead)
Definition: sbgInterface.h:190
SBG_INLINE size_t sbgStreamBufferGetLength(SbgStreamBuffer *pHandle)
#define FALSE
Definition: sbgDefines.h:73
SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint16LE(SbgStreamBuffer *pHandle, uint16_t value)
SbgErrorCode sbgEComProtocolClose(SbgEComProtocol *pHandle)
Implementation of the Ekinox binary communication protocol. You can access low-level communication w...
SBG_INLINE void * sbgStreamBufferGetCursor(SbgStreamBuffer *pHandle)
#define SBG_ECOM_MAX_PAYLOAD_SIZE
enum _SbgErrorCode SbgErrorCode
#define SBG_ECOM_SYNC_2
SBG_INLINE SbgErrorCode sbgStreamBufferGetLastError(SbgStreamBuffer *pHandle)
#define SBG_ECOM_MAX_BUFFER_SIZE


sbg_driver
Author(s): SBG Systems
autogenerated on Sat Sep 3 2022 02:53:36