private void HandleFragment(byte[] packetData, IPEndPoint from, LCMDataInputStream ins)
{
int msgSeqNumber = ins.ReadInt32();
int msgSize = ins.ReadInt32() & unchecked ((int)0xffffffff);
int fragmentOffset = ins.ReadInt32() & unchecked ((int)0xffffffff);
int fragmentId = ins.ReadInt16() & 0xffff;
int fragmentsInMsg = ins.ReadInt16() & 0xffff;
// read entire packet payload
byte[] payload = new byte[ins.Available];
ins.ReadFully(payload);
if (ins.Available > 0)
{
System.Console.Error.WriteLine("Unread data! " + ins.Available);
}
int dataStart = 0;
int fragSize = payload.Length;
FragmentBuffer fbuf;
fragBufs.TryGetValue(from.Serialize(), out fbuf);
if (fbuf != null && ((fbuf.msgSeqNumber != msgSeqNumber) || (fbuf.data_size != msgSize)))
{
fragBufs.Remove(fbuf.from);
fbuf = null;
}
if (fbuf == null && fragmentId == 0)
{
// extract channel name
int channelLen = 0;
for (; channelLen < payload.Length; channelLen++)
{
if (payload[channelLen] == 0)
{
break;
}
}
dataStart = channelLen + 1;
fragSize -= (channelLen + 1);
string tempStr;
tempStr = System.Text.Encoding.GetEncoding("US-ASCII").GetString(payload);
string channel = new string(tempStr.ToCharArray(), 0, channelLen);
fbuf = new FragmentBuffer(from.Serialize(), channel, msgSeqNumber, msgSize, fragmentsInMsg);
fragBufs.Add(fbuf.from, fbuf);
}
if (fbuf == null)
{
// TODO
return;
}
if (fragmentOffset + fragSize > fbuf.data_size)
{
System.Console.Error.WriteLine("LC: dropping invalid fragment");
fragBufs.Remove(fbuf.from);
return;
}
Array.Copy(payload, dataStart, fbuf.data, fragmentOffset, fragSize);
fbuf.fragments_remaining--;
if (0 == fbuf.fragments_remaining)
{
lcm.ReceiveMessage(fbuf.channel, fbuf.data, 0, fbuf.data_size);
fragBufs.Remove(fbuf.from);
}
}