public static byte[] ToBinary(CommandPacket packet)
{
string dataText = packet.Pack();
byte[] dataBin = TextEncoding.GetBytes(dataText);
StringBuilder binPacket = new StringBuilder();
binPacket.Append(PacketBinConverter.Prefix);
binPacket.Append(dataText);
binPacket.Append(PacketBinConverter.Suffix);
binPacket.Append(Checksum.Calculate(dataBin).ToString("x2"));
return TextEncoding.GetBytes(binPacket.ToString());
}
public static ReplyPacket FromBinary(byte[] data, CommandPacket commandSent = null)
{
if (!ValidateBinary(data))
throw new FormatException("[BIN] Invalid packet");
string packet = TextEncoding.GetString(data);
string packetData = packet.Substring(1, packet.Length - 4);
packetData = Rle.Decode(packetData);
// Compare checksum
string receivedChecksum = packet.Substring(packet.Length - 2, 2);
uint calculatedChecksum = Checksum.Calculate(packetData);
if (receivedChecksum != calculatedChecksum.ToString("x2"))
throw new FormatException("[BIN] Invalid checksum");
return ReplyPacketFactory.CreateReplyPacket(packetData, commandSent);
}
public static ReplyPacket CreateReplyPacket(string data, CommandPacket commandSent = null)
{
if (data == "OK")
return new OkReply();
if (data.Length == 3 && data[0] == 'S')
return new StopSignalReply(Convert.ToInt32(data.Substring(1), 16));
if (data.Length == 3 && data[0] == 'E')
return new ErrorReply(Convert.ToInt32(data.Substring(1), 16));
if (commandSent is ReadMemoryCommand) {
try {
byte[] dataBytes = ConvertToByte(data).ToArray();
return new DataReply(dataBytes);
} catch (FormatException) { }
}
if (commandSent is ReadRegisters) {
try {
List<Register> registers = new List<Register>();
var byteList = ConvertToByte(data).ToArray();
// General registers
registers.AddRange(Enumerable.Range(0, 16)
.Select(i => new Register(
(RegisterType)i, BitConverter.ToUInt32(byteList, i * 4)))
);
// CPSR
registers.Add(new Register(
RegisterType.CPSR, BitConverter.ToUInt32(byteList, 164))
);
return new RegistersReply(registers.ToArray());
} catch (FormatException) { }
}
throw new FormatException("Unknown reply");
}
public void SendCommand(CommandPacket command)
{
this.lastCommandSent = command;
this.SendData(PacketBinConverter.ToBinary(command));
}
