public void QueueSend(UInt32 payloadID, Byte[] datagram, UInt32 length) { if (queue.Length <= queueCount) { CdpBufferPoolDatagram[] newQueue = new CdpBufferPoolDatagram[queue.Length + extendLength]; Array.Copy(queue, newQueue, queue.Length); queue = newQueue; } if (queueCount <= 0) { queue[0].datagram = datagram; queue[0].length = length; queueCount = 1; firstPayloadIDInQueue = payloadID; } else { if (payloadID != firstPayloadIDInQueue + queueCount) { throw new InvalidOperationException( String.Format("You queued datagram with payload ID '{0}' but the queue has {1} datagram(s) and the first payload id in the queue is {2}", payloadID, queueCount, firstPayloadIDInQueue)); } queue[queueCount].datagram = datagram; queue[queueCount].length = length; queueCount++; } }
public void ControllerSendPayloadWithAck(UInt32 offsetLimit, ICdpTimeout timeout) { // 1. Check for acks/resends/halts Byte[] bufferToSend = GetRequestedBuffer(offsetLimit); try { // Cdp Header UInt32 payloadID = nextPayloadID++; bufferToSend[0] = (Byte)(((Byte)CdpFlagValue.PayloadWithAck << 4) | (0x0F & (payloadID >> 8))); bufferToSend[1] = (Byte)payloadID; // // Send the datagram // connectedDatagramTransmitter.Send(bufferToSend, 0, offsetLimit); Int64 stopwatchTicksAfterSend = Stopwatch.GetTimestamp(); Int32 timeoutMillis = timeout.WaitForAckInitialRetryTimeout(averageLatency); if (timeoutMillis < 0) { throw new InvalidOperationException(String.Format( "The ICdpTimeout class '{0}' returned negative ({1}) when calling WaitForAckInitialTimeout({2})", timeout.GetType().Name, timeoutMillis, averageLatency)); } Int32 retries = 0; // Keep resending the datagram until a header is recevied or timeout is reached while (true) { Console.WriteLine("Send Retry {0}", retries); Int32 bytesRead = connectedDatagramTransmitter.ReceiveBlocking(headerBuffer, 0, 2, timeoutMillis); if (bytesRead < 0) { Int32 elapsedMillis = (Stopwatch.GetTimestamp() - stopwatchTicksAfterSend).StopwatchTicksAsInt32Milliseconds(); timeoutMillis = timeout.WaitForAckRetryOrTimeout(retries, averageLatency, elapsedMillis, timeoutMillis); if (timeoutMillis <= 0) { throw new TimeoutException(String.Format("Timed out waiting for ack: {0} retries {1} milliseconds elapsed", retries, elapsedMillis)); } // Retry sending the packet connectedDatagramTransmitter.Send(bufferToSend, 0, offsetLimit); retries++; continue; } // // Check the datagram // if (bytesRead == 0) { // It's just a heart beat packet } else { Byte receivedFlagValue = (Byte)(headerBuffer[0] >> 4); UInt32 receivedPayloadID = (UInt32)((0xF00 & (headerBuffer[0] << 8)) | (0xFF & headerBuffer[1])); if (receivedFlagValue == (Byte)CdpFlagValue.Ack) { if (receivedPayloadID == payloadID) { Console.WriteLine("[CdpDebug] Received ACK for payload id {0}", payloadID); break; } Console.WriteLine("[CdpDebug] Got an ack for an old payload id {0}?", receivedPayloadID); } else if (receivedFlagValue == (Byte)CdpFlagValue.Halt) { throw new CdpBadHaltException(); } else if (receivedFlagValue == (Byte)CdpFlagValue.Resend) { if (receivedPayloadID <= payloadID) { UInt32 index = datagramQueue.PayloadIDToIndex(receivedPayloadID); if (index >= 0) { if (receivedPayloadID < payloadID) { do { Console.WriteLine("[Debug] Queue Index {0} Payload ID {1}", index, receivedPayloadID + index); CdpBufferPoolDatagram datagram = datagramQueue.queue[index]; connectedDatagramTransmitter.Send(datagram.datagram, 0, datagram.length); index++; } while (receivedPayloadID + index < payloadID); } connectedDatagramTransmitter.Send(bufferToSend, 0, offsetLimit); } } } else { Console.WriteLine("Unknown flag value {0} from '{1}' (Maybe I should throw an exception? TBD)", receivedFlagValue, connectedDatagramTransmitter.RemoteEndPoint); } } // // Get next timeout // { Int32 elapsedMillis = (Stopwatch.GetTimestamp() - stopwatchTicksAfterSend).StopwatchTicksAsInt32Milliseconds(); timeoutMillis = timeout.WaitForAckRetryOrTimeout(retries, averageLatency, elapsedMillis, timeoutMillis); if (timeoutMillis <= 0) { throw new TimeoutException(String.Format("Timed out waiting for ack: {0} retries {1} milliseconds elapsed", retries, elapsedMillis)); } } } datagramQueue.EmptyAndFree(); // free the queue because everything has been acked } finally { Cdp.BufferPool.FreeBuffer(bufferToSend); } }