diff --git a/P25Control.cpp b/P25Control.cpp
index dae804b..bceeb5f 100644
--- a/P25Control.cpp
+++ b/P25Control.cpp
@@ -414,6 +414,8 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len)
 				m_rfDataFrames    = header[6U] & 0x7FU;
 
 				m_display->writeP25("DATA", false, llId, "R");
+				m_display->writeP25RSSI(m_rssi);
+
 				LogMessage("P25, received RF data transmission to %u, %u blocks", llId, m_rfDataFrames);
 			} else {
 				m_rfPDUCount   = 0U;
@@ -457,7 +459,10 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len)
 				unsigned char pdu[1024U];
 
 				// Add the data
-				CP25Utils::encode(m_rfPDU, pdu + 2U, 0U, bitLength);
+				unsigned int newBitLength = CP25Utils::encode(m_rfPDU, pdu + 2U, bitLength);
+				unsigned int newByteLength = newBitLength / 8U;
+				if ((newBitLength % 8U) > 0U)
+					newByteLength++;
 
 				// Regenerate Sync
 				CSync::addP25Sync(pdu + 2U);
@@ -466,16 +471,12 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len)
 				m_nid.encode(pdu + 2U, P25_DUID_PDU);
 
 				// Add busy bits
-				addBusyBits(pdu + 2U, bitLength, false, true);
+				addBusyBits(pdu + 2U, newBitLength, false, true);
 
 				if (m_duplex) {
-					unsigned int byteLength = bitLength / 8U;
-					if ((bitLength % 8U) > 0U)
-						byteLength++;
-
 					pdu[0U] = TAG_DATA;
 					pdu[1U] = 0x00U;
-					writeQueueRF(pdu, byteLength + 2U);
+					writeQueueRF(pdu, newByteLength + 2U);
 				}
 
 				LogMessage("P25, ended RF data transmission");
diff --git a/P25Utils.cpp b/P25Utils.cpp
index 3575d60..70f5c29 100644
--- a/P25Utils.cpp
+++ b/P25Utils.cpp
@@ -87,6 +87,33 @@ unsigned int CP25Utils::encode(const unsigned char* in, unsigned char* out, unsi
 	return n;
 }
 
+unsigned int CP25Utils::encode(const unsigned char* in, unsigned char* out, unsigned int length)
+{
+	assert(in != NULL);
+	assert(out != NULL);
+
+	// Move the SSx positions to the range needed
+	unsigned int ss0Pos = P25_SS0_START;
+	unsigned int ss1Pos = P25_SS1_START;
+
+	unsigned int n = 0U;
+	unsigned int pos = 0U;
+	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++;
+	}
+
+	return pos;
+}
+
 unsigned int CP25Utils::compare(const unsigned char* data1, const unsigned char* data2, unsigned int length)
 {
 	assert(data1 != NULL);
diff --git a/P25Utils.h b/P25Utils.h
index 142d407..75960e5 100644
--- a/P25Utils.h
+++ b/P25Utils.h
@@ -22,6 +22,7 @@
 class CP25Utils {
 public:
 	static unsigned int encode(const unsigned char* in, unsigned char* out, unsigned int start, unsigned int stop);
+	static unsigned int encode(const unsigned char* in, unsigned char* out, unsigned int length);
 
 	static unsigned int decode(const unsigned char* in, unsigned char* out, unsigned int start, unsigned int stop);