diff --git a/P25Control.cpp b/P25Control.cpp index c9bc7f4..2f87dd4 100644 --- a/P25Control.cpp +++ b/P25Control.cpp @@ -418,6 +418,42 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) unsigned char data[P25_TSDU_FRAME_LENGTH_BYTES + 2U]; switch (m_rfData.getLCF()) { + case P25_LCF_GROUP: + // Handle Group Voice Channel User - respond with Group Voice Channel Grant for talk permit + LogMessage("P25, received RF TSDU transmission, GROUP VOICE CH USER from %s to TG %u", source.c_str(), dstId); + ::memset(data + 2U, 0x00U, P25_TSDU_FRAME_LENGTH_BYTES); + + // Regenerate Sync + CSync::addP25Sync(data + 2U); + + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_TSDU); + + // Build a Group Voice Channel Grant response + // Save original LCF and set Grant LCF for encoding + unsigned char originalLcf = m_rfData.getLCF(); + m_rfData.setLCF(P25_LCF_GROUP); + m_rfData.setServiceType(0x00U); // Service options: 0x00 = routine priority, no emergency + + // Encode TSDU with Grant response + m_rfData.encodeTSDU(data + 2U); + + // Restore original LCF + m_rfData.setLCF(originalLcf); + + // Add busy bits - outbound busy (channel granted) + addBusyBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, true, false); + + // Set first busy bits to 1,0 (outbound busy) + setBusyBits(data + 2U, P25_SS0_START, true, false); + + if (m_duplex) { + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeQueueRF(data, P25_TSDU_FRAME_LENGTH_BYTES + 2U); + } + break; case P25_LCF_TSBK_CALL_ALERT: LogMessage("P25, received RF TSDU transmission, CALL ALERT from %s to %s%u", source.c_str(), grp ? "TG " : "", dstId); ::memset(data + 2U, 0x00U, P25_TSDU_FRAME_LENGTH_BYTES); diff --git a/P25Data.cpp b/P25Data.cpp index 12e190e..d4fa563 100644 --- a/P25Data.cpp +++ b/P25Data.cpp @@ -387,6 +387,11 @@ bool CP25Data::decodeTSDU(const unsigned char* data) tsbkValue = (tsbkValue << 8) + tsbk[9U]; switch (m_lcf) { + case P25_LCF_GROUP: + m_emergency = ((tsbkValue >> 63) & 0x01U) == 0x01U; // Emergency flag (bit 63) + m_dstId = (unsigned int)((tsbkValue >> 24) & 0xFFFFU); // Group Address (16 bits) + m_srcId = (unsigned int)(tsbkValue & 0xFFFFFFU); // Source Radio Address (24 bits) + break; case P25_LCF_TSBK_CALL_ALERT: m_dstId = (unsigned int)((tsbkValue >> 24) & 0xFFFFFFU); // Target Radio Address m_srcId = (unsigned int)(tsbkValue & 0xFFFFFFU); // Source Radio Address @@ -418,6 +423,11 @@ void CP25Data::encodeTSDU(unsigned char* data) tsbk[1U] = m_mfId; switch (m_lcf) { + case P25_LCF_GROUP: + tsbkValue = (unsigned long long)(m_serviceType & 0xFFU); // Service Options (8 bits) + tsbkValue = (tsbkValue << 16) + 0U; // Channel ID/Number (16 bits) - 0 for conventional + tsbkValue = (tsbkValue << 16) + (m_dstId & 0xFFFFU); // Group Address (16 bits) + tsbkValue = (tsbkValue << 24) + (m_srcId & 0xFFFFFFU); // Source Radio Address (24 bits) case P25_LCF_TSBK_CALL_ALERT: tsbkValue = 0U; tsbkValue = (tsbkValue << 16) + 0U;