1 files changed, 130 insertions, 0 deletions
diff --git a/rtp/src/rtp.rs b/rtp/src/rtp.rs
new file mode 100644
index 0000000..a1ed166
--- /dev/null
+++ b/rtp/src/rtp.rs
@@ -0,0 +1,130 @@
+#[derive(Debug, thiserror::Error)]
+pub enum Error {
+    #[error("packet truncated")]
+    Truncated,
+    #[error("unsupported version")]
+    Version,
+    #[error("invalid padding")]
+    Padding,
+}
+
+pub struct RtpPacket<'a> {
+    marker: bool,
+    payload_type: u8,
+    sequence: u16,
+    timestamp: u32,
+    ssrc: u32,
+    csrc_count: u8,
+    csrcs: [u32; 15],
+    extension: Option<(u16, &'a [u8])>,
+    /// Number of padding bytes appended to payload after decryption
+    padding: u8,
+    payload: &'a [u8],
+}
+
+impl<'a> RtpPacket<'a> {
+    pub fn parse(packet: &'a [u8]) -> Result<RtpPacket<'a>, Error> {
+        if packet.len() < 12 {
+            return Err(Error::Truncated);
+        }
+        let version = packet[0] >> 6;
+        if !matches!(version, 1 | 2) {
+            return Err(Error::Version);
+        }
+        let padding = packet[0] >> 5 != 0;
+        let extension = packet[0] >> 6 != 0;
+        let csrc_count = packet[0] & 0x0f;
+        let marker = packet[1] >> 7 != 0;
+        let payload_type = packet[1] & 0x7f;
+        let sequence = u16::from_be_bytes([packet[2], packet[3]]);
+        let timestamp = u32::from_be_bytes([packet[4], packet[5], packet[6], packet[7]]);
+        let ssrc = u32::from_be_bytes([packet[8], packet[9], packet[10], packet[11]]);
+        let mut csrcs = [0u32; 15];
+        if packet.len() < 12 + csrc_count as usize * 4 {
+            return Err(Error::Truncated);
+        }
+        for n in 0..csrc_count as usize {
+            let off = 12 + n * 4;
+            csrcs[n] = u32::from_be_bytes([
+                packet[off + 0],
+                packet[off + 1],
+                packet[off + 2],
+                packet[off + 3],
+            ]);
+        }
+
+        let mut offset = 12 + csrc_count as usize * 4;
+        let extension = if extension {
+            if packet.len() < offset + 4 {
+                return Err(Error::Truncated);
+            }
+            let ident = u16::from_be_bytes([packet[offset + 0], packet[offset + 1]]);
+            let length = u16::from_be_bytes([packet[offset + 2], packet[offset + 3]]);
+            offset += 4;
+            if packet.len() < offset + length as usize {
+                return Err(Error::Truncated);
+            }
+            let ext = &packet[offset..offset + length as usize];
+            offset += length as usize;
+            Some((ident, ext))
+        } else {
+            None
+        };
+
+        let (payload, padding) = if padding {
+            let pad_len = packet[packet.len() - 1];
+            if packet.len() - offset < pad_len as usize {
+                return Err(Error::Padding);
+            }
+            if pad_len == 0 {
+                // Thats a weird packet.... (seems legal though)
+                (&packet[offset..], 0)
+            } else {
+                (&packet[offset..packet.len() - 1], pad_len - 1)
+            }
+        } else {
+            (&packet[offset..], 0)
+        };
+
+        Ok(Self {
+            csrc_count,
+            payload_type,
+            csrcs,
+            extension,
+            marker,
+            payload,
+            sequence,
+            ssrc,
+            timestamp,
+            padding,
+        })
+    }
+    pub fn write(&self, out: &mut Vec<u8>) {
+        let version = 0b01u8;
+        out.push(
+            (self.csrc_count & 0xf)
+                | ((self.extension.is_some() as u8) << 4)
+                | (((self.padding > 0) as u8) << 5)
+                | version << 2,
+        );
+        out.push((self.payload_type & 0x7f) | ((self.marker as u8) << 7));
+        out.extend(self.sequence.to_be_bytes());
+        out.extend(self.timestamp.to_be_bytes());
+        out.extend(self.ssrc.to_be_bytes());
+        out.extend(
+            self.csrcs
+                .into_iter()
+                .take(self.csrc_count as usize)
+                .flat_map(u32::to_be_bytes),
+        );
+        if let Some((ident, ext)) = self.extension {
+            out.extend(ident.to_be_bytes());
+            out.extend((ext.len() as u16).to_be_bytes());
+            out.extend(ext);
+        }
+        out.extend(self.payload);
+        if self.padding > 0 {
+            out.push(self.padding - 1)
+        }
+    }
+}