1 files changed, 96 insertions, 10 deletions
diff --git a/rtp/src/lib.rs b/rtp/src/lib.rs
index 68ca10d..b8c627b 100644
--- a/rtp/src/lib.rs
+++ b/rtp/src/lib.rs
@@ -1,25 +1,111 @@
+#[derive(Debug, thiserror::Error)]
+pub enum Error {
+    #[error("packet truncated")]
+    Truncated,
+    #[error("unsupported version")]
+    Version,
+    #[error("invalid padding")]
+    Padding,
+}
+
 pub struct RtpPacket<'a> {
-    marked: bool,
+    marker: bool,
     payload_type: u8,
-
-    extension: Option<(u16, &'a [u8])>,
-    payload: Option<&'a [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]) -> RtpPacket<'a> {
-        todo!()
+    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>) {
-        out.push(self.csrc_count | (2 << 6) | 0); // TODO
-        out.push(self.payload_type | (self.marked as u8 * 0x80));
+        out.push(
+            (self.csrc_count & 0xf)
+                | ((self.extension.is_some() as u8) << 6)
+                | ((self.marker as u8) << 7),
+        ); // TODO padding
+        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());