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); } }