private static unsafe bool HandlePacket(byte *offset, byte *end, out int increment)
{
Packet.Header *header = (Packet.Header *)offset;
increment = -1;
UInt16 checksum = CRC16.Generate(offset + Marshal.SizeOf(header->HeaderChecksum), Marshal.SizeOf(*header) - Marshal.SizeOf(header->HeaderChecksum));
DebugTools.Print($"Header: HeaderChecksum={header->HeaderChecksum.ToString("X4")}, DataChecksum={header->DataChecksum.ToString("X4")}, Type={header->Type}, Size={header->Size} (Checksum={checksum.ToString("X4")}, Hex={ToHexString(offset, Marshal.SizeOf(*header))})");
if (!Packet.VerifyHeader(header))
{
DebugTools.Print($"Header checksum failed: {header->HeaderChecksum.ToString("X4")} / {checksum.ToString("X4")}");
return(false);
}
if (offset + (Marshal.SizeOf(*header) + header->Size) > end)
{
increment = 0;
return(false);
}
checksum = CRC16.Generate(offset + Marshal.SizeOf(*header), header->Size);
DebugTools.Print($"Data: DataChecksum={header->DataChecksum.ToString("X4")}, Size={header->Size} (Checksum={checksum.ToString("X4")}, Hex={ToHexString(offset + Marshal.SizeOf(*header), header->Size)})");
if (!Packet.VerifyData(header))
{
DebugTools.Print($"Content checksum failed: {header->DataChecksum.ToString("X4")} / {checksum.ToString("X4")}");
return(false);
}
increment = Marshal.SizeOf(*header) + header->Size;
OnPacketReceivedEvent?.Invoke(header);
return(true);
}
private unsafe void PacketReceived(Packet.Header *header)
{
switch (header->Type)
{
case (byte)CustomPackets.Type.OrientationData:
{
Orientation = *((CustomPackets.OrientationDataPacket *)header);
DebugTools.Print($"DATA: {Orientation}");
break;
}
case (byte)CustomPackets.Type.ProximityData:
{
ProximityData data = *((CustomPackets.ProximityDataPacket *)header);
DebugTools.Print($"DATA: {data}");
if (data.ID < 0 || data.ID >= ProximityContent.Length)
{
DebugTools.Print($"Invalid sensor index ID: {data.ID}");
return;
}
int count = ProximityContent[data.ID].Count;
int cap = m_ContentCapacity - 1;
for (; count-- > cap;)
{
ProximityContent[data.ID].RemoveAt(0);
}
ProximityContent[data.ID].Add(data);
break;
}
default:
{
DebugTools.Print($"Unknown packet type: {header->Type}");
break;
}
}
}
private static void Poll()
{
int bufferSize = s_ReadBuffer.Length - 1;
while (ThreadActive)
{
try
{
if (SerialPort.BytesToRead <= 0 || SerialPort.BytesToRead < Settings.ReceivedBytesThreshold /*SerialPort.ReceivedBytesThreshold*/)
{
continue;
}
if (s_ReadOffset >= bufferSize)
{
// Should/could only happen if packet received claims it has an abnormally large data size. This could happen if:
// * Intentional buffer overflow attack / client uncautiously sending too large data
// * The buffer on the server side is smaller than the packet that is currently receiving
DebugTools.Print($"Buffer overflow: offset={s_ReadOffset}, max={bufferSize}");
s_ReadOffset = 0; // Ignore this rubbish
}
int readSize = SerialPort.Read(s_ReadBuffer, s_ReadOffset, bufferSize - s_ReadOffset);
readSize += s_ReadOffset;
s_ReadOffset = 0;
DebugTools.Print("BUFFER BEGIN");
int indexOffset = 0;
int incrementSize = 0;
while (indexOffset + Marshal.SizeOf <Packet.Header>() <= readSize)
{
unsafe
{
fixed(byte *offset = &s_ReadBuffer[indexOffset], end = &s_ReadBuffer[readSize])
{
if (!HandlePacket(offset, end, out incrementSize))
{
break;
}
}
}
indexOffset += incrementSize;
}
if (incrementSize < 0)
{
DebugTools.Print("BUFFER ERROR\n");
PacketFailCount++;
continue;
}
else
{
PacketSuccessCount++;
}
s_ReadOffset = readSize - indexOffset;
for (int i = 0; i < s_ReadOffset; i++)
{
s_ReadBuffer[i] = s_ReadBuffer[indexOffset + i];
}
DebugTools.Print("BUFFER END");
}
catch (TimeoutException) { }
Thread.Sleep(UpdateInterval);
}
}
