code cleanup; further work on PDU CAI registration;

pull/12/head
Bryan Biedenkapp 4 years ago
parent e931d8d26e
commit 66a6201ddc

@ -235,7 +235,7 @@ bool DataPacket::process(uint8_t* data, uint32_t len)
writeNetworkRF(P25_DT_DATA, buffer, P25_PDU_FEC_LENGTH_BYTES);
}
else {
if (m_rfData[i].getConfirmed())
if (m_rfData[i].getFormat() == PDU_FMT_CONFIRMED)
LogWarning(LOG_RF, P25_PDU_STR ", unfixable PDU data (3/4 rate or CRC)");
else
LogWarning(LOG_RF, P25_PDU_STR ", unfixable PDU data (1/2 rate or CRC)");
@ -249,7 +249,7 @@ bool DataPacket::process(uint8_t* data, uint32_t len)
dataOffset += (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
}
if (m_dumpPDUData) {
if (m_dumpPDUData && m_rfDataBlockCnt > 0U) {
Utils::dump(1U, "PDU Packet", m_pduUserData, dataOffset);
}
@ -257,90 +257,102 @@ bool DataPacket::process(uint8_t* data, uint32_t len)
LogWarning(LOG_RF, P25_PDU_STR ", incomplete PDU (%d / %d blocks)", m_rfDataBlockCnt, blocksToFollow);
}
switch (m_rfDataHeader.getSAP()) {
case PDU_SAP_REG:
{
uint8_t regType = (m_pduUserData[0] >> 4) & 0x0F;
switch (regType) {
case PDU_REG_TYPE_REQ_CNCT:
// did we receive a response header?
if (m_rfDataHeader.getFormat() == PDU_FMT_RSP) {
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", response, fmt = $%02X, rspClass = $%02X, rspType = $%02X, rspStatus = $%02X",
m_rfDataHeader.getFormat(), m_rfDataHeader.getResponseClass(), m_rfDataHeader.getResponseType(), m_rfDataHeader.getResponseStatus());
}
}
else {
// handle standard P25 service access points
switch (m_rfDataHeader.getSAP()) {
case PDU_SAP_REG:
{
uint32_t llId = (m_pduUserData[1U] << 16) + (m_pduUserData[2U] << 8) + m_pduUserData[3U];
ulong64_t ipAddr = (m_pduUserData[8U] << 24) + (m_pduUserData[9U] << 16) +
(m_pduUserData[10U] << 8) + m_pduUserData[11U];
uint8_t regType = (m_pduUserData[0] >> 4) & 0x0F;
switch (regType) {
case PDU_REG_TYPE_REQ_CNCT:
{
uint32_t llId = (m_pduUserData[1U] << 16) + (m_pduUserData[2U] << 8) + m_pduUserData[3U];
ulong64_t ipAddr = (m_pduUserData[8U] << 24) + (m_pduUserData[9U] << 16) +
(m_pduUserData[10U] << 8) + m_pduUserData[11U];
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_REQ_CNCT (Registration Request Connect), llId = %u, ipAddr = %u", llId, ipAddr);
}
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_REQ_CNCT (Registration Request Connect), llId = %u, ipAddr = %u", llId, ipAddr);
}
if (!acl::AccessControl::validateSrcId(llId)) {
LogWarning(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_RSP_DENY (Registration Response Deny), llId = %u, ipAddr = %u", llId, ipAddr);
writeRF_PDU_Reg_Response(PDU_REG_TYPE_RSP_DENY, llId, ipAddr);
}
else {
if (!hasLLIdFNEReg(llId)) {
// update dynamic FNE registration table entry
m_fneRegTable[llId] = ipAddr;
writeRF_PDU_Ack_Response(PDU_ACK_CLASS_ACK, PDU_ACK_TYPE_ACK, llId);
if (!acl::AccessControl::validateSrcId(llId)) {
LogWarning(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_RSP_DENY (Registration Response Deny), llId = %u, ipAddr = %u", llId, ipAddr);
writeRF_PDU_Reg_Response(PDU_REG_TYPE_RSP_DENY, llId, ipAddr);
}
else {
if (!hasLLIdFNEReg(llId)) {
// update dynamic FNE registration table entry
m_fneRegTable[llId] = ipAddr;
}
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_RSP_ACCPT (Registration Response Accept), llId = %u, ipAddr = %u", llId, ipAddr);
}
writeRF_PDU_Reg_Response(PDU_REG_TYPE_RSP_ACCPT, llId, ipAddr);
}
}
break;
case PDU_REG_TYPE_REQ_DISCNCT:
{
uint32_t llId = (m_pduUserData[1U] << 16) + (m_pduUserData[2U] << 8) + m_pduUserData[3U];
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_RSP_ACCPT (Registration Response Accept), llId = %u, ipAddr = %u", llId, ipAddr);
LogMessage(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_REQ_DISCNCT (Registration Request Disconnect), llId = %u", llId);
}
writeRF_PDU_Reg_Response(PDU_REG_TYPE_RSP_ACCPT, llId, ipAddr);
if (hasLLIdFNEReg(llId)) {
// remove dynamic FNE registration table entry
try {
m_fneRegTable.at(llId);
m_fneRegTable.erase(llId);
}
catch (...) {
// stub
}
}
}
break;
default:
LogError(LOG_RF, "P25 unhandled PDU registration type, regType = $%02X", regType);
break;
}
}
break;
case PDU_REG_TYPE_REQ_DISCNCT:
case PDU_SAP_TRUNK_CTRL:
{
uint32_t llId = (m_pduUserData[1U] << 16) + (m_pduUserData[2U] << 8) + m_pduUserData[3U];
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", PDU_REG_TYPE_REQ_DISCNCT (Registration Request Disconnect), llId = %u", llId);
LogMessage(LOG_RF, P25_PDU_STR ", PDU_SAP_TRUNK_CTRL (Alternate MBT Packet), lco = $%02X, blocksToFollow = %u",
m_rfDataHeader.getAMBTOpcode(), m_rfDataHeader.getBlocksToFollow());
}
if (hasLLIdFNEReg(llId)) {
// remove dynamic FNE registration table entry
try {
m_fneRegTable.at(llId);
m_fneRegTable.erase(llId);
}
catch (...) {
// stub
}
for (uint32_t i = 0; i < blocksToFollow; i++) {
m_p25->m_trunk->processMBT(m_rfDataHeader, m_rfData);
}
}
break;
default:
LogError(LOG_RF, "P25 unhandled PDU registration type, regType = $%02X", regType);
break;
}
}
break;
case PDU_SAP_TRUNK_CTRL:
{
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", PDU_SAP_TRUNK_CTRL (Alternate MBT Packet), lco = $%02X, blocksToFollow = %u",
m_rfDataHeader.getAMBTOpcode(), m_rfDataHeader.getBlocksToFollow());
}
::ActivityLog("P25", true, "RF data transmission from %u to %u, %u blocks", m_rfDataHeader.getLLId(), m_rfDataHeader.getLLId(), m_rfDataHeader.getBlocksToFollow());
for (uint32_t i = 0; i < blocksToFollow; i++) {
m_p25->m_trunk->processMBT(m_rfDataHeader, m_rfData);
}
}
break;
default:
::ActivityLog("P25", true, "RF data transmission from %u to %u, %u blocks", m_rfDataHeader.getLLId(), m_rfDataHeader.getLLId(), m_rfDataHeader.getBlocksToFollow());
if (m_repeatPDU) {
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", repeating PDU, llId = %u", (m_rfUseSecondHeader) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId());
}
if (m_repeatPDU) {
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", repeating PDU, llId = %u", (m_rfUseSecondHeader) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId());
writeRF_PDU_Buffered(); // re-generate buffered PDU and send it on
}
writeRF_PDU_Buffered(); // re-generate buffered PDU and send it on
::ActivityLog("P25", true, "end of RF data transmission");
break;
}
::ActivityLog("P25", true, "end of RF data transmission");
break;
}
m_rfDataHeader.reset();
@ -351,7 +363,7 @@ bool DataPacket::process(uint8_t* data, uint32_t len)
m_rfPDUBits = 0U;
m_p25->m_rfState = m_prevRfState;
}
} // switch (m_rfDataHeader.getSAP())
}
return true;
@ -722,19 +734,11 @@ void DataPacket::writeRF_PDU_Reg_Response(uint8_t regType, uint32_t llId, ulong6
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
DataHeader rspHeader = DataHeader();
rspHeader.setFormat(PDU_FMT_RSP);
rspHeader.setFormat(PDU_FMT_CONFIRMED);
rspHeader.setAckNeeded(true);
rspHeader.setOutbound(true);
rspHeader.setClass(PDU_ACK_CLASS_ACK);
rspHeader.setType(PDU_ACK_TYPE_ACK);
rspHeader.setStatus(m_rfDataHeader.getNs());
rspHeader.setLLId(llId);
if (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR) {
rspHeader.setSrcLLId(P25_WUID_FNE);
rspHeader.setFullMessage(true);
}
else {
rspHeader.setFullMessage(false);
}
rspHeader.setSAP(PDU_SAP_REG);
rspHeader.setLLId(m_rfDataHeader.getLLId());
rspHeader.setBlocksToFollow(1U);
// Generate the PDU header and 1/2 rate Trellis
@ -743,29 +747,30 @@ void DataPacket::writeRF_PDU_Reg_Response(uint8_t regType, uint32_t llId, ulong6
offset += P25_PDU_FEC_LENGTH_BITS;
// build registration response data
::memset(block, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
uint8_t rspData[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
::memset(rspData, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
block[0U] = (regType << 4); // Registration Type & Options
block[1U] = (llId >> 16) & 0xFFU; // Logical Link ID
block[2U] = (llId >> 8) & 0xFFU;
block[3U] = (llId >> 0) & 0xFFU;
rspData[0U] = ((regType & 0x0FU) << 4); // Registration Type & Options
rspData[1U] = (llId >> 16) & 0xFFU; // Logical Link ID
rspData[2U] = (llId >> 8) & 0xFFU;
rspData[3U] = (llId >> 0) & 0xFFU;
if (regType == PDU_REG_TYPE_RSP_ACCPT) {
block[8U] = (ipAddr >> 24) & 0xFFU; // IP Address
block[9U] = (ipAddr >> 16) & 0xFFU;
block[10U] = (ipAddr >> 8) & 0xFFU;
block[11U] = (ipAddr >> 0) & 0xFFU;
rspData[8U] = (ipAddr >> 24) & 0xFFU; // IP Address
rspData[9U] = (ipAddr >> 16) & 0xFFU;
rspData[10U] = (ipAddr >> 8) & 0xFFU;
rspData[11U] = (ipAddr >> 0) & 0xFFU;
}
edac::CRC::addCRC32(block, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
edac::CRC::addCRC32(rspData, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
// Generate the PDU data
m_rfData[0].setFormat(PDU_FMT_RSP);
m_rfData[0].setConfirmed(true);
m_rfData[0].setSerialNo(0U);
m_rfData[0].setData(block);
DataBlock rspBlock = DataBlock();
rspBlock.setFormat(PDU_FMT_CONFIRMED);
rspBlock.setSerialNo(0U);
rspBlock.setData(rspData);
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
m_rfData[0].encode(block);
rspBlock.encode(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
writeRF_PDU(data, bitLength);
@ -793,9 +798,9 @@ void DataPacket::writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uin
DataHeader rspHeader = DataHeader();
rspHeader.setFormat(PDU_FMT_RSP);
rspHeader.setOutbound(true);
rspHeader.setClass(ackClass);
rspHeader.setType(ackType);
rspHeader.setStatus(m_rfDataHeader.getNs());
rspHeader.setResponseClass(ackClass);
rspHeader.setResponseType(ackType);
rspHeader.setResponseStatus(m_rfDataHeader.getNs());
rspHeader.setLLId(llId);
if (m_rfDataHeader.getSAP() == PDU_SAP_EXT_ADDR) {
rspHeader.setSrcLLId(P25_WUID_FNE);
@ -961,7 +966,7 @@ void DataPacket::writeNet_PDU()
m_netDataBlockCnt++;
}
else {
if (m_netData[m_netDataBlockCnt].getConfirmed())
if (m_netData[m_netDataBlockCnt].getFormat() == PDU_FMT_CONFIRMED)
LogWarning(LOG_NET, P25_PDU_STR ", unfixable PDU data (3/4 rate or CRC)");
else
LogWarning(LOG_NET, P25_PDU_STR ", unfixable PDU data (1/2 rate or CRC)");

@ -216,13 +216,20 @@ namespace p25
// PDU SAP
const uint8_t PDU_SAP_USER_DATA = 0x00U;
const uint8_t PDU_SAP_ENC_USER_DATA = 0x01U;
const uint8_t PDU_SAP_PACKET_DATA = 0x04U;
const uint8_t PDU_SAP_ARP = 0x05U;
const uint8_t PDU_SAP_SNDCP_CTRL_DATA = 0x06U;
const uint8_t PDU_SAP_EXT_ADDR = 0x1FU;
const uint8_t PDU_SAP_REG = 0x20U;
const uint8_t PDU_SAP_UNENC_KMM = 0x28U;
const uint8_t PDU_SAP_ENC_KMM = 0x29U;
const uint8_t PDU_SAP_TRUNK_CTRL = 0x3DU;
// PDU ACK Class

@ -208,6 +208,7 @@ bool TrunkPacket::process(uint8_t* data, uint32_t len, bool mbtDecoded)
uint32_t srcId = m_rfTSBK.getSrcId();
uint32_t dstId = m_rfTSBK.getDstId();
// handle standard P25 reference opcodes
switch (m_rfTSBK.getLCO()) {
case TSBK_IOSP_GRP_VCH:
// make sure control data is supported
@ -494,7 +495,7 @@ bool TrunkPacket::process(uint8_t* data, uint32_t len, bool mbtDecoded)
default:
LogError(LOG_RF, P25_TSDU_STR ", unhandled LCO, mfId = $%02X, lco = $%02X", m_rfTSBK.getMFId(), m_rfTSBK.getLCO());
break;
}
} // switch (m_rfTSBK.getLCO())
// add trailing null pad; only if control data isn't being transmitted
if (!m_p25->m_ccRunning) {
@ -770,7 +771,7 @@ bool TrunkPacket::processNetwork(uint8_t* data, uint32_t len, lc::LC& control, d
default:
LogError(LOG_NET, P25_TSDU_STR ", unhandled LCO, mfId = $%02X, lco = $%02X", m_netTSBK.getMFId(), m_netTSBK.getLCO());
return false;
}
} // switch (m_netTSBK.getLCO())
writeNet_TSDU();
}

@ -44,7 +44,6 @@ using namespace p25;
/// Initializes a new instance of the DataBlock class.
/// </summary>
DataBlock::DataBlock() :
m_confirmed(false),
m_serialNo(0U),
m_lastBlock(false),
m_llId(0U),
@ -85,7 +84,6 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader header)
if (m_fmt == PDU_FMT_CONFIRMED) {
// decode 3/4 rate Trellis
try {
m_confirmed = true;
bool valid = m_trellis.decode34(data, buffer);
if (!valid) {
LogError(LOG_P25, "DataBlock::decode(), failed to decode Trellis 3/4 rate coding");
@ -145,9 +143,8 @@ bool DataBlock::decode(const uint8_t* data, const DataHeader header)
}
}
else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP) || (m_fmt == PDU_FMT_AMBT)) {
// decode 3/4 rate Trellis
// decode 1/2 rate Trellis
try {
m_confirmed = false;
bool valid = m_trellis.decode12(data, buffer);
if (!valid) {
LogError(LOG_P25, "DataBlock::decode(), failed to decode Trellis 1/2 rate coding");
@ -183,7 +180,7 @@ void DataBlock::encode(uint8_t* data)
assert(data != NULL);
assert(m_data != NULL);
if ((m_fmt == PDU_FMT_CONFIRMED) || (m_fmt == PDU_FMT_RSP && m_confirmed)) {
if (m_fmt == PDU_FMT_CONFIRMED) {
uint8_t buffer[P25_PDU_CONFIRMED_LENGTH_BYTES];
::memset(buffer, 0x00U, P25_PDU_CONFIRMED_LENGTH_BYTES);
@ -213,7 +210,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) || (m_fmt == PDU_FMT_AMBT)) {
else if (m_fmt == PDU_FMT_UNCONFIRMED || m_fmt == PDU_FMT_RSP || m_fmt == PDU_FMT_AMBT) {
uint8_t buffer[P25_PDU_UNCONFIRMED_LENGTH_BYTES];
::memset(buffer, 0x00U, P25_PDU_UNCONFIRMED_LENGTH_BYTES);
@ -243,12 +240,6 @@ void DataBlock::setFormat(const uint8_t fmt)
void DataBlock::setFormat(const DataHeader header)
{
m_fmt = header.getFormat();
if (m_fmt == PDU_FMT_CONFIRMED) {
m_confirmed = true;
}
else {
m_confirmed = false;
}
}
/// <summary>Gets the data format.</summary>
@ -265,10 +256,10 @@ void DataBlock::setData(const uint8_t* buffer)
assert(buffer != NULL);
assert(m_data != NULL);
if ((m_fmt == PDU_FMT_CONFIRMED) || (m_fmt == PDU_FMT_RSP && m_confirmed)) {
if (m_fmt == PDU_FMT_CONFIRMED) {
::memcpy(m_data, buffer, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
}
else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP && !m_confirmed) || (m_fmt == PDU_FMT_AMBT)) {
else if (m_fmt == PDU_FMT_UNCONFIRMED || m_fmt == PDU_FMT_RSP || m_fmt == PDU_FMT_AMBT) {
::memcpy(m_data, buffer, P25_PDU_UNCONFIRMED_LENGTH_BYTES);
}
else {
@ -283,11 +274,11 @@ uint32_t DataBlock::getData(uint8_t* buffer) const
assert(buffer != NULL);
assert(m_data != NULL);
if ((m_fmt == PDU_FMT_CONFIRMED) || (m_fmt == PDU_FMT_RSP && m_confirmed)) {
if (m_fmt == PDU_FMT_CONFIRMED) {
::memcpy(buffer, m_data, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
return P25_PDU_CONFIRMED_DATA_LENGTH_BYTES;
}
else if ((m_fmt == PDU_FMT_UNCONFIRMED) || (m_fmt == PDU_FMT_RSP && !m_confirmed) || (m_fmt == PDU_FMT_AMBT)) {
else if (m_fmt == PDU_FMT_UNCONFIRMED || m_fmt == PDU_FMT_RSP || m_fmt == PDU_FMT_AMBT) {
::memcpy(buffer, m_data, P25_PDU_UNCONFIRMED_LENGTH_BYTES);
return P25_PDU_UNCONFIRMED_LENGTH_BYTES;
}

@ -66,15 +66,12 @@ namespace p25
uint32_t getData(uint8_t* buffer) const;
public:
/// <summary>Flag indicating whether or not the data block is confirmed.</summary>
__PROPERTY(bool, confirmed, Confirmed);
/// <summary>Sets the data block serial number.</summary>
__PROPERTY(uint8_t, serialNo, SerialNo);
/// <summary>Flag indicating this is the last block in a sequence of block.</summary>
__PROPERTY(bool, lastBlock, LastBlock);
/// <summary>Logical link ID.</summary>
__PROPERTY(uint32_t, llId, LLId);
/// <summary>Service access point.</summary>

@ -27,6 +27,7 @@
#include "p25/P25Defines.h"
#include "p25/data/DataHeader.h"
#include "edac/CRC.h"
#include "Log.h"
#include "Utils.h"
using namespace p25::data;
@ -131,7 +132,7 @@ bool DataHeader::decode(const uint8_t* data)
case PDU_FMT_CONFIRMED:
m_S = (header[8U] & 0x80U) == 0x80U; // Re-synchronize Flag
m_Ns = (header[8U] >> 4) & 0x07U; // Packet Sequence No.
m_Ns = (header[8U] >> 4) & 0x07U; // Packet Sequence No.
m_fsn = header[8U] & 0x07U; // Fragment Sequence No.
m_lastFragment = (header[8U] & 0x08U) == 0x08U; // Last Fragment Flag
@ -181,6 +182,11 @@ void DataHeader::encode(uint8_t* data)
m_ackNeeded = false;
}
if (m_fmt == PDU_FMT_CONFIRMED && !m_ackNeeded) {
LogWarning(LOG_P25, "DataHeader::encode(), invalid values for PDU_FMT_CONFIRMED, ackNeeded = %u", m_ackNeeded);
m_ackNeeded = true; // force set this to true
}
header[0U] = (m_ackNeeded ? 0x40U : 0x00U) + // Acknowledge Needed
(m_outbound ? 0x20U : 0x00U) + // Inbound/Outbound
(m_fmt & 0x1FU); // Packet Format

@ -99,11 +99,11 @@ namespace p25
/// <summary>Source Logical link ID.</summary>
__PROPERTY(uint32_t, srcLlId, SrcLLId);
/// <summary>Response class.</summary>
__PROPERTY(uint8_t, rspClass, Class);
__PROPERTY(uint8_t, rspClass, ResponseClass);
/// <summary>Response type.</summary>
__PROPERTY(uint8_t, rspType, Type);
__PROPERTY(uint8_t, rspType, ResponseType);
/// <summary>Response status.</summary>
__PROPERTY(uint8_t, rspStatus, Status);
__PROPERTY(uint8_t, rspStatus, ResponseStatus);
/** AMBT Data */
/// <summary>Alternate Trunking Block Opcode</summary>

Loading…
Cancel
Save

Powered by TurnKey Linux.