public bool BeforeUdpMessageReceived(UdpReceiveResult result)
{
Logger.Debug("SPAMCHECK");
lock (padlock)
{
if (SpamList.ContainsKey(result.RemoteEndPoint))
{
if ((DateTime.Now.Ticks / TimeSpan.TicksPerMillisecond) - SpamList[result.RemoteEndPoint].Ticks < 100000)
{
SpamList[result.RemoteEndPoint].SpamCount += 1;
if (SpamList[result.RemoteEndPoint].SpamCount > 10)
{
return false;
}
}
}
else
{
if (SpamList.Count > 5000)
{
var expired = SpamList.Where(kvp => kvp.Value.BanTime.Equals(0)).ToList();
expired.ForEach(kvp => SpamList.Remove(kvp.Key));
}
SpamList.Add(result.RemoteEndPoint, new Spammer()
{
SpamCount = 1,
Ticks = DateTime.Now.Ticks / TimeSpan.TicksPerMillisecond
});
}
}
return true;
}
/// <summary>
/// Return concrete "Torque" message type
/// </summary>
/// <param name="result"></param>
/// <returns></returns>
public static UdpMessage ParseUdpReceiveResult(UdpReceiveResult result)
{
using (var stream = new MemoryStream(result.Buffer))
{
using (var reader = new BinaryReader(stream))
{
var packetType = reader.ReadByte();
switch ((TorqueMessageTypes)packetType)
{
case TorqueMessageTypes.GameHeartbeat:
return result.ToGameHeartbeat(reader, result.RemoteEndPoint);
case TorqueMessageTypes.GameMasterInfoResponse:
return result.ToGameInfoResponse(reader, result.RemoteEndPoint);
default:
return new UdpMessage(result.RemoteEndPoint)
{
PacketType = packetType,
Message = result.Buffer
};
}
}
}
}
public static NetworkData Create(UdpReceiveResult receiveResult)
{
return new NetworkData()
{
Buffer = receiveResult.Buffer,
Length = receiveResult.Buffer.Length,
RemoteHost = receiveResult.RemoteEndPoint.ToNode(TransportType.Udp)
};
}
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
{
while (!stoppingToken.IsCancellationRequested)
{
UdpReceiveResult result = await _udpClient.ReceiveAsync();
var message = Encoding.ASCII.GetString(result.Buffer);
Console.WriteLine($"{result.RemoteEndPoint} - {message}");
}
}
/// <summary>
/// Connects to robot and returns the first received frame
/// </summary>
/// <returns>first received frame</returns>
public async Task <InputFrame> Connect(int port)
{
client = new UdpClient(new IPEndPoint(IPAddress.Any, port));
UdpReceiveResult result = await client.ReceiveAsync();
remoteEndPoint = result.RemoteEndPoint;
byte[] receivedBytes = result.Buffer;
return(new InputFrame(Encoding.ASCII.GetString(receivedBytes, 0, receivedBytes.Length)));
}
public static async Task ProcessPacket(UdpReceiveResult packet)
{
string str = Encoding.ASCII.GetString(packet.Buffer);
JsonReader js = new JsonTextReader(new StringReader(string.Join("",buffer)));
var obj = JObject.Load(js);
buffer.Clear();
ProcessPacket(obj.ToString(), packet.RemoteEndPoint);
//await ProcessPacket(Encoding.ASCII.GetString(packet.Buffer),packet.RemoteEndPoint);
}
public async Task <Received> Receive()
{
UdpReceiveResult result = await Client.ReceiveAsync();
return(new Received
{
Message = Encoding.ASCII.GetString(result.Buffer, 0, result.Buffer.Length),
Sender = result.RemoteEndPoint
});
}
/// <summary>
/// This function processes a packet and decides what to do next
/// </summary>
/// <param name="packet">the packet to process</param>
void ProcessPacket(UdpReceiveResult res)
{
Buffer packet = Buffer.From(res.Buffer); //Buffer from is going to take the res object and give us a packet we can process.
IPAddress sender = res.RemoteEndPoint.Address;
if (packet.Length < 4)
{
return; //do nothing becuase there isn't enough info to use
}
string id = packet.ReadString(0, 4); //we read a string from location 0 and the first four bytes
switch (id)
{
case "REPL":
ProcessPacketREPL(packet);
break;
case "PAWN": // just indicates to player which object(Network ID) they control
if (packet.Length < 5)
{
return;
}
byte networkID = packet.ReadUInt8(4);
NetworkObject obj = NetworkObject.GetObjectByNetworkID(networkID);
if (obj)
{
//Pawn p = (obj as Pawn);
Pawn p = (Pawn)obj;
if (p != null)
{
p.canPlayerControl = true;
}
}
break;
case "HOST":
if (packet.Length < 7)
{
return;
}
ushort port = packet.ReadUInt16BE();
int nameLength = packet.ReadUInt8(6);
if (packet.Length < 7 + nameLength)
{
return; //do nothing
}
string name = packet.ReadString(7, nameLength);
AddToServerList(new RemoteServer(res.RemoteEndPoint, name));
break;
} //end of switch(id)
} //end of void ProcessPacket
/// <summary>
/// This function processes a packet and decides what to do next.
/// </summary>
/// <param name="packet">The packet to process</param>
private void ProcessPacket(UdpReceiveResult res)
{
Buffer packet = Buffer.From(res.Buffer);
if (packet.Length < 4)
{
return; // do nothing
}
string id = packet.ReadString(0, 4);
switch (id)
{
case "REPL":
ProcessPacketREPL(packet);
break;
case "PAWN": // extra packet to identify WHICH object (NetworkID) the player controls
if (packet.Length < 5)
{
return; // do nothing
}
byte networkID = packet.ReadUInt8(4);
NetworkObject obj = NetworkObject.GetObjectByNetworkID(networkID);
if (obj != null)
{
Pawn p = (Pawn)obj;
if (p != null)
{
p.canPlayerControl = true;
}
}
break;
case "HOST":
if (packet.Length < 7)
{
return; // do nothing
}
ushort port = packet.ReadUInt16BE(4);
int nameLength = packet.ReadUInt8(6);
if (packet.Length < 7 + nameLength)
{
return; // do nothing....
}
string name = packet.ReadString(7, nameLength);
AddToServerList(new RemoteServer(res.RemoteEndPoint, name));
break;
}
}
/// <summary>
/// Called by the MulticastClient when a DNS message is received.
/// </summary>
/// <param name="sender">
/// The <see cref="MulticastClient"/> that got the message.
/// </param>
/// <param name="result">
/// The received message <see cref="UdpReceiveResult"/>.
/// </param>
/// <remarks>
/// Decodes the <paramref name="result"/> and then raises
/// either the <see cref="QueryReceived"/> or <see cref="AnswerReceived"/> event.
/// <para>
/// Multicast DNS messages received with an OPCODE or RCODE other than zero
/// are silently ignored.
/// </para>
/// <para>
/// If the message cannot be decoded, then the <see cref="MalformedMessage"/>
/// event is raised.
/// </para>
/// </remarks>
public void OnDnsMessage(object sender, UdpReceiveResult result)
{
// If recently received, then ignore.
// A message has been recently received, if its content, as well as the remote endpoint(!) match, otherwise messages from different senders can be considered equal.
var receivedMessage = new byte[result.Buffer.Length + result.RemoteEndPoint.Address.GetAddressBytes().Length];
result.Buffer.CopyTo(receivedMessage, 0);
result.RemoteEndPoint.Address.GetAddressBytes().CopyTo(receivedMessage, result.Buffer.Length);
if (IgnoreDuplicateMessages && !receivedMessages.TryAdd(receivedMessage))
{
// Not continuing here because it looks like a duplicate message
return;
}
var msg = new Message();
try
{
msg.Read(result.Buffer, 0, result.Buffer.Length);
}
catch (Exception e)
{
log.Warn("Received malformed message", e);
MalformedMessage?.Invoke(this, result.Buffer);
return; // eat the exception
}
if (msg.Opcode != MessageOperation.Query || msg.Status != MessageStatus.NoError)
{
return;
}
// Dispatch the message.
try
{
if (msg.IsQuery && msg.Questions.Count > 0)
{
QueryReceived?.Invoke(this, new MessageEventArgs {
Message = msg, RemoteEndPoint = result.RemoteEndPoint
});
}
else if (msg.IsResponse && msg.Answers.Count > 0)
{
AnswerReceived?.Invoke(this, new MessageEventArgs {
Message = msg, RemoteEndPoint = result.RemoteEndPoint
});
}
}
catch (Exception e)
{
log.Error("Receive handler failed", e);
// eat the exception
}
}
public async Task <ClientResponse> Request(ClientRequest request)
{
IPEndPoint dns = request.Dns;
#if (!PORTABLE)
using (UdpClient udp = new UdpClient())
#else
using (Sockets.Plugin.UdpSocketClient udp = new Sockets.Plugin.UdpSocketClient())
#endif
{
#if (!PORTABLE)
var bytes = request.ToArray();
await udp
.SendAsync(bytes, request.Size, dns)
.WithCancellationTimeout(TIMEOUT);
#else
string ip = request.Dns.Address.ToString();
ip = request.Dns.Address.ToString().Replace(":", ".");
await udp.ConnectAsync(ip, request.Dns.Port);
await udp
.SendAsync(request.ToArray());
#endif
#if (!PORTABLE)
UdpReceiveResult result = await udp.ReceiveAsync().WithCancellationTimeout(TIMEOUT);
if (!result.RemoteEndPoint.ToString().Replace(".", ":").Equals(dns.ToString()))
{
throw new IOException("Remote endpoint mismatch");
}
byte[] buffer = result.Buffer;
#else
ManualResetEvent ev = new ManualResetEvent(false);
byte[] buffer = null;
udp.MessageReceived += (s, e) =>
{
buffer = e.ByteData;
ev.Set();
};
if (!ev.WaitOne(TIMEOUT))
{
throw new System.TimeoutException();
}
#endif
Response response = Response.FromArray(buffer);
if (response.Truncated)
{
return(await fallback.Request(request));
}
return(new ClientResponse(request, response, buffer));
}
}
private async void Listen()
{
try
{
while (!this._disposed)
{
UdpReceiveResult result = await this._client.ReceiveAsync().ConfigureAwait(false);
int errorCode = result.Buffer[0x22] | (result.Buffer[0x23] << 8);
if (errorCode != 0)
{
continue;
}
byte[] encrypted = new byte[result.Buffer.Length - 0x38];
Buffer.BlockCopy(result.Buffer, 0x38, encrypted, 0, encrypted.Length);
byte[] payload = await this.GetDecryptedBytes(encrypted).ConfigureAwait(false);
byte command = result.Buffer[0x26];
switch (command)
{
case 0x0a:
double value = (payload[0x06] * 10 + payload[0x07]) / 10.0;
this.TemperatureReceived?.Invoke(this, new(value));
continue;
case 0xe9:
Buffer.BlockCopy(payload, 0x00, this._id = new byte[0x04], 0x00, 0x04);
Buffer.BlockCopy(payload, 0x04, this._key = new byte[0x10], 0x00, 0x10);
this.Ready?.Invoke(this, EventArgs.Empty);
continue;
case 0xee:
case 0xef:
if (payload[0] == 0x04)
{
this.AckReceived?.Invoke(this, EventArgs.Empty);
}
else
{
byte[] data = new byte[payload.Length - 4];
Buffer.BlockCopy(payload, 6, data, 0, payload.Length - 6);
this.DataReceived?.Invoke(this, new(data));
}
continue;
}
Console.Error.WriteLine("Unhandled command type: {0}", command);
}
}
catch (SocketException)
{
}
catch (ObjectDisposedException)
{
}
}
public async Task StartAsync()
{
listener.ExclusiveAddressUse = false;
listener.DontFragment = true;
listener.Client.Bind(localEP);
await logger.LogDebugAsync($"UDP Gateway started on {localEP.Address.ToString()} and port {localEP.Port}");
while (!token.IsCancellationRequested)
{
try
{
UdpReceiveResult result = await listener.ReceiveAsync();
if (result.Buffer.Length > 0)
{
string key = CreateNamedKey($"{result.RemoteEndPoint.Address.ToString()}:{result.RemoteEndPoint.Port}");
if (cache.Contains(key))
{
Tuple <ProtocolAdapter, CancellationTokenSource> tuple = (Tuple <ProtocolAdapter, CancellationTokenSource>)cache.Get(key);
if (tuple != null && tuple.Item1 != null)
{
cache.Get(CreateNamedKey(tuple.Item1.Channel.Id)); //ensure do not expire sliding
if (tuple.Item1.Channel.State == ChannelState.Open)
{
await tuple.Item1.Channel.AddMessageAsync(result.Buffer);
}
}
}
else
{
CancellationTokenSource cts = new CancellationTokenSource();
ProtocolAdapter adapter = ProtocolAdapterFactory.Create(config, graphManager, authn, listener, result.RemoteEndPoint, logger, cts.Token);
string namedKey = CreateNamedKey(adapter.Channel.Id);
cache.Add(namedKey, key, GetCachePolicy(5.0 * 60.0));
cache.Add(key, new Tuple <ProtocolAdapter, CancellationTokenSource>(adapter, cts), GetCachePolicy(5.0 * 60.0));
adapter.OnError += Adapter_OnError;
adapter.OnClose += Adapter_OnClose;
adapter.OnObserve += Adapter_OnObserve;
await adapter.Channel.OpenAsync();
adapter.Init();
await adapter.Channel.AddMessageAsync(result.Buffer);
}
}
}
catch (Exception ex)
{
logger?.LogErrorAsync(ex, "Fault UDP listener.");
throw ex;
}
}
}
/// <summary>
/// Creates a new EasyTouchUnit if the message contains valid unit information and null otherwise.
/// </summary>
/// <param name="result">The message received from a connection</param>
/// <returns>The EasyTouchUnit if the message is valid</returns>
public static EasyTouchUnit?Create(UdpReceiveResult result)
{
EasyTouchUnit unit = new(result);
if (unit.IsValid)
{
return(unit);
}
return(null);
}
private void HandleRecv(UdpReceiveResult rUdpReceiveResult, uint nConn)
{
KChannel rKChannel;
if (!this.mIdChannels.TryGetValue(nConn, out rKChannel))
{
return;
}
// 处理chanel
rKChannel.HandleRecv(rUdpReceiveResult.Buffer, this.mTimeNow);
}
public async Task GetDataTransmissionAsync(UdpClient DataTransmissionClient)
{
StringBuilder DataTransmissionLog = new StringBuilder();
while (IsTransmitting)
{
UdpReceiveResult receiveBytes = await DataTransmissionClient.ReceiveAsync().ConfigureAwait(false);
Console.WriteLine(BitConverter.ToInt16(receiveBytes.Buffer, 2));
}
}
private async Task ListenerLoop()
{
Logger.Debug("Udp Receiving...");
UdpReceiveResult res = await socket.ReceiveAsync();
byte[] buffer = res.Buffer;
string strres = Encoding.UTF8.GetString(buffer);
Logger.Debug($"Udp Data: {strres}");
}
private async Task RecievePacket(UdpReceiveResult packet)
{
switch ((CommandCode)packet.Buffer[0])
{
case CommandCode.ConnectToParent:
await this.OnChildConnected(packet).ConfigureAwait(false);
break;
case CommandCode.ConnectToParentAck:
this.OnConnectedToParent(packet);
break;
case CommandCode.Message:
await this.MessageRecieved(packet).ConfigureAwait(false);
break;
case CommandCode.MessageAck:
byte[] guidBytes = new byte[packet.Buffer.Length - 1];
Array.Copy(packet.Buffer, 1, guidBytes, 0, guidBytes.Length);
MessageSentData _;
var messageToRemove = new Message(new Guid(guidBytes));
peers[packet.RemoteEndPoint].PendingMessagesLastSendAttempt.TryRemove(messageToRemove, out _);
peers[packet.RemoteEndPoint].MarkAlive();
CheckForTerminationOver();
break;
case CommandCode.Ping:
byte[] message = { (byte)CommandCode.Pong };
await udpclient.SendAsync(message, 1, packet.RemoteEndPoint).ConfigureAwait(false);
break;
case CommandCode.Pong:
peers[packet.RemoteEndPoint].MarkAlive();
break;
case CommandCode.Dead:
await DisconnectPeer(packet.RemoteEndPoint).ConfigureAwait(false);
break;
case CommandCode.DeadAck:
peers[packet.RemoteEndPoint].AwareOfOurDeath = true;
CheckForTerminationOver();
break;
default:
throw new ArgumentOutOfRangeException();
}
}
private void messageReceived(object sender, UdpReceiveResult receiveResult)
{
var chunk = PsnPacketChunk.FromByteArray(receiveResult.Buffer);
if (chunk == null)
{
return;
}
OnPacketReceived(chunk);
}
private void HandleRecv(UdpReceiveResult udpReceiveResult, uint conn)
{
KChannel kChannel;
if (!this.idChannels.TryGetValue(conn, out kChannel))
{
return;
}
// 处理chanel
kChannel.HandleRecv(udpReceiveResult.Buffer, this.TimeNow);
}
async void Start_Serv()
{
while (true)
{
UdpReceiveResult result = await udp.ReceiveAsync();
string str = Encoding.UTF8.GetString(result.Buffer);
handler(str);
Console.WriteLine(str);
}
}
private void SendAcknowledgementMessage(UdpReceiveResult receivedResults, byte messageType, IPEndPoint remoteEndpoint)
{
byte[] response = new byte[4]
{
receivedResults.Buffer[0],
receivedResults.Buffer[1],
receivedResults.Buffer[2],
messageType
};
_ = this.UdpSendMessageAsync(response, remoteEndpoint.Address.ToString(), remoteEndpoint.Port);
}
public static async Task <string> SendMessage(string Message)
{
byte[] datagram = Encoding.UTF8.GetBytes(Message);
await client.SendAsync(datagram, datagram.Length, serverEP);
UdpReceiveResult answerDatagram = await client.ReceiveAsync();
string text = Encoding.UTF8.GetString(answerDatagram.Buffer);
return(text);
}
private void HandlePing(UdpReceiveResult udpReceiveResult)
{
uint id = BitConverter.ToUInt32(udpReceiveResult.Buffer, 4);
KChannel kChannel;
if (!this.idChannels.TryGetValue(id, out kChannel))
{
return;
}
kChannel.HandlePing();
}
public async Task HandleUdpMessage(UdpReceiveResult receiveResult)
{
var buffer = receiveResult.Buffer;
var response = await HandleUdpMessage(receiveResult.RemoteEndPoint, buffer);
if (response.HasValue)
{
var resp = response.Value;
await _udpListener.SendAsync(resp.Array, resp.Count, receiveResult.RemoteEndPoint);
}
}
private async Task Handle(UdpReceiveResult recv, byte[] buff, byte[] validate)
{
await Task.Yield();
if (recv.Buffer != null && this.ValidateBuffer(recv.Buffer, validate))
{
var ep = recv.RemoteEndPoint;
ep = new IPEndPoint(ep.Address, this.Port - 1);
await this.Udp.SendAsync(buff, buff.Length, ep);
}
}
public Task StartAsync()
{
return(Task.Run(() =>
{
IsRunning = true;
CancellationToken cancellationToken = _cancellationTokenSource.Token;
IPEndPoint clientIP = new IPEndPoint(IPAddress.Any, 0);
byte[] clientHelloData;
Task <UdpReceiveResult> udpReceiveTask;
while (!cancellationToken.IsCancellationRequested)
{
try
{
udpReceiveTask = _udpServer.ReceiveAsync();
udpReceiveTask.Wait(cancellationToken);
UdpReceiveResult udpReceiveResult = udpReceiveTask.Result;
clientIP = udpReceiveResult.RemoteEndPoint;
clientHelloData = udpReceiveResult.Buffer;
// Проверить длину пакета (10 символов маркера + 6 байтов MAC)
if (clientHelloData.Length != SERVER_TOKEN.Length + MAC_LENGTH)
{
byte[] tokenReceived = new byte[SERVER_TOKEN.Length];
Array.Copy(clientHelloData, tokenReceived, tokenReceived.Length);
if (!SERVER_TOKEN.SequenceEqual(tokenReceived))
{
throw new Exception(@"Широковещательное сообщение не содержит ASCII ""MultiShare""");
}
}
byte[] macData = new byte[MAC_LENGTH];
Buffer.BlockCopy(clientHelloData, 10, macData, 0, MAC_LENGTH);
PhysicalAddress mac = new PhysicalAddress(macData);
Device device = new Device(clientIP.Address, mac);
if (_connectedDevice != device)
{
OnNewClient(device);
}
}
catch (OperationCanceledException)
{
throw new Exception("Операция отменена");
}
}
IsRunning = false;
}));
}
public void HandleRecv(UdpReceiveResult urr)
{
if (kService != null)
{
if (remoteEndPoint.Port != urr.RemoteEndPoint.Port || remoteEndPoint.Address.ToString() != urr.RemoteEndPoint.Address.ToString())//here is in case wifi toorfrom 4g
{
remoteEndPoint = urr.RemoteEndPoint;
}
}
this.kcp.Input(urr.Buffer);
}
private Task ListenAsync()
{
var task = new Task(async() =>
{
UdpClient listener = new UdpClient(DATA_PORT);
while (true)
{
UdpReceiveResult result = await listener.ReceiveAsync();
int sensorId = BitConverter.ToInt32(result.Buffer, 0);
var accel = new Vector3D();
var gyro = new Vector3D();
var quat = new Quaternion();
int i = 1;
quat.W = BitConverter.ToInt32(result.Buffer, i++ *sizeof(int));
quat.X = BitConverter.ToInt32(result.Buffer, i++ *sizeof(int));
quat.Y = BitConverter.ToInt32(result.Buffer, i++ *sizeof(int));
quat.Z = BitConverter.ToInt32(result.Buffer, i++ *sizeof(int));
accel.X = BitConverter.ToInt16(result.Buffer, i++ *sizeof(int));
accel.Y = BitConverter.ToInt16(result.Buffer, i++ *sizeof(int));
accel.Z = BitConverter.ToInt16(result.Buffer, i++ *sizeof(int));
gyro.X = BitConverter.ToInt16(result.Buffer, i++ *sizeof(int));
gyro.Y = BitConverter.ToInt16(result.Buffer, i++ *sizeof(int));
gyro.Z = BitConverter.ToInt16(result.Buffer, i++ *sizeof(int));
uint timestamp = BitConverter.ToUInt32(result.Buffer, i++ *sizeof(int));
accel = accel / 8192;
gyro = gyro / 16.4;
quat.Normalize();
var value = new SensorValue(quat, accel, gyro, DateTime.Now, timestamp);
var sourceAddr = result.RemoteEndPoint.Address;
// TODO: check performance. this creates a closure on every iteration.
var sensor = Sensors.GetOrAdd(sensorId, (id) =>
{
var newSensor = new Sensor(sourceAddr, id);
// raises the sensor added event on the main thread
startedDispatcher.BeginInvoke(SensorAdded, newSensor);
return(newSensor);
});
sensor.PushValue(value);
}
}, TaskCreationOptions.LongRunning);
task.Start();
return(task);
}
public static async void receiver(UdpClient client)
{
while (true)
{
// afvent meddelelser
UdpReceiveResult result = await client.ReceiveAsync();
// aflæs bytes, oversæt og udskriv beskeden
Console.WriteLine("Received " + Encoding.UTF8.GetString(result.Buffer));
}
}
private static UInt32 modifyBuffer(ref UdpReceiveResult data)
{
//Span<UInt32> field = MemoryMarshal.Cast<byte, UInt32>(new Span<byte>(data.Buffer));
Span<UInt32> field = MemoryMarshal.Cast<byte, UInt32>(data.Buffer);
++field[1];
UInt32 nextIPIdx = (field[2] + 1) % field[3];
field[2] = nextIPIdx;
return field[4 + (Int32)nextIPIdx];
}
//Her modtager den min besked fra tidligere og sender tilbage at den er modtaget
public static async void recieve()
{
IPEndPoint endPoint = new IPEndPoint(IPAddress.Parse("127.0.0.1"), 5000);
UdpClient client = new UdpClient(endPoint);
UdpReceiveResult result = await client.ReceiveAsync();
byte[] buffer = result.Buffer;
string text = Encoding.UTF8.GetString(buffer);
Console.WriteLine("Recieved: " + text);
}
public virtual UdpMessage ParseUdpReceiveResult(UdpReceiveResult result)
{
var message = result.ToUdpMessage();
if (Logger.IsDebugEnabled)
{
Logger.Debug(string.Format("RECEIVE {0}, PacketType {1}, Size {2}", message.RemoteEndPoint, message.PacketType, message.Message.Count()));
}
return(message);
}
public void OnReceive(UdpReceiveResult result)
{
receives.Insert(0, result);
if (receives.Count >= 21)
{
receives.RemoveAt(20);
}
count++;
}
private static bool ProcessReceiveResults(UdpReceiveResult result, Dictionary<string, string> discoveredThings)
{
var response = Encoding.UTF8.GetString(result.Buffer);
DiscoverCommandResponse parsedResponse;
if (!DiscoverCommandResponse.TryParse(response, out parsedResponse))
{
return false;
}
if (!discoveredThings.ContainsKey(parsedResponse.Mac))
{
discoveredThings.Add(parsedResponse.Mac, parsedResponse.IP);
}
return false;
}
private static bool TryParseDiscoverResponse(UdpReceiveResult result, out OrviboS20DiscoverCommandResponse discoverCommandResponse)
{
OrviboS20CommandResponse parsedResponse;
if (!OrviboS20CommandResponseFactory.TryParse(result, out parsedResponse))
{
discoverCommandResponse = null;
return false;
}
discoverCommandResponse = parsedResponse as OrviboS20DiscoverCommandResponse;
if (discoverCommandResponse == null)
{
return false;
}
return true;
}
private static Task AddReceivedToDicAndSendIfShouldAsync(UdpClient server, UdpReceiveResult r,
IDictionary<IPEndPoint, List<byte>> received)
{
var clientEndPoint = r.RemoteEndPoint;
var buffer = r.Buffer;
if (received.ContainsKey(clientEndPoint))
{
received[clientEndPoint].AddRange(buffer);
}
else
{
received.Add(clientEndPoint, buffer.ToList());
}
var shouldSend = !IsReadingShouldContinue(received[clientEndPoint]);
return shouldSend
? SendIfShouldAsync(server, clientEndPoint)
: Task.CompletedTask;
}
internal async void RunMessageReceiver(CancellationToken cancellationToken)
{
while (!cancellationToken.IsCancellationRequested)
{
var didReceive = false;
var msg = new UdpReceiveResult();
try
{
// attempt to read next datagram
msg = await _backingUdpClient
.ReceiveAsync()
.WrapNativeSocketExceptions();
didReceive = true;
}
catch
{
// exception may occur because we stopped listening
// (i.e. cancelled the token) - if so exit loop
// otherwise throw.
if (!cancellationToken.IsCancellationRequested)
throw;
}
if (!didReceive)
return; // cancelled, exit loop;
// generate the message received event
var remoteAddress = msg.RemoteEndPoint.Address.ToString();
var remotePort = msg.RemoteEndPoint.Port.ToString();
var data = msg.Buffer;
var wrapperArgs = new UdpSocketMessageReceivedEventArgs(remoteAddress, remotePort, data);
// fire
if (MessageReceived != null)
MessageReceived(this, wrapperArgs);
}
}
/// <summary>
/// Called when [message received].
/// </summary>
/// <param name="message">The message.</param>
private void OnMessageReceived(UdpReceiveResult message)
{
if (message.RemoteEndPoint.Port == 0)
{
return;
}
var bytes = message.Buffer;
OnMessageReceived(new UdpMessageReceivedEventArgs
{
Bytes = bytes,
RemoteEndPoint = message.RemoteEndPoint.ToString()
});
}
private void ProcessUdpMessage(UdpReceiveResult msg)
{
if (msg.Buffer.Length > 0 && msg.RemoteEndPoint.Equals(_endpoint))
{
byte[] buffer = msg.Buffer;
int length = msg.Buffer.Length;
if (!_isReady)
{
_isReady = true;
if (length != 70)
{
if (_isDebug)
RaiseOnDebugMessage(DebugMessageType.VoiceInput, $"Unexpected message length. Expected >= 70, got {length}.");
return;
}
int port = buffer[68] | buffer[69] << 8;
_myIp = Encoding.ASCII.GetString(buffer, 4, 70 - 6).TrimEnd('\0');
_isReady = true;
var login2 = new VoiceWebSocketCommands.Login2();
login2.Payload.Protocol = "udp";
login2.Payload.SocketData.Address = _myIp;
login2.Payload.SocketData.Mode = _mode;
login2.Payload.SocketData.Port = port;
QueueMessage(login2);
}
else
{
//Parse RTP Data
if (length < 12)
{
if (_isDebug)
RaiseOnDebugMessage(DebugMessageType.VoiceInput, $"Unexpected message length. Expected >= 12, got {length}.");
return;
}
byte flags = buffer[0];
if (flags != 0x80)
{
if (_isDebug)
RaiseOnDebugMessage(DebugMessageType.VoiceInput, $"Unexpected Flags: {flags}");
return;
}
byte payloadType = buffer[1];
if (payloadType != 0x78)
{
if (_isDebug)
RaiseOnDebugMessage(DebugMessageType.VoiceInput, $"Unexpected Payload Type: {flags}");
return;
}
ushort sequenceNumber = (ushort)((buffer[2] << 8) |
buffer[3] << 0);
uint timestamp = (uint)((buffer[4] << 24) |
(buffer[5] << 16) |
(buffer[6] << 8) |
(buffer[7] << 0));
uint ssrc = (uint)((buffer[8] << 24) |
(buffer[9] << 16) |
(buffer[10] << 8) |
(buffer[11] << 0));
//Decrypt
/*if (_mode == "xsalsa20_poly1305")
{
if (length < 36) //12 + 24
throw new Exception($"Unexpected message length. Expected >= 36, got {length}.");
#if !DNXCORE50
byte[] nonce = new byte[24]; //16 bytes static, 8 bytes incrementing?
Buffer.BlockCopy(buffer, 12, nonce, 0, 24);
byte[] cipherText = new byte[buffer.Length - 36];
Buffer.BlockCopy(buffer, 36, cipherText, 0, cipherText.Length);
Sodium.SecretBox.Open(cipherText, nonce, _secretKey);
#endif
}
else //Plain
{
byte[] newBuffer = new byte[buffer.Length - 12];
Buffer.BlockCopy(buffer, 12, newBuffer, 0, newBuffer.Length);
buffer = newBuffer;
}*/
if (_isDebug)
RaiseOnDebugMessage(DebugMessageType.VoiceInput, $"Received {buffer.Length - 12} bytes.");
//TODO: Use Voice Data
}
}
}
public ResponseResult(UdpReceiveResult result)
{
this.result = result;
Shift = 0;
}
private async void ProcessMessageAsync(UdpReceiveResult udpMessage)
{
await Task.Run(async () =>
{
try
{
DnsMessage message;
DnsQuestion question;
var respondedFromCache = false;
try
{
message = DnsMessage.Parse(udpMessage.Buffer);
question = message.Questions[0];
}
catch (Exception)
{
throw new ParsingException();
}
// Check for authorized subnet access
if (_networkWhitelist != null)
{
if (_networkWhitelist.All(pair =>
!pair.Key.GetNetworkAddress(pair.Value)
.Equals(udpMessage.RemoteEndPoint.Address.GetNetworkAddress(pair.Value))))
{
Logger.Info("-> {0} is not authorized, who requested {1}.",
udpMessage.RemoteEndPoint.Address,
question);
message.ReturnCode = ReturnCode.Refused;
message.IsQuery = false;
}
}
Logger.Info("-> {0} requested {1} (#{2}, {3}).", udpMessage.RemoteEndPoint.Address, question.Name,
message.TransactionID, question.RecordType);
// Query cache
if (Options.CacheResponse)
{
if (Cache.ContainsKey(question.Name) && Cache[question.Name].ContainsKey(question.RecordType))
{
var entry = Cache[question.Name][question.RecordType];
if (!entry.IsExpired)
{
var cachedMessage = entry.Message;
Logger.Info("-> #{0} served from cache.", message.TransactionID,
cachedMessage.TransactionID);
cachedMessage.TransactionID = message.TransactionID; // Update transaction ID
cachedMessage.TSigOptions = message.TSigOptions; // Update TSig options
message = cachedMessage;
respondedFromCache = true;
}
}
}
var targetNameServer = Options.DefaultNameServer;
var useHttpQuery = Options.UseHttpQuery;
var queryTimeout = Options.QueryTimeout;
var useCompressionMutation = Options.CompressionMutation;
// Match rules
if (message.IsQuery &&
(question.RecordType == RecordType.A || question.RecordType == RecordType.Aaaa))
{
for (var i = Rules.Count - 1; i >= 0; i--)
{
var match = Regex.Match(question.Name, Rules[i].Pattern);
if (!match.Success) continue;
// Domain name matched
var recordType = question.RecordType;
if (Rules[i].ForceAAAA != null && Rules[i].ForceAAAA.Value) // RecordType override
recordType = RecordType.Aaaa;
if (Rules[i].NameServer != null) // Name server override
targetNameServer = Rules[i].NameServer;
if (Rules[i].UseHttpQuery != null) // HTTP query override
useHttpQuery = Rules[i].UseHttpQuery.Value;
if (Rules[i].QueryTimeout != null) // Query timeout override
queryTimeout = Rules[i].QueryTimeout.Value;
if (Rules[i].CompressionMutation != null) // Compression pointer mutation override
useCompressionMutation = Rules[i].CompressionMutation.Value;
if (Rules[i].Address != null)
{
IPAddress ip;
IPAddress.TryParse(Rules[i].Address, out ip);
if (ip == null) // Invalid IP, may be a domain name
{
var address = string.Format(Rules[i].Address, match.Groups.Cast<object>().ToArray());
if (recordType == RecordType.A && useHttpQuery)
{
await ResolveWithHttp(targetNameServer, address, queryTimeout, message);
}
else
{
var serverEndpoint = Utils.CreateIpEndPoint(targetNameServer, 53);
var dnsClient = new DnsClient(serverEndpoint.Address, queryTimeout,
serverEndpoint.Port);
var response =
await
Task<DnsMessage>.Factory.FromAsync(dnsClient.BeginResolve,
dnsClient.EndResolve,
address, recordType, question.RecordClass, null);
if (response == null)
{
Logger.Warning($"Remote resolve failed for {address}.");
return;
}
foreach (var answerRecord in response.AnswerRecords)
{
answerRecord.Name = question.Name;
message.AnswerRecords.Add(answerRecord);
}
message.ReturnCode = response.ReturnCode;
message.IsQuery = false;
}
}
else
{
if (recordType == RecordType.A &&
ip.AddressFamily == AddressFamily.InterNetwork)
message.AnswerRecords.Add(new ARecord(question.Name, 600, ip));
else if (recordType == RecordType.Aaaa &&
ip.AddressFamily == AddressFamily.InterNetworkV6)
message.AnswerRecords.Add(new AaaaRecord(question.Name, 600, ip));
else // Type mismatch
continue;
message.ReturnCode = ReturnCode.NoError;
message.IsQuery = false;
}
}
break;
}
}
// TODO: Consider how to integrate System.Net.Dns with this project.
// Using System.Net.Dns to forward query if compression mutation is disabled
//if (message.IsQuery && !useCompressionMutation &&
// (question.RecordType == RecordType.A || question.RecordType == RecordType.Aaaa))
//{
// var dnsResponse = await Dns.GetHostAddressesAsync(question.Name);
// if (question.RecordType == RecordType.A)
// {
// message.AnswerRecords.AddRange(dnsResponse.Where(
// ip => ip.AddressFamily == AddressFamily.InterNetwork).Select(
// ip => new ARecord(question.Name, 0, ip)));
// else if (question.RecordType == RecordType.Aaaa)
// {
// message.AnswerRecords.AddRange(dnsResponse.Where(
// ip => ip.AddressFamily == AddressFamily.InterNetworkV6).Select(
// ip => new AaaaRecord(question.Name, 0, ip)));
// }
// message.ReturnCode = ReturnCode.NoError;
// message.IsQuery = false;
//}
if (message.IsQuery && question.RecordType == RecordType.A && useHttpQuery)
{
await ResolveWithHttp(targetNameServer, question.Name, queryTimeout, message);
}
if (message.IsQuery)
{
// Use internal forwarder to forward query to another name server
await ForwardMessage(message, udpMessage, Utils.CreateIpEndPoint(targetNameServer, 53),
queryTimeout, useCompressionMutation);
}
else
{
// Already answered, directly return to the client
byte[] responseBuffer;
message.Encode(false, out responseBuffer);
if (responseBuffer != null)
{
await
_udpListener.SendAsync(responseBuffer, responseBuffer.Length, udpMessage.RemoteEndPoint);
// Update cache
if (Options.CacheResponse && !respondedFromCache)
Cache.Update(question, message, Options.CacheAge);
}
}
}
catch (ParsingException)
{
}
catch (SocketException e)
{
Logger.Error("[Listener.Send] Unexpected socket error:\n{0}", e);
}
catch (Exception e)
{
Logger.Error("[Processor] Unexpected exception:\n{0}", e);
}
});
}
public override UdpMessage ParseUdpReceiveResult(UdpReceiveResult result)
{
return TorqueMessageFactory.ParseUdpReceiveResult(result);
}
public void AfterUdpMessageReceived(UdpReceiveResult result)
{
}
private ITransaction<CanFrame> CreateCanTransaction(UdpReceiveResult Packet)
{
CanFrame frame = CanFrame.NewWithId(BitConverter.ToInt32(Packet.Buffer, 0),
Packet.Buffer, 4, Packet.Buffer.Length - 4);
return new InstantaneousTransaction<CanFrame>(frame);
}
public bool BeforeUdpMessageReceived(UdpReceiveResult result)
{
if (SessionHandler.HasSession(result.RemoteEndPoint) == false)
{
SessionHandler.CreateSession(result.RemoteEndPoint);
}
return true;
}
public void UpdateFromDatagram(UdpReceiveResult receiveResult)
{
var buffer = receiveResult.Buffer;
var byteIndex = 0;
// Note: This version number must match the version that this decoder expects to see
//Debug.WriteLine("{0:x} {1:x} {2:x} {3:x}", buffer[0], buffer[1], buffer[2], buffer[3]);
var telemetryVersionNumber = BitConverter.ToInt32(buffer, byteIndex);
if (telemetryVersionNumber != TelemetryVersionNumber) throw new TelemetryVersionMismatchException("Telemetry version mismatch. Expected: " + TelemetryVersionNumber + ", got " + telemetryVersionNumber);
byteIndex += sizeof (Int32);
_dispatcher.Invoke(() =>
{
// Telemetry data starts here
FrontLeftWheelCommandedSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
FrontLeftWheelActualSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
FrontLeftWheelSteeringSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
FrontLeftWheelActualAngle = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
FrontRightWheelCommandedSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
FrontRightWheelActualSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
FrontRightWheelSteeringSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
FrontRightWheelActualAngle = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
RearWheelCommandedSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
RearWheelActualSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
RearWheelSteeringSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
RearWheelActualAngle = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
ShoulderJointSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
ShoulderJointAngle = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
ElbowJointSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
ElbowJointAngle = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
WristJointSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
WristJointAngle = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
GrabberCommandedSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof (double);
GrabberTickCount = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof (double);
LiftCommandedSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof (double);
LiftActualHeight = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof (double);
LeftRangeSensorValue = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
RightRangeSensorValue = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
LeftGetterCommandedSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
RightGetterCommandedSpeed = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
NavxPitch = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
NavxRoll = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
NavxYaw = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
NavxXAccel = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
NavxYAccel = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
NavxZAccel = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
NavxQuaternionW = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
NavxQuaternionX = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
NavxQuaternionY = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
NavxQuaternionZ = BitConverter.ToDouble(buffer, byteIndex);
byteIndex += sizeof(double);
ElbowWristAngleConstraintViolation = buffer[byteIndex] != 0;
ForearmLiftConstraintViolation = buffer[byteIndex] != 0;
GrabberCeilingConstraintViolation = buffer[byteIndex] != 0;
GrabberGroundConstraintViolation = buffer[byteIndex] != 0;
GrabberLiftConstraintViolation = buffer[byteIndex] != 0;
ShoulderAngleConstraintViolation = buffer[byteIndex] != 0;
ShoulderElbowAngleConstraintViolation = buffer[byteIndex] != 0;
ShoulderWristAngleConstraintViolation = buffer[byteIndex] != 0;
WristCeilingConstraintViolation = buffer[byteIndex] != 0;
WristGroundConstraintViolation = buffer[byteIndex] != 0;
LastUpdate = DateTime.Now;
Status = "Receiving";
//Debug.WriteLine("Telemetry receiving");
});
}
private async Task ForwardMessage(DnsMessage message, UdpReceiveResult originalUdpMessage,
IPEndPoint targetNameServer, int queryTimeout,
bool useCompressionMutation)
{
DnsQuestion question = null;
if (message.Questions.Count > 0)
question = message.Questions[0];
byte[] responseBuffer = null;
try
{
if ((Equals(targetNameServer.Address, IPAddress.Loopback) ||
Equals(targetNameServer.Address, IPAddress.IPv6Loopback)) &&
targetNameServer.Port == ((IPEndPoint) _udpListener.Client.LocalEndPoint).Port)
throw new InfiniteForwardingException(question);
byte[] sendBuffer;
if (useCompressionMutation)
message.Encode(false, out sendBuffer, true);
else
sendBuffer = originalUdpMessage.Buffer;
_transactionClients[message.TransactionID] = originalUdpMessage.RemoteEndPoint;
// Send to Forwarder
await _udpForwarder.SendAsync(sendBuffer, sendBuffer.Length, targetNameServer);
if (_transactionTimeoutCancellationTokenSources.ContainsKey(message.TransactionID))
_transactionTimeoutCancellationTokenSources[message.TransactionID].Cancel();
var cancellationTokenSource = new CancellationTokenSource();
_transactionTimeoutCancellationTokenSources[message.TransactionID] = cancellationTokenSource;
// Timeout task to cancel the request
try
{
await Task.Delay(queryTimeout, cancellationTokenSource.Token);
if (!_transactionClients.ContainsKey(message.TransactionID)) return;
IPEndPoint ignoreEndPoint;
CancellationTokenSource ignoreTokenSource;
_transactionClients.TryRemove(message.TransactionID, out ignoreEndPoint);
_transactionTimeoutCancellationTokenSources.TryRemove(message.TransactionID,
out ignoreTokenSource);
var warningText = message.Questions.Count > 0
? $"{message.Questions[0].Name} (Type {message.Questions[0].RecordType})"
: $"Transaction #{message.TransactionID}";
Logger.Warning("Query timeout for: {0}", warningText);
}
catch (TaskCanceledException)
{
}
}
catch (InfiniteForwardingException e)
{
Logger.Warning("[Forwarder.Send] Infinite forwarding detected for: {0} (Type {1})", e.Question.Name,
e.Question.RecordType);
Utils.ReturnDnsMessageServerFailure(message, out responseBuffer);
}
catch (SocketException e)
{
if (e.SocketErrorCode == SocketError.ConnectionReset) // Target name server port unreachable
Logger.Warning("[Forwarder.Send] Name server port unreachable: {0}", targetNameServer);
else
Logger.Error("[Forwarder.Send] Unhandled socket error: {0}", e.Message);
Utils.ReturnDnsMessageServerFailure(message, out responseBuffer);
}
catch (Exception e)
{
Logger.Error("[Forwarder] Unexpected exception:\n{0}", e);
Utils.ReturnDnsMessageServerFailure(message, out responseBuffer);
}
// If we got some errors
if (responseBuffer != null)
await _udpListener.SendAsync(responseBuffer, responseBuffer.Length, originalUdpMessage.RemoteEndPoint);
}
/// <summary>
/// Called when [message received].
/// </summary>
/// <param name="message">The message.</param>
private void OnMessageReceived(UdpReceiveResult message)
{
if (message.RemoteEndPoint.Port == 0)
{
return;
}
var bytes = message.Buffer;
try
{
OnMessageReceived(new UdpMessageReceivedEventArgs
{
Bytes = bytes,
RemoteEndPoint = message.RemoteEndPoint.ToString()
});
}
catch (Exception ex)
{
_logger.ErrorException("Error handling UDP message", ex);
}
}
protected virtual void ProcessReceiveResult(UdpReceiveResult udpReceiveResult)
{
UdpMessage message = null;
try
{
//If all "before" callbacks return true we can proceed; logging, security etc
if (MessageLifecycleCallbacks.All(handler => handler.BeforeUdpMessageReceived(udpReceiveResult)))
{
message = MessageHandler.ParseUdpReceiveResult(udpReceiveResult);
MessageHandler.ProcessUdpMessage(message);
MessageLifecycleCallbacks.ForEach(
handler => handler.AfterUdpMessageReceived(udpReceiveResult));
}
}
catch (Exception e)
{
MessageHandler.OnUdpMessageReceivedError(udpReceiveResult, message, e);
}
}
