more very very experimental PDU AMBT support;

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];
+                        m_p25->m_trunk->processMBT(m_rfDataHeader, m_rfData);
-                        ::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);
                     }
                 }
                 break;

p25/TrunkPacket.cpp

@@ -34,6 +34,7 @@
 #include "Utils.h"
 
 using namespace p25;
+using namespace p25::data;
 
 #include <cassert>
 #include <cstdio>
@@ -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;
 }
 
+/// <summary>
+/// Helper used to process AMBTs from PDU data.
+/// </summary>
+/// <param name="dataHeader"></param>
+/// <param name="dataBlock"></param>
+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;
+}
+
 /// <summary>
 /// Helper to write P25 adjacent site information to the network.
 /// </summary>

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
         /// <summary>Process a data frame from the network.</summary>
         bool processNetwork(uint8_t* data, uint32_t len, lc::LC& control, data::LowSpeedData& lsd, uint8_t& duid);
 
+        /// <summary>Helper used to process AMBTs from PDU data.</summary>
+        bool processMBT(data::DataHeader dataHeader, data::DataBlock* blocks);
+
         /// <summary>Helper to write P25 adjacent site information to the network.</summary>
         void writeAdjSSNetwork();
 

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;
+        // decode 3/4 rate Trellis
-        bool valid = m_trellis.decode34(data, buffer);
+        try {
-        if (!valid) {
+            m_confirmed = true;
-            return false;
+            bool valid = m_trellis.decode34(data, buffer);
-        }
+            if (!valid) {
+                return false;
+            }
 
-        // determine the number of user data bytes
+            // determine the number of user data bytes
-        uint32_t count = P25_PDU_CONFIRMED_DATA_LENGTH_BYTES;
+            uint32_t count = P25_PDU_CONFIRMED_DATA_LENGTH_BYTES;
-        if (m_serialNo == (header.getBlocksToFollow() - 1) ||
+            if (m_serialNo == (header.getBlocksToFollow() - 1) ||
-            (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U))
+                (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U))
-            count = P25_PDU_CONFIRMED_DATA_LENGTH_BYTES - 4U;
+                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.
+            m_serialNo = (buffer[0] & 0xFEU) >> 1;                                       // Confirmed Data Serial No.
-        uint16_t crc = ((buffer[0] & 0x01U) << 8) + buffer[1];                           // CRC-9 Check Sum
+            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 this is extended addressing and the first block decode the SAP and LLId
-        if (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U) {
+            if (m_headerSap == PDU_SAP_EXT_ADDR && m_serialNo == 0U) {
-            m_sap = buffer[5U] & 0x3FU;                                                 // Service Access Point
+                m_sap = buffer[5U] & 0x3FU;                                              // Service Access Point
-            m_llId = (buffer[2U] << 16) + (buffer[3U] << 8) + buffer[4U];               // Logical Link ID
+                m_llId = (buffer[2U] << 16) + (buffer[3U] << 8) + buffer[4U];            // Logical Link ID
 
-            // re-copy buffer to remove SAP and llId
+                // re-copy buffer to remove SAP and llId
-            for (uint32_t i = 6U; i < count; i++) {
+                for (uint32_t i = 6U; i < count; i++) {
-                m_data[i - 6U] = buffer[i];                                             // Payload Data
+                    m_data[i - 6U] = buffer[i];                                          // Payload Data
+                }
             }
-        }
+            else {
-        else {
+                for (uint32_t i = 2U; i < count; i++) {
-            for (uint32_t i = 2U; i < count; i++) {
+                    m_data[i - 2U] = buffer[i];                                          // Payload Data
-                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
+            // compute CRC-9 for the packet
-        uint16_t computedCRC = confirmedCRC9(buffer);
+            uint16_t computedCRC = confirmedCRC9(buffer);
 
-        if (crc != computedCRC) {
+            if (crc != computedCRC) {
-            LogWarning(LOG_P25, "P25_DUID_PDU, fmt = $%02X, invalid crc = $%04X != $%04X (computed)", m_fmt, crc, computedCRC);
+                LogWarning(LOG_P25, "P25_DUID_PDU, fmt = $%02X, invalid crc = $%04X != $%04X (computed)", m_fmt, crc, computedCRC);
+            }
         }
-    }
+        catch (...) {
-    else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP)) {
+            Utils::dump(2U, "P25, decoding excepted with input data", data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
-        m_confirmed = false;
-        bool valid = m_trellis.decode12(data, buffer);
-        if (!valid) {
             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++) {
+            for (uint32_t i = 0U; i < P25_PDU_UNCONFIRMED_LENGTH_BYTES; i++) {
-            m_data[i] = buffer[i];                                                       // Payload Data
+                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;

p25/lc/TSBK.cpp

@@ -176,6 +176,104 @@ TSBK& TSBK::operator=(const TSBK& data)
     return *this;
 }
 
+/// <summary>
+/// Decode a alternate trunking signalling block.
+/// </summary>
+/// <param name="dataHeader"></param>
+/// <param name="block"></param>
+/// <returns>True, if TSBK was decoded, otherwise false.</returns>
+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;
+}
+
 /// <summary>
 /// Decode a trunking signalling block.
 /// </summary>
@@ -183,6 +281,11 @@ TSBK& TSBK::operator=(const TSBK& data)
 /// <returns>True, if TSBK was decoded, otherwise false.</returns>
 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];

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
             /// <summary>Equals operator.</summary>
             TSBK& operator=(const TSBK& data);
 
+            /// <summary>Decode a alternate trunking signalling block.</summary>
+            bool decodeMBT(const data::DataHeader dataHeader, data::DataBlock block);
+
             /// <summary>Decode a trunking signalling block.</summary>
             bool decode(const uint8_t* data);
             /// <summary>Encode a trunking signalling block.</summary>
@@ -212,6 +217,8 @@ namespace p25
             bool m_sndcpReqAccess;
             uint16_t m_sndcpDAC;
 
+            bool m_decodedMBT;
+
             /** Local Site data */
             uint8_t* m_siteCallsign;
         };