initial commit
This commit is contained in:
577
Infrastructure/Kwp/KW1281Connection.cs
Normal file
577
Infrastructure/Kwp/KW1281Connection.cs
Normal file
@@ -0,0 +1,577 @@
|
||||
using HC_APTBS.Infrastructure.Kwp.Packets;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace HC_APTBS.Infrastructure.Kwp
|
||||
{
|
||||
/// <summary>
|
||||
/// Manages a dialog with a VW controller using the KW1281 protocol.
|
||||
/// </summary>
|
||||
public interface IKW1281Connection
|
||||
{
|
||||
ControllerInfo ReadEcuInfo();
|
||||
|
||||
void EndCommunication();
|
||||
|
||||
List<ControllerIdent> ReadIdent();
|
||||
|
||||
List<byte> ReadEeprom(ushort address, byte count);
|
||||
|
||||
List<byte> ReadRomEeprom(ushort address, byte count);
|
||||
|
||||
void CustomReset();
|
||||
|
||||
void SendPacket(List<byte> packetBytes);
|
||||
|
||||
List<Packet> SendCustom(List<byte> packetCustomBytes);
|
||||
|
||||
/// <summary>
|
||||
/// Keep the dialog alive by sending an ACK and receiving a response.
|
||||
/// </summary>
|
||||
void KeepAlive();
|
||||
|
||||
List<FaultCode> ReadFaultCodes();
|
||||
|
||||
/// <summary>
|
||||
/// Clear all of the controllers fault codes.
|
||||
/// </summary>
|
||||
/// <returns>True if successful.</returns>
|
||||
bool ClearFaultCodes();
|
||||
|
||||
/// <summary>
|
||||
/// Set the controller's software coding and workshop code.
|
||||
/// </summary>
|
||||
/// <param name="controllerAddress"></param>
|
||||
/// <param name="softwareCoding"></param>
|
||||
/// <param name="workshopCode"></param>
|
||||
/// <returns>True if successful.</returns>
|
||||
bool SetSoftwareCoding(int controllerAddress, int softwareCoding, int workshopCode);
|
||||
|
||||
IKwpCommon KwpCommon { get; }
|
||||
}
|
||||
|
||||
public class KW1281Connection : IKW1281Connection
|
||||
{
|
||||
public ControllerInfo ReadEcuInfo()
|
||||
{
|
||||
var packets = ReceivePackets();
|
||||
return new ControllerInfo(packets.Where(b => !b.IsAckNak));
|
||||
}
|
||||
|
||||
public ControllerInfo ReadEcuInfoCustom(Int32 pkt_count)
|
||||
{
|
||||
var packets = new List<Packet>();
|
||||
|
||||
for(var i=0;i<pkt_count; i++)
|
||||
{
|
||||
var packet = ReceivePacket();
|
||||
packets.Add(packet); // TODO: Maybe don't add the packet if it's an Ack
|
||||
if (packet.Bytes.Count < 0x10) {
|
||||
break;
|
||||
}
|
||||
if (packet is AckPacket || packet is NakPacket)
|
||||
{
|
||||
break;
|
||||
}
|
||||
if (i == pkt_count - 1) break;
|
||||
SendAckPacket();
|
||||
}
|
||||
return new ControllerInfo(packets.Where(b => !b.IsAckNak));
|
||||
}
|
||||
|
||||
public List<ControllerIdent> ReadIdent()
|
||||
{
|
||||
var idents = new List<ControllerIdent>();
|
||||
bool moreAvailable;
|
||||
do
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine("Sending ReadIdent packet");
|
||||
|
||||
SendPacket(new List<byte> { (byte)PacketCommand.ReadIdent });
|
||||
|
||||
var packets = ReceivePackets();
|
||||
var ident = new ControllerIdent(packets.Where(b => !b.IsAckNak));
|
||||
idents.Add(ident);
|
||||
|
||||
moreAvailable = packets
|
||||
.OfType<AsciiDataPacket>()
|
||||
.Any(b => b.MoreDataAvailable);
|
||||
} while (moreAvailable);
|
||||
|
||||
return idents;
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// Reads a range of bytes from the EEPROM.
|
||||
/// </summary>
|
||||
/// <param name="address"></param>
|
||||
/// <param name="count"></param>
|
||||
/// <returns>The bytes or null if the bytes could not be read</returns>
|
||||
public List<byte> ReadEeprom(ushort address, byte count)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"Sending ReadEeprom packet (Address: ${address:X4}, Count: ${count:X2})");
|
||||
SendPacket(new List<byte>
|
||||
{
|
||||
(byte)PacketCommand.ReadEeprom,
|
||||
count,
|
||||
(byte)(address >> 8),
|
||||
(byte)(address & 0xFF)
|
||||
});
|
||||
var packets = ReceivePackets();
|
||||
|
||||
if (packets.Count == 1 && packets[0] is NakPacket)
|
||||
{
|
||||
// Permissions issue
|
||||
return null;
|
||||
}
|
||||
|
||||
packets = packets.Where(b => !b.IsAckNak).ToList();
|
||||
if (packets.Count != 1)
|
||||
{
|
||||
throw new InvalidOperationException($"ReadEeprom returned {packets.Count} blocks instead of 1");
|
||||
}
|
||||
return packets[0].Body.ToList();
|
||||
}
|
||||
|
||||
public List<byte> ReadRomEeprom(ushort address, byte count)
|
||||
{
|
||||
//System.Diagnostics.Debug.WriteLine($"Sending ReadRomEeprom packet (Address: ${address:X4}, Count: ${count:X2})");
|
||||
SendPacket(new List<byte>
|
||||
{
|
||||
(byte)PacketCommand.ReadRomEeprom,
|
||||
count,
|
||||
(byte)(address >> 8),
|
||||
(byte)(address & 0xFF)
|
||||
});
|
||||
var packets = ReceivePackets();
|
||||
|
||||
if (packets.Count == 1 && packets[0] is NakPacket)
|
||||
{
|
||||
return new List<byte>();
|
||||
}
|
||||
|
||||
packets = packets.Where(b => !b.IsAckNak).ToList();
|
||||
if (packets.Count != 1)
|
||||
{
|
||||
throw new InvalidOperationException($"ReadRomEeprom returned {packets.Count} blocks instead of 1");
|
||||
}
|
||||
return packets[0].Body.ToList();
|
||||
}
|
||||
|
||||
|
||||
public Dictionary<int, Packet> CustomReadSoftwareVersion()
|
||||
{
|
||||
var versionPackets = new Dictionary<int, Packet>();
|
||||
|
||||
System.Diagnostics.Debug.WriteLine("Sending Custom \"Read Software Version\" packets");
|
||||
|
||||
// The cluster can return 4 variations of software version, specified by the 2nd byte
|
||||
// of the packet:
|
||||
// 0x00 - Cluster software version
|
||||
// 0x01 - Unknown
|
||||
// 0x02 - Unknown
|
||||
// 0x03 - Unknown
|
||||
for (byte variation = 0x00; variation < 0x04; variation++)
|
||||
{
|
||||
var packets = SendCustom(new List<byte> { 0x84, variation });
|
||||
foreach (var packet in packets.Where(b => !b.IsAckNak))
|
||||
{
|
||||
if (variation == 0x00 || variation == 0x03)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"{variation:X2}: {DumpMixedContent(packet)}");
|
||||
}
|
||||
else
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"{variation:X2}: {DumpBinaryContent(packet)}");
|
||||
}
|
||||
versionPackets[variation] = packet;
|
||||
}
|
||||
}
|
||||
|
||||
return versionPackets;
|
||||
}
|
||||
|
||||
private static string DumpMixedContent(Packet packet)
|
||||
{
|
||||
if (packet.IsNak)
|
||||
{
|
||||
return "NAK";
|
||||
}
|
||||
|
||||
return DumpMixedContent(packet.Body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Todo: Move to utility class
|
||||
/// </summary>
|
||||
public static string DumpMixedContent(IEnumerable<byte> content)
|
||||
{
|
||||
char mode = '?';
|
||||
var sb = new StringBuilder();
|
||||
foreach (var b in content)
|
||||
{
|
||||
if (b >= 32 && b <= 126)
|
||||
{
|
||||
mode = 'A';
|
||||
|
||||
sb.Append((char)b);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (mode == 'A')
|
||||
{
|
||||
sb.Append(' ');
|
||||
}
|
||||
mode = 'X';
|
||||
|
||||
sb.Append($"${b:X2} ");
|
||||
}
|
||||
}
|
||||
return sb.ToString();
|
||||
}
|
||||
|
||||
private static string DumpBinaryContent(Packet packet)
|
||||
{
|
||||
if (packet.IsNak)
|
||||
{
|
||||
return "NAK";
|
||||
}
|
||||
|
||||
return DumpBytes(packet.Body);
|
||||
}
|
||||
|
||||
private static string DumpBytes(IEnumerable<byte> bytes)
|
||||
{
|
||||
var sb = new StringBuilder();
|
||||
foreach (var b in bytes)
|
||||
{
|
||||
sb.Append($"${b:X2} ");
|
||||
}
|
||||
return sb.ToString();
|
||||
}
|
||||
|
||||
public void CustomReset()
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine("Sending Custom Reset packet");
|
||||
SendCustom(new List<byte> { 0x82 });
|
||||
}
|
||||
|
||||
public List<Packet> SendCustom(List<byte> packetCustomBytes)
|
||||
{
|
||||
SendPacket(packetCustomBytes);
|
||||
return ReceivePackets();
|
||||
}
|
||||
|
||||
public void EndCommunication()
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine("Sending EndCommunication packet");
|
||||
SendPacket(new List<byte> { (byte)PacketCommand.End });
|
||||
}
|
||||
|
||||
public void SendPacket(List<byte> packetBytes)
|
||||
{
|
||||
var packetLength = (byte)(packetBytes.Count + 2);
|
||||
|
||||
packetBytes.Insert(0, _packetCounter.Value);
|
||||
_packetCounter++;
|
||||
|
||||
packetBytes.Insert(0, packetLength);
|
||||
|
||||
foreach (var b in packetBytes)
|
||||
{
|
||||
WriteByteAndReadAck(b);
|
||||
Thread.Sleep(5);
|
||||
}
|
||||
|
||||
KwpCommon.WriteByte(0x03); // Packet end, does not get ACK'd
|
||||
}
|
||||
|
||||
private List<Packet> ReceivePackets()
|
||||
{
|
||||
var packets = new List<Packet>();
|
||||
|
||||
while (true)
|
||||
{
|
||||
var packet = ReceivePacket();
|
||||
packets.Add(packet); // TODO: Maybe don't add the packet if it's an Ack
|
||||
if (packet is AckPacket || packet is NakPacket)
|
||||
{
|
||||
break;
|
||||
}
|
||||
SendAckPacket();
|
||||
}
|
||||
|
||||
return packets;
|
||||
}
|
||||
|
||||
private void WriteByteAndReadAck(byte b)
|
||||
{
|
||||
KwpCommon.WriteByte(b);
|
||||
KwpCommon.ReadComplement(b);
|
||||
}
|
||||
|
||||
private Packet ReceivePacket()
|
||||
{
|
||||
var packetBytes = new List<byte>();
|
||||
|
||||
var packetLength = KwpCommon.ReadAndAckByte();
|
||||
packetBytes.Add(packetLength);
|
||||
|
||||
var packetCounter = ReadPacketCounter();
|
||||
packetBytes.Add(packetCounter);
|
||||
|
||||
var packetCommand = KwpCommon.ReadAndAckByte();
|
||||
packetBytes.Add(packetCommand);
|
||||
|
||||
for (int i = 0; i < packetLength - 3; i++)
|
||||
{
|
||||
var b = KwpCommon.ReadAndAckByte();
|
||||
packetBytes.Add(b);
|
||||
}
|
||||
|
||||
var packetEnd = KwpCommon.ReadByte();
|
||||
packetBytes.Add(packetEnd);
|
||||
if (packetEnd != 0x03)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"Received packet end ${packetEnd:X2} but expected $03");
|
||||
}
|
||||
|
||||
|
||||
switch (packetCommand)
|
||||
{
|
||||
case (byte)PacketCommand.ACK:
|
||||
return new AckPacket(packetBytes);
|
||||
|
||||
case (byte)PacketCommand.AsciiData:
|
||||
if (packetBytes[3] == 0x00) return new CodingWscPacket(packetBytes);
|
||||
return new AsciiDataPacket(packetBytes);
|
||||
|
||||
case (byte)PacketCommand.ReadEepromResponse:
|
||||
return new ReadEepromResponsePacket(packetBytes);
|
||||
|
||||
case (byte)PacketCommand.ReadRomEepromResponse:
|
||||
return new ReadRomEepromResponse(packetBytes);
|
||||
|
||||
case (byte)PacketCommand.Custom:
|
||||
return new CustomPacket(packetBytes);
|
||||
break;
|
||||
|
||||
case (byte)PacketCommand.NAK:
|
||||
return new NakPacket(packetBytes);
|
||||
|
||||
case (byte)PacketCommand.FaultCodesResponse:
|
||||
return new FaultCodesPacket(packetBytes);
|
||||
|
||||
case (byte)PacketCommand.WriteEepromResponse:
|
||||
return new WriteEepromResponsePacket(packetBytes);
|
||||
|
||||
default:
|
||||
return new UnknownPacket(packetBytes);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
private void SendAckPacket()
|
||||
{
|
||||
var packetBytes = new List<byte> { (byte)PacketCommand.ACK };
|
||||
SendPacket(packetBytes);
|
||||
}
|
||||
|
||||
private byte ReadPacketCounter()
|
||||
{
|
||||
var packetCounter = KwpCommon.ReadAndAckByte();
|
||||
if (!_packetCounter.HasValue)
|
||||
{
|
||||
// First packet
|
||||
_packetCounter = packetCounter;
|
||||
}
|
||||
else if (packetCounter != _packetCounter)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"Received packet counter ${packetCounter:X2} but expected ${_packetCounter:X2}");
|
||||
}
|
||||
_packetCounter++;
|
||||
return packetCounter;
|
||||
}
|
||||
|
||||
public void KeepAlive()
|
||||
{
|
||||
SendAckPacket();
|
||||
var packet = ReceivePacket();
|
||||
if (!(packet is AckPacket))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"Received 0x{packet.Title:X2} packet but expected ACK");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public List<FaultCode> ReadFaultCodes()
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"Sending ReadFaultCodes packet");
|
||||
SendPacket(new List<byte>
|
||||
{
|
||||
(byte)PacketCommand.FaultCodesRead
|
||||
});
|
||||
|
||||
var packets = ReceivePackets();
|
||||
packets = packets.Where(b => !b.IsAckNak).ToList();
|
||||
|
||||
var faultCodes = new List<FaultCode>();
|
||||
var faultCodesData = new List<byte>();
|
||||
foreach (var packet in packets)
|
||||
{
|
||||
if (!(packet is FaultCodesPacket))
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"Expected FaultCodesPacket but got {packet.GetType()}");
|
||||
return null;
|
||||
}
|
||||
|
||||
faultCodesData.AddRange(packet.Body);
|
||||
}
|
||||
|
||||
IEnumerable<byte> data = faultCodesData;
|
||||
while (true)
|
||||
{
|
||||
var code = data.Take(3).ToArray();
|
||||
if (code.Length == 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
var dtc = code[0];
|
||||
var status = code[1];
|
||||
|
||||
var faultCode = new FaultCode(dtc, status);
|
||||
if (faultCode.Dtc != FaultCode.None.Dtc)
|
||||
{
|
||||
faultCodes.Add(faultCode);
|
||||
}
|
||||
|
||||
data = data.Skip(8);
|
||||
}
|
||||
|
||||
return faultCodes;
|
||||
}
|
||||
|
||||
public bool ClearFaultCodes()
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"Sending ClearFaultCodes packet");
|
||||
SendPacket(new List<byte>
|
||||
{
|
||||
(byte)PacketCommand.FaultCodesDelete
|
||||
});
|
||||
|
||||
var packets = ReceivePackets();
|
||||
if (packets.Count == 1)
|
||||
{
|
||||
var packet = packets[0];
|
||||
if (packet is NakPacket)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else if (packet is AckPacket)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
throw new InvalidOperationException($"ClearFaultCodes returned {packet.GetType()} packet instead of ACK/NAK");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
throw new InvalidOperationException($"ClearFaultCodes returned {packets.Count} packets instead of 1");
|
||||
}
|
||||
}
|
||||
|
||||
public bool SetSoftwareCoding(int controllerAddress, int softwareCoding, int workshopCode)
|
||||
{
|
||||
// Workshop codes > 65535 overflow into the low bit of the software coding
|
||||
var bytes = new List<byte>
|
||||
{
|
||||
(byte)PacketCommand.SoftwareCoding,
|
||||
(byte)((softwareCoding * 2) / 256),
|
||||
(byte)((softwareCoding * 2) % 256),
|
||||
(byte)((workshopCode & 65535) / 256),
|
||||
(byte)(workshopCode % 256)
|
||||
};
|
||||
|
||||
if (workshopCode > 65535)
|
||||
{
|
||||
bytes[2]++;
|
||||
}
|
||||
|
||||
System.Diagnostics.Debug.WriteLine($"Sending SoftwareCoding packet");
|
||||
SendPacket(bytes);
|
||||
|
||||
var packets = ReceivePackets();
|
||||
if (packets.Count == 1 && packets[0] is NakPacket)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var controllerInfo = new ControllerInfo(packets.Where(b => !b.IsAckNak));
|
||||
return
|
||||
controllerInfo.SoftwareCoding == softwareCoding &&
|
||||
controllerInfo.WorkshopCode == workshopCode;
|
||||
}
|
||||
|
||||
public IKwpCommon KwpCommon { get; }
|
||||
|
||||
private byte? _packetCounter = null;
|
||||
|
||||
public KW1281Connection(IKwpCommon kwpCommon)
|
||||
{
|
||||
KwpCommon = kwpCommon;
|
||||
}
|
||||
}
|
||||
|
||||
public class KW1281KeepAlive : IDisposable
|
||||
{
|
||||
private readonly IKW1281Connection _kw1281Connection;
|
||||
private volatile bool _cancel = false;
|
||||
private Task _keepAliveTask = null;
|
||||
|
||||
public KW1281KeepAlive(IKW1281Connection kw1281Connection)
|
||||
{
|
||||
_kw1281Connection = kw1281Connection;
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
Pause();
|
||||
}
|
||||
|
||||
private void Pause()
|
||||
{
|
||||
_cancel = true;
|
||||
if (_keepAliveTask != null)
|
||||
{
|
||||
_keepAliveTask.Wait();
|
||||
}
|
||||
}
|
||||
|
||||
private void Resume()
|
||||
{
|
||||
_keepAliveTask = Task.Run(KeepAlive);
|
||||
}
|
||||
|
||||
private void KeepAlive()
|
||||
{
|
||||
_cancel = false;
|
||||
while (!_cancel)
|
||||
{
|
||||
_kw1281Connection.KeepAlive();
|
||||
Console.Write(".");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user