diff --git a/P25Control.cpp b/P25Control.cpp index 1ed8bcd..22e200a 100644 --- a/P25Control.cpp +++ b/P25Control.cpp @@ -1,5 +1,6 @@ /* * Copyright (C) 2016,2017,2018 by Jonathan Naylor G4KLX +* Copyright (C) 2018 by Bryan Biedenkapp * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -176,6 +177,9 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) case P25_DUID_PDU: duid = P25_DUID_PDU; break; + case P25_DUID_TSDU: + duid = P25_DUID_TSDU; + break; default: break; } @@ -346,6 +350,85 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) return true; } + } else if (duid == P25_DUID_TSDU) { + if (m_rfState != RS_RF_DATA) { + m_rfPDUCount = 0U; + m_rfPDUBits = 0U; + m_rfState = RS_RF_DATA; + m_rfDataFrames = 0U; + } + + bool ret = m_rfData.decodeTSDU(data + 2U); + if (!ret) { + m_lastDUID = duid; + return false; + } + + unsigned int srcId = m_rfData.getSrcId(); + unsigned int dstId = m_rfData.getDstId(); + + unsigned char data[P25_TSDU_FRAME_LENGTH_BYTES + 2U]; + + switch (m_rfData.getLCF()) { + case P25_LCF_TSBK_CALL_ALERT: + LogMessage("P25, received RF TSDU transmission, CALL ALERT from %u to %u", srcId, dstId); + ::memset(data + 2U, 0x00U, P25_TSDU_FRAME_LENGTH_BYTES); + + // Regenerate Sync + CSync::addP25Sync(data + 2U); + + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_TSDU); + + // Regenerate TDULC Data + m_rfData.encodeTSDU(data + 2U); + + // Add busy bits + addBusyBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, true, false); + + // Set first busy bits to 1,1 + setBusyBits(data + 2U, P25_SS0_START, true, true); + + if (m_duplex) { + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeQueueRF(data, P25_TSDU_FRAME_LENGTH_BYTES + 2U); + } + break; + case P25_LCF_TSBK_ACK_RSP_FNE: + LogMessage("P25, received RF TSDU transmission, ACK RESPONSE FNE from %u to %u", srcId, dstId); + ::memset(data + 2U, 0x00U, P25_TSDU_FRAME_LENGTH_BYTES); + + // Regenerate Sync + CSync::addP25Sync(data + 2U); + + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_TSDU); + + // Regenerate TDULC Data + m_rfData.encodeTSDU(data + 2U); + + // Add busy bits + addBusyBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, true, false); + + // Set first busy bits to 1,1 + setBusyBits(data + 2U, P25_SS0_START, true, true); + + if (m_duplex) { + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeQueueRF(data, P25_TSDU_FRAME_LENGTH_BYTES + 2U); + } + break; + default: + LogMessage("P25, recieved RF TSDU transmission, unhandled LCF $%02X", m_rfData.getLCF()); + break; + } + + m_rfState = RS_RF_LISTENING; + return true; } else if (duid == P25_DUID_TERM || duid == P25_DUID_TERM_LC) { if (m_rfState == RS_RF_AUDIO) { writeNetwork(m_rfLDU, m_lastDUID, true); @@ -693,6 +776,14 @@ void CP25Control::writeNetwork(const unsigned char *data, unsigned char type, bo } } +void CP25Control::setBusyBits(unsigned char* data, unsigned int ssOffset, bool b1, bool b2) +{ + assert(data != NULL); + + WRITE_BIT(data, ssOffset, b1); + WRITE_BIT(data, ssOffset + 1U, b2); +} + void CP25Control::addBusyBits(unsigned char* data, unsigned int length, bool b1, bool b2) { assert(data != NULL); diff --git a/P25Control.h b/P25Control.h index 79e8d8a..fa069d3 100644 --- a/P25Control.h +++ b/P25Control.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2016,2017,2018 by Jonathan Naylor G4KLX +* Copyright (C) 2018 by Bryan Biedenkapp * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -94,6 +95,7 @@ private: void writeNetwork(const unsigned char *data, unsigned char type, bool end); void writeNetwork(); + void setBusyBits(unsigned char* data, unsigned int ssOffset, bool b1, bool b2); void addBusyBits(unsigned char* data, unsigned int length, bool b1, bool b2); void checkNetLDU1(); diff --git a/P25Data.cpp b/P25Data.cpp index 3c9eb66..618fdc2 100644 --- a/P25Data.cpp +++ b/P25Data.cpp @@ -1,5 +1,6 @@ /* * Copyright (C) 2016,2017 by Jonathan Naylor G4KLX +* Copyright (C) 2018 by Bryan Biedenkapp * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -19,6 +20,7 @@ #include "P25Data.h" #include "P25Defines.h" #include "P25Utils.h" +#include "CRC.h" #include "Hamming.h" #include "Utils.h" #include "Log.h" @@ -54,7 +56,8 @@ m_lcf(0x00U), m_emergency(false), m_srcId(0U), m_dstId(0U), -m_rs241213() +m_rs241213(), +m_trellis() { m_mi = new unsigned char[P25_MI_LENGTH_BYTES]; } @@ -206,6 +209,116 @@ void CP25Data::encodeLDU2(unsigned char* data) CP25Utils::encode(raw, data, 1356U, 1398U); } +bool CP25Data::decodeTSDU(const unsigned char* data) +{ + assert(data != NULL); + + // deinterleave + unsigned char tsbk[12U]; + unsigned char raw[25U]; + CP25Utils::decode(data, raw, 114U, 318U); + + // decode 1/2 rate Trellis & check CRC-CCITT 16 + try { + bool ret = m_trellis.decode12(raw, tsbk); + if (ret) + ret = CCRC::checkCCITT162(tsbk, 12U); + if (!ret) + return false; + } + catch (...) { + CUtils::dump(2U, "P25, CRC failed with input data", tsbk, 12U); + return false; + } + + m_lcf = tsbk[0U] & 0x3F; + m_mfId = tsbk[1U]; + + unsigned long long tsbkValue = 0U; + + // combine bytes into rs value + tsbkValue = tsbk[2U]; + tsbkValue = (tsbkValue << 8) + tsbk[3U]; + tsbkValue = (tsbkValue << 8) + tsbk[4U]; + tsbkValue = (tsbkValue << 8) + tsbk[5U]; + tsbkValue = (tsbkValue << 8) + tsbk[6U]; + tsbkValue = (tsbkValue << 8) + tsbk[7U]; + tsbkValue = (tsbkValue << 8) + tsbk[8U]; + tsbkValue = (tsbkValue << 8) + tsbk[9U]; + + switch (m_lcf) { + case P25_LCF_TSBK_CALL_ALERT: + m_dstId = (unsigned int)((tsbkValue >> 24) & 0xFFFFFFU); // Target Radio Address + m_srcId = (unsigned int)(tsbkValue & 0xFFFFFFU); // Source Radio Address + break; + case P25_LCF_TSBK_ACK_RSP_FNE: + m_serviceType = (unsigned char)((tsbkValue >> 56) & 0xFFU); // Service Type + m_dstId = (unsigned int)((tsbkValue >> 24) & 0xFFFFFFU); // Target Radio Address + m_srcId = (unsigned int)(tsbkValue & 0xFFFFFFU); // Source Radio Address + break; + default: + LogMessage("P25, unknown LCF value in TSDU - $%02X", m_lcf); + break; + } + + return true; +} + +void CP25Data::encodeTSDU(unsigned char* data) +{ + assert(data != NULL); + + unsigned char tsbk[12U]; + ::memset(tsbk, 0x00U, 12U); + + unsigned long long tsbkValue = 0U; + tsbk[0U] = m_lcf; + tsbk[0U] |= 0x80; + + tsbk[1U] = m_mfId; + + switch (m_lcf) { + case P25_LCF_TSBK_CALL_ALERT: + tsbkValue = 0U; + tsbkValue = (tsbkValue << 16) + 0U; + tsbkValue = (tsbkValue << 24) + m_dstId; // Target Radio Address + tsbkValue = (tsbkValue << 24) + m_srcId; // Source Radio Address + break; + case P25_LCF_TSBK_ACK_RSP_FNE: + tsbkValue = 0U; // Additional Info. Flag + tsbkValue = (tsbkValue << 1) + 0U; // Extended Address Flag + tsbkValue = (tsbkValue << 16) + (m_serviceType & 0xFF); // Service Type + tsbkValue = (tsbkValue << 32) + m_dstId; // Target Radio Address + tsbkValue = (tsbkValue << 24) + m_srcId; // Source Radio Address + break; + default: + LogMessage("P25, unknown LCF value in TSDU - $%02X", m_lcf); + break; + } + + // split rs value into bytes + tsbk[2U] = (unsigned char)((tsbkValue >> 56) & 0xFFU); + tsbk[3U] = (unsigned char)((tsbkValue >> 48) & 0xFFU); + tsbk[4U] = (unsigned char)((tsbkValue >> 40) & 0xFFU); + tsbk[5U] = (unsigned char)((tsbkValue >> 32) & 0xFFU); + tsbk[6U] = (unsigned char)((tsbkValue >> 24) & 0xFFU); + tsbk[7U] = (unsigned char)((tsbkValue >> 16) & 0xFFU); + tsbk[8U] = (unsigned char)((tsbkValue >> 8) & 0xFFU); + tsbk[9U] = (unsigned char)((tsbkValue >> 0) & 0xFFU); + + // compute CRC-CCITT 16 + CCRC::addCCITT162(tsbk, 12U); + + unsigned char raw[25U]; + ::memset(raw, 0x00U, 25U); + + // encode 1/2 rate Trellis + m_trellis.encode12(tsbk, raw); + + // interleave + CP25Utils::encode(raw, data, 114U, 318U); +} + void CP25Data::setMI(const unsigned char* mi) { assert(mi != NULL); @@ -290,6 +403,16 @@ unsigned int CP25Data::getDstId() const return m_dstId; } +void CP25Data::setServiceType(unsigned char type) +{ + m_serviceType = type; +} + +unsigned char CP25Data::getServiceType() const +{ + return m_serviceType; +} + void CP25Data::reset() { ::memset(m_mi, 0x00U, P25_MI_LENGTH_BYTES); diff --git a/P25Data.h b/P25Data.h index 6618da2..1b9eec9 100644 --- a/P25Data.h +++ b/P25Data.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2016,2017 by Jonathan Naylor G4KLX +* Copyright (C) 2018 by Bryan Biedenkapp * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -20,6 +21,7 @@ #define P25Data_H #include "RS241213.h" +#include "P25Trellis.h" class CP25Data { public: @@ -33,6 +35,9 @@ public: void encodeLDU2(unsigned char* data); + bool decodeTSDU(const unsigned char* data); + void encodeTSDU(unsigned char* data); + void setMI(const unsigned char* mi); void getMI(unsigned char* mi) const; @@ -57,6 +62,9 @@ public: void setDstId(unsigned int id); unsigned int getDstId() const; + void setServiceType(unsigned char type); + unsigned char getServiceType() const; + void reset(); private: @@ -68,7 +76,9 @@ private: bool m_emergency; unsigned int m_srcId; unsigned int m_dstId; + unsigned char m_serviceType; CRS241213 m_rs241213; + CP25Trellis m_trellis; 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 f04260f..6ceda75 100644 --- a/P25Defines.h +++ b/P25Defines.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2016 by Jonathan Naylor G4KLX + * Copyright (C) 2018 by Bryan Biedenkapp * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -31,6 +32,9 @@ const unsigned int P25_TERM_FRAME_LENGTH_BITS = P25_TERM_FRAME_LENGTH_BYTES * 8 const unsigned int P25_TERMLC_FRAME_LENGTH_BYTES = 54U; const unsigned int P25_TERMLC_FRAME_LENGTH_BITS = P25_TERMLC_FRAME_LENGTH_BYTES * 8U; +const unsigned int P25_TSDU_FRAME_LENGTH_BYTES = 45U; +const unsigned int P25_TSDU_FRAME_LENGTH_BITS = P25_TSDU_FRAME_LENGTH_BYTES * 8U; + const unsigned int P25_SYNC_LENGTH_BYTES = 6U; const unsigned int P25_SYNC_LENGTH_BITS = P25_SYNC_LENGTH_BYTES * 8U; @@ -54,6 +58,9 @@ const unsigned int P25_MI_LENGTH_BYTES = 9U; const unsigned char P25_LCF_GROUP = 0x00U; const unsigned char P25_LCF_PRIVATE = 0x03U; +const unsigned char P25_LCF_TSBK_CALL_ALERT = 0x1FU; +const unsigned char P25_LCF_TSBK_ACK_RSP_FNE = 0x20U; + const unsigned int P25_SS0_START = 70U; const unsigned int P25_SS1_START = 71U; const unsigned int P25_SS_INCREMENT = 72U; @@ -61,6 +68,7 @@ const unsigned int P25_SS_INCREMENT = 72U; const unsigned char P25_DUID_HEADER = 0x00U; const unsigned char P25_DUID_TERM = 0x03U; const unsigned char P25_DUID_LDU1 = 0x05U; +const unsigned char P25_DUID_TSDU = 0x07U; const unsigned char P25_DUID_LDU2 = 0x0AU; const unsigned char P25_DUID_PDU = 0x0CU; const unsigned char P25_DUID_TERM_LC = 0x0FU; diff --git a/P25NID.cpp b/P25NID.cpp index 145dce4..7e4f1e8 100644 --- a/P25NID.cpp +++ b/P25NID.cpp @@ -1,5 +1,6 @@ /* * Copyright (C) 2016,2018 by Jonathan Naylor G4KLX +* Copyright (C) 2018 by Bryan Biedenkapp * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -72,6 +73,13 @@ m_pdu(NULL) bch.encode(m_term); m_term[7U] &= 0xFEU; // Clear the parity bit + m_tsdu = new unsigned char[P25_NID_LENGTH_BYTES]; + m_tsdu[0U] = (nac >> 4) & 0xFFU; + m_tsdu[1U] = (nac << 4) & 0xF0U; + m_tsdu[1U] |= P25_DUID_TSDU; + bch.encode(m_tsdu); + m_tsdu[7U] &= 0xFEU; // Clear the parity bit + m_pdu = new unsigned char[P25_NID_LENGTH_BYTES]; m_pdu[0U] = (nac >> 4) & 0xFFU; m_pdu[1U] = (nac << 4) & 0xF0U; @@ -127,6 +135,12 @@ bool CP25NID::decode(const unsigned char* data) return true; } + errs = CP25Utils::compare(nid, m_tsdu, P25_NID_LENGTH_BYTES); + if (errs < MAX_NID_ERRS) { + m_duid = P25_DUID_TSDU; + return true; + } + errs = CP25Utils::compare(nid, m_pdu, P25_NID_LENGTH_BYTES); if (errs < MAX_NID_ERRS) { m_duid = P25_DUID_PDU; @@ -156,6 +170,9 @@ void CP25NID::encode(unsigned char* data, unsigned char duid) const case P25_DUID_TERM_LC: CP25Utils::encode(m_termlc, data, 48U, 114U); break; + case P25_DUID_TSDU: + CP25Utils::encode(m_tsdu, data, 48U, 114U); + break; case P25_DUID_PDU: CP25Utils::encode(m_pdu, data, 48U, 114U); break; diff --git a/P25NID.h b/P25NID.h index bb0031f..fe7ee74 100644 --- a/P25NID.h +++ b/P25NID.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2016,2018 by Jonathan Naylor G4KLX +* Copyright (C) 2018 by Bryan Biedenkapp * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -37,6 +38,7 @@ private: unsigned char* m_ldu2; unsigned char* m_termlc; unsigned char* m_term; + unsigned char* m_tsdu; unsigned char* m_pdu; };