moved embedded-mqtt crate to a module

11 files changed, 1335 insertions, 6 deletions

diff --git a/src/lib.rs b/src/lib.rs
index 3e91272..714e186 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -28,6 +28,8 @@ use heapless::{
 };
 use serde::Serialize;
 
+mod mqtt;
+
 pub mod log;
 pub use log::Format;
 
@@ -215,7 +217,7 @@ pub struct DeviceResources {
     waker: AtomicWaker,
     entities: [RefCell<Option<EntityData>>; Self::ENTITY_LIMIT],
 
-    mqtt_resources: embedded_mqtt::ClientResources,
+    mqtt_resources: mqtt::ClientResources,
     publish_buffer: Vec<u8, 2048>,
     subscribe_buffer: Vec<u8, 128>,
     discovery_buffer: Vec<u8, 2048>,
@@ -422,7 +424,7 @@ pub struct Device<'a> {
     waker: &'a AtomicWaker,
     entities: &'a [RefCell<Option<EntityData>>],
 
-    mqtt_resources: &'a mut embedded_mqtt::ClientResources,
+    mqtt_resources: &'a mut mqtt::ClientResources,
     publish_buffer: &'a mut VecView<u8>,
     subscribe_buffer: &'a mut VecView<u8>,
     discovery_buffer: &'a mut VecView<u8>,
@@ -585,8 +587,8 @@ pub async fn run<T: Transport>(device: &mut Device<'_>, transport: &mut T) -> Re
     .expect("device availability buffer too small");
     let availability_topic = device.availability_topic_buffer.as_str();
 
-    let mut client = embedded_mqtt::Client::new(device.mqtt_resources, transport);
-    let connect_params = embedded_mqtt::ConnectParams {
+    let mut client = mqtt::Client::new(device.mqtt_resources, transport);
+    let connect_params = mqtt::ConnectParams {
         will_topic: Some(availability_topic),
         will_payload: Some(NOT_AVAILABLE_PAYLOAD.as_bytes()),
         will_retain: true,
@@ -744,7 +746,7 @@ pub async fn run<T: Transport>(device: &mut Device<'_>, transport: &mut T) -> Re
         client.publish_with(
             availability_topic,
             AVAILABLE_PAYLOAD.as_bytes(),
-            embedded_mqtt::PublishParams {
+            mqtt::PublishParams {
                 retain: true,
                 ..Default::default()
             },
@@ -858,7 +860,7 @@ pub async fn run<T: Transport>(device: &mut Device<'_>, transport: &mut T) -> Re
         .await
         {
             Ok(embassy_futures::select::Either::First(packet)) => match packet {
-                Ok(embedded_mqtt::Packet::Publish(publish)) => publish,
+                Ok(mqtt::Packet::Publish(publish)) => publish,
                 Err(err) => {
                     crate::log::error!(
                         "mqtt receive failed with: {:?}",
diff --git a/src/mqtt/connect_code.rs b/src/mqtt/connect_code.rs
new file mode 100644
index 0000000..570ce0f
--- /dev/null
+++ b/src/mqtt/connect_code.rs
@@ -0,0 +1,58 @@
+use super::protocol;
+
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum ConnectCode {
+    ConnectionAccepted,
+    UnacceptableProtocolVersion,
+    IdentifierRejected,
+    ServerUnavailable,
+    BadUsernamePassword,
+    NotAuthorized,
+    Unknown(u8),
+}
+
+impl core::fmt::Display for ConnectCode {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        match self {
+            ConnectCode::ConnectionAccepted => write!(f, "Connection Accepted"),
+            ConnectCode::UnacceptableProtocolVersion => write!(f, "Unacceptable Protocol Version"),
+            ConnectCode::IdentifierRejected => write!(f, "Identifier Rejected"),
+            ConnectCode::ServerUnavailable => write!(f, "Server Unavailable"),
+            ConnectCode::BadUsernamePassword => write!(f, "Bad Username or Password"),
+            ConnectCode::NotAuthorized => write!(f, "Not Authorized"),
+            ConnectCode::Unknown(code) => write!(f, "Unknown({})", code),
+        }
+    }
+}
+
+impl From<u8> for ConnectCode {
+    fn from(value: u8) -> Self {
+        match value {
+            protocol::CONNACK_CODE_ACCEPTED => ConnectCode::ConnectionAccepted,
+            protocol::CONNACK_CODE_UNACCEPTABLE_PROTOCOL_VERSION => {
+                ConnectCode::UnacceptableProtocolVersion
+            }
+            protocol::CONNACK_CODE_IDENTIFIER_REJECTED => ConnectCode::IdentifierRejected,
+            protocol::CONNACK_CODE_SERVER_UNAVAILABLE => ConnectCode::ServerUnavailable,
+            protocol::CONNACK_CODE_BAD_USERNAME_PASSWORD => ConnectCode::BadUsernamePassword,
+            protocol::CONNACK_CODE_NOT_AUTHORIZED => ConnectCode::NotAuthorized,
+            code => ConnectCode::Unknown(code),
+        }
+    }
+}
+
+impl From<ConnectCode> for u8 {
+    fn from(value: ConnectCode) -> Self {
+        match value {
+            ConnectCode::ConnectionAccepted => protocol::CONNACK_CODE_ACCEPTED,
+            ConnectCode::UnacceptableProtocolVersion => {
+                protocol::CONNACK_CODE_UNACCEPTABLE_PROTOCOL_VERSION
+            }
+            ConnectCode::IdentifierRejected => protocol::CONNACK_CODE_IDENTIFIER_REJECTED,
+            ConnectCode::ServerUnavailable => protocol::CONNACK_CODE_SERVER_UNAVAILABLE,
+            ConnectCode::BadUsernamePassword => protocol::CONNACK_CODE_BAD_USERNAME_PASSWORD,
+            ConnectCode::NotAuthorized => protocol::CONNACK_CODE_NOT_AUTHORIZED,
+            ConnectCode::Unknown(code) => code,
+        }
+    }
+}
diff --git a/src/mqtt/field.rs b/src/mqtt/field.rs
new file mode 100644
index 0000000..9e67e63
--- /dev/null
+++ b/src/mqtt/field.rs
@@ -0,0 +1,89 @@
+use core::{mem::MaybeUninit, ops::Deref};
+
+use super::varint;
+
+const DEFAULT_FIELD_BUFFER_CAP: usize = 32;
+
+pub enum Field<'a> {
+    U8(u8),
+    U16(u16),
+    VarInt(u32),
+    Buffer(&'a [u8]),
+    LenPrefixedBuffer(&'a [u8]),
+    LenPrefixedString(&'a str),
+}
+
+pub struct FieldBuffer<'a, const N: usize = DEFAULT_FIELD_BUFFER_CAP> {
+    data: [MaybeUninit<Field<'a>>; N],
+    len: usize,
+}
+
+impl<'a, const N: usize> Default for FieldBuffer<'a, N> {
+    fn default() -> Self {
+        Self {
+            data: [const { MaybeUninit::uninit() }; N],
+            len: 0,
+        }
+    }
+}
+
+impl<'a, const N: usize> FieldBuffer<'a, N> {
+    pub fn clear(&mut self) {
+        self.len = 0;
+    }
+
+    pub fn push(&mut self, field: Field<'a>) {
+        assert!(self.len < N, "field buffer lenght limit exceeded");
+        self.data[self.len].write(field);
+        self.len += 1;
+    }
+
+    pub fn set(&mut self, n: usize, field: Field<'a>) {
+        assert!(self.len > n);
+        self.data[n].write(field);
+    }
+
+    pub fn as_slice<'s>(&'s self) -> &'s [Field<'a>] {
+        unsafe {
+            core::mem::transmute::<&'s [MaybeUninit<Field<'a>>], &'s [Field<'a>]>(
+                &self.data[..self.len],
+            )
+        }
+    }
+}
+
+impl<'a, const N: usize> AsRef<[Field<'a>]> for FieldBuffer<'a, N> {
+    fn as_ref(&self) -> &[Field<'a>] {
+        self.as_slice()
+    }
+}
+
+impl<'a, const N: usize> Deref for FieldBuffer<'a, N> {
+    type Target = [Field<'a>];
+
+    fn deref(&self) -> &Self::Target {
+        self.as_slice()
+    }
+}
+
+pub fn field_size(field: &Field) -> usize {
+    match field {
+        Field::U8(_) => 1,
+        Field::U16(_) => 2,
+        Field::VarInt(v) => {
+            let (_, n) = varint::encode(*v);
+            n
+        }
+        Field::Buffer(v) => v.len(),
+        Field::LenPrefixedBuffer(v) => v.len().strict_add(2),
+        Field::LenPrefixedString(v) => v.len().strict_add(2),
+    }
+}
+
+pub fn fields_size(fields: &[Field]) -> usize {
+    let mut total_size = 0usize;
+    for field in fields {
+        total_size = total_size.strict_add(field_size(field));
+    }
+    total_size
+}
diff --git a/src/mqtt/mod.rs b/src/mqtt/mod.rs
new file mode 100644
index 0000000..30e3a33
--- /dev/null
+++ b/src/mqtt/mod.rs
@@ -0,0 +1,485 @@
+mod connect_code;
+mod field;
+mod packet_id;
+mod protocol;
+mod qos;
+mod rx;
+mod transport;
+mod tx;
+mod varint;
+
+pub use connect_code::ConnectCode;
+use embedded_io_async::ReadExactError;
+pub use packet_id::PacketId;
+pub use qos::Qos;
+pub use transport::Transport;
+
+use self::{field::FieldBuffer, transport::TransportExt as _};
+
+const DEFAULT_CLIENT_RX_BUFFER_SIZE: usize = 512;
+const DEFAULT_CLIENT_TX_BUFFER_SIZE: usize = 512;
+
+pub enum Error<T: Transport> {
+    Transport(T::Error),
+    TransportEOF,
+    InsufficientBufferSpace,
+    ProtocolError(&'static str),
+    ConnectFailed(ConnectCode),
+}
+
+impl<T: Transport> core::fmt::Debug for Error<T> {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        match self {
+            Error::Transport(err) => f.debug_tuple("Transport").field(err).finish(),
+            Error::TransportEOF => f.write_str("TransportEOF"),
+            Error::InsufficientBufferSpace => f.write_str("InsufficientBufferSpace"),
+            Error::ProtocolError(msg) => f.debug_tuple("ProtocolError").field(msg).finish(),
+            Error::ConnectFailed(code) => f.debug_tuple("ConnectFailed").field(code).finish(),
+        }
+    }
+}
+
+impl<T: Transport> core::fmt::Display for Error<T> {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        match self {
+            Error::Transport(err) => write!(f, "transport error: {:?}", err),
+            Error::TransportEOF => write!(f, "unexpected end of transport stream"),
+            Error::InsufficientBufferSpace => {
+                write!(f, "insufficient buffer space to receive packet")
+            }
+            Error::ProtocolError(msg) => write!(f, "MQTT protocol error: {}", msg),
+            Error::ConnectFailed(code) => write!(f, "connection failed: {}", code),
+        }
+    }
+}
+
+impl<T: Transport> core::error::Error for Error<T>
+where
+    T::Error: core::error::Error + 'static,
+{
+    fn source(&self) -> Option<&(dyn core::error::Error + 'static)> {
+        match self {
+            Error::Transport(err) => Some(err),
+            _ => None,
+        }
+    }
+}
+
+#[derive(Debug, Default)]
+pub struct ConnectParams<'a> {
+    pub will_topic: Option<&'a str>,
+    pub will_payload: Option<&'a [u8]>,
+    pub will_retain: bool,
+    pub username: Option<&'a str>,
+    pub password: Option<&'a [u8]>,
+    pub keepalive: Option<u16>,
+}
+
+#[derive(Debug, Default)]
+pub struct PublishParams {
+    pub qos: Qos,
+    pub retain: bool,
+}
+
+#[derive(Debug)]
+pub enum PublishData<'a> {
+    Inline(&'a [u8]),
+    Deferred(usize),
+}
+
+#[derive(Debug)]
+pub struct Publish<'a> {
+    pub topic: &'a str,
+    pub packet_id: Option<PacketId>,
+    pub qos: Qos,
+    pub retain: bool,
+    pub data_len: usize,
+}
+
+#[derive(Debug)]
+pub struct PublishAck {
+    pub packet_id: PacketId,
+}
+
+#[derive(Debug)]
+pub struct SubscribeAck {
+    pub packet_id: PacketId,
+    pub success: bool,
+}
+
+#[derive(Debug)]
+pub struct UnsubscribeAck {
+    pub packet_id: PacketId,
+}
+
+#[derive(Debug)]
+pub enum Packet<'a> {
+    Publish(Publish<'a>),
+    PublishAck(PublishAck),
+    SubscribeAck(SubscribeAck),
+    UnsubscribeAck(UnsubscribeAck),
+}
+
+pub struct ClientResources<
+    const RX: usize = DEFAULT_CLIENT_RX_BUFFER_SIZE,
+    const TX: usize = DEFAULT_CLIENT_TX_BUFFER_SIZE,
+> {
+    rx_buffer: [u8; RX],
+    tx_buffer: [u8; TX],
+}
+
+impl<const RX: usize, const TX: usize> Default for ClientResources<RX, TX> {
+    fn default() -> Self {
+        Self {
+            rx_buffer: [0u8; RX],
+            tx_buffer: [0u8; TX],
+        }
+    }
+}
+
+pub struct Client<'a, T> {
+    transport: T,
+    rx_buffer: &'a mut [u8],
+    rx_buffer_len: usize,
+    rx_buffer_skip: usize,
+    rx_buffer_data: usize,
+    tx_buffer: &'a mut [u8],
+    next_packet_id: u16,
+}
+
+impl<'a, T> Client<'a, T> {
+    pub fn new<const RX: usize, const TX: usize>(
+        resources: &'a mut ClientResources<RX, TX>,
+        transport: T,
+    ) -> Self {
+        Self {
+            transport,
+            rx_buffer: &mut resources.rx_buffer,
+            rx_buffer_len: 0,
+            rx_buffer_skip: 0,
+            rx_buffer_data: 0,
+            tx_buffer: &mut resources.tx_buffer,
+            next_packet_id: 1,
+        }
+    }
+}
+
+impl<'a, T> Client<'a, T>
+where
+    T: Transport,
+{
+    fn allocate_packet_id(&mut self) -> PacketId {
+        let packet_id = self.next_packet_id;
+        self.next_packet_id = self.next_packet_id.wrapping_add(1);
+        if self.next_packet_id == 0 {
+            self.next_packet_id = 1;
+        }
+        PacketId::from(packet_id)
+    }
+
+    pub async fn connect(&mut self, client_id: &str) -> Result<(), Error<T>> {
+        self.connect_with(client_id, Default::default()).await
+    }
+
+    pub async fn connect_with(
+        &mut self,
+        client_id: &str,
+        params: ConnectParams<'_>,
+    ) -> Result<(), Error<T>> {
+        let mut buffer = FieldBuffer::default();
+        tx::connect(
+            &mut buffer,
+            tx::Connect {
+                client_id,
+                clean_session: true,
+                username: params.username,
+                password: params.password,
+                will_topic: params.will_topic,
+                will_payload: params.will_payload,
+                will_retain: params.will_retain,
+                keepalive: None,
+            },
+        );
+        self.transport
+            .write_fields(&buffer)
+            .await
+            .map_err(Error::Transport)?;
+        self.transport.flush().await.map_err(Error::Transport)?;
+
+        // Wait for CONNACK response
+        match self.receive_inner().await? {
+            rx::Packet::ConnAck {
+                session_present,
+                code,
+            } => {
+                if code == ConnectCode::ConnectionAccepted {
+                    Ok(())
+                } else {
+                    Err(Error::ConnectFailed(code))
+                }
+            }
+            _ => Err(Error::ProtocolError(
+                "expected CONNACK packet after CONNECT",
+            )),
+        }
+    }
+
+    pub async fn publish(&mut self, topic: &str, data: &[u8]) -> Result<PacketId, Error<T>> {
+        self.publish_with(topic, data, Default::default()).await
+    }
+
+    pub async fn publish_with(
+        &mut self,
+        topic: &str,
+        data: &[u8],
+        params: PublishParams,
+    ) -> Result<PacketId, Error<T>> {
+        let packet_id = if params.qos.to_u8() > 0 {
+            Some(self.allocate_packet_id())
+        } else {
+            None
+        };
+
+        let mut buffer = FieldBuffer::default();
+        tx::publish(
+            &mut buffer,
+            tx::Publish {
+                topic,
+                payload: data,
+                qos: params.qos,
+                retain: params.retain,
+                dup: false,
+                packet_id,
+            },
+        );
+
+        self.transport
+            .write_fields(&buffer)
+            .await
+            .map_err(Error::Transport)?;
+        self.transport.flush().await.map_err(Error::Transport)?;
+
+        Ok(packet_id.unwrap_or(PacketId::from(0)))
+    }
+
+    pub async fn publish_ack(&mut self, packet_id: PacketId, qos: Qos) -> Result<(), Error<T>> {
+        let mut buffer = FieldBuffer::default();
+
+        match qos {
+            Qos::AtMostOnce => {
+                // QoS 0: No acknowledgment needed
+                return Ok(());
+            }
+            Qos::AtLeastOnce => {
+                // QoS 1: Send PUBACK
+                tx::puback(&mut buffer, packet_id);
+            }
+            Qos::ExactlyOnce => todo!("not implemented"),
+        }
+
+        self.transport
+            .write_fields(&buffer)
+            .await
+            .map_err(Error::Transport)?;
+        self.transport.flush().await.map_err(Error::Transport)?;
+
+        Ok(())
+    }
+
+    pub async fn subscribe(&mut self, topic: &str) -> Result<PacketId, Error<T>> {
+        self.subscribe_with(topic, Qos::AtMostOnce).await
+    }
+
+    pub async fn subscribe_with(&mut self, topic: &str, qos: Qos) -> Result<PacketId, Error<T>> {
+        let packet_id = self.allocate_packet_id();
+
+        let mut buffer = FieldBuffer::default();
+        tx::subscribe(
+            &mut buffer,
+            tx::Subscribe {
+                topic,
+                qos,
+                packet_id,
+            },
+        );
+
+        self.transport
+            .write_fields(&buffer)
+            .await
+            .map_err(Error::Transport)?;
+        self.transport.flush().await.map_err(Error::Transport)?;
+
+        Ok(packet_id)
+    }
+
+    pub async fn unsubscribe(&mut self, topic: &str) -> Result<PacketId, Error<T>> {
+        let packet_id = self.allocate_packet_id();
+
+        let mut buffer = FieldBuffer::default();
+        tx::unsubscribe(&mut buffer, tx::Unsubscribe { topic, packet_id });
+
+        self.transport
+            .write_fields(&buffer)
+            .await
+            .map_err(Error::Transport)?;
+        self.transport.flush().await.map_err(Error::Transport)?;
+
+        Ok(packet_id)
+    }
+
+    async fn receive_inner<'s>(&'s mut self) -> Result<rx::Packet<'s>, Error<T>> {
+        self.skip_if_required();
+        self.discard_data().await?;
+
+        loop {
+            let buf = &self.rx_buffer[..self.rx_buffer_len];
+            match rx::decode(buf) {
+                Ok(_) => {
+                    // NOTE: stupid workaround for borrow checker, should not
+                    // need to decode twice
+                    let buf = &self.rx_buffer[..self.rx_buffer_len];
+                    let (packet, n) = rx::decode(buf).unwrap();
+                    self.rx_buffer_skip = n;
+                    if let rx::Packet::Publish { data_len, .. } = &packet {
+                        self.rx_buffer_data = *data_len;
+                    }
+                    return Ok(packet);
+                }
+                Err(err) => match err {
+                    rx::Error::NeedMoreData => {
+                        if self.rx_buffer.len() == self.rx_buffer_len {
+                            return Err(Error::InsufficientBufferSpace);
+                        }
+                    }
+                    rx::Error::InvalidPacket(msg) => return Err(Error::ProtocolError(msg)),
+                    rx::Error::UnsupportedPacket { packet_type, .. } => {
+                        return Err(Error::ProtocolError("unsupported packet type"));
+                    }
+                    rx::Error::UnknownPacket { packet_type, .. } => {
+                        return Err(Error::ProtocolError("unknown packet type"));
+                    }
+                },
+            }
+
+            self.fill_rx_buffer().await?;
+        }
+    }
+
+    pub async fn receive<'s>(&'s mut self) -> Result<Packet<'s>, Error<T>> {
+        match self.receive_inner().await? {
+            rx::Packet::ConnAck { .. } => {
+                return Err(Error::ProtocolError("unexpected CONNACK packet"));
+            }
+            rx::Packet::Publish {
+                topic,
+                packet_id,
+                qos,
+                retain,
+                dup: _dup,
+                data_len,
+            } => {
+                return Ok(Packet::Publish(Publish {
+                    topic,
+                    packet_id,
+                    qos,
+                    retain,
+                    data_len,
+                }));
+            }
+            rx::Packet::PubAck { packet_id } => {
+                return Ok(Packet::PublishAck(PublishAck { packet_id }));
+            }
+            rx::Packet::SubscribeAck { packet_id, success } => {
+                return Ok(Packet::SubscribeAck(SubscribeAck { packet_id, success }));
+            }
+            rx::Packet::UnsubscribeAck { packet_id } => {
+                return Ok(Packet::UnsubscribeAck(UnsubscribeAck { packet_id }));
+            }
+        }
+    }
+
+    pub async fn receive_data(&mut self, buf: &mut [u8]) -> Result<(), Error<T>> {
+        self.skip_if_required();
+        if buf.len() != self.rx_buffer_data {
+            return Err(Error::InsufficientBufferSpace);
+        }
+
+        assert_eq!(self.rx_buffer_skip, 0);
+        let from_buffer = self.rx_buffer_data.min(self.rx_buffer_len);
+        let from_transport = self.rx_buffer_data.strict_sub(from_buffer);
+
+        buf[..from_buffer].copy_from_slice(&self.rx_buffer[..from_buffer]);
+        self.rx_buffer_len -= from_buffer;
+
+        if from_transport > 0 {
+            assert_eq!(self.rx_buffer_len, 0);
+            self.transport
+                .read_exact(&mut buf[from_buffer..])
+                .await
+                .map_err(|err| match err {
+                    ReadExactError::UnexpectedEof => Error::<T>::TransportEOF,
+                    ReadExactError::Other(e) => Error::Transport(e),
+                })?;
+        }
+        self.rx_buffer_data = 0;
+
+        Ok(())
+    }
+
+    pub async fn disconnect(&mut self) -> Result<(), Error<T>> {
+        let mut buffer = FieldBuffer::default();
+        tx::disconnect(&mut buffer);
+
+        self.transport
+            .write_fields(&buffer)
+            .await
+            .map_err(Error::Transport)?;
+        self.transport.flush().await.map_err(Error::Transport)?;
+
+        Ok(())
+    }
+
+    async fn fill_rx_buffer(&mut self) -> Result<(), Error<T>> {
+        let n = self
+            .transport
+            .read(&mut self.rx_buffer[self.rx_buffer_len..])
+            .await
+            .map_err(Error::Transport)?;
+        if n == 0 {
+            return Err(Error::TransportEOF);
+        }
+        self.rx_buffer_len += n;
+
+        Ok(())
+    }
+
+    fn skip_if_required(&mut self) {
+        assert!(self.rx_buffer_len >= self.rx_buffer_skip);
+        if self.rx_buffer_skip != 0 {
+            self.rx_buffer.copy_within(self.rx_buffer_skip.., 0);
+            self.rx_buffer_len = self.rx_buffer_len.strict_sub(self.rx_buffer_skip);
+            self.rx_buffer_skip = 0;
+        }
+    }
+
+    async fn discard_data(&mut self) -> Result<(), Error<T>> {
+        if self.rx_buffer_data == 0 {
+            return Ok(());
+        }
+
+        assert_eq!(self.rx_buffer_skip, 0);
+        while self.rx_buffer_data > 0 {
+            if self.rx_buffer_len <= self.rx_buffer_data {
+                self.rx_buffer_data -= self.rx_buffer_len;
+                self.rx_buffer_len = 0;
+            } else {
+                self.rx_buffer.copy_within(self.rx_buffer_data.., 0);
+                self.rx_buffer_len -= self.rx_buffer_data;
+                self.rx_buffer_data = 0;
+            }
+            self.fill_rx_buffer().await?;
+        }
+
+        Ok(())
+    }
+}
diff --git a/src/mqtt/packet_id.rs b/src/mqtt/packet_id.rs
new file mode 100644
index 0000000..4e0158f
--- /dev/null
+++ b/src/mqtt/packet_id.rs
@@ -0,0 +1,14 @@
+#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
+pub struct PacketId(u16);
+
+impl From<u16> for PacketId {
+    fn from(value: u16) -> Self {
+        Self(value)
+    }
+}
+
+impl From<PacketId> for u16 {
+    fn from(value: PacketId) -> Self {
+        value.0
+    }
+}
diff --git a/src/mqtt/protocol.rs b/src/mqtt/protocol.rs
new file mode 100644
index 0000000..bf77d78
--- /dev/null
+++ b/src/mqtt/protocol.rs
@@ -0,0 +1,65 @@
+pub const PACKET_TYPE_CONNECT: u8 = 1;
+pub const PACKET_TYPE_CONNACK: u8 = 2;
+pub const PACKET_TYPE_PUBLISH: u8 = 3;
+pub const PACKET_TYPE_PUBACK: u8 = 4;
+pub const PACKET_TYPE_PUBREC: u8 = 5;
+pub const PACKET_TYPE_PUBREL: u8 = 6;
+pub const PACKET_TYPE_PUBCOMP: u8 = 7;
+pub const PACKET_TYPE_SUBSCRIBE: u8 = 8;
+pub const PACKET_TYPE_SUBACK: u8 = 9;
+pub const PACKET_TYPE_UNSUBSCRIBE: u8 = 10;
+pub const PACKET_TYPE_UNSUBACK: u8 = 11;
+pub const PACKET_TYPE_PINGREQ: u8 = 12;
+pub const PACKET_TYPE_PINGRESP: u8 = 13;
+pub const PACKET_TYPE_DISCONNECT: u8 = 14;
+
+pub const PROTOCOL_NAME: &str = "MQTT";
+
+pub const PROTOCOL_LEVEL_3_1_1: u8 = 0x04;
+pub const PROTOCOL_LEVEL_5_0_0: u8 = 0x05;
+
+pub const CONNECT_FLAG_USERNAME: u8 = 1 << 7;
+pub const CONNECT_FLAG_PASSWORD: u8 = 1 << 6;
+pub const CONNECT_FLAG_WILL_RETAIN: u8 = 1 << 5;
+pub const CONNECT_FLAG_WILL_FLAG: u8 = 1 << 2;
+pub const CONNECT_FLAG_CLEAN_SESSION: u8 = 1 << 1;
+
+pub const SUBSCRIBE_HEADER_FLAGS: u8 = 0x02;
+pub const UNSUBSCRIBE_HEADER_FLAGS: u8 = 0x02;
+pub const PUBREL_HEADER_FLAGS: u8 = 0x02;
+
+pub const CONNACK_CODE_ACCEPTED: u8 = 0;
+pub const CONNACK_CODE_UNACCEPTABLE_PROTOCOL_VERSION: u8 = 1;
+pub const CONNACK_CODE_IDENTIFIER_REJECTED: u8 = 2;
+pub const CONNACK_CODE_SERVER_UNAVAILABLE: u8 = 3;
+pub const CONNACK_CODE_BAD_USERNAME_PASSWORD: u8 = 4;
+pub const CONNACK_CODE_NOT_AUTHORIZED: u8 = 5;
+
+pub const CONNACK_FLAG_SESSION_PRESENT: u8 = 0x01;
+pub const CONNACK_FLAG_RESERVED: u8 = 0xFE;
+
+pub const SUBACK_FAILURE: u8 = 0x80;
+
+pub const PUBLISH_FLAG_RETAIN: u8 = 0x01;
+pub const PUBLISH_FLAG_QOS_MASK: u8 = 0x06;
+pub const PUBLISH_FLAG_QOS_SHIFT: u8 = 1;
+pub const PUBLISH_FLAG_DUP: u8 = 0x08;
+
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub struct HeaderControl {
+    pub packet_type: u8,
+    pub packet_flags: u8,
+}
+
+pub fn create_header_control(packet_type: u8, flags: u8) -> u8 {
+    assert!(packet_type & 0xF0 == 0);
+    assert!(flags & 0xF0 == 0);
+    packet_type << 4 | flags
+}
+
+pub fn split_header_control(control: u8) -> HeaderControl {
+    HeaderControl {
+        packet_type: control >> 4,
+        packet_flags: control & 0x0F,
+    }
+}
diff --git a/src/mqtt/qos.rs b/src/mqtt/qos.rs
new file mode 100644
index 0000000..0d464b4
--- /dev/null
+++ b/src/mqtt/qos.rs
@@ -0,0 +1,64 @@
+#[derive(Debug)]
+pub struct InvalidQos(u8);
+
+impl core::fmt::Display for InvalidQos {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        write!(f, "invalid QoS value: '{}'", self.0)
+    }
+}
+
+impl core::error::Error for InvalidQos {}
+
+#[derive(Debug, Default, Clone, Copy, PartialEq, Eq, Hash)]
+pub enum Qos {
+    #[default]
+    AtMostOnce,
+    AtLeastOnce,
+    ExactlyOnce,
+}
+
+impl core::fmt::Display for Qos {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        f.write_str(match self {
+            Qos::AtMostOnce => "QoS::AtMostOnce",
+            Qos::AtLeastOnce => "Qos::AtLeastOnce",
+            Qos::ExactlyOnce => "Qos::ExactlyOnce",
+        })
+    }
+}
+
+impl Qos {
+    pub fn to_u8(&self) -> u8 {
+        match self {
+            Qos::AtMostOnce => 0,
+            Qos::AtLeastOnce => 1,
+            Qos::ExactlyOnce => 2,
+        }
+    }
+
+    pub fn from_u8(v: u8) -> Option<Self> {
+        match v {
+            0 => Some(Self::AtMostOnce),
+            1 => Some(Self::AtLeastOnce),
+            2 => Some(Self::ExactlyOnce),
+            _ => None,
+        }
+    }
+}
+
+impl TryFrom<u8> for Qos {
+    type Error = InvalidQos;
+
+    fn try_from(value: u8) -> Result<Self, Self::Error> {
+        match Self::from_u8(value) {
+            Some(v) => Ok(v),
+            None => Err(InvalidQos(value)),
+        }
+    }
+}
+
+impl From<Qos> for u8 {
+    fn from(value: Qos) -> Self {
+        value.to_u8()
+    }
+}
diff --git a/src/mqtt/rx.rs b/src/mqtt/rx.rs
new file mode 100644
index 0000000..e81171d
--- /dev/null
+++ b/src/mqtt/rx.rs
@@ -0,0 +1,241 @@
+use super::{ConnectCode, PacketId, Qos, protocol, varint};
+
+#[derive(Debug)]
+pub enum Error {
+    NeedMoreData,
+    InvalidPacket(&'static str),
+    UnsupportedPacket { packet_type: u8, packet_len: u32 },
+    UnknownPacket { packet_type: u8, packet_len: u32 },
+}
+
+impl core::fmt::Display for Error {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        match self {
+            Error::NeedMoreData => f.write_str("need more data"),
+            Error::InvalidPacket(msg) => write!(f, "invalid packet: {}", msg),
+            Error::UnsupportedPacket {
+                packet_type,
+                packet_len,
+            } => write!(
+                f,
+                "unsupported packet type {} with length {}",
+                packet_type, packet_len
+            ),
+            Error::UnknownPacket {
+                packet_type,
+                packet_len,
+            } => write!(
+                f,
+                "unknown packet type {} with length {}",
+                packet_type, packet_len
+            ),
+        }
+    }
+}
+
+impl From<varint::Error> for Error {
+    fn from(value: varint::Error) -> Self {
+        match value {
+            varint::Error::NeedMoreData => Self::NeedMoreData,
+            varint::Error::InvalidVarInt => Self::InvalidPacket("invalid variable integer encoding"),
+        }
+    }
+}
+
+pub enum Packet<'a> {
+    ConnAck {
+        session_present: bool,
+        code: ConnectCode,
+    },
+    Publish {
+        topic: &'a str,
+        packet_id: Option<PacketId>,
+        qos: Qos,
+        retain: bool,
+        dup: bool,
+        data_len: usize,
+    },
+    PubAck {
+        packet_id: PacketId,
+    },
+    SubscribeAck {
+        packet_id: PacketId,
+        success: bool,
+    },
+    UnsubscribeAck {
+        packet_id: PacketId,
+    },
+}
+
+pub fn decode<'a>(buf: &'a [u8]) -> Result<(Packet<'a>, usize), Error> {
+    let mut reader = Reader::new(buf);
+    let protocol::HeaderControl {
+        packet_type,
+        packet_flags,
+    } = protocol::split_header_control(reader.read_u8()?);
+    let packet_len = reader.read_varint()?;
+
+    let packet = match packet_type {
+        protocol::PACKET_TYPE_CONNACK => {
+            let flags = reader.read_u8()?;
+            let code = ConnectCode::from(reader.read_u8()?);
+            let session_present = flags & protocol::CONNACK_FLAG_SESSION_PRESENT != 0;
+            if flags & protocol::CONNACK_FLAG_RESERVED != 0 {
+                return Err(Error::InvalidPacket("CONNACK reserved flags must be zero"));
+            }
+            Packet::ConnAck {
+                session_present,
+                code,
+            }
+        }
+        protocol::PACKET_TYPE_PUBLISH => {
+            // Extract flags from the fixed header
+            let retain = (packet_flags & protocol::PUBLISH_FLAG_RETAIN) != 0;
+            let qos_value = (packet_flags & protocol::PUBLISH_FLAG_QOS_MASK) >> protocol::PUBLISH_FLAG_QOS_SHIFT;
+            let qos = Qos::from_u8(qos_value).ok_or(Error::InvalidPacket("PUBLISH has invalid QoS value"))?;
+            let dup = (packet_flags & protocol::PUBLISH_FLAG_DUP) != 0;
+
+            // Track position after fixed header to calculate data length
+            let variable_header_start = reader.num_read();
+
+            // Read topic name
+            let topic = reader.read_len_prefix_str()?;
+
+            // Read packet ID if QoS > 0
+            let packet_id = if qos.to_u8() > 0 {
+                Some(PacketId::from(reader.read_u16()?))
+            } else {
+                None
+            };
+
+            // Calculate payload length without reading it
+            let variable_header_len = reader.num_read() - variable_header_start;
+            let data_len = (packet_len as usize)
+                .checked_sub(variable_header_len)
+                .ok_or(Error::InvalidPacket("PUBLISH remaining length is too short for headers"))?;
+
+            Packet::Publish {
+                topic,
+                packet_id,
+                qos,
+                retain,
+                dup,
+                data_len,
+            }
+        }
+        protocol::PACKET_TYPE_PUBACK => {
+            if packet_flags != 0 {
+                return Err(Error::InvalidPacket("PUBACK flags must be zero"));
+            }
+            if packet_len != 2 {
+                return Err(Error::InvalidPacket("PUBACK remaining length must be 2"));
+            }
+            let packet_id = PacketId::from(reader.read_u16()?);
+            Packet::PubAck { packet_id }
+        }
+        protocol::PACKET_TYPE_SUBACK => {
+            if packet_flags != 0 {
+                return Err(Error::InvalidPacket("SUBACK flags must be zero"));
+            }
+            if packet_len < 3 {
+                // Minimum: 2 bytes packet ID + 1 byte return code
+                return Err(Error::InvalidPacket("SUBACK remaining length must be at least 3"));
+            }
+            let packet_id = PacketId::from(reader.read_u16()?);
+            let return_code = reader.read_u8()?;
+            let success = return_code != protocol::SUBACK_FAILURE;
+            Packet::SubscribeAck { packet_id, success }
+        }
+        protocol::PACKET_TYPE_UNSUBACK => {
+            if packet_flags != 0 {
+                return Err(Error::InvalidPacket("UNSUBACK flags must be zero"));
+            }
+            if packet_len != 2 {
+                return Err(Error::InvalidPacket("UNSUBACK remaining length must be 2"));
+            }
+            let packet_id = PacketId::from(reader.read_u16()?);
+            Packet::UnsubscribeAck { packet_id }
+        }
+        protocol::PACKET_TYPE_CONNECT
+        | protocol::PACKET_TYPE_PUBREC
+        | protocol::PACKET_TYPE_PUBREL
+        | protocol::PACKET_TYPE_PUBCOMP
+        | protocol::PACKET_TYPE_DISCONNECT
+        | protocol::PACKET_TYPE_SUBSCRIBE
+        | protocol::PACKET_TYPE_UNSUBSCRIBE
+        | protocol::PACKET_TYPE_PINGREQ
+        | protocol::PACKET_TYPE_PINGRESP => {
+            return Err(Error::UnsupportedPacket {
+                packet_type,
+                packet_len,
+            });
+        }
+        _ => {
+            return Err(Error::UnknownPacket {
+                packet_type,
+                packet_len,
+            });
+        }
+    };
+
+    Ok((packet, reader.num_read()))
+}
+
+struct Reader<'a> {
+    buf: &'a [u8],
+    off: usize,
+}
+
+impl<'a> Reader<'a> {
+    fn new(buf: &'a [u8]) -> Self {
+        Self { buf, off: 0 }
+    }
+
+    fn remain(&self) -> usize {
+        self.buf.len() - self.off
+    }
+
+    fn remain_slice(&self) -> &'a [u8] {
+        &self.buf[self.off..]
+    }
+
+    fn num_read(&self) -> usize {
+        self.off
+    }
+
+    fn read_buf(&mut self, n: usize) -> Result<&'a [u8], Error> {
+        if self.remain() < n {
+            return Err(Error::NeedMoreData);
+        }
+        let v = &self.buf[self.off..self.off + n];
+        self.off += n;
+        Ok(v)
+    }
+
+    fn read_u8(&mut self) -> Result<u8, Error> {
+        let v = self.read_buf(1)?;
+        Ok(v[0])
+    }
+
+    fn read_u16(&mut self) -> Result<u16, Error> {
+        let v = self.read_buf(2)?;
+        Ok(u16::from_be_bytes([v[0], v[1]]))
+    }
+
+    fn read_len_prefix_buf(&mut self) -> Result<&'a [u8], Error> {
+        let l = self.read_u16()?;
+        let v = self.read_buf(usize::from(l))?;
+        Ok(v)
+    }
+
+    fn read_len_prefix_str(&mut self) -> Result<&'a str, Error> {
+        let v = self.read_len_prefix_buf()?;
+        Ok(str::from_utf8(v).unwrap())
+    }
+
+    fn read_varint(&mut self) -> Result<u32, Error> {
+        let (value, consumed) = varint::decode(self.remain_slice())?;
+        self.off += consumed;
+        Ok(value)
+    }
+}
diff --git a/src/mqtt/transport.rs b/src/mqtt/transport.rs
new file mode 100644
index 0000000..780b0ba
--- /dev/null
+++ b/src/mqtt/transport.rs
@@ -0,0 +1,39 @@
+use super::{field::Field, varint};
+
+pub trait Transport: embedded_io_async::Read + embedded_io_async::Write {}
+
+impl<T> Transport for T where T: embedded_io_async::Read + embedded_io_async::Write {}
+
+pub(crate) trait TransportExt: Transport {
+    async fn write_fields(&mut self, fields: &[Field]) -> Result<(), Self::Error>;
+}
+
+impl<T> TransportExt for T
+where
+    T: Transport,
+{
+    async fn write_fields(&mut self, fields: &[Field<'_>]) -> Result<(), Self::Error> {
+        for field in fields {
+            match field {
+                Field::U8(v) => self.write_all(&[*v]).await?,
+                Field::U16(v) => self.write_all(&u16::to_be_bytes(*v)).await?,
+                Field::VarInt(v) => {
+                    let (v_buf, v_len) = varint::encode(*v);
+                    self.write_all(&v_buf[..v_len]).await?;
+                }
+                Field::Buffer(v) => self.write_all(v).await?,
+                Field::LenPrefixedBuffer(v) => {
+                    self.write_all(&u16::to_be_bytes(u16::try_from(v.len()).unwrap()))
+                        .await?;
+                    self.write_all(v).await?;
+                }
+                Field::LenPrefixedString(v) => {
+                    self.write_all(&u16::to_be_bytes(u16::try_from(v.len()).unwrap()))
+                        .await?;
+                    self.write_all(v.as_bytes()).await?;
+                }
+            }
+        }
+        Ok(())
+    }
+}
diff --git a/src/mqtt/tx.rs b/src/mqtt/tx.rs
new file mode 100644
index 0000000..cdf1c75
--- /dev/null
+++ b/src/mqtt/tx.rs
@@ -0,0 +1,203 @@
+use super::{
+    PacketId,
+    field::{self, Field, FieldBuffer},
+    protocol,
+    qos::Qos,
+};
+
+pub struct Connect<'a> {
+    pub client_id: &'a str,
+    pub clean_session: bool,
+    pub username: Option<&'a str>,
+    pub password: Option<&'a [u8]>,
+    pub will_topic: Option<&'a str>,
+    pub will_payload: Option<&'a [u8]>,
+    pub will_retain: bool,
+    pub keepalive: Option<u16>,
+}
+
+pub fn connect<'a>(buffer: &mut FieldBuffer<'a>, connect: Connect<'a>) {
+    let mut flags = 0;
+    if connect.clean_session {
+        flags |= protocol::CONNECT_FLAG_CLEAN_SESSION;
+    }
+    if connect.username.is_some() {
+        flags |= protocol::CONNECT_FLAG_USERNAME;
+    }
+    if connect.password.is_some() {
+        flags |= protocol::CONNECT_FLAG_PASSWORD;
+    }
+    if connect.will_topic.is_some() {
+        flags |= protocol::CONNECT_FLAG_WILL_FLAG;
+    }
+    if connect.will_retain {
+        flags |= protocol::CONNECT_FLAG_WILL_RETAIN;
+    }
+
+    buffer.push(Field::U8(protocol::create_header_control(
+        protocol::PACKET_TYPE_CONNECT,
+        0,
+    )));
+    buffer.push(Field::VarInt(0));
+
+    buffer.push(Field::LenPrefixedString(protocol::PROTOCOL_NAME));
+    buffer.push(Field::U8(protocol::PROTOCOL_LEVEL_3_1_1));
+    buffer.push(Field::U8(flags));
+    buffer.push(Field::U16(connect.keepalive.unwrap_or(0)));
+    buffer.push(Field::LenPrefixedString(connect.client_id));
+    if let Some(will_topic) = connect.will_topic {
+        buffer.push(Field::LenPrefixedString(will_topic));
+        buffer.push(Field::LenPrefixedBuffer(
+            connect.will_payload.unwrap_or(&[]),
+        ));
+    }
+    if let Some(username) = connect.username {
+        buffer.push(Field::LenPrefixedString(username));
+    }
+    if let Some(password) = connect.password {
+        buffer.push(Field::LenPrefixedBuffer(password));
+    }
+
+    let message_size = field::fields_size(&buffer.as_slice()[2..]);
+    buffer.set(1, Field::VarInt(u32::try_from(message_size).unwrap()));
+}
+
+pub struct Publish<'a> {
+    pub topic: &'a str,
+    pub payload: &'a [u8],
+    pub qos: Qos,
+    pub retain: bool,
+    pub dup: bool,
+    pub packet_id: Option<PacketId>,
+}
+
+pub fn publish<'a>(buffer: &mut FieldBuffer<'a>, publish: Publish<'a>) {
+    let mut flags = 0u8;
+
+    // Set QoS bits (bits 1-2)
+    flags |= (publish.qos.to_u8() & 0x03) << 1;
+
+    // Set RETAIN flag (bit 0)
+    if publish.retain {
+        flags |= 0x01;
+    }
+
+    // Set DUP flag (bit 3)
+    if publish.dup {
+        flags |= 0x08;
+    }
+
+    buffer.push(Field::U8(protocol::create_header_control(
+        protocol::PACKET_TYPE_PUBLISH,
+        flags,
+    )));
+    buffer.push(Field::VarInt(0));
+
+    buffer.push(Field::LenPrefixedString(publish.topic));
+
+    // Packet ID is only present for QoS 1 and 2
+    if publish.qos.to_u8() > 0 {
+        // TODO: turn this into a warning
+        let packet_id = publish.packet_id.expect("packet_id required for QoS > 0");
+        buffer.push(Field::U16(packet_id.into()));
+    }
+
+    buffer.push(Field::Buffer(publish.payload));
+
+    let message_size = field::fields_size(&buffer.as_slice()[2..]);
+    buffer.set(1, Field::VarInt(u32::try_from(message_size).unwrap()));
+}
+
+pub struct Subscribe<'a> {
+    pub topic: &'a str,
+    pub qos: Qos,
+    pub packet_id: PacketId,
+}
+
+pub fn subscribe<'a>(buffer: &mut FieldBuffer<'a>, subscribe: Subscribe<'a>) {
+    // SUBSCRIBE packets have fixed header flags (reserved bits)
+    buffer.push(Field::U8(protocol::create_header_control(
+        protocol::PACKET_TYPE_SUBSCRIBE,
+        protocol::SUBSCRIBE_HEADER_FLAGS,
+    )));
+    buffer.push(Field::VarInt(0));
+
+    // Variable header: packet identifier
+    buffer.push(Field::U16(subscribe.packet_id.into()));
+
+    // Payload: topic filter + QoS
+    buffer.push(Field::LenPrefixedString(subscribe.topic));
+    buffer.push(Field::U8(subscribe.qos.to_u8()));
+
+    let message_size = field::fields_size(&buffer.as_slice()[2..]);
+    buffer.set(1, Field::VarInt(u32::try_from(message_size).unwrap()));
+}
+
+pub struct Unsubscribe<'a> {
+    pub topic: &'a str,
+    pub packet_id: PacketId,
+}
+
+pub fn unsubscribe<'a>(buffer: &mut FieldBuffer<'a>, unsubscribe: Unsubscribe<'a>) {
+    // UNSUBSCRIBE packets have fixed header flags (reserved bits)
+    buffer.push(Field::U8(protocol::create_header_control(
+        protocol::PACKET_TYPE_UNSUBSCRIBE,
+        protocol::UNSUBSCRIBE_HEADER_FLAGS,
+    )));
+    buffer.push(Field::VarInt(0));
+
+    // Variable header: packet identifier
+    buffer.push(Field::U16(unsubscribe.packet_id.into()));
+
+    // Payload: topic filter (no QoS)
+    buffer.push(Field::LenPrefixedString(unsubscribe.topic));
+
+    let message_size = field::fields_size(&buffer.as_slice()[2..]);
+    buffer.set(1, Field::VarInt(u32::try_from(message_size).unwrap()));
+}
+
+pub fn disconnect(buffer: &mut FieldBuffer) {
+    // DISCONNECT has no variable header or payload
+    buffer.push(Field::U8(protocol::create_header_control(
+        protocol::PACKET_TYPE_DISCONNECT,
+        0,
+    )));
+    buffer.push(Field::VarInt(0));
+}
+
+pub fn puback(buffer: &mut FieldBuffer, packet_id: PacketId) {
+    buffer.push(Field::U8(protocol::create_header_control(
+        protocol::PACKET_TYPE_PUBACK,
+        0,
+    )));
+    buffer.push(Field::VarInt(2)); // Remaining length is always 2 (packet ID)
+    buffer.push(Field::U16(packet_id.into()));
+}
+
+pub fn pubrec(buffer: &mut FieldBuffer, packet_id: PacketId) {
+    buffer.push(Field::U8(protocol::create_header_control(
+        protocol::PACKET_TYPE_PUBREC,
+        0,
+    )));
+    buffer.push(Field::VarInt(2)); // Remaining length is always 2 (packet ID)
+    buffer.push(Field::U16(packet_id.into()));
+}
+
+pub fn pubrel(buffer: &mut FieldBuffer, packet_id: PacketId) {
+    buffer.push(Field::U8(protocol::create_header_control(
+        protocol::PACKET_TYPE_PUBREL,
+        protocol::PUBREL_HEADER_FLAGS,
+    )));
+    buffer.push(Field::VarInt(2)); // Remaining length is always 2 (packet ID)
+    buffer.push(Field::U16(packet_id.into()));
+}
+
+pub fn pubcomp(buffer: &mut FieldBuffer, packet_id: PacketId) {
+    buffer.push(Field::U8(protocol::create_header_control(
+        protocol::PACKET_TYPE_PUBCOMP,
+        0,
+    )));
+    buffer.push(Field::VarInt(2)); // Remaining length is always 2 (packet ID)
+    buffer.push(Field::U16(packet_id.into()));
+}
+
diff --git a/src/mqtt/varint.rs b/src/mqtt/varint.rs
new file mode 100644
index 0000000..63bdd06
--- /dev/null
+++ b/src/mqtt/varint.rs
@@ -0,0 +1,69 @@
+#[derive(Debug)]
+pub enum Error {
+    NeedMoreData,
+    InvalidVarInt,
+}
+
+impl core::fmt::Display for Error {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        match self {
+            Error::NeedMoreData => f.write_str("NeedMoreData"),
+            Error::InvalidVarInt => f.write_str("InvalidVarInt"),
+        }
+    }
+}
+
+impl core::error::Error for Error {}
+
+pub fn encode(mut v: u32) -> ([u8; 4], usize) {
+    let mut encoded = [0u8; 4];
+    let mut count = 0;
+
+    loop {
+        let mut byte = (v % 128) as u8;
+        v /= 128;
+
+        if v > 0 {
+            byte |= 0x80; // Set continuation bit
+        }
+
+        encoded[count] = byte;
+        count += 1;
+
+        if v == 0 {
+            break;
+        }
+    }
+
+    (encoded, count)
+}
+
+pub fn decode(buf: &[u8]) -> Result<(u32, usize), Error> {
+    let mut value = 0u32;
+
+    let v = buf.get(0).ok_or(Error::NeedMoreData)?;
+    value |= ((v & 0x7F) as u32) << 0;
+    if v & 0x80 == 0 {
+        return Ok((value, 1));
+    }
+
+    let v = buf.get(1).ok_or(Error::NeedMoreData)?;
+    value |= ((v & 0x7F) as u32) << 7;
+    if v & 0x80 == 0 {
+        return Ok((value, 2));
+    }
+
+    let v = buf.get(2).ok_or(Error::NeedMoreData)?;
+    value |= ((v & 0x7F) as u32) << 14;
+    if v & 0x80 == 0 {
+        return Ok((value, 3));
+    }
+
+    let v = buf.get(3).ok_or(Error::NeedMoreData)?;
+    value |= ((v & 0x7F) as u32) << 21;
+    if v & 0x80 != 0 {
+        return Err(Error::InvalidVarInt);
+    }
+
+    Ok((value, 4))
+}