diff --git a/p25/DataPacket.cpp b/p25/DataPacket.cpp index 305a5db1..00884274 100644 --- a/p25/DataPacket.cpp +++ b/p25/DataPacket.cpp @@ -310,11 +310,7 @@ bool DataPacket::process(uint8_t* data, uint32_t len) } for (uint32_t i = 0; i < blocksToFollow; i++) { - uint8_t data[P25_TSBK_FEC_LENGTH_BYTES + 2U]; - ::memset(data, 0x00U, P25_TSBK_FEC_LENGTH_BYTES + 2U); - - uint32_t len = m_rfData[i].getData(data + 2U); - m_p25->m_trunk->process(data, len, true); + m_p25->m_trunk->processMBT(m_rfDataHeader, m_rfData); } } break; diff --git a/p25/TrunkPacket.cpp b/p25/TrunkPacket.cpp index 3924c180..3652fbc6 100644 --- a/p25/TrunkPacket.cpp +++ b/p25/TrunkPacket.cpp @@ -34,6 +34,7 @@ #include "Utils.h" using namespace p25; +using namespace p25::data; #include #include @@ -189,7 +190,10 @@ bool TrunkPacket::process(uint8_t* data, uint32_t len, bool blockData) m_p25->m_queue.clear(); - resetRF(); + // special case to avoid resetting the TSBK state and handle MBTs (this is a horrible hack) + if (len != 1U && !blockData) { + resetRF(); + } resetNet(); bool ret = m_rfTSBK.decode(data + 2U); @@ -750,6 +754,29 @@ bool TrunkPacket::processNetwork(uint8_t* data, uint32_t len, lc::LC& control, d return true; } +/// +/// Helper used to process AMBTs from PDU data. +/// +/// +/// +bool TrunkPacket::processMBT(DataHeader dataHeader, DataBlock* blocks) +{ + uint8_t data[1U]; + ::memset(data, 0x00U, 1U); + + bool ret = true; + for (uint32_t i = 0; i < dataHeader.getBlocksToFollow(); i++) { + bool decodeRet = m_rfTSBK.decodeMBT(dataHeader, blocks[i]); + if (decodeRet) { + process(data, 0U, true); + } else { + ret = false; + } + } + + return ret; +} + /// /// Helper to write P25 adjacent site information to the network. /// diff --git a/p25/TrunkPacket.h b/p25/TrunkPacket.h index 8d21e4ff..a0062798 100644 --- a/p25/TrunkPacket.h +++ b/p25/TrunkPacket.h @@ -27,6 +27,8 @@ #define __P25_TRUNK_PACKET_H__ #include "Defines.h" +#include "p25/data/DataHeader.h" +#include "p25/data/DataBlock.h" #include "p25/lc/TSBK.h" #include "p25/lc/TDULC.h" #include "p25/Control.h" @@ -65,6 +67,9 @@ namespace p25 /// Process a data frame from the network. bool processNetwork(uint8_t* data, uint32_t len, lc::LC& control, data::LowSpeedData& lsd, uint8_t& duid); + /// Helper used to process AMBTs from PDU data. + bool processMBT(data::DataHeader dataHeader, data::DataBlock* blocks); + /// Helper to write P25 adjacent site information to the network. void writeAdjSSNetwork(); diff --git a/p25/data/DataBlock.cpp b/p25/data/DataBlock.cpp index e02840ae..59289de6 100644 --- a/p25/data/DataBlock.cpp +++ b/p25/data/DataBlock.cpp @@ -82,65 +82,76 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader header) m_headerSap = header.getSAP(); if (m_fmt == PDU_FMT_CONFIRMED) { - m_confirmed = true; - bool valid = m_trellis.decode34(data, buffer); - if (!valid) { - return false; - } + // decode 3/4 rate Trellis + try { + m_confirmed = true; + bool valid = m_trellis.decode34(data, buffer); + if (!valid) { + return false; + } - // determine the number of user data bytes - uint32_t count = P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; - if (m_serialNo == (header.getBlocksToFollow() - 1) || - (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U)) - count = P25_PDU_CONFIRMED_DATA_LENGTH_BYTES - 4U; + // determine the number of user data bytes + uint32_t count = P25_PDU_CONFIRMED_DATA_LENGTH_BYTES; + if (m_serialNo == (header.getBlocksToFollow() - 1) || + (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U)) + count = P25_PDU_CONFIRMED_DATA_LENGTH_BYTES - 4U; - // Utils::dump(1U, "PDU Confirmed Data Block", buffer, P25_PDU_CONFIRMED_LENGTH_BYTES); + // Utils::dump(1U, "PDU Confirmed Data Block", buffer, P25_PDU_CONFIRMED_LENGTH_BYTES); - m_serialNo = (buffer[0] & 0xFEU) >> 1; // Confirmed Data Serial No. - uint16_t crc = ((buffer[0] & 0x01U) << 8) + buffer[1]; // CRC-9 Check Sum + m_serialNo = (buffer[0] & 0xFEU) >> 1; // Confirmed Data Serial No. + uint16_t crc = ((buffer[0] & 0x01U) << 8) + buffer[1]; // CRC-9 Check Sum - ::memset(m_data, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); + ::memset(m_data, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); - // if this is extended addressing and the first block decode the SAP and LLId - if (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U) { - m_sap = buffer[5U] & 0x3FU; // Service Access Point - m_llId = (buffer[2U] << 16) + (buffer[3U] << 8) + buffer[4U]; // Logical Link ID + // if this is extended addressing and the first block decode the SAP and LLId + if (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U) { + m_sap = buffer[5U] & 0x3FU; // Service Access Point + m_llId = (buffer[2U] << 16) + (buffer[3U] << 8) + buffer[4U]; // Logical Link ID - // re-copy buffer to remove SAP and llId - for (uint32_t i = 6U; i < count; i++) { - m_data[i - 6U] = buffer[i]; // Payload Data + // re-copy buffer to remove SAP and llId + for (uint32_t i = 6U; i < count; i++) { + m_data[i - 6U] = buffer[i]; // Payload Data + } } - } - else { - for (uint32_t i = 2U; i < count; i++) { - m_data[i - 2U] = buffer[i]; // Payload Data + else { + for (uint32_t i = 2U; i < count; i++) { + m_data[i - 2U] = buffer[i]; // Payload Data + } } - } - // Utils::dump(1U, "PDU Data", m_data, count); + // Utils::dump(1U, "PDU Data", m_data, count); - // compute CRC-9 for the packet - uint16_t computedCRC = confirmedCRC9(buffer); + // compute CRC-9 for the packet + uint16_t computedCRC = confirmedCRC9(buffer); - if (crc != computedCRC) { - LogWarning(LOG_P25, "P25_DUID_PDU, fmt = $%02X, invalid crc = $%04X != $%04X (computed)", m_fmt, crc, computedCRC); + if (crc != computedCRC) { + LogWarning(LOG_P25, "P25_DUID_PDU, fmt = $%02X, invalid crc = $%04X != $%04X (computed)", m_fmt, crc, computedCRC); + } } - } - else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP)) { - m_confirmed = false; - bool valid = m_trellis.decode12(data, buffer); - if (!valid) { + catch (...) { + Utils::dump(2U, "P25, decoding excepted with input data", data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES); return false; } + } + else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP) || (m_fmt == PDU_FMT_AMBT)) { + // decode 3/4 rate Trellis + try { + m_confirmed = false; + bool valid = m_trellis.decode12(data, buffer); + if (!valid) { + return false; + } - // Utils::dump(1U, "PDU Unconfirmed Data Block", buffer, P25_PDU_UNCONFIRMED_LENGTH_BYTES); + // Utils::dump(1U, "PDU Unconfirmed Data Block", buffer, P25_PDU_UNCONFIRMED_LENGTH_BYTES); - for (uint32_t i = 0U; i < P25_PDU_UNCONFIRMED_LENGTH_BYTES; i++) { - m_data[i] = buffer[i]; // Payload Data + for (uint32_t i = 0U; i < P25_PDU_UNCONFIRMED_LENGTH_BYTES; i++) { + m_data[i] = buffer[i]; // Payload Data + } + } + catch (...) { + Utils::dump(2U, "P25, decoding excepted with input data", data, P25_PDU_UNCONFIRMED_LENGTH_BYTES); + return false; } - } - else if (m_fmt == PDU_FMT_AMBT) { - // ignore AMBT this is handled else where } else { LogError(LOG_P25, "unknown FMT value in P25_DUID_PDU, fmt = $%02X", m_fmt); @@ -186,7 +197,7 @@ void DataBlock::encode(uint8_t* data) m_trellis.encode34(buffer, data); } - else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP && !m_confirmed)) { + else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP && !m_confirmed) || (m_fmt == PDU_FMT_AMBT)) { uint8_t buffer[P25_PDU_UNCONFIRMED_LENGTH_BYTES]; ::memset(buffer, 0x00U, P25_PDU_UNCONFIRMED_LENGTH_BYTES); @@ -196,9 +207,6 @@ void DataBlock::encode(uint8_t* data) m_trellis.encode12(buffer, data); } - else if (m_fmt == PDU_FMT_AMBT) { - // ignore AMBT this is handled else where - } else { LogError(LOG_P25, "unknown FMT value in P25_DUID_PDU, fmt = $%02X", m_fmt); return; diff --git a/p25/lc/TSBK.cpp b/p25/lc/TSBK.cpp index 76037a93..59713f5d 100644 --- a/p25/lc/TSBK.cpp +++ b/p25/lc/TSBK.cpp @@ -176,6 +176,104 @@ TSBK& TSBK::operator=(const TSBK& data) return *this; } +/// +/// Decode a alternate trunking signalling block. +/// +/// +/// +/// True, if TSBK was decoded, otherwise false. +bool TSBK::decodeMBT(const data::DataHeader dataHeader, data::DataBlock block) +{ + // get the raw block data + uint8_t pduBlock[P25_PDU_UNCONFIRMED_LENGTH_BYTES]; + bool ret = block.decode(pduBlock, dataHeader); + if (!ret) { + m_decodedMBT = false; + LogError(LOG_P25, "TSBK::decode(), failed to decode PDU data block"); + return false; + } + + m_lco = dataHeader.getAMBTOpcode(); // LCO + m_lastBlock = true; + m_mfId = dataHeader.getMFId(); // Mfg Id. + + ulong64_t tsbkValue = 0U; + + if (dataHeader.getFormat() == PDU_FMT_AMBT) { + // combine bytes into rs value + tsbkValue = dataHeader.getAMBTField8(); + tsbkValue = (tsbkValue << 8) + dataHeader.getAMBTField9(); + tsbkValue = (tsbkValue << 8) + pduBlock[0U]; + tsbkValue = (tsbkValue << 8) + pduBlock[1U]; + tsbkValue = (tsbkValue << 8) + pduBlock[2U]; + tsbkValue = (tsbkValue << 8) + pduBlock[3U]; + tsbkValue = (tsbkValue << 8) + pduBlock[4U]; + tsbkValue = (tsbkValue << 8) + pduBlock[5U]; + } else { + // combine bytes into rs value + tsbkValue = pduBlock[0U]; + tsbkValue = (tsbkValue << 8) + pduBlock[1U]; + tsbkValue = (tsbkValue << 8) + pduBlock[2U]; + tsbkValue = (tsbkValue << 8) + pduBlock[3U]; + tsbkValue = (tsbkValue << 8) + pduBlock[4U]; + tsbkValue = (tsbkValue << 8) + pduBlock[5U]; + tsbkValue = (tsbkValue << 8) + pduBlock[6U]; + tsbkValue = (tsbkValue << 8) + pduBlock[7U]; + } + + // Motorola P25 vendor opcodes + if (m_mfId == P25_MFG_MOT) { + switch (m_lco) { + case TSBK_IOSP_GRP_VCH: + case TSBK_IOSP_UU_VCH: + case TSBK_IOSP_UU_ANS: + case TSBK_IOSP_TELE_INT_ANS: + case TSBK_IOSP_STS_UPDT: + case TSBK_IOSP_STS_Q: + case TSBK_IOSP_MSG_UPDT: + case TSBK_IOSP_CALL_ALRT: + case TSBK_IOSP_ACK_RSP: + case TSBK_IOSP_GRP_AFF: + case TSBK_IOSP_U_REG: + case TSBK_ISP_CAN_SRV_REQ: + case TSBK_ISP_GRP_AFF_Q_RSP: + case TSBK_OSP_DENY_RSP: + case TSBK_OSP_QUE_RSP: + case TSBK_ISP_U_DEREG_REQ: + case TSBK_OSP_U_DEREG_ACK: + case TSBK_ISP_LOC_REG_REQ: + m_mfId = P25_MFG_STANDARD; + break; + default: + LogError(LOG_P25, "TSBK::decodeMBT(), unknown TSBK LCO value, mfId = $%02X, lco = $%02X", m_mfId, m_lco); + break; + } + + if (m_mfId == P25_MFG_MOT) { + return true; + } + else { + m_mfId = dataHeader.getMFId(); + } + } + + // standard P25 reference opcodes + switch (m_lco) { + case TSBK_IOSP_GRP_AFF: + m_netId = (uint32_t)((tsbkValue >> 44) & 0xFFFFFU); // Network ID + m_sysId = (uint32_t)((tsbkValue >> 32) & 0xFFFU); // System ID + m_dstId = (uint32_t)((tsbkValue >> 24) & 0xFFFFU); // Talkgroup Address + m_srcId = dataHeader.getLLId(); // Source Radio Address + break; + default: + LogError(LOG_P25, "TSBK::decodeMBT(), unknown TSBK LCO value, mfId = $%02X, lco = $%02X", m_mfId, m_lco); + break; + } + + m_decodedMBT = true; + return true; +} + /// /// Decode a trunking signalling block. /// @@ -183,6 +281,11 @@ TSBK& TSBK::operator=(const TSBK& data) /// True, if TSBK was decoded, otherwise false. bool TSBK::decode(const uint8_t* data) { + if (!m_decodedMBT) { + m_decodedMBT = false; + return true; + } + assert(data != NULL); // deinterleave @@ -1002,6 +1105,7 @@ TSBK::TSBK(SiteData siteData) : m_sndcpAutoAccess(true), m_sndcpReqAccess(false), m_sndcpDAC(1U), + m_decodedMBT(false), m_siteCallsign(NULL) { m_siteCallsign = new uint8_t[P25_MOT_CALLSIGN_LENGTH_BYTES]; diff --git a/p25/lc/TSBK.h b/p25/lc/TSBK.h index bf4f03c1..5862f7dd 100644 --- a/p25/lc/TSBK.h +++ b/p25/lc/TSBK.h @@ -27,6 +27,8 @@ #define __P25_LC__TSBK_H__ #include "Defines.h" +#include "p25/data/DataHeader.h" +#include "p25/data/DataBlock.h" #include "p25/edac/Trellis.h" #include "p25/lc/LC.h" #include "p25/lc/TDULC.h" @@ -75,6 +77,9 @@ namespace p25 /// Equals operator. TSBK& operator=(const TSBK& data); + /// Decode a alternate trunking signalling block. + bool decodeMBT(const data::DataHeader dataHeader, data::DataBlock block); + /// Decode a trunking signalling block. bool decode(const uint8_t* data); /// Encode a trunking signalling block. @@ -212,6 +217,8 @@ namespace p25 bool m_sndcpReqAccess; uint16_t m_sndcpDAC; + bool m_decodedMBT; + /** Local Site data */ uint8_t* m_siteCallsign; };