diff --git a/P25Control.cpp b/P25Control.cpp index bb63230..007a681 100644 --- a/P25Control.cpp +++ b/P25Control.cpp @@ -350,86 +350,85 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) return true; } - else if (duid == P25_DUID_TSDU) { - if (m_rfState != RS_RF_DATA) { - m_rfPDUCount = 0U; - m_rfPDUBits = 0U; - m_rfState = RS_RF_DATA; - m_rfDataFrames = 0U; - } - - bool ret = m_rfData.decodeTSDU(data + 2U); - if (!ret) { + } else if (duid == P25_DUID_TSDU) { + if (m_rfState != RS_RF_DATA) { + m_rfPDUCount = 0U; + m_rfPDUBits = 0U; + m_rfState = RS_RF_DATA; + m_rfDataFrames = 0U; + } + + bool ret = m_rfData.decodeTSDU(data + 2U); + if (!ret) { m_lastDUID = duid; - return false; - } + return false; + } + + unsigned int srcId = m_rfData.getSrcId(); + unsigned int dstId = m_rfData.getDstId(); + + unsigned char data[P25_TSDU_FRAME_LENGTH_BYTES + 2U]; + + switch (m_rfData.getLCF()) { + case P25_LCF_TSBK_CALL_ALERT: + LogMessage("P25, received RF TSDU transmission, CALL ALERT from %u to %u", srcId, dstId); + ::memset(data + 2U, 0x00U, P25_TSDU_FRAME_LENGTH_BYTES); + + // Regenerate Sync + CSync::addP25Sync(data + 2U); - unsigned int srcId = m_rfData.getSrcId(); - unsigned int dstId = m_rfData.getDstId(); + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_TSDU); - unsigned char data[P25_TSDU_FRAME_LENGTH_BYTES + 2U]; + // Regenerate TDULC Data + m_rfData.encodeTSDU(data + 2U); - switch (m_rfData.getLCF()) { - case P25_LCF_TSBK_CALL_ALERT: - LogMessage("P25, received RF TSDU transmission, CALL ALERT from %u to %u", srcId, dstId); - ::memset(data + 2U, 0x00U, P25_TSDU_FRAME_LENGTH_BYTES); + // Add busy bits + addBusyBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, true, false); + + // Set first busy bits to 1,1 + setBusyBits(data + 2U, P25_SS0_START, true, true); + + if (m_duplex) { + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeQueueRF(data, P25_TSDU_FRAME_LENGTH_BYTES + 2U); + } + break; + case P25_LCF_TSBK_ACK_RSP_FNE: + LogMessage("P25, received RF TSDU transmission, ACK RESPONSE FNE from %u to %u", srcId, 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); + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_TSDU); - // Regenerate TDULC Data - m_rfData.encodeTSDU(data + 2U); + // Regenerate TDULC Data + m_rfData.encodeTSDU(data + 2U); - // Add busy bits - addBusyBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, true, false); + // Add busy bits + addBusyBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, true, false); - // Set first busy bits to 1,1 - setBusyBits(data + 2U, P25_SS0_START, true, true); + // Set first busy bits to 1,1 + setBusyBits(data + 2U, P25_SS0_START, true, true); - if (m_duplex) { - data[0U] = TAG_DATA; - data[1U] = 0x00U; + if (m_duplex) { + data[0U] = TAG_DATA; + data[1U] = 0x00U; - writeQueueRF(data, P25_TSDU_FRAME_LENGTH_BYTES + 2U); - } - break; - case P25_LCF_TSBK_ACK_RSP_FNE: - LogMessage("P25, received RF TSDU transmission, ACK RESPONSE FNE from %u to %u", srcId, dstId); - ::memset(data + 2U, 0x00U, P25_TSDU_FRAME_LENGTH_BYTES); + writeQueueRF(data, P25_TSDU_FRAME_LENGTH_BYTES + 2U); + } + break; + default: + LogMessage("P25, recieved RF TSDU transmission, unhandled LCF $%02X", m_rfData.getLCF()); + break; + } - // Regenerate Sync - CSync::addP25Sync(data + 2U); - - // Regenerate NID - m_nid.encode(data + 2U, P25_DUID_TSDU); - - // Regenerate TDULC Data - m_rfData.encodeTSDU(data + 2U); - - // Add busy bits - addBusyBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, true, false); - - // Set first busy bits to 1,1 - setBusyBits(data + 2U, P25_SS0_START, true, true); - - if (m_duplex) { - data[0U] = TAG_DATA; - data[1U] = 0x00U; - - writeQueueRF(data, P25_TSDU_FRAME_LENGTH_BYTES + 2U); - } - break; - default: - LogMessage("P25, recieved RF TSDU transmission, unhandled LCF $%02X", m_rfData.getLCF()); - break; - } - - m_rfState = RS_RF_LISTENING; - return true; - } + m_rfState = RS_RF_LISTENING; + return true; } else if (duid == P25_DUID_TERM || duid == P25_DUID_TERM_LC) { if (m_rfState == RS_RF_AUDIO) { writeNetwork(m_rfLDU, m_lastDUID, true); diff --git a/P25Data.h b/P25Data.h index b2727ca..1b9eec9 100644 --- a/P25Data.h +++ b/P25Data.h @@ -35,8 +35,8 @@ public: void encodeLDU2(unsigned char* data); - bool decodeTSDU(const unsigned char* data); - void encodeTSDU(unsigned char* data); + bool decodeTSDU(const unsigned char* data); + void encodeTSDU(unsigned char* data); void setMI(const unsigned char* mi); void getMI(unsigned char* mi) const; @@ -62,8 +62,8 @@ public: void setDstId(unsigned int id); unsigned int getDstId() const; - void setServiceType(unsigned char type); - unsigned char getServiceType() const; + void setServiceType(unsigned char type); + unsigned char getServiceType() const; void reset(); @@ -76,9 +76,9 @@ private: bool m_emergency; unsigned int m_srcId; unsigned int m_dstId; - unsigned char m_serviceType; + unsigned char m_serviceType; CRS241213 m_rs241213; - CP25Trellis m_trellis; + CP25Trellis m_trellis; void decodeLDUHamming(const unsigned char* raw, unsigned char* data); void encodeLDUHamming(unsigned char* data, const unsigned char* raw);