637 lines
16 KiB
C++
637 lines
16 KiB
C++
#include "Header_Files/sbgbuffer.h"
|
|
|
|
using namespace sbgtc;
|
|
|
|
rawBuffer sbgtc::initRawBuffer(QByteArray * sbgMessage)
|
|
{
|
|
rawBuffer tmp;
|
|
|
|
tmp.rxBuffer=(uint8_t *)sbgMessage->data();
|
|
tmp.rxBufferSize=sbgMessage->size();
|
|
|
|
return tmp;
|
|
}
|
|
|
|
SbgErrorCode sbgtc::sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize)
|
|
{
|
|
//
|
|
// Check input parameters
|
|
//
|
|
// assert(pHandle);
|
|
// assert(pLinkedBuffer);
|
|
|
|
//
|
|
// Initialize stream parameters
|
|
//
|
|
pHandle->bufferSize = bufferSize;
|
|
pHandle->errorCode = SBG_NO_ERROR;
|
|
|
|
//
|
|
// Initialize the buffer
|
|
//
|
|
pHandle->pBufferPtr = (uint8_t*)pLinkedBuffer;
|
|
pHandle->pCurrentPtr = (uint8_t*)pLinkedBuffer;
|
|
|
|
//
|
|
// For now, we don't handle any error, maybe we could add checks in debug mode only
|
|
//
|
|
return SBG_NO_ERROR;
|
|
}
|
|
|
|
SbgErrorCode sbgtc::sbgStreamBufferSeek(SbgStreamBuffer *pHandle, size_t offset, SbgSBSeekOrigin origin)
|
|
{
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
//
|
|
// Test if we haven't already an error
|
|
//
|
|
if (pHandle->errorCode == SBG_NO_ERROR)
|
|
{
|
|
//
|
|
// According to the origin reference point
|
|
//
|
|
switch (origin)
|
|
{
|
|
case SB_SEEK_SET:
|
|
pHandle->pCurrentPtr = pHandle->pBufferPtr + offset;
|
|
break;
|
|
case SB_SEEK_CUR_INC:
|
|
pHandle->pCurrentPtr += offset;
|
|
break;
|
|
case SB_SEEK_CUR_DEC:
|
|
pHandle->pCurrentPtr -= offset;
|
|
break;
|
|
case SB_SEEK_END:
|
|
pHandle->pCurrentPtr = pHandle->pBufferPtr + (pHandle->bufferSize - offset);
|
|
break;
|
|
default:
|
|
pHandle->errorCode = SBG_INVALID_PARAMETER;
|
|
//SBG_LOG_ERROR(pHandle->errorCode, "Invalid origin parameter");
|
|
}
|
|
|
|
//
|
|
// Make sure that no error has occurred
|
|
//
|
|
if (pHandle->errorCode == SBG_NO_ERROR)
|
|
{
|
|
//
|
|
// Test if the current ptr is still within the buffer bounds
|
|
//
|
|
if (pHandle->pCurrentPtr < pHandle->pBufferPtr)
|
|
{
|
|
//
|
|
// We are before the buffer so clamp to the begining of the buffer and raise an error
|
|
//
|
|
pHandle->pCurrentPtr = pHandle->pBufferPtr;
|
|
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
|
|
|
//
|
|
// Stream buffer underflow
|
|
//
|
|
//SBG_LOG_ERROR(pHandle->errorCode, "Trying to seek before the buffer");
|
|
}
|
|
else if (pHandle->pCurrentPtr > pHandle->pBufferPtr + pHandle->bufferSize)
|
|
{
|
|
//
|
|
// We are after the buffer so clamp to the end of the buffer and raise an error
|
|
//
|
|
pHandle->pCurrentPtr = pHandle->pBufferPtr + pHandle->bufferSize;
|
|
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
|
|
|
//
|
|
// Stream buffer overflow
|
|
//
|
|
//SBG_LOG_ERROR(pHandle->errorCode, "Trying to seek after the buffer");
|
|
}
|
|
}
|
|
}
|
|
|
|
return pHandle->errorCode;
|
|
}
|
|
|
|
size_t sbgtc::sbgStreamBufferTell(SbgStreamBuffer *pHandle)
|
|
{
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
return (size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr;
|
|
}
|
|
|
|
void * sbgtc::sbgStreamBufferGetCursor(SbgStreamBuffer *pHandle)
|
|
{
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
return pHandle->pCurrentPtr;
|
|
}
|
|
|
|
uint8_t sbgtc::sbgStreamBufferReadUint8LE(SbgStreamBuffer *pHandle)
|
|
{
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
//
|
|
// Test if we haven't already an error
|
|
//
|
|
if (pHandle->errorCode == SBG_NO_ERROR)
|
|
{
|
|
//
|
|
// Test if we can access this item
|
|
//
|
|
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint8_t))
|
|
{
|
|
//
|
|
// Read the byte
|
|
//
|
|
return *((uint8_t*)(pHandle->pCurrentPtr++));
|
|
}
|
|
else
|
|
{
|
|
//
|
|
// We have a buffer overflow
|
|
//
|
|
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
|
}
|
|
}
|
|
|
|
//
|
|
// If we are here, it means we have an error so return 0
|
|
//
|
|
return 0;
|
|
}
|
|
|
|
uint16_t sbgtc::sbgStreamBufferReadUint16LE(SbgStreamBuffer *pHandle)
|
|
{
|
|
uint16_t bytesValues[2];
|
|
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
//
|
|
// Test if we haven't already an error
|
|
//
|
|
if (pHandle->errorCode == SBG_NO_ERROR)
|
|
{
|
|
//
|
|
// Test if we can access this item
|
|
//
|
|
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint16_t))
|
|
{
|
|
//
|
|
// Test if the platform supports un-aligned access and if the endianness is the same
|
|
//
|
|
#if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
|
|
//
|
|
// Read the current value
|
|
//
|
|
bytesValues[0] = *((uint16_t*)pHandle->pCurrentPtr);
|
|
|
|
//
|
|
// Increment the current pointer
|
|
//
|
|
pHandle->pCurrentPtr += sizeof(uint16_t);
|
|
|
|
return bytesValues[0];
|
|
#else
|
|
//
|
|
// Read the each bytes
|
|
//
|
|
bytesValues[0] = *(pHandle->pCurrentPtr++);
|
|
bytesValues[1] = *(pHandle->pCurrentPtr++);
|
|
|
|
//
|
|
// Store data according to platform endianness
|
|
//
|
|
#if (SBG_CONFIG_BIG_ENDIAN == 1)
|
|
return bytesValues[1] | (bytesValues[0] << 8);
|
|
#else
|
|
return bytesValues[0] | (bytesValues[1] << 8);
|
|
#endif
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
//
|
|
// We have a buffer overflow so return 0
|
|
//
|
|
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
|
}
|
|
}
|
|
|
|
//
|
|
// If we are here, it means we have an error so return 0
|
|
//
|
|
return 0;
|
|
}
|
|
|
|
uint32_t sbgtc::sbgStreamBufferReadUint32LE(SbgStreamBuffer *pHandle)
|
|
{
|
|
uint32_t bytesValues[4];
|
|
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
//
|
|
// Test if we haven't already an error
|
|
//
|
|
if (pHandle->errorCode == SBG_NO_ERROR)
|
|
{
|
|
//
|
|
// Test if we can access this item
|
|
//
|
|
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint32_t))
|
|
{
|
|
//
|
|
// Test if the platform supports un-aligned access and if the endianness is the same
|
|
//
|
|
#if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
|
|
//
|
|
// Read the current value
|
|
//
|
|
bytesValues[0] = *((uint32_t*)pHandle->pCurrentPtr);
|
|
|
|
//
|
|
// Increment the current pointer
|
|
//
|
|
pHandle->pCurrentPtr += sizeof(uint32_t);
|
|
|
|
return bytesValues[0];
|
|
#else
|
|
//
|
|
// Read the each bytes
|
|
//
|
|
bytesValues[0] = *(pHandle->pCurrentPtr++);
|
|
bytesValues[1] = *(pHandle->pCurrentPtr++);
|
|
bytesValues[2] = *(pHandle->pCurrentPtr++);
|
|
bytesValues[3] = *(pHandle->pCurrentPtr++);
|
|
|
|
//
|
|
// Store data according to platform endianness
|
|
//
|
|
#if (SBG_CONFIG_BIG_ENDIAN == 1)
|
|
return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24);
|
|
#else
|
|
return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24);
|
|
#endif
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
//
|
|
// We have a buffer overflow so return 0
|
|
//
|
|
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
|
}
|
|
}
|
|
|
|
//
|
|
// If we are here, it means we have an error so return 0
|
|
//
|
|
return 0;
|
|
}
|
|
|
|
/*!
|
|
* Read an uint64_t from a stream buffer (Little endian version).
|
|
* \param[in] pHandle Valid stream buffer handle that supports read operations.
|
|
* \return The read value or 0 if we have an error.
|
|
*/
|
|
uint64_t sbgtc::sbgStreamBufferReadUint64LE(SbgStreamBuffer *pHandle)
|
|
{
|
|
uint64_t lowPart;
|
|
uint64_t highPart;
|
|
|
|
//
|
|
// Check input parameters
|
|
//
|
|
// assert(pHandle);
|
|
|
|
//
|
|
// Test if we haven't already an error
|
|
//
|
|
if (pHandle->errorCode == SBG_NO_ERROR)
|
|
{
|
|
//
|
|
// Test if we can access this item
|
|
//
|
|
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint64_t))
|
|
{
|
|
//
|
|
// Test if the platform supports un-aligned access and if the endianness is the same
|
|
//
|
|
#if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
|
|
//
|
|
// Read the current value
|
|
//
|
|
lowPart = *((uint64_t*)pHandle->pCurrentPtr);
|
|
|
|
//
|
|
// Increment the current pointer
|
|
//
|
|
pHandle->pCurrentPtr += sizeof(uint64_t);
|
|
|
|
return lowPart;
|
|
#else
|
|
//
|
|
// Read 64 bit value using two 32 bits read to avoid too much 64 bits operations
|
|
//
|
|
lowPart = sbgStreamBufferReadUint32LE(pHandle);
|
|
highPart = sbgStreamBufferReadUint32LE(pHandle);
|
|
|
|
//
|
|
// Store data according to platform endianness
|
|
//
|
|
#if (SBG_CONFIG_BIG_ENDIAN == 1)
|
|
return (lowPart << 32) | highPart;
|
|
#else
|
|
return lowPart | (highPart << 32);
|
|
#endif
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
//
|
|
// We have a buffer overflow so return 0
|
|
//
|
|
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
|
}
|
|
}
|
|
|
|
//
|
|
// If we are here, it means we have an error so return 0
|
|
//
|
|
return 0ull;
|
|
}
|
|
|
|
int8_t sbgtc::sbgStreamBufferReadInt8LE(SbgStreamBuffer *pHandle)
|
|
{
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
//
|
|
// Test if we haven't already an error
|
|
//
|
|
if (pHandle->errorCode == SBG_NO_ERROR)
|
|
{
|
|
//
|
|
// Test if we can access this item
|
|
//
|
|
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int8_t))
|
|
{
|
|
//
|
|
// Read the byte
|
|
//
|
|
return *((int8_t*)(pHandle->pCurrentPtr++));
|
|
}
|
|
else
|
|
{
|
|
//
|
|
// We have a buffer overflow
|
|
//
|
|
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
|
}
|
|
}
|
|
|
|
//
|
|
// If we are here, it means we have an error so return 0
|
|
//
|
|
return 0;
|
|
}
|
|
|
|
int32_t sbgtc::sbgStreamBufferReadInt32LE(SbgStreamBuffer *pHandle)
|
|
{
|
|
int32_t bytesValues[4];
|
|
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
//
|
|
// Test if we haven't already an error
|
|
//
|
|
if (pHandle->errorCode == SBG_NO_ERROR)
|
|
{
|
|
//
|
|
// Test if we can access this item
|
|
//
|
|
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int32_t))
|
|
{
|
|
//
|
|
// Test if the platform supports un-aligned access and if the endianness is the same
|
|
//
|
|
#if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
|
|
//
|
|
// Read the current value
|
|
//
|
|
bytesValues[0] = *((int32_t*)pHandle->pCurrentPtr);
|
|
|
|
//
|
|
// Increment the current pointer
|
|
//
|
|
pHandle->pCurrentPtr += sizeof(int32_t);
|
|
|
|
return bytesValues[0];
|
|
#else
|
|
//
|
|
// Read the each bytes
|
|
//
|
|
bytesValues[0] = *(pHandle->pCurrentPtr++);
|
|
bytesValues[1] = *(pHandle->pCurrentPtr++);
|
|
bytesValues[2] = *(pHandle->pCurrentPtr++);
|
|
bytesValues[3] = *(pHandle->pCurrentPtr++);
|
|
|
|
//
|
|
// Store data according to platform endianness
|
|
//
|
|
#if (SBG_CONFIG_BIG_ENDIAN == 1)
|
|
return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24);
|
|
#else
|
|
return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24);
|
|
#endif
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
//
|
|
// We have a buffer overflow so return 0
|
|
//
|
|
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
|
}
|
|
}
|
|
|
|
//
|
|
// If we are here, it means we have an error so return 0
|
|
//
|
|
return 0;
|
|
}
|
|
|
|
float sbgtc::sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle)
|
|
{
|
|
FloatNint floatInt;
|
|
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
//
|
|
// Test if we haven't already an error
|
|
//
|
|
if (pHandle->errorCode == SBG_NO_ERROR)
|
|
{
|
|
//
|
|
// Test if we can access this item
|
|
//
|
|
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(float))
|
|
{
|
|
//
|
|
// Read the float as an uint32_t
|
|
//
|
|
floatInt.valU = sbgStreamBufferReadUint32LE(pHandle);
|
|
|
|
//
|
|
// Return the float using an union to avoid compiler cast
|
|
//
|
|
return floatInt.valF;
|
|
}
|
|
else
|
|
{
|
|
//
|
|
// We have a buffer overflow so return 0
|
|
//
|
|
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
|
}
|
|
}
|
|
|
|
//
|
|
// If we are here, it means we have an error so return 0
|
|
//
|
|
return 0.0f;
|
|
}
|
|
|
|
/*!
|
|
* Read an double from a stream buffer (Little endian version).
|
|
* \param[in] pHandle Valid stream buffer handle that supports read operations.
|
|
* \return The read value or 0 if we have an error.
|
|
*/
|
|
double sbgtc::sbgStreamBufferReadDoubleLE(SbgStreamBuffer *pHandle)
|
|
{
|
|
DoubleNint doubleInt;
|
|
|
|
//
|
|
// Check input parameters
|
|
//
|
|
// assert(pHandle);
|
|
|
|
//
|
|
// Test if we haven't already an error
|
|
//
|
|
if (pHandle->errorCode == SBG_NO_ERROR)
|
|
{
|
|
//
|
|
// Test if we can access this item
|
|
//
|
|
if (sbgStreamBufferGetSpace(pHandle) >= sizeof(double))
|
|
{
|
|
//
|
|
// Read the float as an uint64_t
|
|
//
|
|
doubleInt.valU = sbgStreamBufferReadUint64LE(pHandle);
|
|
|
|
//
|
|
// Return the double using an union to avoid compiler cast
|
|
//
|
|
return doubleInt.valF;
|
|
}
|
|
else
|
|
{
|
|
//
|
|
// We have a buffer overflow so return 0
|
|
//
|
|
pHandle->errorCode = SBG_BUFFER_OVERFLOW;
|
|
}
|
|
}
|
|
|
|
//
|
|
// If we are here, it means we have an error so return 0
|
|
//
|
|
return 0.0;
|
|
}
|
|
|
|
SbgErrorCode sbgtc::sbgStreamBufferGetLastError(SbgStreamBuffer *pHandle)
|
|
{
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
//
|
|
// Return error code
|
|
//
|
|
return pHandle->errorCode;
|
|
}
|
|
|
|
size_t sbgtc::sbgStreamBufferGetSpace(SbgStreamBuffer *pHandle)
|
|
{
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
//
|
|
// Return the space left in bytes
|
|
//
|
|
return sbgStreamBufferGetSize(pHandle) - sbgStreamBufferGetLength(pHandle);
|
|
}
|
|
|
|
size_t sbgtc::sbgStreamBufferGetSize(SbgStreamBuffer *pHandle)
|
|
{
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
//
|
|
// Return the linked buffer size
|
|
//
|
|
return pHandle->bufferSize;
|
|
}
|
|
|
|
size_t sbgtc::sbgStreamBufferGetLength(SbgStreamBuffer *pHandle)
|
|
{
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
//
|
|
// Return the number of bytes between the begin of the stream and the current pointer
|
|
//
|
|
return ((size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr);
|
|
}
|
|
|
|
void * sbgtc::sbgStreamBufferGetLinkedBuffer(SbgStreamBuffer *pHandle)
|
|
{
|
|
//
|
|
// Check input parameters
|
|
//
|
|
//assert(pHandle);
|
|
|
|
return pHandle->pBufferPtr;
|
|
}
|