public int TryParseNextIncomingPacket(byte[] outBuffer, out byte channelId)
{
ushort dataLength;
byte crc, headerLength, headerCrc;
if (_bufferLength == 0)
{
channelId = 0;
return(0);
}
if (!ParseHeader(_buffer, 0, out crc, out headerCrc, out channelId, out dataLength, out headerLength))
{
Debug.LogError("invalid message header!!!");
AdvanceToNextViablePacket();
return(0);
}
var dataLengthBytes = BitConverter.GetBytes(dataLength);
var actualHeaderCrc = CrcUtils.Checksum(new byte[] { channelId, dataLengthBytes[0], dataLengthBytes[1] }, 0, 3, HEADER_CRC_SIZE);
if (actualHeaderCrc != headerCrc)
{
Debug.LogError("header crc check failed");
AdvanceToNextViablePacket();
return(0);
}
if (_bufferLength == 0)
{
return(0);
}
if (_bufferLength < (headerLength + dataLength))
{
return(0); //not enough data
}
// copy the data to outBuffer
Array.Copy(_buffer, headerLength, outBuffer, 0, dataLength);
// Trim the packet from the buffer
_bufferLength -= (dataLength + headerLength);
Array.Copy(_buffer, headerLength + dataLength, _buffer, 0, _bufferLength);
if (!ValidateCrc(crc, outBuffer, 0, dataLength))
{
Debug.LogError("data crc check failed");
AdvanceToNextViablePacket();
return(0);
}
return(dataLength);
}
/// <summary>
/// Header structure:
/// ========================================
/// 1. Fixed packet prefix (2 bits)
/// 2. Channel Id (2 bits)
/// 3. Data CRC (4 bits)
/// 4. Header CRC (4 bits)
/// 5. Data Length (12 bits, trimmed ushort, max=4096)
///
/// Illustrated header structure
/// ========================================
/// BYTE 1 +-+[] FIXED PACKET PREFIX (2 bits)
/// | []
/// |
/// | [] CHANNEL ID (2 bits)
/// | []
/// |
/// | [] DATA CRC (4 bits)
/// | []
/// | []
/// +-+[]
///
/// BYTE 2 +-+[] HEADER CRC (4 bits)
/// | []
/// | []
/// | []
/// |
/// | [] DATA LENGTH (12 bits)
/// | []
/// | []
/// +-+[]
/// BYTE 3 +-+[]
/// | []
/// | []
/// | []
/// | []
/// | []
/// | []
/// +-+[]
///
/// </summary>
/// <param name="outHeader">byte[] buffer to write the header into</param>
/// <param name="channelId">Channel ID to embed into the header</param>
/// <param name="data">Data of the packet, given for calculating the data CRC</param>
/// <param name="startPosition">Start position of the data in the given Data buffer</param>
/// <param name="length">Length of the data in the given data buffer</param>
/// <returns>The length of the header in bytes</returns>
private byte CreateHeader(byte[] outHeader, byte channelId, byte[] data, int startPosition, ushort length)
{
if (length > 4096)
{
throw new Exception("Maximum data length is 4096 bytes! Got " + length);
}
var random = new System.Random();
var lengthBytes = BitConverter.GetBytes(length);
var crc = CrcUtils.Checksum(data, startPosition, length, DATA_CRC_SIZE);
var headerCrc = CrcUtils.Checksum(new byte[] { channelId, lengthBytes[0], lengthBytes[1] }, 0, 3, HEADER_CRC_SIZE);
outHeader[0] = crc;
outHeader[0] |= (byte)(channelId << 4);
outHeader[0] |= (byte)(FIXED_PACKET_PREFIX << 6);
outHeader[1] = (byte)(length >> 8 & FOUR_BITS_MASK);
outHeader[2] = (byte)(length & FULL_BYTE_MASK);
outHeader[1] |= (byte)(headerCrc << 4);
return(HEADER_LENGTH);
}
private bool ValidateCrc(byte crc, byte[] data, byte position, ushort length)
{
var dataCrc = CrcUtils.Checksum(data, position, length, DATA_CRC_SIZE);
return(dataCrc == crc);
}
