diff --git a/P25Control.cpp b/P25Control.cpp index 4ba0ae5..265d5bb 100644 --- a/P25Control.cpp +++ b/P25Control.cpp @@ -57,7 +57,8 @@ m_lastDUID(P25_DUID_TERM), m_audio(), m_rfData(), m_netData(), -m_lsd(), +m_rfLSD(), +m_netLSD(), m_netLDU1(NULL), m_netLDU2(NULL), m_fp(NULL) @@ -178,7 +179,7 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) m_rfData.processLDU1(data + 2U); // Regenerate the Low Speed Data - m_lsd.process(data + 2U); + m_rfLSD.process(data + 2U); // Regenerate Audio unsigned int errors = m_audio.process(data + 2U); @@ -205,12 +206,12 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) } if (m_rfState == RS_RF_LISTENING) { - unsigned int src = m_rfData.getSource(); - bool grp = m_rfData.getGroup(); - unsigned int dst = m_rfData.getDest(); - std::string source = m_lookup->find(src); - LogMessage("P25, received RF transmission from %s to %s%u", source.c_str(), grp ? "TG " : "", dst); - m_display->writeP25(source.c_str(), grp, dst, "R"); + unsigned int srcId = m_rfData.getSrcId(); + bool grp = m_rfData.getLCF() == P25_LCF_GROUP; + unsigned int dstId = m_rfData.getDstId(); + std::string source = m_lookup->find(srcId); + LogMessage("P25, received RF transmission from %s to %s%u", source.c_str(), grp ? "TG " : "", dstId); + m_display->writeP25(source.c_str(), grp, dstId, "R"); m_rfState = RS_RF_AUDIO; } } else if (duid == P25_DUID_LDU2) { @@ -223,11 +224,11 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) // Regenerate NID m_nid.encode(data + 2U, P25_DUID_LDU2); - // Add the dummy LDU2 data - m_netData.createLDU2(data + 2U); + // Regenerate the LDU2 data + m_rfData.processLDU2(data + 2U); // Regenerate the Low Speed Data - m_lsd.process(data + 2U); + m_rfLSD.process(data + 2U); // Regenerate Audio unsigned int errors = m_audio.process(data + 2U); @@ -495,10 +496,10 @@ void CP25Control::writeNetwork(const unsigned char *data, unsigned char type) switch (type) { case P25_DUID_LDU1: - m_network->writeLDU1(data); + m_network->writeLDU1(data, m_rfData, m_rfLSD); break; case P25_DUID_LDU2: - m_network->writeLDU2(data); + m_network->writeLDU2(data, m_rfData, m_rfLSD); break; case P25_DUID_TERM: case P25_DUID_TERM_LC: @@ -542,18 +543,32 @@ void CP25Control::createHeader() writeQueueNet(buffer, P25_HDR_FRAME_LENGTH_BYTES + 2U); - bool grp = m_netLDU1[51U] == 0x00U; - unsigned int dst = (m_netLDU1[76U] << 16) + (m_netLDU1[77U] << 8) + m_netLDU1[78U]; - unsigned int src = (m_netLDU1[101U] << 16) + (m_netLDU1[102U] << 8) + m_netLDU1[103U]; - std::string source = m_lookup->find(src); + unsigned char lcf = m_netLDU1[51U]; + unsigned char mfId = m_netLDU1[52U]; + unsigned int dstId = (m_netLDU1[76U] << 16) + (m_netLDU1[77U] << 8) + m_netLDU1[78U]; + unsigned int srcId = (m_netLDU1[101U] << 16) + (m_netLDU1[102U] << 8) + m_netLDU1[103U]; - LogMessage("P25, received network transmission from %s to %s%u", source.c_str(), grp ? "TG " : "", dst); + unsigned char algId = m_netLDU2[126U]; + unsigned int kId = (m_netLDU2[127U] << 8) + m_netLDU2[128U]; - m_display->writeP25(source.c_str(), grp, dst, "N"); + unsigned char mi[P25_MI_LENGTH_BYTES]; + ::memcpy(mi + 0U, m_netLDU2 + 51U, 3U); + ::memcpy(mi + 3U, m_netLDU2 + 76U, 3U); + ::memcpy(mi + 6U, m_netLDU2 + 101U, 3U); - m_netData.setSource(src); - m_netData.setGroup(grp); - m_netData.setDest(dst); + m_netData.setMI(mi); + m_netData.setAlgId(algId); + m_netData.setKId(kId); + m_netData.setLCF(lcf); + m_netData.setMFId(mfId); + m_netData.setSrcId(srcId); + m_netData.setDstId(dstId); + + std::string source = m_lookup->find(srcId); + + LogMessage("P25, received network transmission from %s to %s%u", source.c_str(), lcf == P25_LCF_GROUP ? "TG " : "", dstId); + + m_display->writeP25(source.c_str(), lcf == P25_LCF_GROUP, dstId, "N"); m_netState = RS_NET_AUDIO; m_netTimeout.start(); @@ -576,7 +591,7 @@ void CP25Control::createLDU1() m_nid.encode(buffer + 2U, P25_DUID_LDU1); // Add the LDU1 data - m_netData.createLDU1(buffer + 2U); + m_netData.encodeLDU1(buffer + 2U); // Add the Audio m_audio.encode(buffer + 2U, m_netLDU1 + 10U, 0U); @@ -590,7 +605,9 @@ void CP25Control::createLDU1() m_audio.encode(buffer + 2U, m_netLDU1 + 204U, 8U); // Add the Low Speed Data - m_lsd.encode(buffer + 2U, m_netLDU1[201U], m_netLDU1[202U]); + m_netLSD.setLSD1(m_netLDU1[201U]); + m_netLSD.setLSD2(m_netLDU1[202U]); + m_netLSD.encode(buffer + 2U); // Add busy bits addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); @@ -614,8 +631,8 @@ void CP25Control::createLDU2() // Add the NID m_nid.encode(buffer + 2U, P25_DUID_LDU2); - // Add the dummy LDU2 data - m_netData.createLDU2(buffer + 2U); + // Add the LDU2 data + m_netData.encodeLDU2(buffer + 2U); // Add the Audio m_audio.encode(buffer + 2U, m_netLDU2 + 10U, 0U); @@ -629,7 +646,9 @@ void CP25Control::createLDU2() m_audio.encode(buffer + 2U, m_netLDU2 + 204U, 8U); // Add the Low Speed Data - m_lsd.encode(buffer + 2U, m_netLDU2[201U], m_netLDU2[202U]); + m_netLSD.setLSD1(m_netLDU2[201U]); + m_netLSD.setLSD2(m_netLDU2[202U]); + m_netLSD.encode(buffer + 2U); // Add busy bits addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); diff --git a/P25Control.h b/P25Control.h index f70c279..45200e6 100644 --- a/P25Control.h +++ b/P25Control.h @@ -68,7 +68,8 @@ private: CP25Audio m_audio; CP25Data m_rfData; CP25Data m_netData; - CP25LowSpeedData m_lsd; + CP25LowSpeedData m_rfLSD; + CP25LowSpeedData m_netLSD; unsigned char* m_netLDU1; unsigned char* m_netLDU2; FILE* m_fp; diff --git a/P25Data.cpp b/P25Data.cpp index 4b486b4..f91837a 100644 --- a/P25Data.cpp +++ b/P25Data.cpp @@ -34,26 +34,29 @@ const unsigned char DUMMY_HEADER[] = { 0xE3U, 0x28U, 0xB0U, 0xB7U, 0x73U, 0x76U, 0x1EU, 0x26U, 0x0CU, 0x75U, 0x5BU, 0xF7U, 0x4DU, 0x5FU, 0x5AU, 0x37U, 0x18U}; -const unsigned char DUMMY_LDU2[] = { - 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x80U, 0x00U, 0x00U, 0xACU, 0xB8U, 0xA4U, 0x9BU, - 0xDCU, 0x75U}; - const unsigned char BIT_MASK_TABLE[] = { 0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U }; #define WRITE_BIT(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7]) #define READ_BIT(p,i) (p[(i)>>3] & BIT_MASK_TABLE[(i)&7]) CP25Data::CP25Data() : -m_source(0U), -m_group(true), -m_dest(0U), +m_mi(NULL), +m_mfId(0U), +m_algId(0U), +m_kId(0U), +m_lcf(0x00U), +m_emergency(false), +m_srcId(0U), +m_dstId(0U), m_rs241213(), m_rs24169() { + m_mi = new unsigned char[P25_MI_LENGTH_BYTES]; } CP25Data::~CP25Data() { + delete[] m_mi; } void CP25Data::processHeader(unsigned char* data) @@ -100,26 +103,33 @@ void CP25Data::processLDU1(unsigned char* data) CP25Utils::decode(data, raw, 1356U, 1398U); decodeLDUHamming(raw, rs + 15U); + // CUtils::dump(1U, "P25, LDU1 Data before", rs, 18U); + m_rs241213.decode(rs); - // CUtils::dump(1U, "P25, LDU1 Data", rs, 9U); + m_lcf = rs[0U]; + m_mfId = rs[1U]; - switch (rs[0U]) { + switch (m_lcf) { case P25_LCF_GROUP: - m_dest = (rs[4U] << 8) + rs[5U]; - m_source = (rs[6U] << 16) + (rs[7U] << 8) + rs[8U]; - m_group = true; + m_emergency = (rs[2U] & 0x80U) == 0x80U; + m_dstId = (rs[4U] << 8) + rs[5U]; + m_srcId = (rs[6U] << 16) + (rs[7U] << 8) + rs[8U]; break; case P25_LCF_PRIVATE: - m_dest = (rs[3U] << 16) + (rs[4U] << 8) + rs[5U]; - m_source = (rs[6U] << 16) + (rs[7U] << 8) + rs[8U]; - m_group = false; + m_emergency = false; + m_dstId = (rs[3U] << 16) + (rs[4U] << 8) + rs[5U]; + m_srcId = (rs[6U] << 16) + (rs[7U] << 8) + rs[8U]; break; default: - LogMessage("P25, unknown LCF value in LDU1 - $%02X", rs[0U]); + LogMessage("P25, unknown LCF value in LDU1 - $%02X", m_lcf); break; } + // m_rs241213.encode(rs); + + // CUtils::dump(1U, "P25, LDU1 Data after", rs, 18U); + encodeLDUHamming(raw, rs + 0U); CP25Utils::encode(raw, data, 410U, 452U); @@ -139,9 +149,60 @@ void CP25Data::processLDU1(unsigned char* data) CP25Utils::encode(raw, data, 1356U, 1398U); } -void CP25Data::createLDU1(unsigned char* data) +void CP25Data::encodeLDU1(unsigned char* data) { assert(data != NULL); + + unsigned char rs[18U]; + ::memset(rs, 0x00U, 18U); + + rs[0U] = m_lcf; + rs[1U] = m_mfId; + + switch (m_lcf) { + case P25_LCF_GROUP: + rs[2U] = m_emergency ? 0x80U : 0x00U; + rs[4U] = (m_dstId >> 8) & 0xFFU; + rs[5U] = (m_dstId >> 0) & 0xFFU; + rs[6U] = (m_srcId >> 16) & 0xFFU; + rs[7U] = (m_srcId >> 8) & 0xFFU; + rs[8U] = (m_srcId >> 0) & 0xFFU; + break; + case P25_LCF_PRIVATE: + rs[3U] = (m_dstId >> 16) & 0xFFU; + rs[4U] = (m_dstId >> 8) & 0xFFU; + rs[5U] = (m_dstId >> 0) & 0xFFU; + rs[6U] = (m_srcId >> 16) & 0xFFU; + rs[7U] = (m_srcId >> 8) & 0xFFU; + rs[8U] = (m_srcId >> 0) & 0xFFU; + break; + default: + LogMessage("P25, unknown LCF value in LDU1 - $%02X", m_lcf); + break; + } + + // m_rs241213.encode(rs); + + // CUtils::dump(1U, "P25, LDU1 Data", rs, 18U); + + unsigned char raw[5U]; + encodeLDUHamming(raw, rs + 0U); + CP25Utils::encode(raw, data, 410U, 452U); + + encodeLDUHamming(raw, rs + 3U); + CP25Utils::encode(raw, data, 600U, 640U); + + encodeLDUHamming(raw, rs + 6U); + CP25Utils::encode(raw, data, 788U, 830U); + + encodeLDUHamming(raw, rs + 9U); + CP25Utils::encode(raw, data, 978U, 1020U); + + encodeLDUHamming(raw, rs + 12U); + CP25Utils::encode(raw, data, 1168U, 1208U); + + encodeLDUHamming(raw, rs + 15U); + CP25Utils::encode(raw, data, 1356U, 1398U); } void CP25Data::processLDU2(unsigned char* data) @@ -169,9 +230,17 @@ void CP25Data::processLDU2(unsigned char* data) CP25Utils::decode(data, raw, 1356U, 1398U); decodeLDUHamming(raw, rs + 15U); + // CUtils::dump(1U, "P25, LDU2 Data before", rs, 18U); + m_rs24169.decode(rs); - // CUtils::dump(1U, "P25, LDU2 Data", rs, 18U); + ::memcpy(m_mi, rs + 0U, P25_MI_LENGTH_BYTES); + m_algId = rs[9U]; + m_kId = (rs[10U] << 8) + rs[11U]; + + // m_rs24169.encode(rs); + + // CUtils::dump(1U, "P25, LDU2 Data after", rs, 18U); encodeLDUHamming(raw, rs + 0U); CP25Utils::encode(raw, data, 410U, 452U); @@ -192,64 +261,132 @@ void CP25Data::processLDU2(unsigned char* data) CP25Utils::encode(raw, data, 1356U, 1398U); } -void CP25Data::createLDU2(unsigned char* data) +void CP25Data::encodeLDU2(unsigned char* data) { assert(data != NULL); + unsigned char rs[18U]; + ::memset(rs, 0x00U, 18U); + + ::memcpy(rs + 0U, m_mi, P25_MI_LENGTH_BYTES); + + rs[9U] = m_algId; + + rs[10U] = (m_kId >> 8) & 0xFFU; + rs[11U] = (m_kId >> 0) & 0xFFU; + + // m_rs24169.encode(rs); + + // CUtils::dump(1U, "P25, LDU2 Data", rs, 18U); + unsigned char raw[5U]; - encodeLDUHamming(raw, DUMMY_LDU2 + 0U); + encodeLDUHamming(raw, rs + 0U); CP25Utils::encode(raw, data, 410U, 452U); - encodeLDUHamming(raw, DUMMY_LDU2 + 3U); + encodeLDUHamming(raw, rs + 3U); CP25Utils::encode(raw, data, 600U, 640U); - encodeLDUHamming(raw, DUMMY_LDU2 + 6U); + encodeLDUHamming(raw, rs + 6U); CP25Utils::encode(raw, data, 788U, 830U); - encodeLDUHamming(raw, DUMMY_LDU2 + 9U); + encodeLDUHamming(raw, rs + 9U); CP25Utils::encode(raw, data, 978U, 1020U); - encodeLDUHamming(raw, DUMMY_LDU2 + 12U); + encodeLDUHamming(raw, rs + 12U); CP25Utils::encode(raw, data, 1168U, 1208U); - encodeLDUHamming(raw, DUMMY_LDU2 + 15U); + encodeLDUHamming(raw, rs + 15U); CP25Utils::encode(raw, data, 1356U, 1398U); } -void CP25Data::setSource(unsigned int source) +void CP25Data::setMI(const unsigned char* mi) { - m_source = source; + assert(mi != NULL); + + ::memcpy(m_mi, mi, P25_MI_LENGTH_BYTES); } -unsigned int CP25Data::getSource() const +void CP25Data::getMI(unsigned char* mi) const { - return m_source; + assert(mi != NULL); + + ::memcpy(mi, m_mi, P25_MI_LENGTH_BYTES); } -void CP25Data::setGroup(bool yes) +void CP25Data::setMFId(unsigned char id) { - m_group = yes; + m_mfId = id; } -bool CP25Data::getGroup() const +unsigned char CP25Data::getMFId() const { - return m_group; + return m_mfId; } -void CP25Data::setDest(unsigned int dest) +void CP25Data::setAlgId(unsigned char id) { - m_dest = dest; + m_algId = id; } -unsigned int CP25Data::getDest() const +unsigned char CP25Data::getAlgId() const { - return m_dest; + return m_algId; +} + +void CP25Data::setKId(unsigned int id) +{ + m_kId = id; +} + +unsigned int CP25Data::getKId() const +{ + return m_kId; +} + +void CP25Data::setSrcId(unsigned int id) +{ + m_srcId = id; +} + +unsigned int CP25Data::getSrcId() const +{ + return m_srcId; +} + +void CP25Data::setEmergency(bool on) +{ + m_emergency = on; +} + +bool CP25Data::getEmergency() const +{ + return m_emergency; +} + +void CP25Data::setLCF(unsigned char lcf) +{ + m_lcf = lcf; +} + +unsigned char CP25Data::getLCF() const +{ + return m_lcf; +} + +void CP25Data::setDstId(unsigned int id) +{ + m_dstId = id; +} + +unsigned int CP25Data::getDstId() const +{ + return m_dstId; } void CP25Data::reset() { - m_source = 0U; - m_dest = 0U; + m_srcId = 0U; + m_dstId = 0U; } void CP25Data::decodeLDUHamming(const unsigned char* data, unsigned char* raw) diff --git a/P25Data.h b/P25Data.h index 4172478..779f518 100644 --- a/P25Data.h +++ b/P25Data.h @@ -28,30 +28,52 @@ public: void processHeader(unsigned char* data); void createHeader(unsigned char* data); + void decodeHeader(const unsigned char* data); + void encodeHeader(unsigned char* data); void processLDU1(unsigned char* data); - void createLDU1(unsigned char* data); + void encodeLDU1(unsigned char* data); void processLDU2(unsigned char* data); - void createLDU2(unsigned char* data); + void encodeLDU2(unsigned char* data); - void setSource(unsigned int source); - unsigned int getSource() const; + void setMI(const unsigned char* mi); + void getMI(unsigned char* mi) const; - void setGroup(bool yes); - bool getGroup() const; + void setMFId(unsigned char id); + unsigned char getMFId() const; - void setDest(unsigned int dest); - unsigned int getDest() const; + void setAlgId(unsigned char id); + unsigned char getAlgId() const; + + void setKId(unsigned int id); + unsigned int getKId() const; + + void setEmergency(bool on); + bool getEmergency() const; + + void setSrcId(unsigned int Id); + unsigned int getSrcId() const; + + void setLCF(unsigned char lcf); + unsigned char getLCF() const; + + void setDstId(unsigned int id); + unsigned int getDstId() const; void reset(); private: - unsigned int m_source; - bool m_group; - unsigned int m_dest; - CRS241213 m_rs241213; - CRS24169 m_rs24169; + unsigned char* m_mi; + unsigned char m_mfId; + unsigned char m_algId; + unsigned int m_kId; + unsigned char m_lcf; + bool m_emergency; + unsigned int m_srcId; + unsigned int m_dstId; + CRS241213 m_rs241213; + CRS24169 m_rs24169; void decodeLDUHamming(const unsigned char* raw, unsigned char* data); void encodeLDUHamming(unsigned char* data, const unsigned char* raw); diff --git a/P25Defines.h b/P25Defines.h index 79d006b..d929cf1 100644 --- a/P25Defines.h +++ b/P25Defines.h @@ -39,6 +39,8 @@ const unsigned int P25_NID_LENGTH_BITS = P25_NID_LENGTH_BYTES * 8U; const unsigned char P25_SYNC_BYTES[] = {0x55U, 0x75U, 0xF5U, 0xFFU, 0x77U, 0xFFU}; const unsigned char P25_SYNC_BYTES_LENGTH = 6U; +const unsigned int P25_MI_LENGTH_BYTES = 9U; + const unsigned char P25_LCF_GROUP = 0x00U; const unsigned char P25_LCF_PRIVATE = 0x03U; diff --git a/P25LowSpeedData.cpp b/P25LowSpeedData.cpp index c868461..45989e2 100644 --- a/P25LowSpeedData.cpp +++ b/P25LowSpeedData.cpp @@ -42,7 +42,9 @@ const unsigned char CCS_PARITY[] = { const unsigned int MAX_CCS_ERRS = 4U; -CP25LowSpeedData::CP25LowSpeedData() +CP25LowSpeedData::CP25LowSpeedData() : +m_lsd1(0x00U), +m_lsd2(0x00U) { } @@ -50,7 +52,7 @@ CP25LowSpeedData::~CP25LowSpeedData() { } -void CP25LowSpeedData::process(unsigned char* data) const +void CP25LowSpeedData::process(unsigned char* data) { assert(data != NULL); @@ -83,22 +85,45 @@ void CP25LowSpeedData::process(unsigned char* data) const } } + m_lsd1 = lsd[0U]; + m_lsd2 = lsd[2U]; + CP25Utils::encode(lsd, data, 1546U, 1578U); } -void CP25LowSpeedData::encode(unsigned char* data, unsigned char lsd1, unsigned char lsd2) const +void CP25LowSpeedData::encode(unsigned char* data) const { assert(data != NULL); unsigned char lsd[4U]; - lsd[0U] = lsd1; - lsd[1U] = encode(lsd1); - lsd[2U] = lsd2; - lsd[3U] = encode(lsd2); + lsd[0U] = m_lsd1; + lsd[1U] = encode(m_lsd1); + lsd[2U] = m_lsd2; + lsd[3U] = encode(m_lsd2); CP25Utils::encode(lsd, data, 1546U, 1578U); } +unsigned char CP25LowSpeedData::getLSD1() const +{ + return m_lsd1; +} + +void CP25LowSpeedData::setLSD1(unsigned char lsd1) +{ + m_lsd1 = lsd1; +} + +unsigned char CP25LowSpeedData::getLSD2() const +{ + return m_lsd2; +} + +void CP25LowSpeedData::setLSD2(unsigned char lsd2) +{ + m_lsd2 = lsd2; +} + unsigned char CP25LowSpeedData::encode(unsigned char in) const { return CCS_PARITY[in]; diff --git a/P25LowSpeedData.h b/P25LowSpeedData.h index ac3fb83..41ba306 100644 --- a/P25LowSpeedData.h +++ b/P25LowSpeedData.h @@ -24,11 +24,20 @@ public: CP25LowSpeedData(); ~CP25LowSpeedData(); - void process(unsigned char* data) const; + void process(unsigned char* data); - void encode(unsigned char* data, unsigned char lsd1, unsigned char lsd2) const; + void encode(unsigned char* data) const; + + unsigned char getLSD1() const; + void setLSD1(unsigned char lsd1); + + unsigned char getLSD2() const; + void setLSD2(unsigned char lsd2); private: + unsigned char m_lsd1; + unsigned char m_lsd2; + unsigned char encode(const unsigned char in) const; }; diff --git a/P25Network.cpp b/P25Network.cpp index 84acb86..c278a90 100644 --- a/P25Network.cpp +++ b/P25Network.cpp @@ -17,7 +17,7 @@ */ #include "P25Network.h" -#include "StopWatch.h" +#include "P25Defines.h" #include "Defines.h" #include "Utils.h" #include "Log.h" @@ -48,9 +48,6 @@ m_buffer(1000U, "P25 Network"), m_audio() { m_address = CUDPSocket::lookup(gatewayAddress); - - CStopWatch stopWatch; - ::srand(stopWatch.start()); } CP25Network::~CP25Network() @@ -107,7 +104,7 @@ bool CP25Network::writeHeader(unsigned int tgid) return true; } -bool CP25Network::writeLDU1(const unsigned char* ldu1) +bool CP25Network::writeLDU1(const unsigned char* ldu1, const CP25Data& control, const CP25LowSpeedData& lsd) { assert(ldu1 != NULL); @@ -147,6 +144,8 @@ bool CP25Network::writeLDU1(const unsigned char* ldu1) // The '64' record ::memset(buffer, 0x00U, 17U); buffer[0U] = 0x64U; + buffer[1U] = control.getLCF(); + buffer[2U] = control.getMFId(); m_audio.decode(ldu1, buffer + 5U, 2U); buffer[16U] = 0x02U; @@ -162,6 +161,10 @@ bool CP25Network::writeLDU1(const unsigned char* ldu1) // The '65' record ::memset(buffer, 0x00U, 17U); buffer[0U] = 0x65U; + unsigned int id = control.getDstId(); + buffer[1U] = (id >> 16) & 0xFFU; + buffer[2U] = (id >> 8) & 0xFFU; + buffer[3U] = (id >> 0) & 0xFFU; m_audio.decode(ldu1, buffer + 5U, 3U); buffer[16U] = 0x02U; @@ -177,6 +180,10 @@ bool CP25Network::writeLDU1(const unsigned char* ldu1) // The '66' record ::memset(buffer, 0x00U, 17U); buffer[0U] = 0x66U; + id = control.getSrcId(); + buffer[1U] = (id >> 16) & 0xFFU; + buffer[2U] = (id >> 8) & 0xFFU; + buffer[3U] = (id >> 0) & 0xFFU; m_audio.decode(ldu1, buffer + 5U, 4U); buffer[16U] = 0x02U; @@ -237,6 +244,8 @@ bool CP25Network::writeLDU1(const unsigned char* ldu1) // The '6A' record ::memset(buffer, 0x00U, 16U); buffer[0U] = 0x6AU; + buffer[1U] = lsd.getLSD1(); + buffer[2U] = lsd.getLSD2(); m_audio.decode(ldu1, buffer + 4U, 8U); buffer[15U] = 0x02U; @@ -252,7 +261,7 @@ bool CP25Network::writeLDU1(const unsigned char* ldu1) return true; } -bool CP25Network::writeLDU2(const unsigned char* ldu2) +bool CP25Network::writeLDU2(const unsigned char* ldu2, const CP25Data& control, const CP25LowSpeedData& lsd) { assert(ldu2 != NULL); @@ -289,9 +298,15 @@ bool CP25Network::writeLDU2(const unsigned char* ldu2) return false; #endif + unsigned char mi[P25_MI_LENGTH_BYTES]; + control.getMI(mi); + // The '6D' record ::memset(buffer, 0x00U, 17U); buffer[0U] = 0x6DU; + buffer[1U] = mi[0U]; + buffer[2U] = mi[1U]; + buffer[3U] = mi[2U]; m_audio.decode(ldu2, buffer + 5U, 2U); buffer[16U] = 0x02U; @@ -307,6 +322,9 @@ bool CP25Network::writeLDU2(const unsigned char* ldu2) // The '6E' record ::memset(buffer, 0x00U, 17U); buffer[0U] = 0x6EU; + buffer[1U] = mi[3U]; + buffer[2U] = mi[4U]; + buffer[3U] = mi[5U]; m_audio.decode(ldu2, buffer + 5U, 3U); buffer[16U] = 0x02U; @@ -322,6 +340,9 @@ bool CP25Network::writeLDU2(const unsigned char* ldu2) // The '6F' record ::memset(buffer, 0x00U, 17U); buffer[0U] = 0x6FU; + buffer[1U] = mi[6U]; + buffer[2U] = mi[7U]; + buffer[3U] = mi[8U]; m_audio.decode(ldu2, buffer + 5U, 4U); buffer[16U] = 0x02U; @@ -337,6 +358,10 @@ bool CP25Network::writeLDU2(const unsigned char* ldu2) // The '70' record ::memset(buffer, 0x00U, 17U); buffer[0U] = 0x70U; + buffer[1U] = control.getAlgId(); + unsigned int id = control.getKId(); + buffer[2U] = (id >> 8) & 0xFFU; + buffer[3U] = (id >> 0) & 0xFFU; m_audio.decode(ldu2, buffer + 5U, 5U); buffer[16U] = 0x02U; @@ -382,6 +407,8 @@ bool CP25Network::writeLDU2(const unsigned char* ldu2) // The '73' record ::memset(buffer, 0x00U, 16U); buffer[0U] = 0x73U; + buffer[1U] = lsd.getLSD1(); + buffer[2U] = lsd.getLSD2(); m_audio.decode(ldu2, buffer + 4U, 8U); buffer[15U] = 0x02U; diff --git a/P25Network.h b/P25Network.h index aa9d7f0..82a8e1e 100644 --- a/P25Network.h +++ b/P25Network.h @@ -19,9 +19,11 @@ #ifndef P25Network_H #define P25Network_H +#include "P25LowSpeedData.h" #include "RingBuffer.h" #include "UDPSocket.h" #include "P25Audio.h" +#include "P25Data.h" #include #include @@ -35,8 +37,8 @@ public: void enable(bool enabled); - bool writeLDU1(const unsigned char* ldu1); - bool writeLDU2(const unsigned char* ldu2); + bool writeLDU1(const unsigned char* ldu1, const CP25Data& control, const CP25LowSpeedData& lsd); + bool writeLDU2(const unsigned char* ldu2, const CP25Data& control, const CP25LowSpeedData& lsd); bool writeTerminator(); unsigned int read(unsigned char* data, unsigned int length);