1 files changed, 75 insertions, 0 deletions
diff --git a/src/proto2.rs b/src/proto2.rs
new file mode 100644
index 0000000..4e126d3
--- /dev/null
+++ b/src/proto2.rs
@@ -0,0 +1,75 @@
+use crate::proto::*;
+use crate::error::*;
+
+pub(crate) struct QueryMessage<'a> {
+	pub(crate) prio: RequestPriority,
+	pub(crate) path: &'a [u8],
+	pub(crate) telemetry_id: Option<Vec<u8>>,
+	pub(crate) body: &'a [u8],
+}
+
+/// QueryMessage encoding:
+/// - priority: u8
+/// - path length: u8
+/// - path: [u8; path length]
+/// - telemetry id length: u8
+/// - telemetry id: [u8; telemetry id length]
+/// - body [u8; ..]
+impl<'a> QueryMessage<'a> {
+	pub(crate) fn encode(self) -> Vec<u8> {
+		let tel_len = match &self.telemetry_id {
+			Some(t) => t.len(),
+			None => 0,
+		};
+
+		let mut ret = Vec::with_capacity(10 + self.path.len() + tel_len + self.body.len());
+
+		ret.push(self.prio);
+
+		ret.push(self.path.len() as u8);
+		ret.extend_from_slice(self.path);
+
+		if let Some(t) = self.telemetry_id {
+			ret.push(t.len() as u8);
+			ret.extend(t);
+		} else {
+			ret.push(0u8);
+		}
+
+		ret.extend_from_slice(self.body);
+
+		ret
+	}
+
+	pub(crate) fn decode(bytes: &'a [u8]) -> Result<Self, Error> {
+		if bytes.len() < 3 {
+			return Err(Error::Message("Invalid protocol message".into()));
+		}
+
+		let path_length = bytes[1] as usize;
+		if bytes.len() < 3 + path_length {
+			return Err(Error::Message("Invalid protocol message".into()));
+		}
+
+		let telemetry_id_len = bytes[2 + path_length] as usize;
+		if bytes.len() < 3 + path_length + telemetry_id_len {
+			return Err(Error::Message("Invalid protocol message".into()));
+		}
+
+		let path = &bytes[2..2 + path_length];
+		let telemetry_id = if telemetry_id_len > 0 {
+			Some(bytes[3 + path_length .. 3 + path_length + telemetry_id_len].to_vec())
+		} else {
+			None
+		};
+
+		let body = &bytes[3 + path_length + telemetry_id_len..];
+
+		Ok(Self {
+			prio: bytes[0],
+			path,
+			telemetry_id,
+			body,
+		})
+	}
+}