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

[NOTICE: this commit is *EXPERIMENTAL* and implements *very* early data support, it is expected to be buggy, incomplete or broken] implement VTUN interface (fne0) on the dvmfne (this requires dvmfne to be run as root on Linux system [untested on non-Linux] to create the tun interface); implement VIFace random MAC generation; implement P25 TIA-102.BAEB SCEP ARP (SCEP is always used for conventional data) on dvmfne; refactor P25 packet status bits (this may break things else where, beware!); properly implement P25 status bit flipping on inbound channel activity; implement functionality in the P25 and DMR DataHeader classes to calculate the proper raw PDU frame length; implement dvmfne ARP table creation and maintainence (this is buggy and requires more implementation and test); implement dvmfne VTUN -> PDU and PDU -> VTUN IP traffic forwarding (this is incomplete and requires more implementation and test); correct several issues on dvmhost p25::packet::Data with overlapped buffers;

Browse Source
4.01b_maint
Bryan Biedenkapp 2 years ago
parent ac64946b3f
commit da2f6130a8
24 changed files with 951 additions and 141 deletions
Show Stats Download Patch File Download Diff File

19
configs/fne-config.example.yml
Unescape View File

@ -226,3 +226,22 @@ system:
file: peer_list.dat
# Amount of time between updates of white/blacklist file. (minutes)
time: 2
#
# Packet Data Virtual Network Tunnel Configuration
#
vtun:
# Flag indicating the virtual network tunnel is enabled.
# (If this is enabled, dvmfne must be run as root to create the TUN interface.)
enabled: false
# Operational mode for the network tunnel (dmr or p25).
digitalMode: p25
# Kernel Interface Name
interfaceName: fne0
# IP address of the tunnel network interface
address: 192.168.1.254
# Netmask of the tunnel network interface
netmask: 255.255.255.0
# Broadcast address of the tunnel network interface
broadcast: 192.168.1.255

16
src/common/dmr/data/DataHeader.cpp
Unescape View File

@ -418,6 +418,22 @@ uint32_t DataHeader::getPacketLength() const
}
}
/* Gets the total length in bytes of entire PDU. */
uint32_t DataHeader::getPDULength() const
{
if (m_DPF == DPF::RESPONSE) {
return 0U; // responses have no packet length as they are header only
}
if (m_DPF == DPF::CONFIRMED_DATA) {
return DMR_PDU_CONFIRMED_DATA_LENGTH_BYTES * m_blocksToFollow;
}
else {
return DMR_PDU_UNCONFIRMED_LENGTH_BYTES * m_blocksToFollow;
}
}
/* Gets the raw header data. */
uint32_t DataHeader::getData(uint8_t* buffer) const

5
src/common/dmr/data/DataHeader.h
Unescape View File

@ -75,6 +75,11 @@ namespace dmr
* @returns uint32_t Total length of packet in bytes.
*/
uint32_t getPacketLength() const;
/**
* @brief Gets the total length in bytes of entire PDU.
* @returns uint32_t Total length of PDU in bytes.
*/
uint32_t getPDULength() const;
/**
* @brief Gets the raw header data.

29
src/common/network/viface/VIFace.cpp
Unescape View File

@ -19,6 +19,7 @@ using namespace network::viface;
#include <sstream>
#include <fstream>
#include <iomanip>
#include <random>
#include <cassert>
#include <cstring>
@ -332,7 +333,7 @@ VIFace::VIFace(std::string name, bool tap, int id) :
throw std::runtime_error("Unable to create IPv4 socket channel to the NET kernel.");
}
// Set id
// set id
if (id < 0) {
m_id = m_idSeq;
m_idSeq++;
@ -543,6 +544,30 @@ bool VIFace::write(const uint8_t* buffer, uint32_t length, ssize_t* lenWritten)
return result;
}
/* Set the MAC address of the virtual interface to a random value. */
void VIFace::setRandomMAC()
{
// generate random MAC
std::random_device rd;
std::mt19937 mt(rd());
std::ostringstream addr;
addr << std::hex << std::setfill('0');
for (int i = 0; i < 6; i++) {
uint8_t macByte = 0U;
std::uniform_int_distribution<uint8_t> dist(1U, 254U);
macByte = dist(mt);
addr << std::setw(2) << (uint8_t)(0xFFU & macByte);
if (i != 5) {
addr << ":";
}
}
m_mac = addr.str();
}
/* Set the MAC address of the virtual interface. */
void VIFace::setMAC(std::string mac)
@ -567,7 +592,7 @@ std::string VIFace::getMAC() const
std::ostringstream addr;
addr << std::hex << std::setfill('0');
for (int i = 0; i < 6; i++) {
addr << std::setw(2) << (uint8_t)(0xFF & ifr.ifr_hwaddr.sa_data[i]);
addr << std::setw(2) << (uint8_t)(0xFFU & ifr.ifr_hwaddr.sa_data[i]);
if (i != 5) {
addr << ":";
}

4
src/common/network/viface/VIFace.h
Unescape View File

@ -121,6 +121,10 @@ namespace network
*/
bool write(const uint8_t* buffer, uint32_t length, ssize_t* lenWritten = nullptr);
/**
* @brief Set the MAC address of the virtual interface to a random value.
*/
void setRandomMAC();
/**
* @brief Set the MAC address of the virtual interface.
*

7
src/common/p25/P25Defines.h
Unescape View File

@ -92,6 +92,10 @@ namespace p25
const uint32_t P25_PDU_CONFIRMED_DATA_LENGTH_BYTES = 16U;
const uint32_t P25_PDU_UNCONFIRMED_LENGTH_BYTES = 12U;
const uint32_t P25_PDU_ARP_PCKT_LENGTH = 22U;
const uint8_t P25_PDU_ARP_HW_ADDR_LENGTH = 3U;
const uint8_t P25_PDU_ARP_PROTO_ADDR_LENGTH = 4U;
const uint32_t P25_PDU_FEC_LENGTH_BYTES = 25U;
const uint32_t P25_PDU_FEC_LENGTH_BITS = P25_PDU_FEC_LENGTH_BYTES * 8U - 4U; // Trellis is actually 196 bits
@ -169,6 +173,9 @@ namespace p25
/** @brief All-call Talkgroup ID */
const uint32_t TGID_ALL = 0xFFFFU;
/** @brief ARP Hardware Type */
const uint16_t P25_PDU_ARP_CAI_TYPE = 0x21U;
/** @brief ARP Request */
const uint8_t P25_PDU_ARP_REQUEST = 0x01U;
/** @brief ARP Reply */

41
src/common/p25/P25Utils.cpp
Unescape View File

@ -21,9 +21,9 @@ using namespace p25::defines;
// Static Class Members
// ---------------------------------------------------------------------------
/* Helper to set the busy status bits on P25 frame data. */
/* Helper to set the status bits on P25 frame data. */
void P25Utils::setBusyBits(uint8_t* data, uint32_t ssOffset, bool b1, bool b2)
void P25Utils::setStatusBits(uint8_t* data, uint32_t ssOffset, bool b1, bool b2)
{
assert(data != nullptr);
@ -31,9 +31,9 @@ void P25Utils::setBusyBits(uint8_t* data, uint32_t ssOffset, bool b1, bool b2)
WRITE_BIT(data, ssOffset + 1U, b2);
}
/* Helper to add the busy status bits on P25 frame data. */
/* Helper to add the status bits on P25 frame data. */
void P25Utils::addBusyBits(uint8_t* data, uint32_t length, bool b1, bool b2)
void P25Utils::addStatusBits(uint8_t* data, uint32_t length, bool inbound, bool control)
{
assert(data != nullptr);
@ -47,21 +47,44 @@ void P25Utils::addBusyBits(uint8_t* data, uint32_t length, bool b1, bool b2)
// interleave the requested status bits (every other)
for (uint32_t ss0Pos = P25_SS0_START; ss0Pos < length; ss0Pos += (P25_SS_INCREMENT * 2U)) {
uint32_t ss1Pos = ss0Pos + 1U;
WRITE_BIT(data, ss0Pos, b1);
WRITE_BIT(data, ss1Pos, b2);
if (inbound) {
WRITE_BIT(data, ss0Pos, false); // 0
WRITE_BIT(data, ss1Pos, true); // 1
} else {
if (control) {
WRITE_BIT(data, ss0Pos, true); // 1
WRITE_BIT(data, ss1Pos, false); // 0
} else {
WRITE_BIT(data, ss0Pos, true); // 1
WRITE_BIT(data, ss1Pos, true); // 1
}
}
}
}
/* Helper to add the idle status bits on P25 frame data. */
void P25Utils::addIdleBits(uint8_t* data, uint32_t length, bool b1, bool b2)
void P25Utils::addIdleStatusBits(uint8_t* data, uint32_t length)
{
assert(data != nullptr);
for (uint32_t ss0Pos = P25_SS0_START; ss0Pos < length; ss0Pos += (P25_SS_INCREMENT * 5U)) {
uint32_t ss1Pos = ss0Pos + 1U;
WRITE_BIT(data, ss0Pos, b1);
WRITE_BIT(data, ss1Pos, b2);
WRITE_BIT(data, ss0Pos, true); // 1
WRITE_BIT(data, ss1Pos, false); // 0
}
}
/* Helper to add the trunk start slot status bits on P25 frame data. */
void P25Utils::addTrunkSlotStatusBits(uint8_t* data, uint32_t length)
{
assert(data != nullptr);
for (uint32_t ss0Pos = P25_SS0_START; ss0Pos < length; ss0Pos += (P25_SS_INCREMENT * 5U)) {
uint32_t ss1Pos = ss0Pos + 1U;
WRITE_BIT(data, ss0Pos, true); // 1
WRITE_BIT(data, ss1Pos, true); // 1
}
}

26
src/common/p25/P25Utils.h
Unescape View File

@ -118,29 +118,37 @@ namespace p25
}
/**
* @brief Helper to set the busy status bits on P25 frame data.
* @brief Helper to set the status bits on P25 frame data.
* @param data P25 frame data buffer.
* @param ssOffset
* @param b1 Status Bit 1
* @param b2 Status Bit 2
*/
static void setBusyBits(uint8_t* data, uint32_t ssOffset, bool b1, bool b2);
static void setStatusBits(uint8_t* data, uint32_t ssOffset, bool b1, bool b2);
/**
* @brief Helper to add the busy status bits on P25 frame data.
* @brief Helper to add the status bits on P25 frame data.
* This appropriately sets the status bits for the P25 frame, starting with 1,0 and then
* properly setting 0,1 for inbound traffic, or 1,1 for idle (or 1,0 for control channels).
* @param data P25 frame data buffer.
* @param length
* @param b1 Status Bit 1
* @param b2 Status Bit 2
* @param inbound Flag indicating inbound channel is busy.
* @param control Flag indicating the channel is a control channel.
*/
static void addBusyBits(uint8_t* data, uint32_t length, bool b1, bool b2);
static void addStatusBits(uint8_t *data, uint32_t length, bool inbound, bool control = false);
/**
* @brief Helper to add the idle status bits on P25 frame data.
* This sets the status bits to 1,0 interleaved every 5th status bit pair.
* @param data P25 frame data buffer.
* @param length
*/
static void addIdleStatusBits(uint8_t* data, uint32_t length);
/**
* @brief Helper to add the trunk start slot status bits on P25 frame data.
* This sets the status bits to 1,1 interleaved every 5th status bit pair.
* @param data P25 frame data buffer.
* @param length
* @param b1 Status Bit 1
* @param b2 Status Bit 2
*/
static void addIdleBits(uint8_t* data, uint32_t length, bool b1, bool b2);
static void addTrunkSlotStatusBits(uint8_t* data, uint32_t length);
/**
* @brief Decode bit interleaving.

24
src/common/p25/data/DataHeader.cpp
Unescape View File

@ -430,6 +430,22 @@ uint32_t DataHeader::getPacketLength() const
}
}
/* Gets the total length in bytes of entire PDU. */
uint32_t DataHeader::getPDULength() const
{
if (m_fmt == PDUFormatType::RSP) {
return 0U; // responses have no packet length as they are header only
}
if (m_fmt == PDUFormatType::CONFIRMED) {
return P25_PDU_CONFIRMED_DATA_LENGTH_BYTES * m_blocksToFollow;
}
else {
return P25_PDU_UNCONFIRMED_LENGTH_BYTES * m_blocksToFollow;
}
}
/* Gets the raw header data. */
uint32_t DataHeader::getData(uint8_t* buffer) const
@ -457,6 +473,14 @@ uint32_t DataHeader::getExtAddrData(uint8_t* buffer) const
void DataHeader::calculateLength(uint32_t packetLength)
{
uint32_t len = packetLength + 4U; // packet length + CRC32
if (m_fmt == PDUFormatType::UNCONFIRMED && m_sap == PDUSAP::EXT_ADDR) {
len += P25_PDU_HEADER_LENGTH_BYTES;
}
if (m_fmt == PDUFormatType::CONFIRMED && m_sap == PDUSAP::EXT_ADDR) {
len += 4U;
}
uint32_t blockLen = (m_fmt == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
if (len > blockLen) {

5
src/common/p25/data/DataHeader.h
Unescape View File

@ -86,6 +86,11 @@ namespace p25
* @returns uint32_t Total length of packet in bytes.
*/
uint32_t getPacketLength() const;
/**
* @brief Gets the total length in bytes of entire PDU.
* @returns uint32_t Total length of PDU in bytes.
*/
uint32_t getPDULength() const;
/**
* @brief Gets the raw header data.

97
src/fne/HostFNE.cpp
Unescape View File

@ -21,6 +21,7 @@
#include "FNEMain.h"
using namespace network;
using namespace network::viface;
using namespace lookups;
#include <cstdio>
@ -35,6 +36,7 @@ using namespace lookups;
// ---------------------------------------------------------------------------
#define IDLE_WARMUP_MS 5U
#define DEFAULT_MTU_SIZE 496
// ---------------------------------------------------------------------------
// Public Class Members
@ -47,6 +49,9 @@ HostFNE::HostFNE(const std::string& confFile) :
m_conf(),
m_network(nullptr),
m_diagNetwork(nullptr),
m_vtunEnabled(false),
m_packetDataMode(PacketDataMode::PROJECT25),
m_tun(nullptr),
m_dmrEnabled(false),
m_p25Enabled(false),
m_nxdnEnabled(false),
@ -171,6 +176,11 @@ int HostFNE::run()
if (!ret)
return EXIT_FAILURE;
// initialize virtual networking
ret = createVirtualNetworking();
if (!ret)
return EXIT_FAILURE;
::LogInfoEx(LOG_HOST, "FNE is up and running");
StopWatch stopWatch;
@ -207,6 +217,38 @@ int HostFNE::run()
diagNetworkLoop.setName("dvmfne:diag-network-loop");
}
ThreadFunc vtunLoop([&, this]() {
if (g_killed)
return;
if (!m_vtunEnabled)
return;
if (m_tun != nullptr) {
while (!g_killed) {
uint8_t packet[DEFAULT_MTU_SIZE];
::memset(packet, 0x00U, DEFAULT_MTU_SIZE);
ssize_t len = m_tun->read(packet);
if (len > 0) {
switch (m_packetDataMode) {
case PacketDataMode::DMR:
// TODO: not supported yet
break;
case PacketDataMode::PROJECT25:
m_network->p25TrafficHandler()->processPacketFrame(packet, DEFAULT_MTU_SIZE);
break;
}
}
Thread::sleep(5U);
}
}
});
vtunLoop.run();
vtunLoop.setName("dvmfne:vtun-loop");
// main execution loop
while (!g_killed) {
uint32_t ms = stopWatch.elapsed();
@ -250,6 +292,10 @@ int HostFNE::run()
diagNetworkLoop.wait();
}
if (m_vtunEnabled) {
vtunLoop.wait();
}
if (m_network != nullptr) {
m_network->close();
delete m_network;
@ -287,6 +333,14 @@ int HostFNE::run()
delete m_peerListLookup;
}
if (m_tun != nullptr) {
if (m_tun->isUp()) {
m_tun->down();
}
delete m_tun;
}
return EXIT_SUCCESS;
}
@ -676,6 +730,49 @@ bool HostFNE::createPeerNetworks()
return true;
}
/* Initializes virtual networking. */
bool HostFNE::createVirtualNetworking()
{
yaml::Node vtunConf = m_conf["vtun"];
bool vtunEnabled = vtunConf["enabled"].as<bool>(false);
if (vtunEnabled) {
m_vtunEnabled = vtunEnabled;
std::string vtunName = vtunConf["interfaceName"].as<std::string>("fne0");
std::string ipv4Address = vtunConf["address"].as<std::string>("192.168.1.254");
std::string ipv4Netmask = vtunConf["netmask"].as<std::string>("255.255.255.0");
std::string ipv4Broadcast = vtunConf["broadcast"].as<std::string>("192.168.1.255");
std::string packetDataModeStr = vtunConf["digitalMode"].as<std::string>("p25");
if (packetDataModeStr == "dmr") {
m_packetDataMode = PacketDataMode::DMR;
} else {
m_packetDataMode = PacketDataMode::PROJECT25;
}
LogInfo("Virtual Network Parameters");
LogInfo(" Interface Name: %s", vtunName.c_str());
LogInfo(" Address: %s", ipv4Address.c_str());
LogInfo(" Netmask: %s", ipv4Netmask.c_str());
LogInfo(" Broadcast: %s", ipv4Broadcast.c_str());
LogInfo(" Digital Packet Mode: %s", packetDataModeStr.c_str());
// initialize networking
m_tun = new VIFace(vtunName, false);
m_tun->setIPv4(ipv4Address);
m_tun->setIPv4Netmask(ipv4Netmask);
m_tun->setIPv4Broadcast(ipv4Broadcast);
m_tun->setMTU(DEFAULT_MTU_SIZE);
m_tun->up();
}
return true;
}
/* Processes any peer network traffic. */
void HostFNE::processPeer(network::PeerNetwork* peerNetwork)

19
src/fne/HostFNE.h
Unescape View File

@ -20,6 +20,7 @@
#include "common/lookups/RadioIdLookup.h"
#include "common/lookups/TalkgroupRulesLookup.h"
#include "common/lookups/PeerListLookup.h"
#include "common/network/viface/VIFace.h"
#include "common/yaml/Yaml.h"
#include "common/Timer.h"
#include "network/FNENetwork.h"
@ -51,6 +52,14 @@ namespace network { namespace callhandler { class HOST_SW_API TagNXDNData; } }
*/
class HOST_SW_API HostFNE {
public:
/**
* @brief Virtual Network Packet Data Digital Mode
*/
enum PacketDataMode {
DMR, //! Digital Mobile Radio
PROJECT25 //! Project 25
};
/**
* @brief Initializes a new instance of the HostFNE class.
* @param confFile Full-path to the configuration file.
@ -80,6 +89,10 @@ private:
network::FNENetwork* m_network;
network::DiagNetwork* m_diagNetwork;
bool m_vtunEnabled;
PacketDataMode m_packetDataMode;
network::viface::VIFace* m_tun;
bool m_dmrEnabled;
bool m_p25Enabled;
bool m_nxdnEnabled;
@ -123,6 +136,12 @@ private:
*/
bool createPeerNetworks();
/**
* @brief Initializes virtual networking.
* @returns bool True, if network connectivity was initialized, otherwise false.
*/
bool createVirtualNetworking();
/**
* @brief Processes any peer network traffic.
* @param peerNetwork Instance of PeerNetwork to process traffic for.

11
src/fne/network/callhandler/TagP25Data.cpp
Unescape View File

@ -421,6 +421,13 @@ bool TagP25Data::processGrantReq(uint32_t srcId, uint32_t dstId, bool unitToUnit
return true;
}
/* Process a data frame from the virtual IP network. */
void TagP25Data::processPacketFrame(const uint8_t* data, uint32_t len)
{
m_packetData->processPacketFrame(data, len);
}
/* Helper to playback a parrot frame to the network. */
void TagP25Data::playbackParrot()
@ -1224,10 +1231,10 @@ void TagP25Data::write_TSDU(uint32_t peerId, lc::TSBK* tsbk)
tsbk->encode(data);
// Add busy bits
P25Utils::addBusyBits(data, P25_TSDU_FRAME_LENGTH_BYTES, true, false);
P25Utils::addStatusBits(data, P25_TSDU_FRAME_LENGTH_BYTES, false);
// Set first busy bits to 1,1
P25Utils::setBusyBits(data, P25_SS0_START, true, true);
P25Utils::setStatusBits(data, P25_SS0_START, true, true);
if (m_debug) {
LogDebug(LOG_RF, P25_TSDU_STR ", lco = $%02X, mfId = $%02X, lastBlock = %u, AIV = %u, EX = %u, srcId = %u, dstId = %u, sysId = $%03X, netId = $%05X",

7
src/fne/network/callhandler/TagP25Data.h
Unescape View File

@ -79,6 +79,13 @@ namespace network
*/
bool processGrantReq(uint32_t srcId, uint32_t dstId, bool unitToUnit, uint32_t peerId, uint16_t pktSeq, uint32_t streamId);
/**
* @brief Process a data frame from the virtual IP network.
* @param data Network data buffer.
* @param len Length of data.
*/
void processPacketFrame(const uint8_t* data, uint32_t len);
/**
* @brief Helper to playback a parrot frame to the network.
*/

433
src/fne/network/callhandler/packetdata/P25PacketData.cpp
Unescape View File

@ -8,6 +8,7 @@
*
*/
#include "fne/Defines.h"
#include "common/p25/sndcp/SNDCPFactory.h"
#include "common/p25/Sync.h"
#include "common/edac/CRC.h"
#include "common/Clock.h"
@ -24,10 +25,19 @@ using namespace network::callhandler;
using namespace network::callhandler::packetdata;
using namespace p25;
using namespace p25::defines;
using namespace p25::sndcp;
#include <cassert>
#include <chrono>
#include <netinet/ip.h>
// ---------------------------------------------------------------------------
// Static Class Members
// ---------------------------------------------------------------------------
uint8_t P25PacketData::m_sendSeqNo = 0U;
// ---------------------------------------------------------------------------
// Constants
// ---------------------------------------------------------------------------
@ -43,7 +53,9 @@ const uint8_t DATA_CALL_COLL_TIMEOUT = 60U;
P25PacketData::P25PacketData(FNENetwork* network, TagP25Data* tag, bool debug) :
m_network(network),
m_tag(tag),
m_dataFrames(),
m_status(),
m_arpTable(),
m_debug(debug)
{
assert(network != nullptr);
@ -121,6 +133,15 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee
m_status[peerId] = status;
// a PDU header only with no blocks to follow is usually a response header
if (status->header.getBlocksToFollow() == 0U) {
dispatch(peerId);
delete status;
m_status.erase(peerId);
return true;
}
LogMessage(LOG_NET, "P25, Data Call Start, peer = %u, llId = %u, streamId = %u, external = %u", peerId, status->llId, streamId, external);
return true;
} else {
@ -154,18 +175,6 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee
}
}
// a PDU header only with no blocks to follow is usually a response header
if (status->header.getBlocksToFollow() == 0U) {
dispatch(peerId);
LogMessage(LOG_NET, "P25, Data Call End, peer = %u, srcId = %u, dstId = %u, streamId = %u, external = %u",
peerId, status->header.getSrcLLId(), status->header.getLLId(), streamId, external);
delete status;
m_status.erase(peerId);
return true;
}
::memcpy(status->netPDU + status->dataOffset, data + 24U, blockLength);
status->dataOffset += blockLength;
status->netPDUCount++;
@ -306,6 +315,61 @@ bool P25PacketData::processFrame(const uint8_t* data, uint32_t len, uint32_t pee
return true;
}
/* Process a data frame from the virtual IP network. */
void P25PacketData::processPacketFrame(const uint8_t* data, uint32_t len, bool alreadyQueued)
{
struct ip* ipHeader = (struct ip*)data;
char srcIp[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(ipHeader->ip_src), srcIp, INET_ADDRSTRLEN);
char dstIp[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(ipHeader->ip_dst), dstIp, INET_ADDRSTRLEN);
uint8_t proto = ipHeader->ip_p;
uint16_t pktLen = Utils::reverseEndian(ipHeader->ip_len); // bryanb: this could be problematic on different endianness
LogMessage(LOG_NET, "P25, VTUN -> PDU IP Data, srcIp = %s, dstIp = %s, pktLen = %u, proto = %02X", srcIp, dstIp, pktLen, proto);
Utils::dump(1U, "P25PacketData::processPacketFrame() packet", data, pktLen);
uint32_t dstLlId = getLLIdAddress(Utils::reverseEndian(ipHeader->ip_dst.s_addr));
if (dstLlId == 0U) {
LogMessage(LOG_NET, "P25, no ARP entry for, dstIp = %s", dstIp);
write_PDU_ARP(Utils::reverseEndian(ipHeader->ip_dst.s_addr));
// TODO: queue data frame for retransmission?
}
else {
// assemble a P25 PDU frame header for transport...
data::DataHeader rspHeader = data::DataHeader();
rspHeader.setFormat(PDUFormatType::CONFIRMED);
rspHeader.setMFId(MFG_STANDARD);
rspHeader.setAckNeeded(true);
rspHeader.setOutbound(true);
rspHeader.setSAP(PDUSAP::EXT_ADDR);
rspHeader.setLLId(dstLlId);
rspHeader.setBlocksToFollow(1U);
rspHeader.setEXSAP(PDUSAP::PACKET_DATA);
rspHeader.setSrcLLId(WUID_FNE);
rspHeader.calculateLength(pktLen);
uint32_t pduLength = rspHeader.getPDULength();
uint8_t pduUserData[pduLength];
::memset(pduUserData, 0x00U, pduLength);
::memcpy(pduUserData + 4U, data, pktLen);
Utils::dump(1U, "P25PacketData::processPacketFrame() pduUserData", pduUserData, pduLength);
dispatchUserFrameToFNE(rspHeader, true, pduUserData);
}
Thread::sleep(1750);
}
// ---------------------------------------------------------------------------
// Private Class Members
// ---------------------------------------------------------------------------
@ -327,7 +391,98 @@ void P25PacketData::dispatch(uint32_t peerId)
Utils::dump(1U, "PDU Packet", status->pduUserData, status->pduUserDataLength);
}
uint8_t sap = (status->extendedAddress) ? status->header.getEXSAP() : status->header.getSAP();
// don't dispatch SNDCP control, conventional data registration or ARP
if (sap != PDUSAP::SNDCP_CTRL_DATA && sap != PDUSAP::CONV_DATA_REG &&
sap != PDUSAP::ARP) {
dispatchToFNE(peerId);
}
// handle standard P25 service access points
switch (sap) {
case PDUSAP::ARP:
{
// is the host virtual tunneling enabled?
if (!m_network->m_host->m_vtunEnabled)
break;
uint32_t fneIPv4 = __IP_FROM_STR(m_network->m_host->m_tun->getIPv4());
uint8_t arpPacket[P25_PDU_ARP_PCKT_LENGTH];
::memset(arpPacket, 0x00U, P25_PDU_ARP_PCKT_LENGTH);
::memcpy(arpPacket, status->pduUserData + 12U, P25_PDU_ARP_PCKT_LENGTH);
uint16_t opcode = __GET_UINT16B(arpPacket, 6U);
uint32_t srcHWAddr = __GET_UINT16(arpPacket, 8U);
uint32_t srcProtoAddr = __GET_UINT32(arpPacket, 11U);
uint32_t tgtHWAddr = __GET_UINT16(arpPacket, 15U);
uint32_t tgtProtoAddr = __GET_UINT32(arpPacket, 18U);
if (opcode == P25_PDU_ARP_REQUEST) {
LogMessage(LOG_NET, P25_PDU_STR ", ARP request, who has %s? tell %s (%u)", __IP_FROM_UINT(tgtProtoAddr).c_str(), __IP_FROM_UINT(srcProtoAddr).c_str(), srcHWAddr);
if (fneIPv4 == tgtProtoAddr) {
write_PDU_ARP_Reply(fneIPv4, srcHWAddr, srcProtoAddr, WUID_FNE);
} else {
write_PDU_ARP_Reply(tgtProtoAddr, srcHWAddr, srcProtoAddr);
}
} else if (opcode == P25_PDU_ARP_REPLY) {
LogMessage(LOG_NET, P25_PDU_STR ", ARP reply, %s is at %u", __IP_FROM_UINT(srcProtoAddr).c_str(), srcHWAddr);
if (fneIPv4 == srcProtoAddr) {
LogWarning(LOG_NET, P25_PDU_STR ", ARP reply, %u is trying to masquerade as us...", srcHWAddr);
} else {
m_arpTable[srcHWAddr] = srcProtoAddr;
}
}
}
break;
case PDUSAP::PACKET_DATA:
{
// is the host virtual tunneling enabled?
if (!m_network->m_host->m_vtunEnabled)
break;
int dataPktOffset = 0U;
if (status->header.getFormat() == PDUFormatType::CONFIRMED && status->extendedAddress)
dataPktOffset = 4U;
if (status->header.getFormat() == PDUFormatType::UNCONFIRMED && status->extendedAddress)
dataPktOffset = 12U;
struct ip* ipHeader = (struct ip*)(status->pduUserData + dataPktOffset);
char srcIp[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(ipHeader->ip_src), srcIp, INET_ADDRSTRLEN);
char dstIp[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(ipHeader->ip_dst), dstIp, INET_ADDRSTRLEN);
uint8_t proto = ipHeader->ip_p;
uint16_t pktLen = Utils::reverseEndian(ipHeader->ip_len); // bryanb: this could be problematic on different endianness
LogMessage(LOG_NET, "P25, PDU -> VTUN, IP Data, srcIp = %s, dstIp = %s, pktLen = %u, proto = %02X", srcIp, dstIp, pktLen, proto);
uint8_t ipFrame[pktLen];
::memset(ipFrame, 0x00U, pktLen);
::memcpy(ipFrame, status->pduUserData + dataPktOffset, pktLen);
Utils::dump(1U, "P25PacketData::dispatch() ipFrame", ipFrame, pktLen);
if (!m_network->m_host->m_tun->write(ipFrame, pktLen)) {
LogError(LOG_NET, P25_PDU_STR ", failed to write IP frame to virtual tunnel, len %u", pktLen);
}
}
break;
case PDUSAP::SNDCP_CTRL_DATA:
{
LogMessage(LOG_NET, P25_PDU_STR ", SNDCP_CTRL_DATA (SNDCP Control Data), blocksToFollow = %u",
status->header.getBlocksToFollow());
processSNDCPControl(status);
}
break;
default:
break;
}
}
/* Helper to dispatch PDU user data back to the FNE network. */
@ -389,6 +544,198 @@ void P25PacketData::dispatchToFNE(uint32_t peerId)
}
}
/* Helper to dispatch PDU user data back to the FNE network. */
void P25PacketData::dispatchUserFrameToFNE(p25::data::DataHeader& dataHeader, bool extendedAddress, uint8_t* pduUserData)
{
uint32_t srcId = (extendedAddress) ? dataHeader.getSrcLLId() : dataHeader.getLLId();
uint32_t dstId = dataHeader.getLLId();
dataHeader.setNs(m_sendSeqNo);
++m_sendSeqNo;
if (m_sendSeqNo > 7U)
m_sendSeqNo = 0U;
// repeat traffic to the connected peers
if (m_network->m_peers.size() > 0U) {
uint32_t i = 0U;
for (auto peer : m_network->m_peers) {
// every 2 peers flush the queue
if (i % 2U == 0U) {
m_network->m_frameQueue->flushQueue();
}
write_PDU_User(peer.first, nullptr, dataHeader, extendedAddress, pduUserData, true);
if (m_network->m_debug) {
LogDebug(LOG_NET, "P25, dstPeer = %u, duid = $%02X, srcId = %u, dstId = %u",
peer.first, DUID::PDU, srcId, dstId);
}
i++;
}
m_network->m_frameQueue->flushQueue();
}
// repeat traffic to external peers
if (m_network->m_host->m_peerNetworks.size() > 0U) {
for (auto peer : m_network->m_host->m_peerNetworks) {
uint32_t dstPeerId = peer.second->getPeerId();
write_PDU_User(dstPeerId, peer.second, dataHeader, extendedAddress, pduUserData);
if (m_network->m_debug) {
LogDebug(LOG_NET, "P25, dstPeer = %u, duid = $%02X, srcId = %u, dstId = %u",
dstPeerId, DUID::PDU, srcId, dstId);
}
}
}
}
/* Helper used to process SNDCP control data from PDU data. */
bool P25PacketData::processSNDCPControl(RxStatus* status)
{
std::unique_ptr<sndcp::SNDCPPacket> packet = SNDCPFactory::create(status->pduUserData);
if (packet == nullptr) {
LogWarning(LOG_NET, P25_PDU_STR ", undecodable SNDCP packet");
return false;
}
uint32_t llId = status->header.getLLId();
switch (packet->getPDUType()) {
case SNDCP_PDUType::ACT_TDS_CTX:
{
SNDCPCtxActRequest* isp = static_cast<SNDCPCtxActRequest*>(packet.get());
LogMessage(LOG_NET, P25_PDU_STR ", SNDCP context activation request, llId = %u, nsapi = %u, ipAddr = %s, nat = $%02X, dsut = $%02X, mdpco = $%02X", llId,
isp->getNSAPI(), __IP_FROM_UINT(isp->getIPAddress()).c_str(), isp->getNAT(), isp->getDSUT(), isp->getMDPCO());
m_arpTable[llId] = isp->getIPAddress();
}
break;
case SNDCP_PDUType::DEACT_TDS_CTX_REQ:
{
SNDCPCtxDeactivation* isp = static_cast<SNDCPCtxDeactivation*>(packet.get());
LogMessage(LOG_NET, P25_PDU_STR ", SNDCP context deactivation request, llId = %u, deactType = %02X", llId,
isp->getDeactType());
m_arpTable.erase(llId);
}
break;
default:
break;
} // switch (packet->getPDUType())
return true;
}
/* Helper write ARP request to the network. */
void P25PacketData::write_PDU_ARP(uint32_t addr)
{
if (!m_network->m_host->m_vtunEnabled)
return;
uint8_t arpPacket[P25_PDU_ARP_PCKT_LENGTH];
::memset(arpPacket, 0x00U, P25_PDU_ARP_PCKT_LENGTH);
__SET_UINT16B(P25_PDU_ARP_CAI_TYPE, arpPacket, 0U); // Hardware Address Type
__SET_UINT16B(PDUSAP::PACKET_DATA, arpPacket, 2U); // Protocol Address Type
arpPacket[4U] = P25_PDU_ARP_HW_ADDR_LENGTH; // Hardware Address Length
arpPacket[5U] = P25_PDU_ARP_PROTO_ADDR_LENGTH; // Protocol Address Length
__SET_UINT16B(P25_PDU_ARP_REQUEST, arpPacket, 6U); // Opcode
__SET_UINT16(WUID_FNE, arpPacket, 8U); // Sender Hardware Address
std::string fneIPv4 = m_network->m_host->m_tun->getIPv4();
__SET_UINT32(__IP_FROM_STR(fneIPv4), arpPacket, 11U); // Sender Protocol Address
__SET_UINT32(addr, arpPacket, 18U); // Target Protocol Address
Utils::dump(1U, "P25PacketData::write_PDU_ARP() arpPacket", arpPacket, P25_PDU_ARP_PCKT_LENGTH);
LogMessage(LOG_NET, P25_PDU_STR ", ARP request, who has %s? tell %s (%u)", __IP_FROM_UINT(addr).c_str(), fneIPv4.c_str(), WUID_FNE);
// assemble a P25 PDU frame header for transport...
data::DataHeader rspHeader = data::DataHeader();
rspHeader.setFormat(PDUFormatType::UNCONFIRMED);
rspHeader.setMFId(MFG_STANDARD);
rspHeader.setAckNeeded(false);
rspHeader.setOutbound(true);
rspHeader.setSAP(PDUSAP::EXT_ADDR);
rspHeader.setLLId(WUID_ALL);
rspHeader.setBlocksToFollow(1U);
rspHeader.setEXSAP(PDUSAP::ARP);
rspHeader.setSrcLLId(WUID_FNE);
rspHeader.calculateLength(P25_PDU_ARP_PCKT_LENGTH);
uint32_t pduLength = rspHeader.getPDULength();
uint8_t pduUserData[pduLength];
::memset(pduUserData, 0x00U, pduLength);
::memcpy(pduUserData + P25_PDU_HEADER_LENGTH_BYTES, arpPacket, P25_PDU_ARP_PCKT_LENGTH);
dispatchUserFrameToFNE(rspHeader, true, pduUserData);
}
/* Helper write ARP reply to the network. */
void P25PacketData::write_PDU_ARP_Reply(uint32_t targetAddr, uint32_t requestorLlid, uint32_t requestorAddr, uint32_t targetLlid)
{
if (!m_network->m_host->m_vtunEnabled)
return;
uint32_t tgtLlid = getLLIdAddress(targetAddr);
if (targetLlid != 0U) {
tgtLlid = targetLlid; // forcibly override
}
if (tgtLlid == 0U)
return;
uint8_t arpPacket[P25_PDU_ARP_PCKT_LENGTH];
::memset(arpPacket, 0x00U, P25_PDU_ARP_PCKT_LENGTH);
__SET_UINT16B(P25_PDU_ARP_CAI_TYPE, arpPacket, 0U); // Hardware Address Type
__SET_UINT16B(PDUSAP::PACKET_DATA, arpPacket, 2U); // Protocol Address Type
arpPacket[4U] = P25_PDU_ARP_HW_ADDR_LENGTH; // Hardware Address Length
arpPacket[5U] = P25_PDU_ARP_PROTO_ADDR_LENGTH; // Protocol Address Length
__SET_UINT16B(P25_PDU_ARP_REPLY, arpPacket, 6U); // Opcode
__SET_UINT16(tgtLlid, arpPacket, 8U); // Sender Hardware Address
__SET_UINT32(targetAddr, arpPacket, 11U); // Sender Protocol Address
__SET_UINT16(requestorLlid, arpPacket, 15U); // Requestor Hardware Address
__SET_UINT32(requestorAddr, arpPacket, 18U); // Requestor Protocol Address
Utils::dump(1U, "P25PacketData::write_PDU_ARP_Reply() arpPacket", arpPacket, P25_PDU_ARP_PCKT_LENGTH);
LogMessage(LOG_NET, P25_PDU_STR ", ARP reply, %s is at %u", __IP_FROM_UINT(targetAddr).c_str(), tgtLlid);
// assemble a P25 PDU frame header for transport...
data::DataHeader rspHeader = data::DataHeader();
rspHeader.setFormat(PDUFormatType::UNCONFIRMED);
rspHeader.setMFId(MFG_STANDARD);
rspHeader.setAckNeeded(false);
rspHeader.setOutbound(true);
rspHeader.setSAP(PDUSAP::EXT_ADDR);
rspHeader.setLLId(WUID_ALL);
rspHeader.setBlocksToFollow(1U);
rspHeader.setEXSAP(PDUSAP::ARP);
rspHeader.setSrcLLId(WUID_FNE);
rspHeader.calculateLength(P25_PDU_ARP_PCKT_LENGTH);
uint32_t pduLength = rspHeader.getPDULength();
uint8_t pduUserData[pduLength];
::memset(pduUserData, 0x00U, pduLength);
::memcpy(pduUserData + P25_PDU_HEADER_LENGTH_BYTES, arpPacket, P25_PDU_ARP_PCKT_LENGTH);
dispatchUserFrameToFNE(rspHeader, true, pduUserData);
}
/* Helper to write user data as a P25 PDU packet. */
void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNet, data::DataHeader& dataHeader,
@ -414,7 +761,7 @@ void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNe
writeNetwork(peerId, peerNet, dataHeader, 0U, buffer, P25_PDU_FEC_LENGTH_BYTES, pktSeq, streamId, queueOnly);
++pktSeq;
uint32_t packetLength = (dataHeader.getPacketLength() + dataHeader.getPadLength() + 4U);
uint32_t packetLength = dataHeader.getPDULength();
if (blocksToFollow > 0U) {
uint32_t dataOffset = 0U;
@ -430,12 +777,11 @@ void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNe
++pktSeq;
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
packetLength += P25_PDU_HEADER_LENGTH_BYTES;
blocksToFollow--;
networkBlock++;
LogMessage(LOG_RF, P25_PDU_STR ", OSP, extended address, sap = $%02X, srcLlId = %u",
LogMessage(LOG_NET, P25_PDU_STR ", OSP, extended address, sap = $%02X, srcLlId = %u",
dataHeader.getEXSAP(), dataHeader.getSrcLLId());
}
@ -443,7 +789,7 @@ void P25PacketData::write_PDU_User(uint32_t peerId, network::PeerNetwork* peerNe
if ((dataHeader.getFormat() == PDUFormatType::CONFIRMED) && (dataHeader.getSAP() == PDUSAP::EXT_ADDR) && extendedAddress) {
dataHeader.encodeExtAddr(pduUserData);
LogMessage(LOG_RF, P25_PDU_STR ", OSP, sap = $%02X, srcLlId = %u",
LogMessage(LOG_NET, P25_PDU_STR ", OSP, sap = $%02X, srcLlId = %u",
dataHeader.getEXSAP(), dataHeader.getSrcLLId());
}
@ -495,3 +841,58 @@ bool P25PacketData::writeNetwork(uint32_t peerId, network::PeerNetwork* peerNet,
return m_network->writePeer(peerId, { NET_FUNC::PROTOCOL, NET_SUBFUNC::PROTOCOL_SUBFUNC_P25 }, message.get(), messageLength, pktSeq, streamId, false, true);
}
}
/* Helper to determine if the logical link ID has an ARP entry. */
bool P25PacketData::hasARPEntry(uint32_t llId) const
{
if (llId == 0U) {
return false;
}
// lookup ARP table entry
try {
uint32_t addr = m_arpTable.at(llId);
if (addr != 0U) {
return true;
}
else {
return false;
}
} catch (...) {
return false;
}
}
/* Helper to get the IP address for the given logical link ID. */
uint32_t P25PacketData::getIPAddress(uint32_t llId)
{
if (llId == 0U) {
return 0U;
}
if (hasARPEntry(llId)) {
return m_arpTable[llId];
}
return 0U;
}
/* Helper to get the logical link ID for the given IP address. */
uint32_t P25PacketData::getLLIdAddress(uint32_t addr)
{
if (addr == 0U) {
return 0U;
}
// lookup ARP table entry
for (auto entry : m_arpTable) {
if (entry.second == addr) {
return entry.first;
}
}
return 0U;
}

69
src/fne/network/callhandler/packetdata/P25PacketData.h
Unescape View File

@ -67,10 +67,30 @@ namespace network
*/
bool processFrame(const uint8_t* data, uint32_t len, uint32_t peerId, uint16_t pktSeq, uint32_t streamId, bool external = false);
/**
* @brief Process a data frame from the virtual IP network.
* @param data Network data buffer.
* @param len Length of data.
* @param alreaedyQueued Flag indicating the data frame being processed is already queued.
*/
void processPacketFrame(const uint8_t* data, uint32_t len, bool alreadyQueued = false);
private:
FNENetwork* m_network;
TagP25Data *m_tag;
static uint8_t m_sendSeqNo;
/**
* @brief Represents a queued data frame from the VTUN.
*/
class VTUNDataFrame {
public:
uint8_t* buffer;
uint32_t bufferLen;
};
std::deque<VTUNDataFrame> m_dataFrames;
/**
* @brief Represents the receive status of a call.
*/
@ -130,6 +150,8 @@ namespace network
typedef std::pair<const uint32_t, RxStatus*> StatusMapPair;
std::unordered_map<uint32_t, RxStatus*> m_status;
std::unordered_map<uint32_t, uint32_t> m_arpTable;
bool m_debug;
/**
@ -142,6 +164,34 @@ namespace network
* @param peerId Peer ID.
*/
void dispatchToFNE(uint32_t peerId);
/**
* @brief Helper to dispatch PDU user data back to the FNE network.
* @param dataHeader Instance of a PDU data header.
* @param extendedAddress Flag indicating whether or not to extended addressing is in use.
* @param pduUserData Buffer containing user data to transmit.
*/
void dispatchUserFrameToFNE(p25::data::DataHeader& dataHeader, bool extendedAddress, uint8_t* pduUserData);
/**
* @brief Helper used to process SNDCP control data from PDU data.
* @param status Instance of the RxStatus class.
* @returns bool True, if SNDCP control data was processed, otherwise false.
*/
bool processSNDCPControl(RxStatus* status);
/**
* @brief Helper write ARP request to the network.
* @param addr IP Address.
*/
void write_PDU_ARP(uint32_t addr);
/**
* @brief Helper write ARP reply to the network.
* @param targetAddr Target IP Address.
* @param requestorLlid Requestor Logical Link Address.
* @param requestorAddr Requestor IP Address.
* @param targetLlId Target Logical Link Address.
*/
void write_PDU_ARP_Reply(uint32_t targetAddr, uint32_t requestorLlid, uint32_t requestorAddr, uint32_t targetLlid = 0U);
/**
* @brief Helper to write user data as a P25 PDU packet.
@ -167,6 +217,25 @@ namespace network
*/
bool writeNetwork(uint32_t peerId, network::PeerNetwork* peerNet, const p25::data::DataHeader& dataHeader, const uint8_t currentBlock,
const uint8_t* data, uint32_t len, uint16_t pktSeq, uint32_t streamId, bool queueOnly = false);
/**
* @brief Helper to determine if the logical link ID has an ARP entry.
* @param llId Logical Link Address.
* @returns bool True, if the logical link ID has an arp entry, otherwise false.
*/
bool hasARPEntry(uint32_t llId) const;
/**
* @brief Helper to get the IP address for the given logical link ID.
* @param llId Logical Link Address.
* @returns uint32_t Numerical IP address.
*/
uint32_t getIPAddress(uint32_t llId);
/**
* @brief Helper to get the logical link ID granted to the given IP address.
* @param addr Numerical IP address.
* @returns uint32_t Logical Link Address.
*/
uint32_t getLLIdAddress(uint32_t addr);
};
} // namespace packetdata
} // namespace callhandler

2
src/host/p25/Control.cpp
Unescape View File

@ -1638,7 +1638,7 @@ void Control::writeRF_TDU(bool noNetwork, bool imm)
m_nid.encode(data + 2U, DUID::TDU);
// Add busy bits
P25Utils::addBusyBits(data + 2U, P25_TDU_FRAME_LENGTH_BITS, true, true);
P25Utils::addStatusBits(data + 2U, P25_TDU_FRAME_LENGTH_BITS, false);
if (!noNetwork)
m_voice->writeNetwork(data + 2U, DUID::TDU);

28
src/host/p25/packet/ControlSignaling.cpp
Unescape View File

@ -184,6 +184,8 @@ bool ControlSignaling::process(uint8_t* data, uint32_t len, std::unique_ptr<lc::
// handle individual DUIDs
if (duid == DUID::TSDU) {
m_inbound = true;
if (m_p25->m_rfState != RS_RF_DATA) {
m_p25->m_rfState = RS_RF_DATA;
}
@ -666,6 +668,7 @@ bool ControlSignaling::process(uint8_t* data, uint32_t len, std::unique_ptr<lc::
m_p25->writeRF_Nulls();
}
m_inbound = false;
m_p25->m_rfState = prevRfState;
return true;
}
@ -1201,6 +1204,7 @@ ControlSignaling::ControlSignaling(Control* p25, bool dumpTSBKData, bool debug,
m_disableGrantSrcIdCheck(false),
m_redundantImmediate(true),
m_redundantGrant(false),
m_inbound(false),
m_dumpTSBK(dumpTSBKData),
m_verbose(verbose),
m_debug(debug)
@ -1299,7 +1303,7 @@ void ControlSignaling::writeRF_TDULC(lc::TDULC* lc, bool noNetwork)
lc->encode(data + 2U);
// Add busy bits
P25Utils::addBusyBits(data + 2U, P25_TDULC_FRAME_LENGTH_BITS, true, true);
P25Utils::addStatusBits(data + 2U, P25_TDULC_FRAME_LENGTH_BITS, false);
m_p25->m_rfTimeout.stop();
@ -1338,7 +1342,7 @@ void ControlSignaling::writeNet_TDULC(lc::TDULC* lc)
lc->encode(buffer + 2U);
// Add busy bits
P25Utils::addBusyBits(buffer + 2U, P25_TDULC_FRAME_LENGTH_BITS, true, true);
P25Utils::addStatusBits(buffer + 2U, P25_TDULC_FRAME_LENGTH_BITS, false);
m_p25->addFrame(buffer, P25_TDULC_FRAME_LENGTH_BYTES + 2U, true);
@ -1394,10 +1398,11 @@ void ControlSignaling::writeRF_TSDU_SBF(lc::TSBK* tsbk, bool noNetwork, bool for
}
// Add busy bits
P25Utils::addBusyBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, true, false);
P25Utils::addStatusBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, m_inbound, true);
P25Utils::addTrunkSlotStatusBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS);
// Set first busy bits to 1,1
P25Utils::setBusyBits(data + 2U, P25_SS0_START, true, true);
P25Utils::setStatusBits(data + 2U, P25_SS0_START, true, true);
if (!noNetwork)
writeNetworkRF(tsbk, data + 2U, true);
@ -1455,10 +1460,11 @@ void ControlSignaling::writeNet_TSDU(lc::TSBK* tsbk)
tsbk->encode(buffer + 2U);
// Add busy bits
P25Utils::addBusyBits(buffer + 2U, P25_TSDU_FRAME_LENGTH_BYTES, true, false);
P25Utils::addStatusBits(buffer + 2U, P25_TSDU_FRAME_LENGTH_BYTES, false, true);
P25Utils::addTrunkSlotStatusBits(buffer + 2U, P25_TSDU_FRAME_LENGTH_BYTES);
// Set first busy bits to 1,1
P25Utils::setBusyBits(buffer + 2U, P25_SS0_START, true, true);
P25Utils::setStatusBits(buffer + 2U, P25_SS0_START, true, true);
m_p25->addFrame(buffer, P25_TSDU_FRAME_LENGTH_BYTES + 2U, true);
@ -1548,10 +1554,8 @@ void ControlSignaling::writeRF_TSDU_MBF(lc::TSBK* tsbk)
P25Utils::encode(tsdu, data + 2U, 114U, 720U);
// Add busy bits
P25Utils::addBusyBits(data + 2U, P25_TSDU_TRIPLE_FRAME_LENGTH_BITS, true, false);
// Add idle bits
P25Utils::addIdleBits(data + 2U, P25_TSDU_TRIPLE_FRAME_LENGTH_BITS, true, true);
P25Utils::addStatusBits(data + 2U, P25_TSDU_TRIPLE_FRAME_LENGTH_BITS, m_inbound, true);
P25Utils::addTrunkSlotStatusBits(data + 2U, P25_TSDU_TRIPLE_FRAME_LENGTH_BITS);
data[0U] = modem::TAG_DATA;
data[1U] = 0x00U;
@ -2855,10 +2859,10 @@ void ControlSignaling::writeNet_TSDU_From_RF(lc::TSBK* tsbk, uint8_t* data)
tsbk->encode(data);
// Add busy bits
P25Utils::addBusyBits(data, P25_TSDU_FRAME_LENGTH_BYTES, true, false);
P25Utils::addStatusBits(data, P25_TSDU_FRAME_LENGTH_BYTES, false);
// Set first busy bits to 1,1
P25Utils::setBusyBits(data, P25_SS0_START, true, true);
P25Utils::setStatusBits(data, P25_SS0_START, true, true);
}
/* Helper to automatically inhibit a source ID on a denial. */

2
src/host/p25/packet/ControlSignaling.h
Unescape View File

@ -205,6 +205,8 @@ namespace p25
bool m_redundantImmediate;
bool m_redundantGrant;
bool m_inbound;
bool m_dumpTSBK;
bool m_verbose;

181
src/host/p25/packet/Data.cpp
Unescape View File

@ -79,6 +79,8 @@ bool Data::process(uint8_t* data, uint32_t len)
// handle individual DUIDs
if (duid == DUID::PDU) {
m_inbound = true;
if (m_p25->m_rfState != RS_RF_DATA) {
m_rfDataHeader.reset();
m_rfExtendedAddress = false;
@ -90,8 +92,8 @@ bool Data::process(uint8_t* data, uint32_t len)
m_p25->m_rfState = RS_RF_DATA;
::memset(m_pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
m_pduUserDataLength = 0U;
::memset(m_rfPduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
m_rfPduUserDataLength = 0U;
}
uint32_t start = m_rfPDUCount * P25_PDU_FRAME_LENGTH_BITS;
@ -196,9 +198,9 @@ bool Data::process(uint8_t* data, uint32_t len)
blocksToFollow--;
// if we are using a secondary header place it in the PDU user data buffer
m_rfDataHeader.getExtAddrData(m_pduUserData + dataOffset);
m_rfDataHeader.getExtAddrData(m_rfPduUserData + dataOffset);
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
m_pduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES;
m_rfPduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES;
}
uint32_t srcId = (m_rfExtendedAddress) ? m_rfDataHeader.getSrcLLId() : m_rfDataHeader.getLLId();
@ -214,7 +216,7 @@ bool Data::process(uint8_t* data, uint32_t len)
// if the primary header has a header offset ensure data if offset by that amount
if (m_rfDataHeader.getHeaderOffset() > 0U) {
offset += m_rfDataHeader.getHeaderOffset() * 8;
m_pduUserDataLength -= m_rfDataHeader.getHeaderOffset();
m_rfPduUserDataLength -= m_rfDataHeader.getHeaderOffset();
}
// decode data blocks
@ -255,9 +257,9 @@ bool Data::process(uint8_t* data, uint32_t len)
}
}
m_rfData[i].getData(m_pduUserData + dataOffset);
m_rfData[i].getData(m_rfPduUserData + dataOffset);
dataOffset += (m_rfDataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
m_pduUserDataLength = dataOffset;
m_rfPduUserDataLength = dataOffset;
// only send data blocks across the network, if we're not an AMBT,
// an RSP or a registration service
@ -277,9 +279,9 @@ bool Data::process(uint8_t* data, uint32_t len)
// to prevent data block offset errors fill the bad block with 0's
uint8_t blankBuf[P25_PDU_CONFIRMED_DATA_LENGTH_BYTES];
::memset(blankBuf, 0x00U, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
::memcpy(m_pduUserData + dataOffset, blankBuf, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
::memcpy(m_rfPduUserData + dataOffset, blankBuf, P25_PDU_CONFIRMED_DATA_LENGTH_BYTES);
dataOffset += P25_PDU_CONFIRMED_DATA_LENGTH_BYTES;
m_pduUserDataLength = dataOffset;
m_rfPduUserDataLength = dataOffset;
}
else {
LogWarning(LOG_RF, P25_PDU_STR ", unfixable PDU data (1/2 rate or CRC), block %u", i);
@ -287,9 +289,9 @@ bool Data::process(uint8_t* data, uint32_t len)
// to prevent data block offset errors fill the bad block with 0's
uint8_t blankBuf[P25_PDU_UNCONFIRMED_LENGTH_BYTES];
::memset(blankBuf, 0x00U, P25_PDU_UNCONFIRMED_LENGTH_BYTES);
::memcpy(m_pduUserData + dataOffset, blankBuf, P25_PDU_UNCONFIRMED_LENGTH_BYTES);
::memcpy(m_rfPduUserData + dataOffset, blankBuf, P25_PDU_UNCONFIRMED_LENGTH_BYTES);
dataOffset += P25_PDU_UNCONFIRMED_LENGTH_BYTES;
m_pduUserDataLength = dataOffset;
m_rfPduUserDataLength = dataOffset;
}
if (m_dumpPDUData) {
@ -301,14 +303,14 @@ bool Data::process(uint8_t* data, uint32_t len)
}
if (m_rfDataHeader.getBlocksToFollow() > 0U) {
bool crcRet = edac::CRC::checkCRC32(m_pduUserData, m_pduUserDataLength);
bool crcRet = edac::CRC::checkCRC32(m_rfPduUserData, m_rfPduUserDataLength);
if (!crcRet) {
LogWarning(LOG_RF, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", m_rfDataHeader.getBlocksToFollow(), m_pduUserDataLength);
LogWarning(LOG_RF, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", m_rfDataHeader.getBlocksToFollow(), m_rfPduUserDataLength);
}
}
if (m_dumpPDUData && m_rfDataBlockCnt > 0U) {
Utils::dump(1U, "PDU Packet", m_pduUserData, m_pduUserDataLength);
Utils::dump(1U, "PDU Packet", m_rfPduUserData, m_rfPduUserDataLength);
}
if (m_rfDataBlockCnt < blocksToFollow) {
@ -401,9 +403,9 @@ bool Data::process(uint8_t* data, uint32_t len)
case PDUSAP::ARP:
{
/* bryanb: quick and dirty ARP logging */
uint8_t arpPacket[22U];
::memset(arpPacket, 0x00U, 22U);
::memcpy(arpPacket, m_pduUserData + 12U, 22U);
uint8_t arpPacket[P25_PDU_ARP_PCKT_LENGTH];
::memset(arpPacket, 0x00U, P25_PDU_ARP_PCKT_LENGTH);
::memcpy(arpPacket, m_rfPduUserData + P25_PDU_HEADER_LENGTH_BYTES, P25_PDU_ARP_PCKT_LENGTH);
uint16_t opcode = __GET_UINT16B(arpPacket, 6U);
uint32_t srcHWAddr = __GET_UINT16(arpPacket, 8U);
@ -429,7 +431,7 @@ bool Data::process(uint8_t* data, uint32_t len)
m_rfDataHeader.getBlocksToFollow());
}
processSNDCPControl();
processSNDCPControl(m_rfPduUserData);
}
break;
case PDUSAP::CONV_DATA_REG:
@ -439,7 +441,7 @@ bool Data::process(uint8_t* data, uint32_t len)
m_rfDataHeader.getBlocksToFollow());
}
processConvDataReg();
processConvDataReg(m_rfPduUserData);
}
break;
case PDUSAP::TRUNK_CTRL:
@ -473,12 +475,13 @@ bool Data::process(uint8_t* data, uint32_t len)
m_rfDataBlockCnt = 0U;
m_rfPDUCount = 0U;
m_rfPDUBits = 0U;
m_pduUserDataLength = 0U;
m_rfPduUserDataLength = 0U;
m_p25->m_rfState = m_prevRfState;
} // switch (m_rfDataHeader.getSAP())
}
m_inbound = false;
return true;
}
else {
@ -609,7 +612,7 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
m_netDataOffset = 0U;
m_netDataBlockCnt = 0U;
m_netPDUCount = 0U;
m_pduUserDataLength = 0U;
m_netPduUserDataLength = 0U;
}
return true;
@ -621,9 +624,6 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
m_netPDUCount++;
m_netDataBlockCnt++;
uint32_t srcId = (m_netExtendedAddress) ? m_netDataHeader.getSrcLLId() : m_netDataHeader.getLLId();
uint32_t dstId = m_netDataHeader.getLLId();
if (m_netDataBlockCnt >= m_netDataHeader.getBlocksToFollow()) {
uint32_t blocksToFollow = m_netDataHeader.getBlocksToFollow();
uint32_t offset = 0U;
@ -660,9 +660,9 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
blocksToFollow--;
// if we are using a secondary header place it in the PDU user data buffer
m_netDataHeader.getExtAddrData(m_pduUserData + dataOffset);
m_netDataHeader.getExtAddrData(m_netPduUserData + dataOffset);
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
m_pduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES;
m_netPduUserDataLength += P25_PDU_HEADER_LENGTH_BYTES;
}
m_netDataBlockCnt = 0U;
@ -703,9 +703,9 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
m_netData[i].getLastBlock());
}
m_netData[i].getData(m_pduUserData + dataOffset);
m_netData[i].getData(m_netPduUserData + dataOffset);
dataOffset += (m_netDataHeader.getFormat() == PDUFormatType::CONFIRMED) ? P25_PDU_CONFIRMED_DATA_LENGTH_BYTES : P25_PDU_UNCONFIRMED_LENGTH_BYTES;
m_pduUserDataLength = dataOffset;
m_netPduUserDataLength = dataOffset;
m_netDataBlockCnt++;
}
@ -724,24 +724,51 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
}
if (m_netDataHeader.getBlocksToFollow() > 0U) {
bool crcRet = edac::CRC::checkCRC32(m_pduUserData, m_pduUserDataLength);
bool crcRet = edac::CRC::checkCRC32(m_netPduUserData, m_netPduUserDataLength);
if (!crcRet) {
LogWarning(LOG_NET, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", m_netDataHeader.getBlocksToFollow(), m_pduUserDataLength);
LogWarning(LOG_NET, P25_PDU_STR ", failed CRC-32 check, blocks %u, len %u", m_netDataHeader.getBlocksToFollow(), m_netPduUserDataLength);
}
}
if (m_dumpPDUData && m_netDataBlockCnt > 0U) {
Utils::dump(1U, "PDU Packet", m_pduUserData, m_pduUserDataLength);
Utils::dump(1U, "PDU Packet", m_netPduUserData, m_netPduUserDataLength);
}
if (m_netDataBlockCnt < blocksToFollow) {
LogWarning(LOG_NET, P25_PDU_STR ", incomplete PDU (%d / %d blocks)", m_netDataBlockCnt, blocksToFollow);
}
uint32_t srcId = (m_netExtendedAddress) ? m_netDataHeader.getSrcLLId() : m_netDataHeader.getLLId();
uint32_t dstId = m_netDataHeader.getLLId();
uint8_t sap = (m_netExtendedAddress) ? m_netDataHeader.getEXSAP() : m_netDataHeader.getSAP();
// handle standard P25 service access points
switch (sap) {
case PDUSAP::ARP:
{
/* bryanb: quick and dirty ARP logging */
uint8_t arpPacket[P25_PDU_ARP_PCKT_LENGTH];
::memset(arpPacket, 0x00U, P25_PDU_ARP_PCKT_LENGTH);
::memcpy(arpPacket, m_netPduUserData + P25_PDU_HEADER_LENGTH_BYTES, P25_PDU_ARP_PCKT_LENGTH);
uint16_t opcode = __GET_UINT16B(arpPacket, 6U);
uint32_t srcHWAddr = __GET_UINT16(arpPacket, 8U);
uint32_t srcProtoAddr = __GET_UINT32(arpPacket, 11U);
uint32_t tgtHWAddr = __GET_UINT16(arpPacket, 15U);
uint32_t tgtProtoAddr = __GET_UINT32(arpPacket, 18U);
if (m_verbose) {
if (opcode == P25_PDU_ARP_REQUEST) {
LogMessage(LOG_NET, P25_PDU_STR ", ARP request, who has %s? tell %s (%u)", __IP_FROM_UINT(tgtProtoAddr).c_str(), __IP_FROM_UINT(srcProtoAddr).c_str(), srcHWAddr);
} else if (opcode == P25_PDU_ARP_REPLY) {
LogMessage(LOG_NET, P25_PDU_STR ", ARP reply, %s is at %u", __IP_FROM_UINT(srcProtoAddr).c_str(), srcHWAddr);
}
}
writeNet_PDU_Buffered(); // re-generate buffered PDU and send it on
}
break;
default:
::ActivityLog("P25", false, "Net data transmission from %u to %u, %u blocks", srcId, dstId, m_netDataHeader.getBlocksToFollow());
@ -760,7 +787,7 @@ bool Data::processNetwork(uint8_t* data, uint32_t len, uint32_t blockLength)
m_netDataOffset = 0U;
m_netDataBlockCnt = 0U;
m_netPDUCount = 0U;
m_pduUserDataLength = 0U;
m_netPduUserDataLength = 0U;
m_p25->m_netState = RS_NET_IDLE;
}
@ -819,7 +846,7 @@ void Data::writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress,
if (blocksToFollow > 0U) {
uint32_t dataOffset = 0U;
uint32_t packetLength = (dataHeader.getPacketLength() + dataHeader.getPadLength() + 4U);
uint32_t packetLength = dataHeader.getPDULength();
// generate the second PDU header
if ((dataHeader.getFormat() == PDUFormatType::UNCONFIRMED) && (dataHeader.getSAP() == PDUSAP::EXT_ADDR) && extendedAddress) {
@ -833,7 +860,6 @@ void Data::writeRF_PDU_User(data::DataHeader& dataHeader, bool extendedAddress,
offset += P25_PDU_FEC_LENGTH_BITS;
dataOffset += P25_PDU_HEADER_LENGTH_BYTES;
packetLength += P25_PDU_HEADER_LENGTH_BYTES;
blocksToFollow--;
@ -1091,14 +1117,17 @@ Data::Data(Control* p25, bool dumpPDUData, bool repeatPDU, bool debug, bool verb
m_retryPDUData(nullptr),
m_retryPDUBitLength(0U),
m_retryCount(0U),
m_pduUserData(nullptr),
m_pduUserDataLength(0U),
m_rfPduUserData(nullptr),
m_rfPduUserDataLength(0U),
m_netPduUserData(nullptr),
m_netPduUserDataLength(0U),
m_fneRegTable(),
m_convRegQueueTable(),
m_convRegTimerTable(),
m_sndcpStateTable(),
m_sndcpReadyTimers(),
m_sndcpStandbyTimers(),
m_inbound(false),
m_dumpPDUData(dumpPDUData),
m_repeatPDU(repeatPDU),
m_verbose(verbose),
@ -1114,8 +1143,10 @@ Data::Data(Control* p25, bool dumpPDUData, bool repeatPDU, bool debug, bool verb
m_netPDU = new uint8_t[P25_PDU_FRAME_LENGTH_BYTES + 2U];
::memset(m_netPDU, 0x00U, P25_PDU_FRAME_LENGTH_BYTES + 2U);
m_pduUserData = new uint8_t[P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U];
::memset(m_pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
m_rfPduUserData = new uint8_t[P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U];
::memset(m_rfPduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
m_netPduUserData = new uint8_t[P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U];
::memset(m_netPduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_CONFIRMED_LENGTH_BYTES + 2U);
m_fneRegTable.clear();
@ -1139,19 +1170,20 @@ Data::~Data()
if (m_retryPDUData != nullptr)
delete m_retryPDUData;
delete[] m_pduUserData;
delete[] m_rfPduUserData;
delete[] m_netPduUserData;
}
/* Helper used to process conventional data registration from PDU data. */
bool Data::processConvDataReg()
bool Data::processConvDataReg(const uint8_t* pduUserData)
{
uint8_t regType = (m_pduUserData[0] >> 4) & 0x0F;
uint8_t regType = (pduUserData[0U] >> 4) & 0x0F;
switch (regType) {
case PDURegType::CONNECT:
{
uint32_t llId = (m_pduUserData[1U] << 16) + (m_pduUserData[2U] << 8) + m_pduUserData[3U];
uint32_t ipAddr = (m_pduUserData[8U] << 24) + (m_pduUserData[9U] << 16) + (m_pduUserData[10U] << 8) + m_pduUserData[11U];
uint32_t llId = (pduUserData[1U] << 16) + (pduUserData[2U] << 8) + pduUserData[3U];
uint32_t ipAddr = (pduUserData[8U] << 24) + (pduUserData[9U] << 16) + (pduUserData[10U] << 8) + pduUserData[11U];
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", CONNECT (Registration Request Connect), llId = %u, ipAddr = %s", llId, __IP_FROM_UINT(ipAddr).c_str());
@ -1168,7 +1200,7 @@ bool Data::processConvDataReg()
break;
case PDURegType::DISCONNECT:
{
uint32_t llId = (m_pduUserData[1U] << 16) + (m_pduUserData[2U] << 8) + m_pduUserData[3U];
uint32_t llId = (pduUserData[1U] << 16) + (pduUserData[2U] << 8) + pduUserData[3U];
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", DISCONNECT (Registration Request Disconnect), llId = %u", llId);
@ -1199,16 +1231,16 @@ bool Data::processConvDataReg()
/* Helper used to process SNDCP control data from PDU data. */
bool Data::processSNDCPControl()
bool Data::processSNDCPControl(const uint8_t* pduUserData)
{
if (!m_p25->m_sndcpSupport) {
return false;
}
uint8_t pduUserData[P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES];
::memset(pduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES);
uint8_t txPduUserData[P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES];
::memset(txPduUserData, 0x00U, P25_MAX_PDU_BLOCKS * P25_PDU_UNCONFIRMED_LENGTH_BYTES);
std::unique_ptr<sndcp::SNDCPPacket> packet = SNDCPFactory::create(m_pduUserData);
std::unique_ptr<sndcp::SNDCPPacket> packet = SNDCPFactory::create(pduUserData);
if (packet == nullptr) {
LogWarning(LOG_RF, P25_PDU_STR ", undecodable SNDCP packet");
return false;
@ -1242,10 +1274,10 @@ bool Data::processSNDCPControl()
osp->setNSAPI(DEFAULT_NSAPI);
osp->setRejectCode(SNDCPRejectReason::SU_NOT_PROVISIONED);
osp->encode(pduUserData);
osp->encode(txPduUserData);
rspHeader.calculateLength(2U);
writeRF_PDU_User(rspHeader, false, pduUserData);
writeRF_PDU_User(rspHeader, false, txPduUserData);
return true;
}
@ -1257,10 +1289,10 @@ bool Data::processSNDCPControl()
osp->setNSAPI(DEFAULT_NSAPI);
osp->setRejectCode(SNDCPRejectReason::STATIC_IP_ALLOCATION_UNSUPPORTED);
osp->encode(pduUserData);
osp->encode(txPduUserData);
rspHeader.calculateLength(2U);
writeRF_PDU_User(rspHeader, false, pduUserData);
writeRF_PDU_User(rspHeader, false, txPduUserData);
sndcpReset(llId, true);
}
@ -1272,10 +1304,10 @@ bool Data::processSNDCPControl()
osp->setNSAPI(DEFAULT_NSAPI);
osp->setRejectCode(SNDCPRejectReason::DYN_IP_ALLOCATION_UNSUPPORTED);
osp->encode(pduUserData);
osp->encode(txPduUserData);
rspHeader.calculateLength(2U);
writeRF_PDU_User(rspHeader, false, pduUserData);
writeRF_PDU_User(rspHeader, false, txPduUserData);
sndcpReset(llId, true);
@ -1290,10 +1322,10 @@ bool Data::processSNDCPControl()
osp->setMTU(SNDCP_MTU_510);
osp->setMDPCO(isp->getMDPCO());
osp->encode(pduUserData);
osp->encode(txPduUserData);
rspHeader.calculateLength(13U);
writeRF_PDU_User(rspHeader, rspHeader, false, pduUserData);
writeRF_PDU_User(rspHeader, rspHeader, false, txPduUserData);
m_sndcpStateTable[llId] = SNDCPState::STANDBY;
m_sndcpReadyTimers[llId].stop();
@ -1308,10 +1340,10 @@ bool Data::processSNDCPControl()
osp->setNSAPI(DEFAULT_NSAPI);
osp->setRejectCode(SNDCPRejectReason::ANY_REASON);
osp->encode(pduUserData);
osp->encode(txPduUserData);
rspHeader.calculateLength(2U);
writeRF_PDU_User(rspHeader, false, pduUserData);
writeRF_PDU_User(rspHeader, false, txPduUserData);
sndcpReset(llId, true);
}
@ -1401,11 +1433,12 @@ void Data::writeRF_PDU(const uint8_t* pdu, uint32_t bitLength, bool noNulls, boo
// Regenerate NID
m_p25->m_nid.encode(data + 2U, DUID::PDU);
// Add busy bits
P25Utils::addBusyBits(data + 2U, newBitLength, true, false);
// Add status bits
P25Utils::addStatusBits(data + 2U, newBitLength, false);
P25Utils::addIdleStatusBits(data + 2U, newBitLength);
// Add idle bits
P25Utils::addIdleBits(data + 2U, newBitLength, true, true);
// Set first busy bits to 1,1
P25Utils::setStatusBits(data + 2U, P25_SS0_START, true, true);
if (m_p25->m_duplex) {
data[0U] = modem::TAG_DATA;
@ -1451,7 +1484,7 @@ void Data::writeNet_PDU_Buffered()
// generate the second PDU header
if ((m_netDataHeader.getFormat() == PDUFormatType::UNCONFIRMED) && (m_netDataHeader.getSAP() == PDUSAP::EXT_ADDR) && m_netExtendedAddress) {
m_netDataHeader.encodeExtAddr(m_pduUserData, true);
m_netDataHeader.encodeExtAddr(m_netPduUserData, true);
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
m_netDataHeader.encodeExtAddr(block);
@ -1471,7 +1504,7 @@ void Data::writeNet_PDU_Buffered()
// are we processing extended address data from the first block?
if ((m_netDataHeader.getFormat() == PDUFormatType::CONFIRMED) && (m_netDataHeader.getSAP() == PDUSAP::EXT_ADDR) && m_netExtendedAddress) {
m_netDataHeader.encodeExtAddr(m_pduUserData);
m_netDataHeader.encodeExtAddr(m_netPduUserData);
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", OSP, extended address, sap = $%02X, srcLlId = %u",
@ -1479,13 +1512,17 @@ void Data::writeNet_PDU_Buffered()
}
}
edac::CRC::addCRC32(m_pduUserData, m_pduUserDataLength);
edac::CRC::addCRC32(m_netPduUserData, m_netPduUserDataLength);
if (m_dumpPDUData) {
Utils::dump("OSP PDU User Data (NET)", m_netPduUserData, m_netPduUserDataLength);
}
// generate the PDU data
for (uint32_t i = 0U; i < blocksToFollow; i++) {
m_netData[i].setFormat(m_netDataHeader);
m_netData[i].setSerialNo(i);
m_netData[i].setData(m_pduUserData + dataOffset);
m_netData[i].setData(m_netPduUserData + dataOffset);
if (m_verbose) {
LogMessage(LOG_NET, P25_PDU_STR ", OSP, block %u, fmt = $%02X, lastBlock = %u",
@ -1536,7 +1573,7 @@ void Data::writeRF_PDU_Buffered()
// generate the second PDU header
if ((m_rfDataHeader.getFormat() == PDUFormatType::UNCONFIRMED) && (m_rfDataHeader.getSAP() == PDUSAP::EXT_ADDR) && m_rfExtendedAddress) {
m_rfDataHeader.encodeExtAddr(m_pduUserData, true);
m_rfDataHeader.encodeExtAddr(m_rfPduUserData, true);
::memset(block, 0x00U, P25_PDU_FEC_LENGTH_BYTES);
m_rfDataHeader.encodeExtAddr(block);
@ -1556,7 +1593,7 @@ void Data::writeRF_PDU_Buffered()
// are we processing extended address data from the first block?
if ((m_rfDataHeader.getFormat() == PDUFormatType::CONFIRMED) && (m_rfDataHeader.getSAP() == PDUSAP::EXT_ADDR) && m_rfExtendedAddress) {
m_rfDataHeader.encodeExtAddr(m_pduUserData);
m_rfDataHeader.encodeExtAddr(m_rfPduUserData);
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", OSP, extended address, sap = $%02X, srcLlId = %u",
@ -1564,13 +1601,17 @@ void Data::writeRF_PDU_Buffered()
}
}
edac::CRC::addCRC32(m_pduUserData, m_pduUserDataLength);
edac::CRC::addCRC32(m_rfPduUserData, m_rfPduUserDataLength);
if (m_dumpPDUData) {
Utils::dump("OSP PDU User Data (RF)", m_rfPduUserData, m_rfPduUserDataLength);
}
// generate the PDU data
for (uint32_t i = 0U; i < blocksToFollow; i++) {
m_rfData[i].setFormat(m_rfDataHeader);
m_rfData[i].setSerialNo(i);
m_rfData[i].setData(m_pduUserData + dataOffset);
m_rfData[i].setData(m_rfPduUserData + dataOffset);
if (m_verbose) {
LogMessage(LOG_RF, P25_PDU_STR ", OSP, block %u, fmt = $%02X, lastBlock = %u",

16
src/host/p25/packet/Data.h
Unescape View File

@ -140,8 +140,10 @@ namespace p25
uint32_t m_retryPDUBitLength;
uint8_t m_retryCount;
uint8_t* m_pduUserData;
uint32_t m_pduUserDataLength;
uint8_t* m_rfPduUserData;
uint32_t m_rfPduUserDataLength;
uint8_t* m_netPduUserData;
uint32_t m_netPduUserDataLength;
std::unordered_map<uint32_t, uint32_t> m_fneRegTable;
@ -152,6 +154,8 @@ namespace p25
std::unordered_map<uint32_t, Timer> m_sndcpReadyTimers;
std::unordered_map<uint32_t, Timer> m_sndcpStandbyTimers;
bool m_inbound;
bool m_dumpPDUData;
bool m_repeatPDU;
@ -174,14 +178,16 @@ namespace p25
/**
* @brief Helper used to process conventional data registration from PDU data.
* @returns bool True, if SNDCP control data was processed, otherwise false.
* @param pduUserData Buffer containing user data to transmit.
* @returns bool True, if conventional data registration data was processed, otherwise false.
*/
bool processConvDataReg();
bool processConvDataReg(const uint8_t* pduUserData);
/**
* @brief Helper used to process SNDCP control data from PDU data.
* @param pduUserData Buffer containing user data to transmit.
* @returns bool True, if SNDCP control data was processed, otherwise false.
*/
bool processSNDCPControl();
bool processSNDCPControl(const uint8_t* pduUserData);
/**
* @brief Write data processed from RF to the network.

39
src/host/p25/packet/Voice.cpp
Unescape View File

@ -58,6 +58,8 @@ void Voice::resetRF()
m_rfUndecodableLC = 0U;
m_vocLDU1Count = 0U;
m_roamLDU1Count = 0U;
m_inbound = false;
}
/* Resets the data states for the network. */
@ -106,6 +108,8 @@ bool Voice::process(uint8_t* data, uint32_t len)
if (m_p25->m_rfState == RS_RF_LISTENING || m_p25->m_rfState == RS_RF_AUDIO) {
resetRF();
m_inbound = true;
lc::LC lc = lc::LC();
bool ret = lc.decodeHDU(data + 2U);
if (!ret) {
@ -192,9 +196,12 @@ bool Voice::process(uint8_t* data, uint32_t len)
lc::LC lc = lc::LC();
bool ret = lc.decodeLDU1(data + 2U);
if (!ret) {
m_inbound = false;
return false;
}
m_inbound = true;
rsValue = lc.getRS();
uint32_t srcId = lc.getSrcId();
@ -495,7 +502,7 @@ bool Voice::process(uint8_t* data, uint32_t len)
m_rfLC.encodeHDU(buffer + 2U);
// Add busy bits
P25Utils::addBusyBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, false, true);
P25Utils::addStatusBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, m_inbound);
writeNetwork(buffer, DUID::HDU);
@ -637,6 +644,8 @@ bool Voice::process(uint8_t* data, uint32_t len)
}
}
m_inbound = true;
rsValue = m_rfLC.getRS();
alreadyDecoded = false;
@ -715,7 +724,7 @@ bool Voice::process(uint8_t* data, uint32_t len)
m_rfFrames++;
// add busy bits
P25Utils::addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true);
P25Utils::addStatusBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, m_inbound);
writeNetwork(data + 2U, DUID::LDU1, frameType);
@ -773,6 +782,8 @@ bool Voice::process(uint8_t* data, uint32_t len)
m_rfLastLDU2 = m_rfLC;
}
m_inbound = true;
// generate Sync
Sync::addP25Sync(data + 2U);
@ -821,7 +832,7 @@ bool Voice::process(uint8_t* data, uint32_t len)
m_rfFrames++;
// add busy bits
P25Utils::addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true);
P25Utils::addStatusBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, m_inbound);
writeNetwork(data + 2U, DUID::LDU2);
@ -868,6 +879,8 @@ bool Voice::process(uint8_t* data, uint32_t len)
resetRF();
}
m_inbound = true;
m_lastRejectId = 0U;
::ActivityLog("P25", true, "RF VSELP voice transmission");
@ -908,7 +921,7 @@ bool Voice::process(uint8_t* data, uint32_t len)
m_rfLC.encodeHDU(buffer + 2U);
// Add busy bits
P25Utils::addBusyBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, false, true);
P25Utils::addStatusBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, m_inbound);
writeNetwork(buffer, DUID::HDU);
@ -942,6 +955,8 @@ bool Voice::process(uint8_t* data, uint32_t len)
if (m_p25->m_rfState == RS_RF_AUDIO) {
m_rfFrames++;
m_inbound = true;
// generate Sync
Sync::addP25Sync(data + 2U);
@ -949,7 +964,7 @@ bool Voice::process(uint8_t* data, uint32_t len)
m_p25->m_nid.encode(data + 2U, DUID::VSELP1);
// add busy bits
P25Utils::addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true);
P25Utils::addStatusBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, m_inbound);
writeNetwork(data + 2U, DUID::VSELP1);
@ -982,6 +997,8 @@ bool Voice::process(uint8_t* data, uint32_t len)
else if (m_p25->m_rfState == RS_RF_AUDIO) {
m_rfFrames++;
m_inbound = true;
// generate Sync
Sync::addP25Sync(data + 2U);
@ -989,7 +1006,7 @@ bool Voice::process(uint8_t* data, uint32_t len)
m_p25->m_nid.encode(data + 2U, DUID::VSELP2);
// add busy bits
P25Utils::addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true);
P25Utils::addStatusBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, m_inbound);
writeNetwork(data + 2U, DUID::VSELP2);
@ -1063,6 +1080,7 @@ bool Voice::process(uint8_t* data, uint32_t len)
m_p25->writeRF_ControlData();
}
m_inbound = false;
m_p25->m_rfState = RS_RF_LISTENING;
return true;
}
@ -1371,6 +1389,7 @@ Voice::Voice(Control* p25, bool debug, bool verbose) :
m_silenceThreshold(DEFAULT_SILENCE_THRESHOLD),
m_vocLDU1Count(0U),
m_roamLDU1Count(0U),
m_inbound(false),
m_verbose(verbose),
m_debug(debug)
{
@ -1465,7 +1484,7 @@ void Voice::writeNet_TDU()
m_p25->m_nid.encode(buffer + 2U, DUID::TDU);
// Add busy bits
P25Utils::addBusyBits(buffer + 2U, P25_TDU_FRAME_LENGTH_BITS, true, true);
P25Utils::addStatusBits(buffer + 2U, P25_TDU_FRAME_LENGTH_BITS, false);
m_p25->addFrame(buffer, P25_TDU_FRAME_LENGTH_BYTES + 2U, true);
@ -1733,7 +1752,7 @@ void Voice::writeNet_LDU1()
m_netLC.encodeHDU(buffer + 2U);
// Add busy bits
P25Utils::addBusyBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, false, true);
P25Utils::addStatusBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, false);
buffer[0U] = modem::TAG_DATA;
buffer[1U] = 0x00U;
@ -1844,7 +1863,7 @@ void Voice::writeNet_LDU1()
m_netLSD.encode(buffer + 2U);
// Add busy bits
P25Utils::addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true);
P25Utils::addStatusBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false);
buffer[0U] = modem::TAG_DATA;
buffer[1U] = 0x00U;
@ -1934,7 +1953,7 @@ void Voice::writeNet_LDU2()
m_netLSD.encode(buffer + 2U);
// Add busy bits
P25Utils::addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true);
P25Utils::addStatusBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false);
buffer[0U] = modem::TAG_DATA;
buffer[1U] = 0x00U;

2
src/host/p25/packet/Voice.h
Unescape View File

@ -124,6 +124,8 @@ namespace p25
uint8_t m_vocLDU1Count;
uint8_t m_roamLDU1Count;
bool m_inbound;
bool m_verbose;
bool m_debug;

2
src/host/setup/SetupApplication.h
Unescape View File

@ -63,7 +63,7 @@ protected:
nid->encode(data + 2U, DUID::TDU);
// Add busy bits
p25::P25Utils::addBusyBits(data + 2U, P25_TDU_FRAME_LENGTH_BITS, true, true);
p25::P25Utils::addStatusBits(data + 2U, P25_TDU_FRAME_LENGTH_BITS, false);
data[0U] = modem::TAG_EOT;
data[1U] = 0x00U;

Write Preview
Loading…
Cancel
Save

Powered by TurnKey Linux.