diff --git a/src/common/p25/P25Utils.cpp b/src/common/p25/P25Utils.cpp
index 548c98d2..a9d57b36 100644
--- a/src/common/p25/P25Utils.cpp
+++ b/src/common/p25/P25Utils.cpp
@@ -191,9 +191,9 @@ uint32_t P25Utils::encode(const uint8_t* in, uint8_t* out, uint32_t start, uint3
     return n;
 }
 
-/* Encode bit interleaving. */
+/* Encode bit interleaving for a given length. */
 
-uint32_t P25Utils::encode(const uint8_t* in, uint8_t* out, uint32_t length)
+uint32_t P25Utils::encodeByLength(const uint8_t* in, uint8_t* out, uint32_t length)
 {
     assert(in != nullptr);
     assert(out != nullptr);
@@ -207,17 +207,14 @@ uint32_t P25Utils::encode(const uint8_t* in, uint8_t* out, uint32_t length)
     while (n < length) {
         if (pos == ss0Pos) {
             ss0Pos += P25_SS_INCREMENT;
-
         }
         else if (pos == ss1Pos) {
             ss1Pos += P25_SS_INCREMENT;
-
         }
         else {
             bool b = READ_BIT(in, n);
             WRITE_BIT(out, pos, b);
             n++;
-
         }
         pos++;
 
diff --git a/src/common/p25/P25Utils.h b/src/common/p25/P25Utils.h
index bba98a28..bc030a43 100644
--- a/src/common/p25/P25Utils.h
+++ b/src/common/p25/P25Utils.h
@@ -188,7 +188,7 @@ namespace p25
          * @param length 
          * @returns uint32_t 
          */
-        static uint32_t encode(const uint8_t* in, uint8_t* out, uint32_t length);
+        static uint32_t encodeByLength(const uint8_t* in, uint8_t* out, uint32_t length);
 
         /**
          * @brief Compare two datasets for the given length.
diff --git a/src/host/modem/ModemV24.cpp b/src/host/modem/ModemV24.cpp
index 36cf66a5..bea2a2fe 100644
--- a/src/host/modem/ModemV24.cpp
+++ b/src/host/modem/ModemV24.cpp
@@ -754,15 +754,15 @@ void ModemV24::convertToAir(const uint8_t *data, uint32_t length)
                 ::memset(buffer, 0x00U, P25_PDU_FRAME_LENGTH_BYTES + 2U);
 
                 // add the data
-                uint32_t newBitLength = P25Utils::encode(data, buffer + 2U, bitLength);
+                uint32_t newBitLength = P25Utils::encodeByLength(data, buffer + 2U, bitLength);
                 uint32_t newByteLength = newBitLength / 8U;
                 if ((newBitLength % 8U) > 0U)
                     newByteLength++;
 
-                // regenerate Sync
+                // generate Sync
                 Sync::addP25Sync(buffer + 2U);
 
-                // regenerate NID
+                // generate NID
                 m_nid->encode(buffer + 2U, DUID::PDU);
 
                 // add status bits
diff --git a/src/host/p25/packet/Data.cpp b/src/host/p25/packet/Data.cpp
index f91902ad..30071bca 100644
--- a/src/host/p25/packet/Data.cpp
+++ b/src/host/p25/packet/Data.cpp
@@ -96,6 +96,8 @@ bool Data::process(uint8_t* data, uint32_t len)
             m_rfPduUserDataLength = 0U;
         }
 
+        //Utils::dump(1U, "Raw PDU ISP", data, len);
+
         uint32_t start = m_rfPDUCount * P25_PDU_FRAME_LENGTH_BITS;
 
         uint8_t buffer[P25_PDU_FRAME_LENGTH_BYTES];
@@ -815,6 +817,9 @@ void Data::writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress,
     m_p25->writeRF_TDU(true, imm);
 
     uint32_t bitLength = ((dataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS;
+    if (dataHeader.getPadLength() > 0U)
+        bitLength += (dataHeader.getPadLength() * 8U);
+
     uint32_t offset = P25_PREAMBLE_LENGTH_BITS;
 
     UInt8Array __data = std::make_unique<uint8_t[]>((bitLength / 8U) + 1U);
@@ -1392,7 +1397,7 @@ void Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm, bool ac
     assert(pdu != nullptr);
     assert(bitLength > 0U);
 
-    m_p25->writeRF_Preamble();
+    m_p25->writeRF_TDU(true, imm);
 
     if (!ackRetry) {
         if (m_retryPDUData != nullptr)
@@ -1416,21 +1421,23 @@ void Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm, bool ac
     ::memset(data, 0x00U, P25_PDU_FRAME_LENGTH_BYTES + 2U);
 
     // add the data
-    uint32_t newBitLength = P25Utils::encode(pdu, data + 2U, bitLength);
+    uint32_t newBitLength = P25Utils::encodeByLength(pdu, data + 2U, bitLength);
     uint32_t newByteLength = newBitLength / 8U;
     if ((newBitLength % 8U) > 0U)
         newByteLength++;
 
-    // regenerate Sync
+    // generate Sync
     Sync::addP25Sync(data + 2U);
 
-    // regenerate NID
+    // generate NID
     m_p25->m_nid.encode(data + 2U, DUID::PDU);
 
     // add status bits
     P25Utils::addStatusBits(data + 2U, newBitLength, m_inbound, true);
     P25Utils::setStatusBitsStartIdle(data + 2U);
 
+    //Utils::dump("Raw PDU OSP", data, newByteLength + 2U);
+
     if (m_p25->m_duplex) {
         data[0U] = modem::TAG_DATA;
         data[1U] = 0x00U;
@@ -1446,6 +1453,9 @@ void Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool imm, bool ac
 void Data::writeNet_PDU_Buffered()
 {
     uint32_t bitLength = ((m_netDataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS;
+    if (m_netDataHeader.getPadLength() > 0U)
+        bitLength += (m_netDataHeader.getPadLength() * 8U);
+
     uint32_t offset = P25_PREAMBLE_LENGTH_BITS;
 
     UInt8Array __data = std::make_unique<uint8_t[]>((bitLength / 8U) + 1U);
@@ -1537,6 +1547,9 @@ void Data::writeNet_PDU_Buffered()
 void Data::writeRF_PDU_Buffered()
 {
     uint32_t bitLength = ((m_rfDataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS;
+    if (m_rfDataHeader.getPadLength() > 0U)
+        bitLength += (m_rfDataHeader.getPadLength() * 8U);
+
     uint32_t offset = P25_PREAMBLE_LENGTH_BITS;
 
     UInt8Array __data = std::make_unique<uint8_t[]>((bitLength / 8U) + 1U);