Digital_Voice_Modem_Project
/
dvmhost
mirror of https://github.com/DVMProject/dvmhost.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

reorganize PDU transmission to the FNE;

Browse Source
pull/12/head
Bryan Biedenkapp 4 years ago
parent d79fcd6cf2
commit 179ed76ced
5 changed files with 195 additions and 324 deletions
Show Stats Download Patch File Download Diff File

86
network/BaseNetwork.cpp
Unescape View File

@ -226,14 +226,21 @@ uint8_t* BaseNetwork::readP25(bool& ret, p25::lc::LC& control, p25::data::LowSpe
uint8_t* data = NULL; uint8_t* data = NULL;
len = m_buffer[23U]; len = m_buffer[23U];
if (len <= 24) { if (duid == p25::P25_DUID_PDU) {
data = new uint8_t[len]; data = new uint8_t[length];
::memset(data, 0x00U, len); ::memset(data, 0x00U, length);
::memcpy(data, m_buffer, length);
} }
else { else {
data = new uint8_t[len]; if (len <= 24) {
::memset(data, 0x00U, len); data = new uint8_t[len];
::memcpy(data, m_buffer + 24U, len); ::memset(data, 0x00U, len);
}
else {
data = new uint8_t[len];
::memset(data, 0x00U, len);
::memcpy(data, m_buffer + 24U, len);
}
} }
ret = true; ret = true;
@ -359,12 +366,14 @@ bool BaseNetwork::writeP25TSDU(const p25::lc::TSBK& tsbk, const uint8_t* data)
/// <summary> /// <summary>
/// Writes P25 PDU frame data to the network. /// Writes P25 PDU frame data to the network.
/// </summary> /// </summary>
/// <param name="llId"></param> /// <param name="header"></param>
/// <param name="dataType"></param> /// <param name="secHeader"></param>
/// <param name="currentBlock"></param>
/// <param name="data"></param> /// <param name="data"></param>
/// <param name="len"></param> /// <param name="len"></param>
/// <returns></returns> /// <returns></returns>
bool BaseNetwork::writeP25PDU(const uint32_t llId, const uint8_t dataType, const uint8_t* data, const uint32_t len) bool BaseNetwork::writeP25PDU(const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader, const uint8_t currentBlock,
const uint8_t* data, const uint32_t len)
{ {
if (m_status != NET_STAT_RUNNING && m_status != NET_STAT_MST_RUNNING) if (m_status != NET_STAT_RUNNING && m_status != NET_STAT_MST_RUNNING)
return false; return false;
@ -375,7 +384,7 @@ bool BaseNetwork::writeP25PDU(const uint32_t llId, const uint8_t dataType, const
m_streamId[0] = m_p25StreamId; m_streamId[0] = m_p25StreamId;
return writeP25PDU(m_id, m_p25StreamId, llId, dataType, data, len); return writeP25PDU(m_id, m_p25StreamId, header, secHeader, currentBlock, data, len);
} }
/// <summary> /// <summary>
@ -483,7 +492,7 @@ bool BaseNetwork::writeDMR(const uint32_t id, const uint32_t streamId, const dmr
uint32_t dstId = data.getDstId(); // Target Address uint32_t dstId = data.getDstId(); // Target Address
__SET_UINT16(dstId, buffer, 8U); __SET_UINT16(dstId, buffer, 8U);
__SET_UINT32(id, buffer, 11U); __SET_UINT32(id, buffer, 11U); // Peer ID
uint32_t slotNo = data.getSlotNo(); uint32_t slotNo = data.getSlotNo();
@ -521,7 +530,7 @@ bool BaseNetwork::writeDMR(const uint32_t id, const uint32_t streamId, const dmr
buffer[4U] = data.getSeqNo(); // Sequence Number buffer[4U] = data.getSeqNo(); // Sequence Number
__SET_UINT32(streamId, buffer, 16U); __SET_UINT32(streamId, buffer, 16U); // Stream ID
data.getData(buffer + 20U); data.getData(buffer + 20U);
@ -569,11 +578,11 @@ bool BaseNetwork::writeP25LDU1(const uint32_t id, const uint32_t streamId, const
uint32_t dstId = control.getDstId(); // Target Address uint32_t dstId = control.getDstId(); // Target Address
__SET_UINT16(dstId, buffer, 8U); __SET_UINT16(dstId, buffer, 8U);
__SET_UINT32(id, buffer, 11U); __SET_UINT32(id, buffer, 11U); // Peer ID
buffer[15U] = control.getMFId(); // MFId buffer[15U] = control.getMFId(); // MFId
__SET_UINT32(streamId, buffer, 16U); __SET_UINT32(streamId, buffer, 16U); // Stream ID
buffer[20U] = lsd.getLSD1(); // LSD 1 buffer[20U] = lsd.getLSD1(); // LSD 1
buffer[21U] = lsd.getLSD2(); // LSD 2 buffer[21U] = lsd.getLSD2(); // LSD 2
@ -670,11 +679,11 @@ bool BaseNetwork::writeP25LDU2(const uint32_t id, const uint32_t streamId, const
uint32_t dstId = control.getDstId(); // Target Address uint32_t dstId = control.getDstId(); // Target Address
__SET_UINT16(dstId, buffer, 8U); __SET_UINT16(dstId, buffer, 8U);
__SET_UINT32(id, buffer, 11U); __SET_UINT32(id, buffer, 11U); // Peer ID
buffer[15U] = control.getMFId(); // MFId buffer[15U] = control.getMFId(); // MFId
__SET_UINT32(streamId, buffer, 16U); __SET_UINT32(streamId, buffer, 16U); // Stream ID
buffer[20U] = lsd.getLSD1(); // LSD 1 buffer[20U] = lsd.getLSD1(); // LSD 1
buffer[21U] = lsd.getLSD2(); // LSD 2 buffer[21U] = lsd.getLSD2(); // LSD 2
@ -765,11 +774,11 @@ bool BaseNetwork::writeP25TDU(const uint32_t id, const uint32_t streamId, const
uint32_t dstId = control.getDstId(); // Target Address uint32_t dstId = control.getDstId(); // Target Address
__SET_UINT16(dstId, buffer, 8U); __SET_UINT16(dstId, buffer, 8U);
__SET_UINT32(id, buffer, 11U); __SET_UINT32(id, buffer, 11U); // Peer ID
buffer[15U] = control.getMFId(); // MFId buffer[15U] = control.getMFId(); // MFId
__SET_UINT32(streamId, buffer, 16U); __SET_UINT32(streamId, buffer, 16U); // Stream ID
buffer[20U] = lsd.getLSD1(); // LSD 1 buffer[20U] = lsd.getLSD1(); // LSD 1
buffer[21U] = lsd.getLSD2(); // LSD 2 buffer[21U] = lsd.getLSD2(); // LSD 2
@ -813,11 +822,11 @@ bool BaseNetwork::writeP25TSDU(const uint32_t id, const uint32_t streamId, const
uint32_t dstId = tsbk.getDstId(); // Target Address uint32_t dstId = tsbk.getDstId(); // Target Address
__SET_UINT16(dstId, buffer, 8U); __SET_UINT16(dstId, buffer, 8U);
__SET_UINT32(id, buffer, 11U); __SET_UINT32(id, buffer, 11U); // Peer ID
buffer[15U] = tsbk.getMFId(); // MFId buffer[15U] = tsbk.getMFId(); // MFId
__SET_UINT32(streamId, buffer, 16U); __SET_UINT32(streamId, buffer, 16U); // Stream ID
buffer[20U] = 0U; // Reserved (LSD 1) buffer[20U] = 0U; // Reserved (LSD 1)
buffer[21U] = 0U; // Reserved (LSD 2) buffer[21U] = 0U; // Reserved (LSD 2)
@ -844,17 +853,26 @@ bool BaseNetwork::writeP25TSDU(const uint32_t id, const uint32_t streamId, const
/// </summary> /// </summary>
/// <param name="id"></param> /// <param name="id"></param>
/// <param name="streamId"></param> /// <param name="streamId"></param>
/// <param name="llId"></param> /// <param name="header"></param>
/// <param name="dataType"></param> /// <param name="secHeader"></param>
/// <param name="currentBlock"></param>
/// <param name="data"></param> /// <param name="data"></param>
/// <param name="len"></param> /// <param name="len"></param>
/// <returns></returns> /// <returns></returns>
bool BaseNetwork::writeP25PDU(const uint32_t id, const uint32_t streamId, const uint32_t llId, const uint8_t dataType, const uint8_t* data, bool BaseNetwork::writeP25PDU(const uint32_t id, const uint32_t streamId, const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader,
const uint32_t len) const uint8_t currentBlock, const uint8_t* data, const uint32_t len)
{ {
if (m_status != NET_STAT_RUNNING && m_status != NET_STAT_MST_RUNNING) if (m_status != NET_STAT_RUNNING && m_status != NET_STAT_MST_RUNNING)
return false; return false;
bool useSecondHeader = false;
// process second header if we're using enhanced addressing
if (header.getSAP() == p25::PDU_SAP_EXT_ADDR &&
header.getFormat() == p25::PDU_FMT_UNCONFIRMED) {
useSecondHeader = true;
}
assert(data != NULL); assert(data != NULL);
uint8_t buffer[DATA_PACKET_LENGTH]; uint8_t buffer[DATA_PACKET_LENGTH];
@ -862,20 +880,24 @@ bool BaseNetwork::writeP25PDU(const uint32_t id, const uint32_t streamId, const
::memcpy(buffer + 0U, TAG_P25_DATA, 4U); ::memcpy(buffer + 0U, TAG_P25_DATA, 4U);
buffer[4U] = dataType; // Data Type (LCO) buffer[4U] = header.getSAP(); // Service Access Point
if (header.getFormat() == p25::PDU_FMT_CONFIRMED) {
buffer[4U] |= 0x80U;
}
__SET_UINT16(llId, buffer, 5U); // Logical Link Address (Source Address) uint32_t llId = (useSecondHeader) ? secHeader.getLLId() : header.getLLId();
__SET_UINT16(llId, buffer, 5U); // Logical Link Address
__SET_UINT16(len, buffer, 8U); // PDU Length [bytes] (Target Address) __SET_UINT16(len, buffer, 8U); // PDU Length [bytes]
__SET_UINT32(id, buffer, 11U); __SET_UINT32(id, buffer, 11U); // Peer ID
buffer[15U] = p25::P25_MFG_STANDARD; // MFId buffer[15U] = header.getMFId(); // MFId
__SET_UINT32(streamId, buffer, 16U); __SET_UINT32(streamId, buffer, 16U); // Stream ID
buffer[20U] = 0U; // Reserved (LSD 1) buffer[20U] = header.getBlocksToFollow(); // Blocks To Follow
buffer[21U] = 0U; // Reserved (LSD 2) buffer[21U] = currentBlock; // Current Block
buffer[22U] = p25::P25_DUID_PDU; // DUID buffer[22U] = p25::P25_DUID_PDU; // DUID

6
network/BaseNetwork.h
Unescape View File

@ -141,7 +141,8 @@ namespace network
/// <summary>Writes P25 TSDU frame data to the network.</summary> /// <summary>Writes P25 TSDU frame data to the network.</summary>
virtual bool writeP25TSDU(const p25::lc::TSBK& control, const uint8_t* data); virtual bool writeP25TSDU(const p25::lc::TSBK& control, const uint8_t* data);
/// <summary>Writes P25 PDU frame data to the network.</summary> /// <summary>Writes P25 PDU frame data to the network.</summary>
virtual bool writeP25PDU(const uint32_t llId, const uint8_t dataType, const uint8_t* data, const uint32_t len); virtual bool writeP25PDU(const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader, const uint8_t currentBlock,
const uint8_t* data, const uint32_t len);
/// <summary>Writes the local activity log to the network.</summary> /// <summary>Writes the local activity log to the network.</summary>
virtual bool writeActLog(const char* message); virtual bool writeActLog(const char* message);
@ -207,7 +208,8 @@ namespace network
/// <summary>Writes P25 TSDU frame data to the network.</summary> /// <summary>Writes P25 TSDU frame data to the network.</summary>
bool writeP25TSDU(const uint32_t id, const uint32_t streamId, const p25::lc::TSBK& control, const uint8_t* data); bool writeP25TSDU(const uint32_t id, const uint32_t streamId, const p25::lc::TSBK& control, const uint8_t* data);
/// <summary>Writes P25 PDU frame data to the network.</summary> /// <summary>Writes P25 PDU frame data to the network.</summary>
bool writeP25PDU(const uint32_t id, const uint32_t streamId, const uint32_t llId, const uint8_t dataType, const uint8_t* data, const uint32_t len); bool writeP25PDU(const uint32_t id, const uint32_t streamId, const p25::data::DataHeader& header, const p25::data::DataHeader& secHeader, const uint8_t currentBlock,
const uint8_t* data, const uint32_t len);
/// <summary>Writes data to the network.</summary> /// <summary>Writes data to the network.</summary>
virtual bool write(const uint8_t* data, uint32_t length); virtual bool write(const uint8_t* data, uint32_t length);

405
p25/DataPacket.cpp
Unescape View File

@ -150,8 +150,6 @@ bool DataPacket::process(uint8_t* data, uint32_t len)
m_p25->m_rfState = m_prevRfState; m_p25->m_rfState = m_prevRfState;
return false; return false;
} }
writeNetworkRF(P25_DT_DATA_HEADER, buffer, P25_PDU_FEC_LENGTH_BYTES);
} }
if (m_p25->m_rfState == RS_RF_DATA) { if (m_p25->m_rfState == RS_RF_DATA) {
@ -183,8 +181,6 @@ bool DataPacket::process(uint8_t* data, uint32_t len)
m_rfSecondHeader.getHeaderOffset(), m_rfSecondHeader.getLLId()); m_rfSecondHeader.getHeaderOffset(), m_rfSecondHeader.getLLId());
} }
writeNetworkRF(P25_DT_DATA_SEC_HEADER, buffer, P25_PDU_FEC_LENGTH_BYTES);
m_rfUseSecondHeader = true; m_rfUseSecondHeader = true;
offset += P25_PDU_FEC_LENGTH_BITS; offset += P25_PDU_FEC_LENGTH_BITS;
@ -222,7 +218,6 @@ bool DataPacket::process(uint8_t* data, uint32_t len)
m_rfData[i].getData(m_pduUserData + dataOffset); m_rfData[i].getData(m_pduUserData + dataOffset);
m_pduUserDataLength += (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES; m_pduUserDataLength += (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
m_rfDataBlockCnt++;
// is this the last block? // is this the last block?
if (m_rfData[i].getLastBlock()) { if (m_rfData[i].getLastBlock()) {
@ -232,7 +227,8 @@ bool DataPacket::process(uint8_t* data, uint32_t len)
} }
} }
writeNetworkRF(P25_DT_DATA, buffer, P25_PDU_FEC_LENGTH_BYTES); writeNetworkRF(m_rfDataBlockCnt, m_pduUserData + dataOffset, (m_rfDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES);
m_rfDataBlockCnt++;
} }
else { else {
if (m_rfData[i].getFormat() == PDU_FMT_CONFIRMED) if (m_rfData[i].getFormat() == PDU_FMT_CONFIRMED)
@ -392,117 +388,90 @@ bool DataPacket::processNetwork(uint8_t* data, uint32_t len, lc::LC& control, da
switch (duid) { switch (duid) {
case P25_DUID_PDU: case P25_DUID_PDU:
{ {
uint32_t pduLen = control.getDstId(); // PDU's use dstId as the PDU len if (m_p25->m_netState != RS_NET_DATA) {
::memset(m_netPDU, 0x00U, pduLen + 2U); m_netDataHeader.reset();
::memcpy(m_netPDU, data, pduLen); m_netSecondHeader.reset();
m_netDataOffset = 0U;
uint8_t dataType = control.getLCO(); m_netDataBlockCnt = 0U;
if (dataType == P25_DT_DATA_HEADER) { m_netPDUCount = 0U;
writeNet_PDU_Header();
} m_p25->m_netState = RS_NET_DATA;
else if (dataType == P25_DT_DATA_SEC_HEADER) {
writeNet_PDU_Sec_Header(); uint8_t blocksToFollow = data[20U];
} bool confirmed = (data[4U] & 0x80U) == 0x80U;
else if (dataType == P25_DT_DATA) { bool response = (data[4U] & 0x40U) == 0x40U;
writeNet_PDU(); uint8_t sap = data[4U] & 0x3FU;
::ActivityLog("P25", false, "network data transmission from %u to %u, %u blocks", m_netDataHeader.getLLId(), m_netDataHeader.getLLId(), m_netDataHeader.getBlocksToFollow());
} m_netDataHeader.setAckNeeded(confirmed);
m_netDataHeader.setOutbound(true);
m_netDataHeader.setFormat((confirmed) ? PDU_FMT_CONFIRMED : PDU_FMT_UNCONFIRMED);
m_netDataHeader.setSAP(sap);
m_netDataHeader.setFullMessage(true);
m_netDataHeader.setBlocksToFollow(blocksToFollow);
if (m_p25->m_netState == RS_NET_DATA) { if (m_verbose) {
if (m_netDataBlockCnt >= m_netBlocksToFollow) { LogMessage(LOG_NET, P25_PDU_STR ", ack = %u, outbound = %u, fmt = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padCount = %u, n = %u, seqNo = %u, hdrOffset = %u",
if (m_dumpPDUData) { m_netDataHeader.getAckNeeded(), m_netDataHeader.getOutbound(), m_netDataHeader.getFormat(), m_netDataHeader.getSAP(), m_netDataHeader.getFullMessage(),
Utils::dump(1U, "PDU Packet", m_pduUserData, m_netDataOffset); m_netDataHeader.getBlocksToFollow(), m_netDataHeader.getPadCount(), m_netDataHeader.getNs(), m_netDataHeader.getFSN(),
} m_netDataHeader.getHeaderOffset());
} }
// write data to RF interface? // make sure we don't get a PDU with more blocks then we support
if (m_repeatPDU) { if (m_netDataHeader.getBlocksToFollow() >= P25_MAX_PDU_COUNT) {
if (m_netDataBlockCnt >= m_netBlocksToFollow) { LogError(LOG_NET, P25_PDU_STR ", too many PDU blocks to process, %u > %u", m_netDataHeader.getBlocksToFollow(), P25_MAX_PDU_COUNT);
uint32_t bitLength = ((m_netDataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS;
uint32_t offset = P25_PREAMBLE_LENGTH_BITS;
::memset(m_netPDU, 0x00U, P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U);
uint8_t buffer[P25_PDU_FEC_LENGTH_BYTES];
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
uint32_t blocksToFollow = m_netDataHeader.getBlocksToFollow();
// Generate the PDU header and 1/2 rate Trellis
m_netDataHeader.encode(buffer);
Utils::setBitRange(buffer, m_netPDU, offset, P25_PDU_FEC_LENGTH_BITS);
offset += P25_PDU_FEC_LENGTH_BITS;
// Generate the second PDU header m_netDataHeader.reset();
if (m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR) { m_netDataOffset = 0U;
m_netSecondHeader.encode(buffer); m_netDataBlockCnt = 0U;
Utils::setBitRange(buffer, m_netPDU, offset, P25_PDU_FEC_LENGTH_BITS); m_netPDUCount = 0U;
m_p25->m_netState = RS_NET_IDLE;
offset += P25_PDU_FEC_LENGTH_BITS; return false;
blocksToFollow--; }
}
// Generate the PDU data
uint32_t dataOffset = 0U;
for (uint32_t i = 0U; i < blocksToFollow; i++) {
m_netData[i].setFormat((m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR) ? m_netSecondHeader : m_netDataHeader);
m_netData[i].setSerialNo(i);
m_netData[i].setData(m_pduUserData + dataOffset);
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
m_netData[i].encode(buffer);
Utils::setBitRange(buffer, m_netPDU, offset, P25_PDU_FEC_LENGTH_BITS);
offset += P25_PDU_FEC_LENGTH_BITS;
dataOffset += (m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
}
if (m_debug) {
Utils::dump(2U, "!!! *Raw PDU Frame Data - P25_DUID_PDU", m_netPDU, bitLength / 8U);
}
uint8_t pdu[P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U];
// Add the data
uint32_t newBitLength = P25Utils::encode(m_netPDU, pdu + 2U, bitLength);
uint32_t newByteLength = newBitLength / 8U;
if ((newBitLength % 8U) > 0U)
newByteLength++;
// Regenerate Sync
Sync::addP25Sync(pdu + 2U);
// Regenerate NID if (m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR &&
m_p25->m_nid.encode(pdu + 2U, P25_DUID_PDU); m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) {
LogWarning(LOG_NET, P25_PDU_STR ", unsupported confirmed enhanced addressing");
m_netDataHeader.reset();
m_netSecondHeader.reset();
m_netDataOffset = 0U;
m_netDataBlockCnt = 0U;
m_netPDUCount = 0U;
m_p25->m_netState = RS_NET_IDLE;
return false;
}
// Add busy bits ::ActivityLog("P25", false, "network data transmission from %u to %u, %u blocks", m_netDataHeader.getLLId(), m_netDataHeader.getLLId(), m_netDataHeader.getBlocksToFollow());
m_p25->addBusyBits(pdu + 2U, newBitLength, false, true); }
::memset(m_netPDU, 0x00U, P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U); if (m_p25->m_netState == RS_NET_DATA) {
uint32_t pduLen = control.getDstId(); // PDU's use dstId as the PDU len
::memset(m_netPDU, 0x00U, pduLen + 2U);
::memcpy(m_netPDU, data, pduLen);
if (m_p25->m_duplex) { if (m_netDataBlockCnt >= m_netDataHeader.getBlocksToFollow()) {
pdu[0U] = TAG_DATA; if (m_dumpPDUData) {
pdu[1U] = 0x00U; Utils::dump(1U, "PDU Packet", m_pduUserData, m_netDataOffset);
m_p25->writeQueueNet(pdu, newByteLength + 2U); }
}
// add trailing null pad; only if control data isn't being transmitted writeNet_PDU_Buffered();
if (!m_p25->m_ccRunning) {
m_p25->writeRF_Nulls();
}
::ActivityLog("P25", true, "end of RF data transmission"); ::ActivityLog("P25", true, "end of RF data transmission");
m_netDataHeader.reset(); m_netDataHeader.reset();
m_netSecondHeader.reset(); m_netSecondHeader.reset();
m_netBlocksToFollow = 0U; m_netDataOffset = 0U;
m_netDataBlockCnt = 0U; m_netDataBlockCnt = 0U;
m_netBitOffset = 0U; m_netPDUCount = 0U;
m_netDataOffset = 0U; m_p25->m_netState = RS_NET_IDLE;
m_p25->m_netState = RS_NET_IDLE;
}
} }
} // if (m_netState == RS_NET_DATA) else {
uint32_t len = __GET_UINT16(data, 8U);
::memcpy(m_pduUserData, data + 24U, len);
m_netDataOffset += len;
m_netDataBlockCnt++;
}
}
} }
break; break;
default: default:
@ -560,11 +529,11 @@ DataPacket::DataPacket(Control* p25, network::BaseNetwork* network, bool dumpPDU
m_netData(NULL), m_netData(NULL),
m_netDataHeader(), m_netDataHeader(),
m_netSecondHeader(), m_netSecondHeader(),
m_netBlocksToFollow(0U), m_netUseSecondHeader(false),
m_netDataBlockCnt(0U),
m_netBitOffset(0U),
m_netDataOffset(0U), m_netDataOffset(0U),
m_netDataBlockCnt(0U),
m_netPDU(NULL), m_netPDU(NULL),
m_netPDUCount(0U),
m_pduUserData(NULL), m_pduUserData(NULL),
m_pduUserDataLength(0U), m_pduUserDataLength(0U),
m_fneRegTable(), m_fneRegTable(),
@ -602,10 +571,10 @@ DataPacket::~DataPacket()
/// <summary> /// <summary>
/// Write data processed from RF to the network. /// Write data processed from RF to the network.
/// </summary> /// </summary>
/// <param name="dataType"></param> /// <param name="currentBlock"></param>
/// <param name="data"></param> /// <param name="data"></param>
/// <param name="len"></param> /// <param name="len"></param>
void DataPacket::writeNetworkRF(const uint8_t dataType, const uint8_t *data, uint32_t len) void DataPacket::writeNetworkRF(const uint8_t currentBlock, const uint8_t *data, uint32_t len)
{ {
assert(data != NULL); assert(data != NULL);
@ -615,7 +584,7 @@ void DataPacket::writeNetworkRF(const uint8_t dataType, const uint8_t *data, uin
if (m_p25->m_rfTimeout.isRunning() && m_p25->m_rfTimeout.hasExpired()) if (m_p25->m_rfTimeout.isRunning() && m_p25->m_rfTimeout.hasExpired())
return; return;
m_network->writeP25PDU((m_rfUseSecondHeader) ? m_rfSecondHeader.getLLId() : m_rfDataHeader.getLLId(), dataType, data, len); m_network->writeP25PDU(m_rfDataHeader, m_rfSecondHeader, currentBlock, data, len);
} }
/// <summary> /// <summary>
@ -664,6 +633,57 @@ void DataPacket::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool noNull
} }
} }
/// <summary>
/// Helper to write a network P25 PDU packet.
/// </summary>
/// <remarks>This will take buffered network PDU data and repeat it over the air.</remarks>
void DataPacket::writeNet_PDU_Buffered()
{
uint32_t bitLength = ((m_netDataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS;
uint32_t offset = P25_PREAMBLE_LENGTH_BITS;
uint8_t* data = new uint8_t[bitLength / 8U];
::memset(data, 0x00U, bitLength / 8U);
uint8_t block[P25_PDU_FEC_LENGTH_BYTES];
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
uint32_t blocksToFollow = m_netDataHeader.getBlocksToFollow();
// Generate the PDU header and 1/2 rate Trellis
m_netDataHeader.encode(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
offset += P25_PDU_FEC_LENGTH_BITS;
// Generate the second PDU header
if (m_netUseSecondHeader) {
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
m_netSecondHeader.encode(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
offset += P25_PDU_FEC_LENGTH_BITS;
blocksToFollow--;
}
// Generate the PDU data
uint32_t dataOffset = 0U;
for (uint32_t i = 0U; i < blocksToFollow; i++) {
m_netData[i].setFormat((m_netUseSecondHeader) ? m_netSecondHeader : m_netDataHeader);
m_netData[i].setSerialNo(i);
m_netData[i].setData(m_pduUserData + dataOffset);
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
m_netData[i].encode(block);
Utils::setBitRange(block, data, offset, P25_PDU_FEC_LENGTH_BITS);
offset += P25_PDU_FEC_LENGTH_BITS;
dataOffset += (m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
}
writeRF_PDU(data, bitLength);
delete[] data;
}
/// <summary> /// <summary>
/// Helper to re-write a received P25 PDU packet. /// Helper to re-write a received P25 PDU packet.
/// </summary> /// </summary>
@ -822,166 +842,3 @@ void DataPacket::writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uin
writeRF_PDU(data, bitLength); writeRF_PDU(data, bitLength);
delete[] data; delete[] data;
} }
/// <summary>
/// Helper to write a network P25 PDU header packet.
/// </summary>
void DataPacket::writeNet_PDU_Header()
{
if (m_p25->m_netState != RS_NET_DATA) {
m_netDataHeader.reset();
m_netSecondHeader.reset();
m_netBlocksToFollow = 0U;
m_netDataBlockCnt = 0U;
m_netBitOffset = 0U;
m_netDataOffset = 0U;
m_p25->m_netState = RS_NET_DATA;
::memset(m_pduUserData, 0x00U, P25_MAX_PDU_COUNT * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
uint8_t buffer[P25_PDU_FEC_LENGTH_BYTES];
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
::memcpy(buffer, m_netPDU, P25_PDU_FEC_LENGTH_BYTES);
bool ret = m_netDataHeader.decode(buffer);
if (!ret) {
LogWarning(LOG_NET, P25_PDU_STR ", unfixable RF 1/2 rate header data");
Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_FEC_LENGTH_BYTES);
m_netDataHeader.reset();
m_netSecondHeader.reset();
m_netBlocksToFollow = 0U;
m_netDataBlockCnt = 0U;
m_netBitOffset = 0U;
m_netDataOffset = 0U;
m_p25->m_netState = RS_NET_IDLE;
return;
}
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", ack = %u, outbound = %u, fmt = $%02X, sap = $%02X, fullMessage = %u, blocksToFollow = %u, padCount = %u, n = %u, seqNo = %u, hdrOffset = %u",
m_netDataHeader.getAckNeeded(), m_netDataHeader.getOutbound(), m_netDataHeader.getFormat(), m_netDataHeader.getSAP(), m_netDataHeader.getFullMessage(),
m_netDataHeader.getBlocksToFollow(), m_netDataHeader.getPadCount(), m_netDataHeader.getNs(), m_netDataHeader.getFSN(),
m_netDataHeader.getHeaderOffset());
}
// make sure we don't get a PDU with more blocks then we support
if (m_netDataHeader.getBlocksToFollow() >= P25_MAX_PDU_COUNT) {
LogError(LOG_NET, P25_PDU_STR ", too many PDU blocks to process, %u > %u", m_netDataHeader.getBlocksToFollow(), P25_MAX_PDU_COUNT);
m_netDataHeader.reset();
m_netBlocksToFollow = 0U;
m_netDataBlockCnt = 0U;
m_netBitOffset = 0U;
m_netDataOffset = 0U;
return;
}
if (m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR &&
m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) {
LogWarning(LOG_NET, P25_PDU_STR ", unsupported confirmed enhanced addressing");
m_netDataHeader.reset();
m_netSecondHeader.reset();
m_netBlocksToFollow = 0U;
m_netDataBlockCnt = 0U;
m_netBitOffset = 0U;
m_netDataOffset = 0U;
m_p25->m_netState = RS_NET_IDLE;
return;
}
m_netBlocksToFollow = m_netDataHeader.getBlocksToFollow();
//uint32_t bitLength = ((m_netDataHeader.getBlocksToFollow() + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_PREAMBLE_LENGTH_BITS;
m_netBitOffset = P25_PREAMBLE_LENGTH_BITS;
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
// Generate the PDU header and 1/2 rate Trellis
m_netDataHeader.encode(buffer);
Utils::setBitRange(buffer, m_netPDU, m_netBitOffset, P25_PDU_FEC_LENGTH_BITS);
m_netBitOffset += P25_PDU_FEC_LENGTH_BITS;
}
}
/// <summary>
/// Helper to write a network P25 PDU secondary header packet.
/// </summary>
void DataPacket::writeNet_PDU_Sec_Header()
{
if (m_p25->m_netState == RS_NET_DATA) {
// process second header if we're using enhanced addressing
if (m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR) {
uint8_t buffer[P25_PDU_FEC_LENGTH_BYTES];
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
::memcpy(buffer, m_netPDU, P25_PDU_FEC_LENGTH_BYTES);
bool ret = m_netSecondHeader.decode(buffer);
if (!ret) {
LogWarning(LOG_NET, P25_PDU_STR ", unfixable RF 1/2 rate second header data");
Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_HEADER_LENGTH_BYTES);
m_netDataHeader.reset();
m_netSecondHeader.reset();
m_netBlocksToFollow = 0U;
m_netDataBlockCnt = 0U;
m_netBitOffset = 0U;
m_netDataOffset = 0U;
m_p25->m_netState = RS_NET_IDLE;
return;
}
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", fmt = $%02X, sap = $%02X, llId = %u",
m_netSecondHeader.getFormat(), m_netSecondHeader.getSAP(), m_netSecondHeader.getLLId());
}
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
// Generate the PDU header and 1/2 rate Trellis
m_netDataHeader.encode(buffer);
Utils::setBitRange(buffer, m_netPDU, m_netBitOffset, P25_PDU_FEC_LENGTH_BITS);
m_netBitOffset += P25_PDU_FEC_LENGTH_BITS;
m_netBlocksToFollow--;
}
}
}
/// <summary>
/// Helper to write a network P25 PDU data packet.
/// </summary>
void DataPacket::writeNet_PDU()
{
if (m_p25->m_netState == RS_NET_DATA) {
uint8_t buffer[P25_PDU_FEC_LENGTH_BYTES];
::memset(buffer, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
::memcpy(buffer, m_netPDU, P25_PDU_FEC_LENGTH_BYTES);
bool ret = m_netData[m_netDataBlockCnt].decode(buffer, (m_netDataHeader.getSAP() == PDU_SAP_EXT_ADDR) ? m_netSecondHeader : m_netDataHeader);
if (ret) {
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", block %u, fmt = $%02X",
(m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? m_netData[m_netDataBlockCnt].getSerialNo() : m_netDataBlockCnt, m_netData[m_netDataBlockCnt].getFormat());
}
m_netData[m_netDataBlockCnt].getData(m_pduUserData + m_netDataOffset);
m_netDataBlockCnt++;
}
else {
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)");
if (m_dumpPDUData) {
Utils::dump(1U, "Unfixable PDU Data", buffer, P25_PDU_FEC_LENGTH_BYTES);
}
}
m_netBitOffset += P25_PDU_FEC_LENGTH_BITS;
m_netDataOffset += (m_netDataHeader.getFormat() == PDU_FMT_CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
}
}

17
p25/DataPacket.h
Unescape View File

@ -88,11 +88,11 @@ namespace p25
data::DataBlock* m_netData; data::DataBlock* m_netData;
data::DataHeader m_netDataHeader; data::DataHeader m_netDataHeader;
data::DataHeader m_netSecondHeader; data::DataHeader m_netSecondHeader;
uint8_t m_netBlocksToFollow; bool m_netUseSecondHeader;
uint8_t m_netDataBlockCnt;
uint32_t m_netBitOffset;
uint32_t m_netDataOffset; uint32_t m_netDataOffset;
uint8_t m_netDataBlockCnt;
uint8_t* m_netPDU; uint8_t* m_netPDU;
uint32_t m_netPDUCount;
uint8_t* m_pduUserData; uint8_t* m_pduUserData;
uint32_t m_pduUserDataLength; uint32_t m_pduUserDataLength;
@ -111,23 +111,18 @@ namespace p25
~DataPacket(); ~DataPacket();
/// <summary>Write data processed from RF to the network.</summary> /// <summary>Write data processed from RF to the network.</summary>
void writeNetworkRF(const uint8_t dataType, const uint8_t* data, uint32_t len); void writeNetworkRF(const uint8_t currentBlock, const uint8_t* data, uint32_t len);
/// <summary>Helper to write a P25 PDU packet.</summary> /// <summary>Helper to write a P25 PDU packet.</summary>
void writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool noNulls = false); void writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool noNulls = false);
/// <summary>Helper to write a network P25 PDU packet.</summary>
void writeNet_PDU_Buffered();
/// <summary>Helper to re-write a received P25 PDU packet.</summary> /// <summary>Helper to re-write a received P25 PDU packet.</summary>
void writeRF_PDU_Buffered(); void writeRF_PDU_Buffered();
/// <summary>Helper to write a PDU registration response.</summary> /// <summary>Helper to write a PDU registration response.</summary>
void writeRF_PDU_Reg_Response(uint8_t regType, uint32_t llId, ulong64_t ipAddr); void writeRF_PDU_Reg_Response(uint8_t regType, uint32_t llId, ulong64_t ipAddr);
/// <summary>Helper to write a PDU acknowledge response.</summary> /// <summary>Helper to write a PDU acknowledge response.</summary>
void writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint32_t llId); void writeRF_PDU_Ack_Response(uint8_t ackClass, uint8_t ackType, uint32_t llId);
/// <summary>Helper to write a network P25 PDU header packet.</summary>
void writeNet_PDU_Header();
/// <summary>Helper to write a network P25 PDU secondary header packet.</summary>
void writeNet_PDU_Sec_Header();
/// <summary>Helper to write a network P25 PDU data packet.</summary>
void writeNet_PDU();
}; };
} // namespace p25 } // namespace p25

5
p25/P25Defines.h
Unescape View File

@ -340,11 +340,6 @@ namespace p25
const uint8_t P25_DUID_LDU2 = 0x0AU; // Logical Link Data Unit 2 const uint8_t P25_DUID_LDU2 = 0x0AU; // Logical Link Data Unit 2
const uint8_t P25_DUID_PDU = 0x0CU; // Packet Data Unit const uint8_t P25_DUID_PDU = 0x0CU; // Packet Data Unit
const uint8_t P25_DUID_TDULC = 0x0FU; // Terminator Data Unit with Link Control const uint8_t P25_DUID_TDULC = 0x0FU; // Terminator Data Unit with Link Control
// Data Type(s)
const uint8_t P25_DT_DATA_HEADER = 0x06U;
const uint8_t P25_DT_DATA_SEC_HEADER = 0x07U;
const uint8_t P25_DT_DATA = 0x08U;
} // namespace p25 } // namespace p25
// --------------------------------------------------------------------------- // ---------------------------------------------------------------------------

Write Preview
Loading…
Cancel
Save

Powered by TurnKey Linux.