From 26cf88fa1e4105840185752c76babf167addc538 Mon Sep 17 00:00:00 2001
From: The6P4C <watsonjcampbell@gmail.com>
Date: Fri, 29 Sep 2017 10:22:53 +1000
Subject: [PATCH] Process packet in ThunderboltPacket to extract ID and unstuff
 data

---
 ThunderboltTimeSync/FormMain.cs              | 21 +++--
 ThunderboltTimeSync/ThunderboltSerialPort.cs | 92 ++++++++++++++++++--
 2 files changed, 100 insertions(+), 13 deletions(-)

diff --git a/ThunderboltTimeSync/FormMain.cs b/ThunderboltTimeSync/FormMain.cs
index 012bffd..023ea8a 100644
--- a/ThunderboltTimeSync/FormMain.cs
+++ b/ThunderboltTimeSync/FormMain.cs
@@ -22,14 +22,19 @@ namespace ThunderboltTimeSync {
 
 			ThunderboltSerialPort tbsp = new ThunderboltSerialPort(new SerialPort("COM3"));
 
-			tbsp.PacketReceived += (List<byte> packetBuffer) => {
-				List<string> byteStrings = packetBuffer.Select(x => string.Format("{0:X2}", x)).ToList();
-				Debug.WriteLine(
-					string.Format(
-						"Packet received: {0}",
-						string.Join(" ", byteStrings)
-					)
-				);
+			tbsp.PacketReceived += (ThunderboltPacket packet) => {
+				if (packet.IsPacketValid) {
+					List<string> dataByteStrings = packet.Data.Select(x => string.Format("{0:X2}", x)).ToList();
+					List<string> rawDataByteStrings = packet.RawData.Select(x => string.Format("{0:X2}", x)).ToList();
+					Debug.WriteLine(
+						string.Format(
+							"Received packet: ID: {0:X2}, Data: {1}, Raw Data: {2}",
+							packet.ID, string.Join(" ", dataByteStrings), string.Join(" ", rawDataByteStrings)
+						)
+					);
+				} else {
+					Debug.WriteLine("Received invalid packet.");
+				}
 			};
 
 			tbsp.Open();
diff --git a/ThunderboltTimeSync/ThunderboltSerialPort.cs b/ThunderboltTimeSync/ThunderboltSerialPort.cs
index 15a91da..f34ed17 100644
--- a/ThunderboltTimeSync/ThunderboltSerialPort.cs
+++ b/ThunderboltTimeSync/ThunderboltSerialPort.cs
@@ -2,6 +2,39 @@
 using System.IO.Ports;
 
 namespace ThunderboltTimeSync {
+	public class ThunderboltPacket {
+		/// <summary>
+		/// The validity of the packet.
+		/// If <code>false</code>, the values of <code>ID</code>, <code>Data</code> and <code>RawData</code> must not be used.
+		/// If  <code>true</code>, the packet may still contain invalid values or be of incorrect length.
+		/// </summary>
+		public bool IsPacketValid { get; }
+
+		/// <summary>
+		/// The ID of the packet.
+		/// </summary>
+		public byte ID { get; }
+
+		/// <summary>
+		/// The data contained within the packet.
+		/// </summary>
+		public List<byte> Data { get; }
+
+		/// <summary>
+		/// The raw data of the packet.
+		/// Contains the initial [DLE] byte, and the terminating [DLE][ETX] bytes. [DLE] values within the data are still stuffed.
+		/// </summary>
+		public List<byte> RawData { get; }
+
+		protected ThunderboltPacket(bool isPacketValid, byte id, List<byte> data, List<byte> rawData) {
+			IsPacketValid = isPacketValid;
+
+			ID = id;
+			Data = data;
+			RawData = rawData;
+		}
+	}
+
 	public class ThunderboltSerialPort {
 		private static readonly byte CHAR_DLE = 0x10;
 		private static readonly byte CHAR_ETX = 0x03;
@@ -13,14 +46,12 @@ namespace ThunderboltTimeSync {
 
 		/// <summary>
 		/// A delegate which is called when a full packet is received over the serial port.
-		/// The byte buffer passed in contains the initial [DLE], and final [DLE][ETX]. The buffer is completely unparsed and remains stuffed.
 		/// </summary>
-		/// <param name="packetBuffer">A byte buffer which contains the packet.</param>
-		public delegate void PacketReceivedEventHandler(List<byte> packetBuffer);
+		/// <param name="packet">The packet which was received.</param>
+		public delegate void PacketReceivedEventHandler(ThunderboltPacket packet);
 
 		/// <summary>
 		/// An event which is called when a full packet is received over the serial port.
-		/// Refer to <see cref="PacketReceivedEventHandler"/> for more information.
 		/// </summary>
 		public event PacketReceivedEventHandler PacketReceived;
 
@@ -44,6 +75,57 @@ namespace ThunderboltTimeSync {
 			serialPort.Open();
 		}
 
+		/// <summary>
+		/// Converts a stuffed byte list (one where all 0x10 bytes are replaced with 0x10 0x10) into an unstuffed byte list.
+		/// </summary>
+		/// <param name="data">A reference to the list containing the data to be unstuffed.</param>
+		/// <returns>True if the unstuffing was successful, false otherwise.</returns>
+		private bool Unstuff(ref List<byte> data) {
+			List<byte> newData = new List<byte>();
+
+			bool inStuffedDLE = false;
+			foreach (byte b in data) {
+				if (b == CHAR_DLE) {
+					if (!inStuffedDLE) {
+						newData.Add(b);
+						inStuffedDLE = true;
+					} else {
+						// If we see a DLE after we've already seen one (inStuffedDLE == true), we don't need to add the byte to the list because we already did when the first byte was encountered
+						// Just set the flag to false so that if this stuffed DLE is immediately followed by another, it will be correctly parsed
+						inStuffedDLE = false;
+					}
+				} else {
+					if (inStuffedDLE) {
+						return false;
+					}
+
+					newData.Add(b);
+				}
+			}
+
+			data = newData;
+
+			return true;
+		}
+
+		private void ProcessPacket() {
+			byte id = packetBuffer[1];
+
+			// Grab only the data - not the first [DLE]<id> or the last [DLE][ETX]
+			List<byte> data = packetBuffer.GetRange(2, packetBuffer.Count - 4);
+			bool isPacketValid = Unstuff(ref data);
+
+			ThunderboltPacket packet;
+
+			if (isPacketValid) {
+				packet = new ThunderboltPacket(isPacketValid, id, data, packetBuffer);
+			} else {
+				packet = new ThunderboltPacket(isPacketValid, 0, new List<byte>(), new List<byte>());
+			}
+
+			PacketReceived?.Invoke(packet);
+		}
+
 		private void DataReceived(object sender, SerialDataReceivedEventArgs e) {
 			int possibleCurrentByte;
 
@@ -68,7 +150,7 @@ namespace ThunderboltTimeSync {
 
 						// Odd number of DLEs means the ETX does in fact signify the end of the packet
 						if (numberOfPrecedingDLEs % 2 == 1) {
-							PacketReceived?.Invoke(packetBuffer);
+							ProcessPacket();
 
 							packetBuffer.Clear();
 							inPacket = false;