From 0395955ab32817f07392acec8d0625860d25fa57 Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Mon, 14 May 2018 09:36:57 -0400 Subject: [PATCH 1/5] Add support to decode and encode P25 TSDU data; Add support to process P25 TSDU data. --- P25Control.cpp | 2216 +++++++++++++++++++++++++----------------------- P25Control.h | 234 ++--- P25Data.cpp | 806 ++++++++++-------- P25Data.h | 164 ++-- P25Defines.h | 148 ++-- 5 files changed, 1897 insertions(+), 1671 deletions(-) diff --git a/P25Control.cpp b/P25Control.cpp index 1ed8bcd..f6c4d90 100644 --- a/P25Control.cpp +++ b/P25Control.cpp @@ -1,1062 +1,1154 @@ -/* -* Copyright (C) 2016,2017,2018 by Jonathan Naylor G4KLX -* -* 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 -* the Free Software Foundation; either version 2 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software -* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. -*/ - -#include "P25Control.h" -#include "P25Defines.h" -#include "P25Trellis.h" -#include "P25Utils.h" -#include "Utils.h" -#include "Sync.h" -#include "CRC.h" -#include "Log.h" - -#include -#include -#include -#include - -// #define DUMP_P25 - -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]) - -CP25Control::CP25Control(unsigned int nac, unsigned int id, bool selfOnly, bool uidOverride, CP25Network* network, CDisplay* display, unsigned int timeout, bool duplex, CDMRLookup* lookup, bool remoteGateway, CRSSIInterpolator* rssiMapper) : -m_nac(nac), -m_id(id), -m_selfOnly(selfOnly), -m_uidOverride(uidOverride), -m_remoteGateway(remoteGateway), -m_network(network), -m_display(display), -m_duplex(duplex), -m_lookup(lookup), -m_queue(1000U, "P25 Control"), -m_rfState(RS_RF_LISTENING), -m_netState(RS_NET_IDLE), -m_rfTimeout(1000U, timeout), -m_netTimeout(1000U, timeout), -m_networkWatchdog(1000U, 0U, 1500U), -m_rfFrames(0U), -m_rfBits(0U), -m_rfErrs(0U), -m_netFrames(0U), -m_netLost(0U), -m_rfDataFrames(0U), -m_nid(nac), -m_lastDUID(P25_DUID_TERM), -m_audio(), -m_rfData(), -m_netData(), -m_rfLSD(), -m_netLSD(), -m_netLDU1(NULL), -m_netLDU2(NULL), -m_lastIMBE(NULL), -m_rfLDU(NULL), -m_rfPDU(NULL), -m_rfPDUCount(0U), -m_rfPDUBits(0U), -m_rssiMapper(rssiMapper), -m_rssi(0U), -m_maxRSSI(0U), -m_minRSSI(0U), -m_aveRSSI(0U), -m_rssiCount(0U), -m_fp(NULL) -{ - assert(display != NULL); - assert(lookup != NULL); - assert(rssiMapper != NULL); - - m_netLDU1 = new unsigned char[9U * 25U]; - m_netLDU2 = new unsigned char[9U * 25U]; - - ::memset(m_netLDU1, 0x00U, 9U * 25U); - ::memset(m_netLDU2, 0x00U, 9U * 25U); - - m_lastIMBE = new unsigned char[11U]; - ::memcpy(m_lastIMBE, P25_NULL_IMBE, 11U); - - m_rfLDU = new unsigned char[P25_LDU_FRAME_LENGTH_BYTES]; - ::memset(m_rfLDU, 0x00U, P25_LDU_FRAME_LENGTH_BYTES); - - m_rfPDU = new unsigned char[P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U]; - ::memset(m_rfPDU, 0x00U, P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U); -} - -CP25Control::~CP25Control() -{ - delete[] m_netLDU1; - delete[] m_netLDU2; - delete[] m_lastIMBE; - delete[] m_rfLDU; - delete[] m_rfPDU; -} - -bool CP25Control::writeModem(unsigned char* data, unsigned int len) -{ - assert(data != NULL); - - bool sync = data[1U] == 0x01U; - - if (data[0U] == TAG_LOST && m_rfState == RS_RF_AUDIO) { - if (m_rssi != 0U) - LogMessage("P25, transmission lost, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount); - else - LogMessage("P25, transmission lost, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits)); - - if (m_netState == RS_NET_IDLE) - m_display->clearP25(); - - writeNetwork(m_rfLDU, m_lastDUID, true); - writeNetwork(data + 2U, P25_DUID_TERM, true); - m_rfState = RS_RF_LISTENING; - m_rfTimeout.stop(); - m_rfData.reset(); -#if defined(DUMP_P25) - closeFile(); -#endif - return false; - } - - if (data[0U] == TAG_LOST && m_rfState == RS_RF_DATA) { - if (m_netState == RS_NET_IDLE) - m_display->clearP25(); - - m_rfState = RS_RF_LISTENING; - m_rfPDUCount = 0U; - m_rfPDUBits = 0U; -#if defined(DUMP_P25) - closeFile(); -#endif - return false; - } - - if (data[0U] == TAG_LOST) { - m_rfState = RS_RF_LISTENING; - return false; - } - - if (!sync && m_rfState == RS_RF_LISTENING) - return false; - - // Decode the NID - bool valid = m_nid.decode(data + 2U); - - if (m_rfState == RS_RF_LISTENING && !valid) - return false; - - unsigned char duid = m_nid.getDUID(); - if (!valid) { - switch (m_lastDUID) { - case P25_DUID_HEADER: - case P25_DUID_LDU2: - duid = P25_DUID_LDU1; - break; - case P25_DUID_LDU1: - duid = P25_DUID_LDU2; - break; - case P25_DUID_PDU: - duid = P25_DUID_PDU; - break; - default: - break; - } - } - - // Have we got RSSI bytes on the end of a P25 LDU? - if (len == (P25_LDU_FRAME_LENGTH_BYTES + 4U)) { - uint16_t raw = 0U; - raw |= (data[218U] << 8) & 0xFF00U; - raw |= (data[219U] << 0) & 0x00FFU; - - // Convert the raw RSSI to dBm - int rssi = m_rssiMapper->interpolate(raw); - if (rssi != 0) - LogDebug("P25, raw RSSI: %u, reported RSSI: %d dBm", raw, rssi); - - // RSSI is always reported as positive - m_rssi = (rssi >= 0) ? rssi : -rssi; - - if (m_rssi > m_minRSSI) - m_minRSSI = m_rssi; - if (m_rssi < m_maxRSSI) - m_maxRSSI = m_rssi; - - m_aveRSSI += m_rssi; - m_rssiCount++; - } - - if (duid == P25_DUID_LDU1) { - if (m_rfState == RS_RF_LISTENING) { - m_rfData.reset(); - bool ret = m_rfData.decodeLDU1(data + 2U); - if (!ret) { - m_lastDUID = duid; - return false; - } - - unsigned int srcId = m_rfData.getSrcId(); - - if (m_selfOnly) { - if (m_id > 99999999U) { // Check that the Config DMR-ID is bigger than 8 digits - if (srcId != m_id / 100U) - return false; - } - - else if (m_id > 9999999U) { // Check that the Config DMR-ID is bigger than 7 digits - if (srcId != m_id / 10U) - return false; - } - - else if (srcId != m_id) { // All other cases - return false; - } - } - - if (!m_uidOverride) { - bool found = m_lookup->exists(srcId); - if (!found) - return false; - } - - 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 voice 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; - - m_minRSSI = m_rssi; - m_maxRSSI = m_rssi; - m_aveRSSI = m_rssi; - m_rssiCount = 1U; - - createRFHeader(); - writeNetwork(data + 2U, P25_DUID_HEADER, false); - } else if (m_rfState == RS_RF_AUDIO) { - writeNetwork(m_rfLDU, m_lastDUID, false); - } - - if (m_rfState == RS_RF_AUDIO) { - // Regenerate Sync - CSync::addP25Sync(data + 2U); - - // Regenerate NID - m_nid.encode(data + 2U, P25_DUID_LDU1); - - // Regenerate LDU1 Data - m_rfData.encodeLDU1(data + 2U); - - // Regenerate the Low Speed Data - m_rfLSD.process(data + 2U); - - // Regenerate Audio - unsigned int errors = m_audio.process(data + 2U); - LogDebug("P25, LDU1 audio, errs: %u/1233 (%.1f%%)", errors, float(errors) / 12.33F); - - m_display->writeP25BER(float(errors) / 12.33F); - - m_rfBits += 1233U; - m_rfErrs += errors; - m_rfFrames++; - m_lastDUID = duid; - - // Add busy bits - addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); - -#if defined(DUMP_P25) - writeFile(data + 2U, len - 2U); -#endif - - ::memcpy(m_rfLDU, data + 2U, P25_LDU_FRAME_LENGTH_BYTES); - - if (m_duplex) { - data[0U] = TAG_DATA; - data[1U] = 0x00U; - writeQueueRF(data, P25_LDU_FRAME_LENGTH_BYTES + 2U); - } - - m_display->writeP25RSSI(m_rssi); - - return true; - } - } else if (duid == P25_DUID_LDU2) { - if (m_rfState == RS_RF_AUDIO) { - writeNetwork(m_rfLDU, m_lastDUID, false); - - // Regenerate Sync - CSync::addP25Sync(data + 2U); - - // Regenerate NID - m_nid.encode(data + 2U, P25_DUID_LDU2); - - // Add the dummy LDU2 data - m_rfData.encodeLDU2(data + 2U); - - // Regenerate the Low Speed Data - m_rfLSD.process(data + 2U); - - // Regenerate Audio - unsigned int errors = m_audio.process(data + 2U); - LogDebug("P25, LDU2 audio, errs: %u/1233 (%.1f%%)", errors, float(errors) / 12.33F); - - m_display->writeP25BER(float(errors) / 12.33F); - - m_rfBits += 1233U; - m_rfErrs += errors; - m_rfFrames++; - m_lastDUID = duid; - - // Add busy bits - addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); - -#if defined(DUMP_P25) - writeFile(data + 2U, len - 2U); -#endif - - ::memcpy(m_rfLDU, data + 2U, P25_LDU_FRAME_LENGTH_BYTES); - - if (m_duplex) { - data[0U] = TAG_DATA; - data[1U] = 0x00U; - writeQueueRF(data, P25_LDU_FRAME_LENGTH_BYTES + 2U); - } - - m_display->writeP25RSSI(m_rssi); - - 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); - - ::memset(data + 2U, 0x00U, P25_TERM_FRAME_LENGTH_BYTES); - - // Regenerate Sync - CSync::addP25Sync(data + 2U); - - // Regenerate NID - m_nid.encode(data + 2U, P25_DUID_TERM); - - // Add busy bits - addBusyBits(data + 2U, P25_TERM_FRAME_LENGTH_BITS, false, true); - - m_rfState = RS_RF_LISTENING; - m_rfTimeout.stop(); - m_rfData.reset(); - m_lastDUID = duid; - - if (m_rssi != 0U) - LogMessage("P25, received RF end of voice transmission, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount); - else - LogMessage("P25, received RF end of voice transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits)); - - m_display->clearP25(); - -#if defined(DUMP_P25) - closeFile(); -#endif - - writeNetwork(data + 2U, P25_DUID_TERM, true); - - if (m_duplex) { - data[0U] = TAG_EOT; - data[1U] = 0x00U; - writeQueueRF(data, P25_TERM_FRAME_LENGTH_BYTES + 2U); - } - } - } else if (duid == P25_DUID_PDU) { - if (m_rfState != RS_RF_DATA) { - m_rfPDUCount = 0U; - m_rfPDUBits = 0U; - m_rfState = RS_RF_DATA; - m_rfDataFrames = 0U; - } - - unsigned int start = m_rfPDUCount * P25_LDU_FRAME_LENGTH_BITS; - - unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES]; - unsigned int bits = CP25Utils::decode(data + 2U, buffer, start, start + P25_LDU_FRAME_LENGTH_BITS); - - for (unsigned int i = 0U; i < bits; i++, m_rfPDUBits++) { - bool b = READ_BIT(buffer, i); - WRITE_BIT(m_rfPDU, m_rfPDUBits, b); - } - - if (m_rfPDUCount == 0U) { - CP25Trellis trellis; - unsigned char header[P25_PDU_HEADER_LENGTH_BYTES]; - bool valid = trellis.decode12(m_rfPDU + P25_SYNC_LENGTH_BYTES + P25_NID_LENGTH_BYTES, header); - if (valid) - valid = CCRC::checkCCITT162(header, P25_PDU_HEADER_LENGTH_BYTES); - - if (valid) { - unsigned int llId = (header[3U] << 16) + (header[4U] << 8) + header[5U]; - unsigned int sap = header[1U] & 0x3FU; - m_rfDataFrames = header[6U] & 0x7FU; - - LogMessage("P25, received RF data transmission for Local Link Id %u, SAP %u, %u blocks", llId, sap, m_rfDataFrames); - } else { - m_rfPDUCount = 0U; - m_rfPDUBits = 0U; - m_rfState = RS_RF_LISTENING; - m_rfDataFrames = 0U; - } - } - - if (m_rfState == RS_RF_DATA) { - m_rfPDUCount++; - - unsigned int bitLength = ((m_rfDataFrames + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_SYNC_LENGTH_BITS + P25_NID_LENGTH_BITS; - - if (m_rfPDUBits >= bitLength) { - unsigned int offset = P25_SYNC_LENGTH_BYTES + P25_NID_LENGTH_BYTES; - - // Regenerate the PDU header - CP25Trellis trellis; - unsigned char header[P25_PDU_HEADER_LENGTH_BYTES]; - trellis.decode12(m_rfPDU + offset, header); - trellis.encode12(header, m_rfPDU + offset); - offset += P25_PDU_FEC_LENGTH_BITS; - - // Regenerate the PDU data - for (unsigned int i = 0U; i < m_rfDataFrames; i++) { - unsigned char data[P25_PDU_CONFIRMED_LENGTH_BYTES]; - - bool valid = trellis.decode34(m_rfPDU + offset, data); - if (valid) { - trellis.encode34(data, m_rfPDU + offset); - } else { - valid = trellis.decode12(m_rfPDU + offset, data); - if (valid) - trellis.encode12(data, m_rfPDU + offset); - } - - offset += P25_PDU_FEC_LENGTH_BITS; - } - - unsigned char pdu[1024U]; - - // Add the data - unsigned int newBitLength = CP25Utils::encode(m_rfPDU, pdu + 2U, bitLength); - unsigned int newByteLength = newBitLength / 8U; - if ((newBitLength % 8U) > 0U) - newByteLength++; - - // Regenerate Sync - CSync::addP25Sync(pdu + 2U); - - // Regenerate NID - m_nid.encode(pdu + 2U, P25_DUID_PDU); - - // Add busy bits - addBusyBits(pdu + 2U, newBitLength, false, true); - - if (m_duplex) { - pdu[0U] = TAG_DATA; - pdu[1U] = 0x00U; - writeQueueRF(pdu, newByteLength + 2U); - } - - LogMessage("P25, ended RF data transmission"); - m_display->clearP25(); - - m_rfPDUCount = 0U; - m_rfPDUBits = 0U; - m_rfState = RS_RF_LISTENING; - m_rfDataFrames = 0U; - } - - return true; - } - } - - return false; -} - -unsigned int CP25Control::readModem(unsigned char* data) -{ - assert(data != NULL); - - if (m_queue.isEmpty()) - return 0U; - - unsigned char len = 0U; - m_queue.getData(&len, 1U); - - m_queue.getData(data, len); - - return len; -} - -void CP25Control::writeNetwork() -{ - unsigned char data[100U]; - unsigned int length = m_network->read(data, 100U); - if (length == 0U) - return; - - if (m_rfState != RS_RF_LISTENING && m_netState == RS_NET_IDLE) - return; - - m_networkWatchdog.start(); - - switch (data[0U]) { - case 0x62U: - ::memcpy(m_netLDU1 + 0U, data, 22U); - checkNetLDU2(); - break; - case 0x63U: - ::memcpy(m_netLDU1 + 25U, data, 14U); - checkNetLDU2(); - break; - case 0x64U: - ::memcpy(m_netLDU1 + 50U, data, 17U); - checkNetLDU2(); - break; - case 0x65U: - ::memcpy(m_netLDU1 + 75U, data, 17U); - checkNetLDU2(); - break; - case 0x66U: - ::memcpy(m_netLDU1 + 100U, data, 17U); - checkNetLDU2(); - break; - case 0x67U: - ::memcpy(m_netLDU1 + 125U, data, 17U); - checkNetLDU2(); - break; - case 0x68U: - ::memcpy(m_netLDU1 + 150U, data, 17U); - checkNetLDU2(); - break; - case 0x69U: - ::memcpy(m_netLDU1 + 175U, data, 17U); - checkNetLDU2(); - break; - case 0x6AU: - ::memcpy(m_netLDU1 + 200U, data, 16U); - checkNetLDU2(); - if (m_netState != RS_NET_IDLE) - createNetLDU1(); - break; - case 0x6BU: - ::memcpy(m_netLDU2 + 0U, data, 22U); - checkNetLDU1(); - break; - case 0x6CU: - ::memcpy(m_netLDU2 + 25U, data, 14U); - checkNetLDU1(); - break; - case 0x6DU: - ::memcpy(m_netLDU2 + 50U, data, 17U); - checkNetLDU1(); - break; - case 0x6EU: - ::memcpy(m_netLDU2 + 75U, data, 17U); - checkNetLDU1(); - break; - case 0x6FU: - ::memcpy(m_netLDU2 + 100U, data, 17U); - checkNetLDU1(); - break; - case 0x70U: - ::memcpy(m_netLDU2 + 125U, data, 17U); - checkNetLDU1(); - break; - case 0x71U: - ::memcpy(m_netLDU2 + 150U, data, 17U); - checkNetLDU1(); - break; - case 0x72U: - ::memcpy(m_netLDU2 + 175U, data, 17U); - checkNetLDU1(); - break; - case 0x73U: - ::memcpy(m_netLDU2 + 200U, data, 16U); - if (m_netState == RS_NET_IDLE) { - createNetHeader(); - createNetLDU1(); - } else { - checkNetLDU1(); - } - createNetLDU2(); - break; - case 0x80U: - createNetTerminator(); - break; - default: - break; - } -} - -void CP25Control::clock(unsigned int ms) -{ - if (m_network != NULL) - writeNetwork(); - - m_rfTimeout.clock(ms); - m_netTimeout.clock(ms); - - if (m_netState == RS_NET_AUDIO) { - m_networkWatchdog.clock(ms); - - if (m_networkWatchdog.hasExpired()) { - LogMessage("P25, network watchdog has expired, %.1f seconds, %u%% packet loss", float(m_netFrames) / 50.0F, (m_netLost * 100U) / m_netFrames); - m_display->clearP25(); - m_networkWatchdog.stop(); - m_netState = RS_NET_IDLE; - m_netData.reset(); - m_netTimeout.stop(); - } - } -} - -void CP25Control::writeQueueRF(const unsigned char* data, unsigned int length) -{ - assert(data != NULL); - - if (m_rfTimeout.isRunning() && m_rfTimeout.hasExpired()) - return; - - unsigned int space = m_queue.freeSpace(); - if (space < (length + 1U)) { - LogError("P25, overflow in the P25 RF queue"); - return; - } - - unsigned char len = length; - m_queue.addData(&len, 1U); - - m_queue.addData(data, len); -} - -void CP25Control::writeQueueNet(const unsigned char* data, unsigned int length) -{ - assert(data != NULL); - - if (m_netTimeout.isRunning() && m_netTimeout.hasExpired()) - return; - - unsigned int space = m_queue.freeSpace(); - if (space < (length + 1U)) { - LogError("P25, overflow in the P25 RF queue"); - return; - } - - unsigned char len = length; - m_queue.addData(&len, 1U); - - m_queue.addData(data, len); -} - -void CP25Control::writeNetwork(const unsigned char *data, unsigned char type, bool end) -{ - assert(data != NULL); - - if (m_network == NULL) - return; - - if (m_rfTimeout.isRunning() && m_rfTimeout.hasExpired()) - return; - - switch (type) - { - case P25_DUID_LDU1: - m_network->writeLDU1(data, m_rfData, m_rfLSD, end); - break; - case P25_DUID_LDU2: - m_network->writeLDU2(data, m_rfData, m_rfLSD, end); - break; - default: - break; - } -} - -void CP25Control::addBusyBits(unsigned char* data, unsigned int length, bool b1, bool b2) -{ - assert(data != NULL); - - for (unsigned int ss0Pos = P25_SS0_START; ss0Pos < length; ss0Pos += P25_SS_INCREMENT) { - unsigned int ss1Pos = ss0Pos + 1U; - WRITE_BIT(data, ss0Pos, b1); - WRITE_BIT(data, ss1Pos, b2); - } -} - -void CP25Control::checkNetLDU1() -{ - if (m_netState == RS_NET_IDLE) - return; - - // Check for an unflushed LDU1 - if (m_netLDU1[0U] != 0x00U || m_netLDU1[25U] != 0x00U || m_netLDU1[50U] != 0x00U || - m_netLDU1[75U] != 0x00U || m_netLDU1[100U] != 0x00U || m_netLDU1[125U] != 0x00U || - m_netLDU1[150U] != 0x00U || m_netLDU1[175U] != 0x00U || m_netLDU1[200U] != 0x00U) - createNetLDU1(); -} - -void CP25Control::checkNetLDU2() -{ - if (m_netState == RS_NET_IDLE) - return; - - // Check for an unflushed LDU1 - if (m_netLDU2[0U] != 0x00U || m_netLDU2[25U] != 0x00U || m_netLDU2[50U] != 0x00U || - m_netLDU2[75U] != 0x00U || m_netLDU2[100U] != 0x00U || m_netLDU2[125U] != 0x00U || - m_netLDU2[150U] != 0x00U || m_netLDU2[175U] != 0x00U || m_netLDU2[200U] != 0x00U) - createNetLDU2(); -} - -void CP25Control::insertMissingAudio(unsigned char* data) -{ - if (data[0U] == 0x00U) { - ::memcpy(data + 10U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 10U, 11U); - } - - if (data[25U] == 0x00U) { - ::memcpy(data + 26U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 26U, 11U); - } - - if (data[50U] == 0x00U) { - ::memcpy(data + 55U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 55U, 11U); - } - - if (data[75U] == 0x00U) { - ::memcpy(data + 80U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 80U, 11U); - } - - if (data[100U] == 0x00U) { - ::memcpy(data + 105U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 105U, 11U); - } - - if (data[125U] == 0x00U) { - ::memcpy(data + 130U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 130U, 11U); - } - - if (data[150U] == 0x00U) { - ::memcpy(data + 155U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 155U, 11U); - } - - if (data[175U] == 0x00U) { - ::memcpy(data + 180U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 180U, 11U); - } - - if (data[200U] == 0x00U) { - ::memcpy(data + 204U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 204U, 11U); - } -} - -void CP25Control::createRFHeader() -{ - unsigned char buffer[P25_HDR_FRAME_LENGTH_BYTES + 2U]; - ::memset(buffer, 0x00U, P25_HDR_FRAME_LENGTH_BYTES + 2U); - - buffer[0U] = TAG_HEADER; - buffer[1U] = 0x00U; - - // Add the sync - CSync::addP25Sync(buffer + 2U); - - // Add the NID - m_nid.encode(buffer + 2U, P25_DUID_HEADER); - - // Add the dummy header - m_rfData.encodeHeader(buffer + 2U); - - // Add busy bits - addBusyBits(buffer + 2U, P25_HDR_FRAME_LENGTH_BITS, false, true); - - m_rfFrames = 0U; - m_rfErrs = 0U; - m_rfBits = 1U; - m_rfTimeout.start(); - m_lastDUID = P25_DUID_HEADER; - ::memset(m_rfLDU, 0x00U, P25_LDU_FRAME_LENGTH_BYTES); - -#if defined(DUMP_P25) - openFile(); - writeFile(buffer + 2U, buffer - 2U); -#endif - - if (m_duplex) { - buffer[0U] = TAG_HEADER; - buffer[1U] = 0x00U; - writeQueueRF(buffer, P25_HDR_FRAME_LENGTH_BYTES + 2U); - } -} - -void CP25Control::createNetHeader() -{ - 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]; - - unsigned char algId = m_netLDU2[126U]; - unsigned int kId = (m_netLDU2[127U] << 8) + m_netLDU2[128U]; - - 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.reset(); - 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(); - m_netFrames = 0U; - m_netLost = 0U; - - unsigned char buffer[P25_HDR_FRAME_LENGTH_BYTES + 2U]; - ::memset(buffer, 0x00U, P25_HDR_FRAME_LENGTH_BYTES + 2U); - - buffer[0U] = TAG_HEADER; - buffer[1U] = 0x00U; - - // Add the sync - CSync::addP25Sync(buffer + 2U); - - // Add the NID - m_nid.encode(buffer + 2U, P25_DUID_HEADER); - - // Add the dummy header - m_netData.encodeHeader(buffer + 2U); - - // Add busy bits - if (m_remoteGateway) - addBusyBits(buffer + 2U, P25_HDR_FRAME_LENGTH_BITS, false, false); - else - addBusyBits(buffer + 2U, P25_HDR_FRAME_LENGTH_BITS, false, true); - - writeQueueNet(buffer, P25_HDR_FRAME_LENGTH_BYTES + 2U); -} - -void CP25Control::createNetLDU1() -{ - insertMissingAudio(m_netLDU1); - - unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES + 2U]; - ::memset(buffer, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 2U); - - buffer[0U] = TAG_DATA; - buffer[1U] = 0x00U; - - // Add the sync - CSync::addP25Sync(buffer + 2U); - - // Add the NID - m_nid.encode(buffer + 2U, P25_DUID_LDU1); - - // Add the LDU1 data - m_netData.encodeLDU1(buffer + 2U); - - // Add the Audio - m_audio.encode(buffer + 2U, m_netLDU1 + 10U, 0U); - m_audio.encode(buffer + 2U, m_netLDU1 + 26U, 1U); - m_audio.encode(buffer + 2U, m_netLDU1 + 55U, 2U); - m_audio.encode(buffer + 2U, m_netLDU1 + 80U, 3U); - m_audio.encode(buffer + 2U, m_netLDU1 + 105U, 4U); - m_audio.encode(buffer + 2U, m_netLDU1 + 130U, 5U); - m_audio.encode(buffer + 2U, m_netLDU1 + 155U, 6U); - m_audio.encode(buffer + 2U, m_netLDU1 + 180U, 7U); - m_audio.encode(buffer + 2U, m_netLDU1 + 204U, 8U); - - // Add the Low Speed Data - m_netLSD.setLSD1(m_netLDU1[201U]); - m_netLSD.setLSD2(m_netLDU1[202U]); - m_netLSD.encode(buffer + 2U); - - // Add busy bits - if (m_remoteGateway) - addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, false); - else - addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); - - writeQueueNet(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); - - ::memset(m_netLDU1, 0x00U, 9U * 25U); - - m_netFrames += 9U; -} - -void CP25Control::createNetLDU2() -{ - insertMissingAudio(m_netLDU2); - - unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES + 2U]; - ::memset(buffer, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 2U); - - buffer[0U] = TAG_DATA; - buffer[1U] = 0x00U; - - // Add the sync - CSync::addP25Sync(buffer + 2U); - - // Add the NID - m_nid.encode(buffer + 2U, P25_DUID_LDU2); - - // Add the dummy LDU2 data - m_netData.encodeLDU2(buffer + 2U); - - // Add the Audio - m_audio.encode(buffer + 2U, m_netLDU2 + 10U, 0U); - m_audio.encode(buffer + 2U, m_netLDU2 + 26U, 1U); - m_audio.encode(buffer + 2U, m_netLDU2 + 55U, 2U); - m_audio.encode(buffer + 2U, m_netLDU2 + 80U, 3U); - m_audio.encode(buffer + 2U, m_netLDU2 + 105U, 4U); - m_audio.encode(buffer + 2U, m_netLDU2 + 130U, 5U); - m_audio.encode(buffer + 2U, m_netLDU2 + 155U, 6U); - m_audio.encode(buffer + 2U, m_netLDU2 + 180U, 7U); - m_audio.encode(buffer + 2U, m_netLDU2 + 204U, 8U); - - // Add the Low Speed Data - m_netLSD.setLSD1(m_netLDU2[201U]); - m_netLSD.setLSD2(m_netLDU2[202U]); - m_netLSD.encode(buffer + 2U); - - // Add busy bits - if (m_remoteGateway) - addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, false); - else - addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); - - writeQueueNet(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); - - ::memset(m_netLDU2, 0x00U, 9U * 25U); - - m_netFrames += 9U; -} - -void CP25Control::createNetTerminator() -{ - unsigned char buffer[P25_TERM_FRAME_LENGTH_BYTES + 2U]; - ::memset(buffer, 0x00U, P25_TERM_FRAME_LENGTH_BYTES + 2U); - - buffer[0U] = TAG_EOT; - buffer[1U] = 0x00U; - - // Add the sync - CSync::addP25Sync(buffer + 2U); - - // Add the NID - m_nid.encode(buffer + 2U, P25_DUID_TERM); - - // Add busy bits - if (m_remoteGateway) - addBusyBits(buffer + 2U, P25_TERM_FRAME_LENGTH_BITS, false, false); - else - addBusyBits(buffer + 2U, P25_TERM_FRAME_LENGTH_BITS, false, true); - - writeQueueNet(buffer, P25_TERM_FRAME_LENGTH_BYTES + 2U); - - LogMessage("P25, network end of transmission, %.1f seconds, %u%% packet loss", float(m_netFrames) / 50.0F, (m_netLost * 100U) / m_netFrames); - - m_display->clearP25(); - m_netTimeout.stop(); - m_networkWatchdog.stop(); - m_netData.reset(); - m_netState = RS_NET_IDLE; -} - -bool CP25Control::openFile() -{ - if (m_fp != NULL) - return true; - - time_t t; - ::time(&t); - - struct tm* tm = ::localtime(&t); - - char name[100U]; - ::sprintf(name, "P25_%04d%02d%02d_%02d%02d%02d.ambe", tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec); - - m_fp = ::fopen(name, "wb"); - if (m_fp == NULL) - return false; - - ::fwrite("P25", 1U, 3U, m_fp); - - return true; -} - -bool CP25Control::writeFile(const unsigned char* data, unsigned char length) -{ - if (m_fp == NULL) - return false; - - ::fwrite(&length, 1U, 1U, m_fp); - - ::fwrite(data, 1U, length, m_fp); - - return true; -} - -void CP25Control::closeFile() -{ - if (m_fp != NULL) { - ::fclose(m_fp); - m_fp = NULL; - } -} +/* +* 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 +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "P25Control.h" +#include "P25Defines.h" +#include "P25Trellis.h" +#include "P25Utils.h" +#include "Utils.h" +#include "Sync.h" +#include "CRC.h" +#include "Log.h" + +#include +#include +#include +#include + +// #define DUMP_P25 + +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]) + +CP25Control::CP25Control(unsigned int nac, unsigned int id, bool selfOnly, bool uidOverride, CP25Network* network, CDisplay* display, unsigned int timeout, bool duplex, CDMRLookup* lookup, bool remoteGateway, CRSSIInterpolator* rssiMapper) : +m_nac(nac), +m_id(id), +m_selfOnly(selfOnly), +m_uidOverride(uidOverride), +m_remoteGateway(remoteGateway), +m_network(network), +m_display(display), +m_duplex(duplex), +m_lookup(lookup), +m_queue(1000U, "P25 Control"), +m_rfState(RS_RF_LISTENING), +m_netState(RS_NET_IDLE), +m_rfTimeout(1000U, timeout), +m_netTimeout(1000U, timeout), +m_networkWatchdog(1000U, 0U, 1500U), +m_rfFrames(0U), +m_rfBits(0U), +m_rfErrs(0U), +m_netFrames(0U), +m_netLost(0U), +m_rfDataFrames(0U), +m_nid(nac), +m_lastDUID(P25_DUID_TERM), +m_audio(), +m_rfData(), +m_netData(), +m_rfLSD(), +m_netLSD(), +m_netLDU1(NULL), +m_netLDU2(NULL), +m_lastIMBE(NULL), +m_rfLDU(NULL), +m_rfPDU(NULL), +m_rfPDUCount(0U), +m_rfPDUBits(0U), +m_rssiMapper(rssiMapper), +m_rssi(0U), +m_maxRSSI(0U), +m_minRSSI(0U), +m_aveRSSI(0U), +m_rssiCount(0U), +m_fp(NULL) +{ + assert(display != NULL); + assert(lookup != NULL); + assert(rssiMapper != NULL); + + m_netLDU1 = new unsigned char[9U * 25U]; + m_netLDU2 = new unsigned char[9U * 25U]; + + ::memset(m_netLDU1, 0x00U, 9U * 25U); + ::memset(m_netLDU2, 0x00U, 9U * 25U); + + m_lastIMBE = new unsigned char[11U]; + ::memcpy(m_lastIMBE, P25_NULL_IMBE, 11U); + + m_rfLDU = new unsigned char[P25_LDU_FRAME_LENGTH_BYTES]; + ::memset(m_rfLDU, 0x00U, P25_LDU_FRAME_LENGTH_BYTES); + + m_rfPDU = new unsigned char[P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U]; + ::memset(m_rfPDU, 0x00U, P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U); +} + +CP25Control::~CP25Control() +{ + delete[] m_netLDU1; + delete[] m_netLDU2; + delete[] m_lastIMBE; + delete[] m_rfLDU; + delete[] m_rfPDU; +} + +bool CP25Control::writeModem(unsigned char* data, unsigned int len) +{ + assert(data != NULL); + + bool sync = data[1U] == 0x01U; + + if (data[0U] == TAG_LOST && m_rfState == RS_RF_AUDIO) { + if (m_rssi != 0U) + LogMessage("P25, transmission lost, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount); + else + LogMessage("P25, transmission lost, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits)); + + if (m_netState == RS_NET_IDLE) + m_display->clearP25(); + + writeNetwork(m_rfLDU, m_lastDUID, true); + writeNetwork(data + 2U, P25_DUID_TERM, true); + m_rfState = RS_RF_LISTENING; + m_rfTimeout.stop(); + m_rfData.reset(); +#if defined(DUMP_P25) + closeFile(); +#endif + return false; + } + + if (data[0U] == TAG_LOST && m_rfState == RS_RF_DATA) { + if (m_netState == RS_NET_IDLE) + m_display->clearP25(); + + m_rfState = RS_RF_LISTENING; + m_rfPDUCount = 0U; + m_rfPDUBits = 0U; +#if defined(DUMP_P25) + closeFile(); +#endif + return false; + } + + if (data[0U] == TAG_LOST) { + m_rfState = RS_RF_LISTENING; + return false; + } + + if (!sync && m_rfState == RS_RF_LISTENING) + return false; + + // Decode the NID + bool valid = m_nid.decode(data + 2U); + + if (m_rfState == RS_RF_LISTENING && !valid) + return false; + + unsigned char duid = m_nid.getDUID(); + if (!valid) { + switch (m_lastDUID) { + case P25_DUID_HEADER: + case P25_DUID_LDU2: + duid = P25_DUID_LDU1; + break; + case P25_DUID_LDU1: + duid = P25_DUID_LDU2; + break; + case P25_DUID_PDU: + duid = P25_DUID_PDU; + break; + case P25_DUID_TSDU: + duid = P25_DUID_TSDU; + break; + default: + break; + } + } + + // Have we got RSSI bytes on the end of a P25 LDU? + if (len == (P25_LDU_FRAME_LENGTH_BYTES + 4U)) { + uint16_t raw = 0U; + raw |= (data[218U] << 8) & 0xFF00U; + raw |= (data[219U] << 0) & 0x00FFU; + + // Convert the raw RSSI to dBm + int rssi = m_rssiMapper->interpolate(raw); + if (rssi != 0) + LogDebug("P25, raw RSSI: %u, reported RSSI: %d dBm", raw, rssi); + + // RSSI is always reported as positive + m_rssi = (rssi >= 0) ? rssi : -rssi; + + if (m_rssi > m_minRSSI) + m_minRSSI = m_rssi; + if (m_rssi < m_maxRSSI) + m_maxRSSI = m_rssi; + + m_aveRSSI += m_rssi; + m_rssiCount++; + } + + if (duid == P25_DUID_LDU1) { + if (m_rfState == RS_RF_LISTENING) { + m_rfData.reset(); + bool ret = m_rfData.decodeLDU1(data + 2U); + if (!ret) { + m_lastDUID = duid; + return false; + } + + unsigned int srcId = m_rfData.getSrcId(); + + if (m_selfOnly) { + if (m_id > 99999999U) { // Check that the Config DMR-ID is bigger than 8 digits + if (srcId != m_id / 100U) + return false; + } + + else if (m_id > 9999999U) { // Check that the Config DMR-ID is bigger than 7 digits + if (srcId != m_id / 10U) + return false; + } + + else if (srcId != m_id) { // All other cases + return false; + } + } + + if (!m_uidOverride) { + bool found = m_lookup->exists(srcId); + if (!found) + return false; + } + + 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 voice 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; + + m_minRSSI = m_rssi; + m_maxRSSI = m_rssi; + m_aveRSSI = m_rssi; + m_rssiCount = 1U; + + createRFHeader(); + writeNetwork(data + 2U, P25_DUID_HEADER, false); + } else if (m_rfState == RS_RF_AUDIO) { + writeNetwork(m_rfLDU, m_lastDUID, false); + } + + if (m_rfState == RS_RF_AUDIO) { + // Regenerate Sync + CSync::addP25Sync(data + 2U); + + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_LDU1); + + // Regenerate LDU1 Data + m_rfData.encodeLDU1(data + 2U); + + // Regenerate the Low Speed Data + m_rfLSD.process(data + 2U); + + // Regenerate Audio + unsigned int errors = m_audio.process(data + 2U); + LogDebug("P25, LDU1 audio, errs: %u/1233 (%.1f%%)", errors, float(errors) / 12.33F); + + m_display->writeP25BER(float(errors) / 12.33F); + + m_rfBits += 1233U; + m_rfErrs += errors; + m_rfFrames++; + m_lastDUID = duid; + + // Add busy bits + addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + +#if defined(DUMP_P25) + writeFile(data + 2U, len - 2U); +#endif + + ::memcpy(m_rfLDU, data + 2U, P25_LDU_FRAME_LENGTH_BYTES); + + if (m_duplex) { + data[0U] = TAG_DATA; + data[1U] = 0x00U; + writeQueueRF(data, P25_LDU_FRAME_LENGTH_BYTES + 2U); + } + + m_display->writeP25RSSI(m_rssi); + + return true; + } + } else if (duid == P25_DUID_LDU2) { + if (m_rfState == RS_RF_AUDIO) { + writeNetwork(m_rfLDU, m_lastDUID, false); + + // Regenerate Sync + CSync::addP25Sync(data + 2U); + + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_LDU2); + + // Add the dummy LDU2 data + m_rfData.encodeLDU2(data + 2U); + + // Regenerate the Low Speed Data + m_rfLSD.process(data + 2U); + + // Regenerate Audio + unsigned int errors = m_audio.process(data + 2U); + LogDebug("P25, LDU2 audio, errs: %u/1233 (%.1f%%)", errors, float(errors) / 12.33F); + + m_display->writeP25BER(float(errors) / 12.33F); + + m_rfBits += 1233U; + m_rfErrs += errors; + m_rfFrames++; + m_lastDUID = duid; + + // Add busy bits + addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + +#if defined(DUMP_P25) + writeFile(data + 2U, len - 2U); +#endif + + ::memcpy(m_rfLDU, data + 2U, P25_LDU_FRAME_LENGTH_BYTES); + + if (m_duplex) { + data[0U] = TAG_DATA; + data[1U] = 0x00U; + writeQueueRF(data, P25_LDU_FRAME_LENGTH_BYTES + 2U); + } + + m_display->writeP25RSSI(m_rssi); + + 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); + + ::memset(data + 2U, 0x00U, P25_TERM_FRAME_LENGTH_BYTES); + + // Regenerate Sync + CSync::addP25Sync(data + 2U); + + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_TERM); + + // Add busy bits + addBusyBits(data + 2U, P25_TERM_FRAME_LENGTH_BITS, false, true); + + m_rfState = RS_RF_LISTENING; + m_rfTimeout.stop(); + m_rfData.reset(); + m_lastDUID = duid; + + if (m_rssi != 0U) + LogMessage("P25, received RF end of voice transmission, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount); + else + LogMessage("P25, received RF end of voice transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits)); + + m_display->clearP25(); + +#if defined(DUMP_P25) + closeFile(); +#endif + + writeNetwork(data + 2U, P25_DUID_TERM, true); + + if (m_duplex) { + data[0U] = TAG_EOT; + data[1U] = 0x00U; + writeQueueRF(data, P25_TERM_FRAME_LENGTH_BYTES + 2U); + } + } + } else if (duid == P25_DUID_PDU) { + if (m_rfState != RS_RF_DATA) { + m_rfPDUCount = 0U; + m_rfPDUBits = 0U; + m_rfState = RS_RF_DATA; + m_rfDataFrames = 0U; + } + + unsigned int start = m_rfPDUCount * P25_LDU_FRAME_LENGTH_BITS; + + unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES]; + unsigned int bits = CP25Utils::decode(data + 2U, buffer, start, start + P25_LDU_FRAME_LENGTH_BITS); + + for (unsigned int i = 0U; i < bits; i++, m_rfPDUBits++) { + bool b = READ_BIT(buffer, i); + WRITE_BIT(m_rfPDU, m_rfPDUBits, b); + } + + if (m_rfPDUCount == 0U) { + CP25Trellis trellis; + unsigned char header[P25_PDU_HEADER_LENGTH_BYTES]; + bool valid = trellis.decode12(m_rfPDU + P25_SYNC_LENGTH_BYTES + P25_NID_LENGTH_BYTES, header); + if (valid) + valid = CCRC::checkCCITT162(header, P25_PDU_HEADER_LENGTH_BYTES); + + if (valid) { + unsigned int llId = (header[3U] << 16) + (header[4U] << 8) + header[5U]; + unsigned int sap = header[1U] & 0x3FU; + m_rfDataFrames = header[6U] & 0x7FU; + + LogMessage("P25, received RF data transmission for Local Link Id %u, SAP %u, %u blocks", llId, sap, m_rfDataFrames); + } else { + m_rfPDUCount = 0U; + m_rfPDUBits = 0U; + m_rfState = RS_RF_LISTENING; + m_rfDataFrames = 0U; + } + } + + if (m_rfState == RS_RF_DATA) { + m_rfPDUCount++; + + unsigned int bitLength = ((m_rfDataFrames + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_SYNC_LENGTH_BITS + P25_NID_LENGTH_BITS; + + if (m_rfPDUBits >= bitLength) { + unsigned int offset = P25_SYNC_LENGTH_BYTES + P25_NID_LENGTH_BYTES; + + // Regenerate the PDU header + CP25Trellis trellis; + unsigned char header[P25_PDU_HEADER_LENGTH_BYTES]; + trellis.decode12(m_rfPDU + offset, header); + trellis.encode12(header, m_rfPDU + offset); + offset += P25_PDU_FEC_LENGTH_BITS; + + // Regenerate the PDU data + for (unsigned int i = 0U; i < m_rfDataFrames; i++) { + unsigned char data[P25_PDU_CONFIRMED_LENGTH_BYTES]; + + bool valid = trellis.decode34(m_rfPDU + offset, data); + if (valid) { + trellis.encode34(data, m_rfPDU + offset); + } else { + valid = trellis.decode12(m_rfPDU + offset, data); + if (valid) + trellis.encode12(data, m_rfPDU + offset); + } + + offset += P25_PDU_FEC_LENGTH_BITS; + } + + unsigned char pdu[1024U]; + + // Add the data + unsigned int newBitLength = CP25Utils::encode(m_rfPDU, pdu + 2U, bitLength); + unsigned int newByteLength = newBitLength / 8U; + if ((newBitLength % 8U) > 0U) + newByteLength++; + + // Regenerate Sync + CSync::addP25Sync(pdu + 2U); + + // Regenerate NID + m_nid.encode(pdu + 2U, P25_DUID_PDU); + + // Add busy bits + addBusyBits(pdu + 2U, newBitLength, false, true); + + if (m_duplex) { + pdu[0U] = TAG_DATA; + pdu[1U] = 0x00U; + writeQueueRF(pdu, newByteLength + 2U); + } + + LogMessage("P25, ended RF data transmission"); + m_display->clearP25(); + + m_rfPDUCount = 0U; + m_rfPDUBits = 0U; + m_rfState = RS_RF_LISTENING; + m_rfDataFrames = 0U; + } + + return true; + } + } + + return false; +} + +unsigned int CP25Control::readModem(unsigned char* data) +{ + assert(data != NULL); + + if (m_queue.isEmpty()) + return 0U; + + unsigned char len = 0U; + m_queue.getData(&len, 1U); + + m_queue.getData(data, len); + + return len; +} + +void CP25Control::writeNetwork() +{ + unsigned char data[100U]; + unsigned int length = m_network->read(data, 100U); + if (length == 0U) + return; + + if (m_rfState != RS_RF_LISTENING && m_netState == RS_NET_IDLE) + return; + + m_networkWatchdog.start(); + + switch (data[0U]) { + case 0x62U: + ::memcpy(m_netLDU1 + 0U, data, 22U); + checkNetLDU2(); + break; + case 0x63U: + ::memcpy(m_netLDU1 + 25U, data, 14U); + checkNetLDU2(); + break; + case 0x64U: + ::memcpy(m_netLDU1 + 50U, data, 17U); + checkNetLDU2(); + break; + case 0x65U: + ::memcpy(m_netLDU1 + 75U, data, 17U); + checkNetLDU2(); + break; + case 0x66U: + ::memcpy(m_netLDU1 + 100U, data, 17U); + checkNetLDU2(); + break; + case 0x67U: + ::memcpy(m_netLDU1 + 125U, data, 17U); + checkNetLDU2(); + break; + case 0x68U: + ::memcpy(m_netLDU1 + 150U, data, 17U); + checkNetLDU2(); + break; + case 0x69U: + ::memcpy(m_netLDU1 + 175U, data, 17U); + checkNetLDU2(); + break; + case 0x6AU: + ::memcpy(m_netLDU1 + 200U, data, 16U); + checkNetLDU2(); + if (m_netState != RS_NET_IDLE) + createNetLDU1(); + break; + case 0x6BU: + ::memcpy(m_netLDU2 + 0U, data, 22U); + checkNetLDU1(); + break; + case 0x6CU: + ::memcpy(m_netLDU2 + 25U, data, 14U); + checkNetLDU1(); + break; + case 0x6DU: + ::memcpy(m_netLDU2 + 50U, data, 17U); + checkNetLDU1(); + break; + case 0x6EU: + ::memcpy(m_netLDU2 + 75U, data, 17U); + checkNetLDU1(); + break; + case 0x6FU: + ::memcpy(m_netLDU2 + 100U, data, 17U); + checkNetLDU1(); + break; + case 0x70U: + ::memcpy(m_netLDU2 + 125U, data, 17U); + checkNetLDU1(); + break; + case 0x71U: + ::memcpy(m_netLDU2 + 150U, data, 17U); + checkNetLDU1(); + break; + case 0x72U: + ::memcpy(m_netLDU2 + 175U, data, 17U); + checkNetLDU1(); + break; + case 0x73U: + ::memcpy(m_netLDU2 + 200U, data, 16U); + if (m_netState == RS_NET_IDLE) { + createNetHeader(); + createNetLDU1(); + } else { + checkNetLDU1(); + } + createNetLDU2(); + break; + case 0x80U: + createNetTerminator(); + break; + default: + break; + } +} + +void CP25Control::clock(unsigned int ms) +{ + if (m_network != NULL) + writeNetwork(); + + m_rfTimeout.clock(ms); + m_netTimeout.clock(ms); + + if (m_netState == RS_NET_AUDIO) { + m_networkWatchdog.clock(ms); + + if (m_networkWatchdog.hasExpired()) { + LogMessage("P25, network watchdog has expired, %.1f seconds, %u%% packet loss", float(m_netFrames) / 50.0F, (m_netLost * 100U) / m_netFrames); + m_display->clearP25(); + m_networkWatchdog.stop(); + m_netState = RS_NET_IDLE; + m_netData.reset(); + m_netTimeout.stop(); + } + } +} + +void CP25Control::writeQueueRF(const unsigned char* data, unsigned int length) +{ + assert(data != NULL); + + if (m_rfTimeout.isRunning() && m_rfTimeout.hasExpired()) + return; + + unsigned int space = m_queue.freeSpace(); + if (space < (length + 1U)) { + LogError("P25, overflow in the P25 RF queue"); + return; + } + + unsigned char len = length; + m_queue.addData(&len, 1U); + + m_queue.addData(data, len); +} + +void CP25Control::writeQueueNet(const unsigned char* data, unsigned int length) +{ + assert(data != NULL); + + if (m_netTimeout.isRunning() && m_netTimeout.hasExpired()) + return; + + unsigned int space = m_queue.freeSpace(); + if (space < (length + 1U)) { + LogError("P25, overflow in the P25 RF queue"); + return; + } + + unsigned char len = length; + m_queue.addData(&len, 1U); + + m_queue.addData(data, len); +} + +void CP25Control::writeNetwork(const unsigned char *data, unsigned char type, bool end) +{ + assert(data != NULL); + + if (m_network == NULL) + return; + + if (m_rfTimeout.isRunning() && m_rfTimeout.hasExpired()) + return; + + switch (type) + { + case P25_DUID_LDU1: + m_network->writeLDU1(data, m_rfData, m_rfLSD, end); + break; + case P25_DUID_LDU2: + m_network->writeLDU2(data, m_rfData, m_rfLSD, end); + break; + default: + break; + } +} + +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); + + for (unsigned int ss0Pos = P25_SS0_START; ss0Pos < length; ss0Pos += P25_SS_INCREMENT) { + unsigned int ss1Pos = ss0Pos + 1U; + WRITE_BIT(data, ss0Pos, b1); + WRITE_BIT(data, ss1Pos, b2); + } +} + +void CP25Control::checkNetLDU1() +{ + if (m_netState == RS_NET_IDLE) + return; + + // Check for an unflushed LDU1 + if (m_netLDU1[0U] != 0x00U || m_netLDU1[25U] != 0x00U || m_netLDU1[50U] != 0x00U || + m_netLDU1[75U] != 0x00U || m_netLDU1[100U] != 0x00U || m_netLDU1[125U] != 0x00U || + m_netLDU1[150U] != 0x00U || m_netLDU1[175U] != 0x00U || m_netLDU1[200U] != 0x00U) + createNetLDU1(); +} + +void CP25Control::checkNetLDU2() +{ + if (m_netState == RS_NET_IDLE) + return; + + // Check for an unflushed LDU1 + if (m_netLDU2[0U] != 0x00U || m_netLDU2[25U] != 0x00U || m_netLDU2[50U] != 0x00U || + m_netLDU2[75U] != 0x00U || m_netLDU2[100U] != 0x00U || m_netLDU2[125U] != 0x00U || + m_netLDU2[150U] != 0x00U || m_netLDU2[175U] != 0x00U || m_netLDU2[200U] != 0x00U) + createNetLDU2(); +} + +void CP25Control::insertMissingAudio(unsigned char* data) +{ + if (data[0U] == 0x00U) { + ::memcpy(data + 10U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 10U, 11U); + } + + if (data[25U] == 0x00U) { + ::memcpy(data + 26U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 26U, 11U); + } + + if (data[50U] == 0x00U) { + ::memcpy(data + 55U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 55U, 11U); + } + + if (data[75U] == 0x00U) { + ::memcpy(data + 80U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 80U, 11U); + } + + if (data[100U] == 0x00U) { + ::memcpy(data + 105U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 105U, 11U); + } + + if (data[125U] == 0x00U) { + ::memcpy(data + 130U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 130U, 11U); + } + + if (data[150U] == 0x00U) { + ::memcpy(data + 155U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 155U, 11U); + } + + if (data[175U] == 0x00U) { + ::memcpy(data + 180U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 180U, 11U); + } + + if (data[200U] == 0x00U) { + ::memcpy(data + 204U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 204U, 11U); + } +} + +void CP25Control::createRFHeader() +{ + unsigned char buffer[P25_HDR_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_HDR_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = TAG_HEADER; + buffer[1U] = 0x00U; + + // Add the sync + CSync::addP25Sync(buffer + 2U); + + // Add the NID + m_nid.encode(buffer + 2U, P25_DUID_HEADER); + + // Add the dummy header + m_rfData.encodeHeader(buffer + 2U); + + // Add busy bits + addBusyBits(buffer + 2U, P25_HDR_FRAME_LENGTH_BITS, false, true); + + m_rfFrames = 0U; + m_rfErrs = 0U; + m_rfBits = 1U; + m_rfTimeout.start(); + m_lastDUID = P25_DUID_HEADER; + ::memset(m_rfLDU, 0x00U, P25_LDU_FRAME_LENGTH_BYTES); + +#if defined(DUMP_P25) + openFile(); + writeFile(buffer + 2U, buffer - 2U); +#endif + + if (m_duplex) { + buffer[0U] = TAG_HEADER; + buffer[1U] = 0x00U; + writeQueueRF(buffer, P25_HDR_FRAME_LENGTH_BYTES + 2U); + } +} + +void CP25Control::createNetHeader() +{ + 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]; + + unsigned char algId = m_netLDU2[126U]; + unsigned int kId = (m_netLDU2[127U] << 8) + m_netLDU2[128U]; + + 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.reset(); + 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(); + m_netFrames = 0U; + m_netLost = 0U; + + unsigned char buffer[P25_HDR_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_HDR_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = TAG_HEADER; + buffer[1U] = 0x00U; + + // Add the sync + CSync::addP25Sync(buffer + 2U); + + // Add the NID + m_nid.encode(buffer + 2U, P25_DUID_HEADER); + + // Add the dummy header + m_netData.encodeHeader(buffer + 2U); + + // Add busy bits + if (m_remoteGateway) + addBusyBits(buffer + 2U, P25_HDR_FRAME_LENGTH_BITS, false, false); + else + addBusyBits(buffer + 2U, P25_HDR_FRAME_LENGTH_BITS, false, true); + + writeQueueNet(buffer, P25_HDR_FRAME_LENGTH_BYTES + 2U); +} + +void CP25Control::createNetLDU1() +{ + insertMissingAudio(m_netLDU1); + + unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = TAG_DATA; + buffer[1U] = 0x00U; + + // Add the sync + CSync::addP25Sync(buffer + 2U); + + // Add the NID + m_nid.encode(buffer + 2U, P25_DUID_LDU1); + + // Add the LDU1 data + m_netData.encodeLDU1(buffer + 2U); + + // Add the Audio + m_audio.encode(buffer + 2U, m_netLDU1 + 10U, 0U); + m_audio.encode(buffer + 2U, m_netLDU1 + 26U, 1U); + m_audio.encode(buffer + 2U, m_netLDU1 + 55U, 2U); + m_audio.encode(buffer + 2U, m_netLDU1 + 80U, 3U); + m_audio.encode(buffer + 2U, m_netLDU1 + 105U, 4U); + m_audio.encode(buffer + 2U, m_netLDU1 + 130U, 5U); + m_audio.encode(buffer + 2U, m_netLDU1 + 155U, 6U); + m_audio.encode(buffer + 2U, m_netLDU1 + 180U, 7U); + m_audio.encode(buffer + 2U, m_netLDU1 + 204U, 8U); + + // Add the Low Speed Data + m_netLSD.setLSD1(m_netLDU1[201U]); + m_netLSD.setLSD2(m_netLDU1[202U]); + m_netLSD.encode(buffer + 2U); + + // Add busy bits + if (m_remoteGateway) + addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, false); + else + addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + + writeQueueNet(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + ::memset(m_netLDU1, 0x00U, 9U * 25U); + + m_netFrames += 9U; +} + +void CP25Control::createNetLDU2() +{ + insertMissingAudio(m_netLDU2); + + unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = TAG_DATA; + buffer[1U] = 0x00U; + + // Add the sync + CSync::addP25Sync(buffer + 2U); + + // Add the NID + m_nid.encode(buffer + 2U, P25_DUID_LDU2); + + // Add the dummy LDU2 data + m_netData.encodeLDU2(buffer + 2U); + + // Add the Audio + m_audio.encode(buffer + 2U, m_netLDU2 + 10U, 0U); + m_audio.encode(buffer + 2U, m_netLDU2 + 26U, 1U); + m_audio.encode(buffer + 2U, m_netLDU2 + 55U, 2U); + m_audio.encode(buffer + 2U, m_netLDU2 + 80U, 3U); + m_audio.encode(buffer + 2U, m_netLDU2 + 105U, 4U); + m_audio.encode(buffer + 2U, m_netLDU2 + 130U, 5U); + m_audio.encode(buffer + 2U, m_netLDU2 + 155U, 6U); + m_audio.encode(buffer + 2U, m_netLDU2 + 180U, 7U); + m_audio.encode(buffer + 2U, m_netLDU2 + 204U, 8U); + + // Add the Low Speed Data + m_netLSD.setLSD1(m_netLDU2[201U]); + m_netLSD.setLSD2(m_netLDU2[202U]); + m_netLSD.encode(buffer + 2U); + + // Add busy bits + if (m_remoteGateway) + addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, false); + else + addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + + writeQueueNet(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + ::memset(m_netLDU2, 0x00U, 9U * 25U); + + m_netFrames += 9U; +} + +void CP25Control::createNetTerminator() +{ + unsigned char buffer[P25_TERM_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_TERM_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = TAG_EOT; + buffer[1U] = 0x00U; + + // Add the sync + CSync::addP25Sync(buffer + 2U); + + // Add the NID + m_nid.encode(buffer + 2U, P25_DUID_TERM); + + // Add busy bits + if (m_remoteGateway) + addBusyBits(buffer + 2U, P25_TERM_FRAME_LENGTH_BITS, false, false); + else + addBusyBits(buffer + 2U, P25_TERM_FRAME_LENGTH_BITS, false, true); + + writeQueueNet(buffer, P25_TERM_FRAME_LENGTH_BYTES + 2U); + + LogMessage("P25, network end of transmission, %.1f seconds, %u%% packet loss", float(m_netFrames) / 50.0F, (m_netLost * 100U) / m_netFrames); + + m_display->clearP25(); + m_netTimeout.stop(); + m_networkWatchdog.stop(); + m_netData.reset(); + m_netState = RS_NET_IDLE; +} + +bool CP25Control::openFile() +{ + if (m_fp != NULL) + return true; + + time_t t; + ::time(&t); + + struct tm* tm = ::localtime(&t); + + char name[100U]; + ::sprintf(name, "P25_%04d%02d%02d_%02d%02d%02d.ambe", tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec); + + m_fp = ::fopen(name, "wb"); + if (m_fp == NULL) + return false; + + ::fwrite("P25", 1U, 3U, m_fp); + + return true; +} + +bool CP25Control::writeFile(const unsigned char* data, unsigned char length) +{ + if (m_fp == NULL) + return false; + + ::fwrite(&length, 1U, 1U, m_fp); + + ::fwrite(data, 1U, length, m_fp); + + return true; +} + +void CP25Control::closeFile() +{ + if (m_fp != NULL) { + ::fclose(m_fp); + m_fp = NULL; + } +} diff --git a/P25Control.h b/P25Control.h index 79e8d8a..934bdb5 100644 --- a/P25Control.h +++ b/P25Control.h @@ -1,116 +1,118 @@ -/* -* Copyright (C) 2016,2017,2018 by Jonathan Naylor G4KLX -* -* 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 -* the Free Software Foundation; either version 2 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software -* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. -*/ - -#if !defined(P25Control_H) -#define P25Control_H - -#include "RSSIInterpolator.h" -#include "P25LowSpeedData.h" -#include "RingBuffer.h" -#include "P25Network.h" -#include "DMRLookup.h" -#include "P25Audio.h" -#include "Defines.h" -#include "Display.h" -#include "P25Data.h" -#include "P25NID.h" -#include "Modem.h" -#include "Timer.h" - -#include - -class CP25Control { -public: - CP25Control(unsigned int nac, unsigned int id, bool selfOly, bool uidOverride, CP25Network* network, CDisplay* display, unsigned int timeout, bool duplex, CDMRLookup* lookup, bool remoteGateway, CRSSIInterpolator* rssiMapper); - ~CP25Control(); - - bool writeModem(unsigned char* data, unsigned int len); - - unsigned int readModem(unsigned char* data); - - void clock(unsigned int ms); - -private: - unsigned int m_nac; - unsigned int m_id; - bool m_selfOnly; - bool m_uidOverride; - bool m_remoteGateway; - CP25Network* m_network; - CDisplay* m_display; - bool m_duplex; - CDMRLookup* m_lookup; - CRingBuffer m_queue; - RPT_RF_STATE m_rfState; - RPT_NET_STATE m_netState; - CTimer m_rfTimeout; - CTimer m_netTimeout; - CTimer m_networkWatchdog; - unsigned int m_rfFrames; - unsigned int m_rfBits; - unsigned int m_rfErrs; - unsigned int m_netFrames; - unsigned int m_netLost; - unsigned int m_rfDataFrames; - CP25NID m_nid; - unsigned char m_lastDUID; - CP25Audio m_audio; - CP25Data m_rfData; - CP25Data m_netData; - CP25LowSpeedData m_rfLSD; - CP25LowSpeedData m_netLSD; - unsigned char* m_netLDU1; - unsigned char* m_netLDU2; - unsigned char* m_lastIMBE; - unsigned char* m_rfLDU; - unsigned char* m_rfPDU; - unsigned int m_rfPDUCount; - unsigned int m_rfPDUBits; - CRSSIInterpolator* m_rssiMapper; - unsigned char m_rssi; - unsigned char m_maxRSSI; - unsigned char m_minRSSI; - unsigned int m_aveRSSI; - unsigned int m_rssiCount; - FILE* m_fp; - - void writeQueueRF(const unsigned char* data, unsigned int length); - void writeQueueNet(const unsigned char* data, unsigned int length); - void writeNetwork(const unsigned char *data, unsigned char type, bool end); - void writeNetwork(); - - void addBusyBits(unsigned char* data, unsigned int length, bool b1, bool b2); - - void checkNetLDU1(); - void checkNetLDU2(); - - void insertMissingAudio(unsigned char* data); - - void createRFHeader(); - - void createNetHeader(); - void createNetLDU1(); - void createNetLDU2(); - void createNetTerminator(); - - bool openFile(); - bool writeFile(const unsigned char* data, unsigned char length); - void closeFile(); -}; - -#endif +/* +* 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 +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#if !defined(P25Control_H) +#define P25Control_H + +#include "RSSIInterpolator.h" +#include "P25LowSpeedData.h" +#include "RingBuffer.h" +#include "P25Network.h" +#include "DMRLookup.h" +#include "P25Audio.h" +#include "Defines.h" +#include "Display.h" +#include "P25Data.h" +#include "P25NID.h" +#include "Modem.h" +#include "Timer.h" + +#include + +class CP25Control { +public: + CP25Control(unsigned int nac, unsigned int id, bool selfOly, bool uidOverride, CP25Network* network, CDisplay* display, unsigned int timeout, bool duplex, CDMRLookup* lookup, bool remoteGateway, CRSSIInterpolator* rssiMapper); + ~CP25Control(); + + bool writeModem(unsigned char* data, unsigned int len); + + unsigned int readModem(unsigned char* data); + + void clock(unsigned int ms); + +private: + unsigned int m_nac; + unsigned int m_id; + bool m_selfOnly; + bool m_uidOverride; + bool m_remoteGateway; + CP25Network* m_network; + CDisplay* m_display; + bool m_duplex; + CDMRLookup* m_lookup; + CRingBuffer m_queue; + RPT_RF_STATE m_rfState; + RPT_NET_STATE m_netState; + CTimer m_rfTimeout; + CTimer m_netTimeout; + CTimer m_networkWatchdog; + unsigned int m_rfFrames; + unsigned int m_rfBits; + unsigned int m_rfErrs; + unsigned int m_netFrames; + unsigned int m_netLost; + unsigned int m_rfDataFrames; + CP25NID m_nid; + unsigned char m_lastDUID; + CP25Audio m_audio; + CP25Data m_rfData; + CP25Data m_netData; + CP25LowSpeedData m_rfLSD; + CP25LowSpeedData m_netLSD; + unsigned char* m_netLDU1; + unsigned char* m_netLDU2; + unsigned char* m_lastIMBE; + unsigned char* m_rfLDU; + unsigned char* m_rfPDU; + unsigned int m_rfPDUCount; + unsigned int m_rfPDUBits; + CRSSIInterpolator* m_rssiMapper; + unsigned char m_rssi; + unsigned char m_maxRSSI; + unsigned char m_minRSSI; + unsigned int m_aveRSSI; + unsigned int m_rssiCount; + FILE* m_fp; + + void writeQueueRF(const unsigned char* data, unsigned int length); + void writeQueueNet(const unsigned char* data, unsigned int length); + 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(); + void checkNetLDU2(); + + void insertMissingAudio(unsigned char* data); + + void createRFHeader(); + + void createNetHeader(); + void createNetLDU1(); + void createNetLDU2(); + void createNetTerminator(); + + bool openFile(); + bool writeFile(const unsigned char* data, unsigned char length); + void closeFile(); +}; + +#endif diff --git a/P25Data.cpp b/P25Data.cpp index 3c9eb66..8987736 100644 --- a/P25Data.cpp +++ b/P25Data.cpp @@ -1,346 +1,460 @@ -/* -* Copyright (C) 2016,2017 by Jonathan Naylor G4KLX -* -* 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 -* the Free Software Foundation; either version 2 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software -* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. -*/ - -#include "P25Data.h" -#include "P25Defines.h" -#include "P25Utils.h" -#include "Hamming.h" -#include "Utils.h" -#include "Log.h" - -#include -#include -#include - -const unsigned char DUMMY_HEADER[] = { - 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, - 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x08U, 0xDCU, 0x60U, - 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x02U, 0x93U, 0xE7U, 0x73U, 0x77U, 0x57U, 0xD6U, 0xD3U, 0xCFU, 0x77U, - 0xEEU, 0x82U, 0x93U, 0xE2U, 0x2FU, 0xF3U, 0xD5U, 0xF5U, 0xBEU, 0xBCU, 0x54U, 0x0DU, 0x9CU, 0x29U, 0x3EU, 0x46U, - 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_mi(NULL), -m_mfId(0U), -m_algId(0x80U), -m_kId(0U), -m_lcf(0x00U), -m_emergency(false), -m_srcId(0U), -m_dstId(0U), -m_rs241213() -{ - m_mi = new unsigned char[P25_MI_LENGTH_BYTES]; -} - -CP25Data::~CP25Data() -{ - delete[] m_mi; -} - -void CP25Data::encodeHeader(unsigned char* data) -{ - assert(data != NULL); - - CP25Utils::encode(DUMMY_HEADER, data, 114U, 780U); -} - -bool CP25Data::decodeLDU1(const unsigned char* data) -{ - assert(data != NULL); - - unsigned char rs[18U]; - - unsigned char raw[5U]; - CP25Utils::decode(data, raw, 410U, 452U); - decodeLDUHamming(raw, rs + 0U); - - CP25Utils::decode(data, raw, 600U, 640U); - decodeLDUHamming(raw, rs + 3U); - - CP25Utils::decode(data, raw, 788U, 830U); - decodeLDUHamming(raw, rs + 6U); - - CP25Utils::decode(data, raw, 978U, 1020U); - decodeLDUHamming(raw, rs + 9U); - - CP25Utils::decode(data, raw, 1168U, 1208U); - decodeLDUHamming(raw, rs + 12U); - - CP25Utils::decode(data, raw, 1356U, 1398U); - decodeLDUHamming(raw, rs + 15U); - - try { - bool ret = m_rs241213.decode(rs); - if (!ret) - return false; - } catch (...) { - CUtils::dump(2U, "P25, RS carshed with input data", rs, 18U); - return false; - } - - unsigned int srcId = (rs[6U] << 16) + (rs[7U] << 8) + rs[8U]; - - switch (rs[0U]) { - case P25_LCF_GROUP: - m_emergency = (rs[2U] & 0x80U) == 0x80U; - m_dstId = (rs[4U] << 8) + rs[5U]; - m_srcId = srcId; - break; - case P25_LCF_PRIVATE: - m_emergency = false; - m_dstId = (rs[3U] << 16) + (rs[4U] << 8) + rs[5U]; - m_srcId = srcId; - break; - default: - return false; - } - - m_lcf = rs[0U]; - m_mfId = rs[1U]; - - return true; -} - -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); - - 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::encodeLDU2(unsigned char* data) -{ - assert(data != NULL); - - unsigned char raw[5U]; - encodeLDUHamming(raw, DUMMY_LDU2 + 0U); - CP25Utils::encode(raw, data, 410U, 452U); - - encodeLDUHamming(raw, DUMMY_LDU2 + 3U); - CP25Utils::encode(raw, data, 600U, 640U); - - encodeLDUHamming(raw, DUMMY_LDU2 + 6U); - CP25Utils::encode(raw, data, 788U, 830U); - - encodeLDUHamming(raw, DUMMY_LDU2 + 9U); - CP25Utils::encode(raw, data, 978U, 1020U); - - encodeLDUHamming(raw, DUMMY_LDU2 + 12U); - CP25Utils::encode(raw, data, 1168U, 1208U); - - encodeLDUHamming(raw, DUMMY_LDU2 + 15U); - CP25Utils::encode(raw, data, 1356U, 1398U); -} - -void CP25Data::setMI(const unsigned char* mi) -{ - assert(mi != NULL); - - ::memcpy(m_mi, mi, P25_MI_LENGTH_BYTES); -} - -void CP25Data::getMI(unsigned char* mi) const -{ - assert(mi != NULL); - - ::memcpy(mi, m_mi, P25_MI_LENGTH_BYTES); -} - -void CP25Data::setMFId(unsigned char id) -{ - m_mfId = id; -} - -unsigned char CP25Data::getMFId() const -{ - return m_mfId; -} - -void CP25Data::setAlgId(unsigned char id) -{ - m_algId = id; -} - -unsigned char CP25Data::getAlgId() const -{ - 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() -{ - ::memset(m_mi, 0x00U, P25_MI_LENGTH_BYTES); - - m_algId = 0x80U; - m_kId = 0x0000U; - m_lcf = P25_LCF_GROUP; - m_mfId = 0x00U; - m_srcId = 0U; - m_dstId = 0U; - m_emergency = false; -} - -void CP25Data::decodeLDUHamming(const unsigned char* data, unsigned char* raw) -{ - unsigned int n = 0U; - unsigned int m = 0U; - for (unsigned int i = 0U; i < 4U; i++) { - bool hamming[10U]; - - for (unsigned int j = 0U; j < 10U; j++) { - hamming[j] = READ_BIT(data, n); - n++; - } - - CHamming::decode1063(hamming); - - for (unsigned int j = 0U; j < 6U; j++) { - WRITE_BIT(raw, m, hamming[j]); - m++; - } - } -} - -void CP25Data::encodeLDUHamming(unsigned char* data, const unsigned char* raw) -{ - unsigned int n = 0U; - unsigned int m = 0U; - for (unsigned int i = 0U; i < 4U; i++) { - bool hamming[10U]; - - for (unsigned int j = 0U; j < 6U; j++) { - hamming[j] = READ_BIT(raw, m); - m++; - } - - CHamming::encode1063(hamming); - - for (unsigned int j = 0U; j < 10U; j++) { - WRITE_BIT(data, n, hamming[j]); - n++; - } - } -} +/* +* 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 +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "P25Data.h" +#include "P25Defines.h" +#include "P25Utils.h" +#include "CRC.h" +#include "Hamming.h" +#include "Utils.h" +#include "Log.h" + +#include +#include +#include + +const unsigned char DUMMY_HEADER[] = { + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x08U, 0xDCU, 0x60U, + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x02U, 0x93U, 0xE7U, 0x73U, 0x77U, 0x57U, 0xD6U, 0xD3U, 0xCFU, 0x77U, + 0xEEU, 0x82U, 0x93U, 0xE2U, 0x2FU, 0xF3U, 0xD5U, 0xF5U, 0xBEU, 0xBCU, 0x54U, 0x0DU, 0x9CU, 0x29U, 0x3EU, 0x46U, + 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_mi(NULL), +m_mfId(0U), +m_algId(0x80U), +m_kId(0U), +m_lcf(0x00U), +m_emergency(false), +m_srcId(0U), +m_dstId(0U), +m_serviceType(0U), +m_rs241213(), +m_trellis() +{ + m_mi = new unsigned char[P25_MI_LENGTH_BYTES]; +} + +CP25Data::~CP25Data() +{ + delete[] m_mi; +} + +void CP25Data::encodeHeader(unsigned char* data) +{ + assert(data != NULL); + + CP25Utils::encode(DUMMY_HEADER, data, 114U, 780U); +} + +bool CP25Data::decodeLDU1(const unsigned char* data) +{ + assert(data != NULL); + + unsigned char rs[18U]; + + unsigned char raw[5U]; + CP25Utils::decode(data, raw, 410U, 452U); + decodeLDUHamming(raw, rs + 0U); + + CP25Utils::decode(data, raw, 600U, 640U); + decodeLDUHamming(raw, rs + 3U); + + CP25Utils::decode(data, raw, 788U, 830U); + decodeLDUHamming(raw, rs + 6U); + + CP25Utils::decode(data, raw, 978U, 1020U); + decodeLDUHamming(raw, rs + 9U); + + CP25Utils::decode(data, raw, 1168U, 1208U); + decodeLDUHamming(raw, rs + 12U); + + CP25Utils::decode(data, raw, 1356U, 1398U); + decodeLDUHamming(raw, rs + 15U); + + try { + bool ret = m_rs241213.decode(rs); + if (!ret) + return false; + } catch (...) { + CUtils::dump(2U, "P25, RS carshed with input data", rs, 18U); + return false; + } + + unsigned int srcId = (rs[6U] << 16) + (rs[7U] << 8) + rs[8U]; + + switch (rs[0U]) { + case P25_LCF_GROUP: + m_emergency = (rs[2U] & 0x80U) == 0x80U; + m_dstId = (rs[4U] << 8) + rs[5U]; + m_srcId = srcId; + break; + case P25_LCF_PRIVATE: + m_emergency = false; + m_dstId = (rs[3U] << 16) + (rs[4U] << 8) + rs[5U]; + m_srcId = srcId; + break; + default: + return false; + } + + m_lcf = rs[0U]; + m_mfId = rs[1U]; + + return true; +} + +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); + + 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::encodeLDU2(unsigned char* data) +{ + assert(data != NULL); + + unsigned char raw[5U]; + encodeLDUHamming(raw, DUMMY_LDU2 + 0U); + CP25Utils::encode(raw, data, 410U, 452U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 3U); + CP25Utils::encode(raw, data, 600U, 640U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 6U); + CP25Utils::encode(raw, data, 788U, 830U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 9U); + CP25Utils::encode(raw, data, 978U, 1020U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 12U); + CP25Utils::encode(raw, data, 1168U, 1208U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 15U); + 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); + + ::memcpy(m_mi, mi, P25_MI_LENGTH_BYTES); +} + +void CP25Data::getMI(unsigned char* mi) const +{ + assert(mi != NULL); + + ::memcpy(mi, m_mi, P25_MI_LENGTH_BYTES); +} + +void CP25Data::setMFId(unsigned char id) +{ + m_mfId = id; +} + +unsigned char CP25Data::getMFId() const +{ + return m_mfId; +} + +void CP25Data::setAlgId(unsigned char id) +{ + m_algId = id; +} + +unsigned char CP25Data::getAlgId() const +{ + 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() +{ + ::memset(m_mi, 0x00U, P25_MI_LENGTH_BYTES); + + m_algId = 0x80U; + m_kId = 0x0000U; + m_lcf = P25_LCF_GROUP; + m_mfId = 0x00U; + m_srcId = 0U; + m_dstId = 0U; + m_emergency = false; +} + +void CP25Data::decodeLDUHamming(const unsigned char* data, unsigned char* raw) +{ + unsigned int n = 0U; + unsigned int m = 0U; + for (unsigned int i = 0U; i < 4U; i++) { + bool hamming[10U]; + + for (unsigned int j = 0U; j < 10U; j++) { + hamming[j] = READ_BIT(data, n); + n++; + } + + CHamming::decode1063(hamming); + + for (unsigned int j = 0U; j < 6U; j++) { + WRITE_BIT(raw, m, hamming[j]); + m++; + } + } +} + +void CP25Data::encodeLDUHamming(unsigned char* data, const unsigned char* raw) +{ + unsigned int n = 0U; + unsigned int m = 0U; + for (unsigned int i = 0U; i < 4U; i++) { + bool hamming[10U]; + + for (unsigned int j = 0U; j < 6U; j++) { + hamming[j] = READ_BIT(raw, m); + m++; + } + + CHamming::encode1063(hamming); + + for (unsigned int j = 0U; j < 10U; j++) { + WRITE_BIT(data, n, hamming[j]); + n++; + } + } +} diff --git a/P25Data.h b/P25Data.h index 6618da2..40ade19 100644 --- a/P25Data.h +++ b/P25Data.h @@ -1,77 +1,87 @@ -/* -* Copyright (C) 2016,2017 by Jonathan Naylor G4KLX -* -* 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 -* the Free Software Foundation; either version 2 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software -* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. -*/ - -#if !defined(P25Data_H) -#define P25Data_H - -#include "RS241213.h" - -class CP25Data { -public: - CP25Data(); - ~CP25Data(); - - void encodeHeader(unsigned char* data); - - bool decodeLDU1(const unsigned char* data); - void encodeLDU1(unsigned char* data); - - void encodeLDU2(unsigned char* data); - - void setMI(const unsigned char* mi); - void getMI(unsigned char* mi) const; - - void setMFId(unsigned char id); - unsigned char getMFId() 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 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; - - void decodeLDUHamming(const unsigned char* raw, unsigned char* data); - void encodeLDUHamming(unsigned char* data, const unsigned char* raw); -}; - -#endif +/* +* 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 +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#if !defined(P25Data_H) +#define P25Data_H + +#include "RS241213.h" +#include "P25Trellis.h" + +class CP25Data { +public: + CP25Data(); + ~CP25Data(); + + void encodeHeader(unsigned char* data); + + bool decodeLDU1(const unsigned char* data); + void encodeLDU1(unsigned char* data); + + 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; + + void setMFId(unsigned char id); + unsigned char getMFId() 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 setServiceType(unsigned char type); + unsigned char getServiceType() const; + + void reset(); + +private: + 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; + 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); +}; + +#endif diff --git a/P25Defines.h b/P25Defines.h index f04260f..5a61c27 100644 --- a/P25Defines.h +++ b/P25Defines.h @@ -1,70 +1,78 @@ -/* - * Copyright (C) 2016 by Jonathan Naylor G4KLX - * - * 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 - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -#if !defined(P25DEFINES_H) -#define P25DEFINES_H - -const unsigned int P25_HDR_FRAME_LENGTH_BYTES = 99U; -const unsigned int P25_HDR_FRAME_LENGTH_BITS = P25_HDR_FRAME_LENGTH_BYTES * 8U; - -const unsigned int P25_LDU_FRAME_LENGTH_BYTES = 216U; -const unsigned int P25_LDU_FRAME_LENGTH_BITS = P25_LDU_FRAME_LENGTH_BYTES * 8U; - -const unsigned int P25_TERM_FRAME_LENGTH_BYTES = 18U; -const unsigned int P25_TERM_FRAME_LENGTH_BITS = P25_TERM_FRAME_LENGTH_BYTES * 8U; - -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_SYNC_LENGTH_BYTES = 6U; -const unsigned int P25_SYNC_LENGTH_BITS = P25_SYNC_LENGTH_BYTES * 8U; - -const unsigned int P25_NID_LENGTH_BYTES = 8U; -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_MAX_PDU_COUNT = 10U; - -const unsigned int P25_PDU_HEADER_LENGTH_BYTES = 12U; -const unsigned int P25_PDU_CONFIRMED_LENGTH_BYTES = 18U; -const unsigned int P25_PDU_UNCONFIRMED_LENGTH_BYTES = 12U; - -const unsigned int P25_PDU_FEC_LENGTH_BYTES = 24U; -const unsigned int P25_PDU_FEC_LENGTH_BITS = P25_PDU_FEC_LENGTH_BYTES * 8U; - -const unsigned int P25_MI_LENGTH_BYTES = 9U; - -const unsigned char P25_LCF_GROUP = 0x00U; -const unsigned char P25_LCF_PRIVATE = 0x03U; - -const unsigned int P25_SS0_START = 70U; -const unsigned int P25_SS1_START = 71U; -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_LDU2 = 0x0AU; -const unsigned char P25_DUID_PDU = 0x0CU; -const unsigned char P25_DUID_TERM_LC = 0x0FU; - -const unsigned char P25_NULL_IMBE[] = {0x04U, 0x0CU, 0xFDU, 0x7BU, 0xFBU, 0x7DU, 0xF2U, 0x7BU, 0x3DU, 0x9EU, 0x45U}; - -#endif +/* + * 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 + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(P25DEFINES_H) +#define P25DEFINES_H + +const unsigned int P25_HDR_FRAME_LENGTH_BYTES = 99U; +const unsigned int P25_HDR_FRAME_LENGTH_BITS = P25_HDR_FRAME_LENGTH_BYTES * 8U; + +const unsigned int P25_LDU_FRAME_LENGTH_BYTES = 216U; +const unsigned int P25_LDU_FRAME_LENGTH_BITS = P25_LDU_FRAME_LENGTH_BYTES * 8U; + +const unsigned int P25_TERM_FRAME_LENGTH_BYTES = 18U; +const unsigned int P25_TERM_FRAME_LENGTH_BITS = P25_TERM_FRAME_LENGTH_BYTES * 8U; + +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; + +const unsigned int P25_NID_LENGTH_BYTES = 8U; +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_MAX_PDU_COUNT = 10U; + +const unsigned int P25_PDU_HEADER_LENGTH_BYTES = 12U; +const unsigned int P25_PDU_CONFIRMED_LENGTH_BYTES = 18U; +const unsigned int P25_PDU_UNCONFIRMED_LENGTH_BYTES = 12U; + +const unsigned int P25_PDU_FEC_LENGTH_BYTES = 24U; +const unsigned int P25_PDU_FEC_LENGTH_BITS = P25_PDU_FEC_LENGTH_BYTES * 8U; + +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; + +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; + +const unsigned char P25_NULL_IMBE[] = {0x04U, 0x0CU, 0xFDU, 0x7BU, 0xFBU, 0x7DU, 0xF2U, 0x7BU, 0x3DU, 0x9EU, 0x45U}; + +#endif From 471e084723875862907883d398aec4183d53d2fc Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Mon, 14 May 2018 10:25:19 -0400 Subject: [PATCH 2/5] Fix commit with bad line endings. --- P25Control.cpp | 2308 ++++++++++++++++++++++++------------------------ P25Control.h | 236 ++--- P25Data.cpp | 929 +++++++++---------- P25Data.h | 174 ++-- P25Defines.h | 156 ++-- 5 files changed, 1906 insertions(+), 1897 deletions(-) diff --git a/P25Control.cpp b/P25Control.cpp index f6c4d90..bb63230 100644 --- a/P25Control.cpp +++ b/P25Control.cpp @@ -1,1154 +1,1154 @@ -/* -* 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 -* the Free Software Foundation; either version 2 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software -* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. -*/ - -#include "P25Control.h" -#include "P25Defines.h" -#include "P25Trellis.h" -#include "P25Utils.h" -#include "Utils.h" -#include "Sync.h" -#include "CRC.h" -#include "Log.h" - -#include -#include -#include -#include - -// #define DUMP_P25 - -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]) - -CP25Control::CP25Control(unsigned int nac, unsigned int id, bool selfOnly, bool uidOverride, CP25Network* network, CDisplay* display, unsigned int timeout, bool duplex, CDMRLookup* lookup, bool remoteGateway, CRSSIInterpolator* rssiMapper) : -m_nac(nac), -m_id(id), -m_selfOnly(selfOnly), -m_uidOverride(uidOverride), -m_remoteGateway(remoteGateway), -m_network(network), -m_display(display), -m_duplex(duplex), -m_lookup(lookup), -m_queue(1000U, "P25 Control"), -m_rfState(RS_RF_LISTENING), -m_netState(RS_NET_IDLE), -m_rfTimeout(1000U, timeout), -m_netTimeout(1000U, timeout), -m_networkWatchdog(1000U, 0U, 1500U), -m_rfFrames(0U), -m_rfBits(0U), -m_rfErrs(0U), -m_netFrames(0U), -m_netLost(0U), -m_rfDataFrames(0U), -m_nid(nac), -m_lastDUID(P25_DUID_TERM), -m_audio(), -m_rfData(), -m_netData(), -m_rfLSD(), -m_netLSD(), -m_netLDU1(NULL), -m_netLDU2(NULL), -m_lastIMBE(NULL), -m_rfLDU(NULL), -m_rfPDU(NULL), -m_rfPDUCount(0U), -m_rfPDUBits(0U), -m_rssiMapper(rssiMapper), -m_rssi(0U), -m_maxRSSI(0U), -m_minRSSI(0U), -m_aveRSSI(0U), -m_rssiCount(0U), -m_fp(NULL) -{ - assert(display != NULL); - assert(lookup != NULL); - assert(rssiMapper != NULL); - - m_netLDU1 = new unsigned char[9U * 25U]; - m_netLDU2 = new unsigned char[9U * 25U]; - - ::memset(m_netLDU1, 0x00U, 9U * 25U); - ::memset(m_netLDU2, 0x00U, 9U * 25U); - - m_lastIMBE = new unsigned char[11U]; - ::memcpy(m_lastIMBE, P25_NULL_IMBE, 11U); - - m_rfLDU = new unsigned char[P25_LDU_FRAME_LENGTH_BYTES]; - ::memset(m_rfLDU, 0x00U, P25_LDU_FRAME_LENGTH_BYTES); - - m_rfPDU = new unsigned char[P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U]; - ::memset(m_rfPDU, 0x00U, P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U); -} - -CP25Control::~CP25Control() -{ - delete[] m_netLDU1; - delete[] m_netLDU2; - delete[] m_lastIMBE; - delete[] m_rfLDU; - delete[] m_rfPDU; -} - -bool CP25Control::writeModem(unsigned char* data, unsigned int len) -{ - assert(data != NULL); - - bool sync = data[1U] == 0x01U; - - if (data[0U] == TAG_LOST && m_rfState == RS_RF_AUDIO) { - if (m_rssi != 0U) - LogMessage("P25, transmission lost, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount); - else - LogMessage("P25, transmission lost, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits)); - - if (m_netState == RS_NET_IDLE) - m_display->clearP25(); - - writeNetwork(m_rfLDU, m_lastDUID, true); - writeNetwork(data + 2U, P25_DUID_TERM, true); - m_rfState = RS_RF_LISTENING; - m_rfTimeout.stop(); - m_rfData.reset(); -#if defined(DUMP_P25) - closeFile(); -#endif - return false; - } - - if (data[0U] == TAG_LOST && m_rfState == RS_RF_DATA) { - if (m_netState == RS_NET_IDLE) - m_display->clearP25(); - - m_rfState = RS_RF_LISTENING; - m_rfPDUCount = 0U; - m_rfPDUBits = 0U; -#if defined(DUMP_P25) - closeFile(); -#endif - return false; - } - - if (data[0U] == TAG_LOST) { - m_rfState = RS_RF_LISTENING; - return false; - } - - if (!sync && m_rfState == RS_RF_LISTENING) - return false; - - // Decode the NID - bool valid = m_nid.decode(data + 2U); - - if (m_rfState == RS_RF_LISTENING && !valid) - return false; - - unsigned char duid = m_nid.getDUID(); - if (!valid) { - switch (m_lastDUID) { - case P25_DUID_HEADER: - case P25_DUID_LDU2: - duid = P25_DUID_LDU1; - break; - case P25_DUID_LDU1: - duid = P25_DUID_LDU2; - break; - case P25_DUID_PDU: - duid = P25_DUID_PDU; - break; - case P25_DUID_TSDU: - duid = P25_DUID_TSDU; - break; - default: - break; - } - } - - // Have we got RSSI bytes on the end of a P25 LDU? - if (len == (P25_LDU_FRAME_LENGTH_BYTES + 4U)) { - uint16_t raw = 0U; - raw |= (data[218U] << 8) & 0xFF00U; - raw |= (data[219U] << 0) & 0x00FFU; - - // Convert the raw RSSI to dBm - int rssi = m_rssiMapper->interpolate(raw); - if (rssi != 0) - LogDebug("P25, raw RSSI: %u, reported RSSI: %d dBm", raw, rssi); - - // RSSI is always reported as positive - m_rssi = (rssi >= 0) ? rssi : -rssi; - - if (m_rssi > m_minRSSI) - m_minRSSI = m_rssi; - if (m_rssi < m_maxRSSI) - m_maxRSSI = m_rssi; - - m_aveRSSI += m_rssi; - m_rssiCount++; - } - - if (duid == P25_DUID_LDU1) { - if (m_rfState == RS_RF_LISTENING) { - m_rfData.reset(); - bool ret = m_rfData.decodeLDU1(data + 2U); - if (!ret) { - m_lastDUID = duid; - return false; - } - - unsigned int srcId = m_rfData.getSrcId(); - - if (m_selfOnly) { - if (m_id > 99999999U) { // Check that the Config DMR-ID is bigger than 8 digits - if (srcId != m_id / 100U) - return false; - } - - else if (m_id > 9999999U) { // Check that the Config DMR-ID is bigger than 7 digits - if (srcId != m_id / 10U) - return false; - } - - else if (srcId != m_id) { // All other cases - return false; - } - } - - if (!m_uidOverride) { - bool found = m_lookup->exists(srcId); - if (!found) - return false; - } - - 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 voice 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; - - m_minRSSI = m_rssi; - m_maxRSSI = m_rssi; - m_aveRSSI = m_rssi; - m_rssiCount = 1U; - - createRFHeader(); - writeNetwork(data + 2U, P25_DUID_HEADER, false); - } else if (m_rfState == RS_RF_AUDIO) { - writeNetwork(m_rfLDU, m_lastDUID, false); - } - - if (m_rfState == RS_RF_AUDIO) { - // Regenerate Sync - CSync::addP25Sync(data + 2U); - - // Regenerate NID - m_nid.encode(data + 2U, P25_DUID_LDU1); - - // Regenerate LDU1 Data - m_rfData.encodeLDU1(data + 2U); - - // Regenerate the Low Speed Data - m_rfLSD.process(data + 2U); - - // Regenerate Audio - unsigned int errors = m_audio.process(data + 2U); - LogDebug("P25, LDU1 audio, errs: %u/1233 (%.1f%%)", errors, float(errors) / 12.33F); - - m_display->writeP25BER(float(errors) / 12.33F); - - m_rfBits += 1233U; - m_rfErrs += errors; - m_rfFrames++; - m_lastDUID = duid; - - // Add busy bits - addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); - -#if defined(DUMP_P25) - writeFile(data + 2U, len - 2U); -#endif - - ::memcpy(m_rfLDU, data + 2U, P25_LDU_FRAME_LENGTH_BYTES); - - if (m_duplex) { - data[0U] = TAG_DATA; - data[1U] = 0x00U; - writeQueueRF(data, P25_LDU_FRAME_LENGTH_BYTES + 2U); - } - - m_display->writeP25RSSI(m_rssi); - - return true; - } - } else if (duid == P25_DUID_LDU2) { - if (m_rfState == RS_RF_AUDIO) { - writeNetwork(m_rfLDU, m_lastDUID, false); - - // Regenerate Sync - CSync::addP25Sync(data + 2U); - - // Regenerate NID - m_nid.encode(data + 2U, P25_DUID_LDU2); - - // Add the dummy LDU2 data - m_rfData.encodeLDU2(data + 2U); - - // Regenerate the Low Speed Data - m_rfLSD.process(data + 2U); - - // Regenerate Audio - unsigned int errors = m_audio.process(data + 2U); - LogDebug("P25, LDU2 audio, errs: %u/1233 (%.1f%%)", errors, float(errors) / 12.33F); - - m_display->writeP25BER(float(errors) / 12.33F); - - m_rfBits += 1233U; - m_rfErrs += errors; - m_rfFrames++; - m_lastDUID = duid; - - // Add busy bits - addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); - -#if defined(DUMP_P25) - writeFile(data + 2U, len - 2U); -#endif - - ::memcpy(m_rfLDU, data + 2U, P25_LDU_FRAME_LENGTH_BYTES); - - if (m_duplex) { - data[0U] = TAG_DATA; - data[1U] = 0x00U; - writeQueueRF(data, P25_LDU_FRAME_LENGTH_BYTES + 2U); - } - - m_display->writeP25RSSI(m_rssi); - - 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); - - ::memset(data + 2U, 0x00U, P25_TERM_FRAME_LENGTH_BYTES); - - // Regenerate Sync - CSync::addP25Sync(data + 2U); - - // Regenerate NID - m_nid.encode(data + 2U, P25_DUID_TERM); - - // Add busy bits - addBusyBits(data + 2U, P25_TERM_FRAME_LENGTH_BITS, false, true); - - m_rfState = RS_RF_LISTENING; - m_rfTimeout.stop(); - m_rfData.reset(); - m_lastDUID = duid; - - if (m_rssi != 0U) - LogMessage("P25, received RF end of voice transmission, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount); - else - LogMessage("P25, received RF end of voice transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits)); - - m_display->clearP25(); - -#if defined(DUMP_P25) - closeFile(); -#endif - - writeNetwork(data + 2U, P25_DUID_TERM, true); - - if (m_duplex) { - data[0U] = TAG_EOT; - data[1U] = 0x00U; - writeQueueRF(data, P25_TERM_FRAME_LENGTH_BYTES + 2U); - } - } - } else if (duid == P25_DUID_PDU) { - if (m_rfState != RS_RF_DATA) { - m_rfPDUCount = 0U; - m_rfPDUBits = 0U; - m_rfState = RS_RF_DATA; - m_rfDataFrames = 0U; - } - - unsigned int start = m_rfPDUCount * P25_LDU_FRAME_LENGTH_BITS; - - unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES]; - unsigned int bits = CP25Utils::decode(data + 2U, buffer, start, start + P25_LDU_FRAME_LENGTH_BITS); - - for (unsigned int i = 0U; i < bits; i++, m_rfPDUBits++) { - bool b = READ_BIT(buffer, i); - WRITE_BIT(m_rfPDU, m_rfPDUBits, b); - } - - if (m_rfPDUCount == 0U) { - CP25Trellis trellis; - unsigned char header[P25_PDU_HEADER_LENGTH_BYTES]; - bool valid = trellis.decode12(m_rfPDU + P25_SYNC_LENGTH_BYTES + P25_NID_LENGTH_BYTES, header); - if (valid) - valid = CCRC::checkCCITT162(header, P25_PDU_HEADER_LENGTH_BYTES); - - if (valid) { - unsigned int llId = (header[3U] << 16) + (header[4U] << 8) + header[5U]; - unsigned int sap = header[1U] & 0x3FU; - m_rfDataFrames = header[6U] & 0x7FU; - - LogMessage("P25, received RF data transmission for Local Link Id %u, SAP %u, %u blocks", llId, sap, m_rfDataFrames); - } else { - m_rfPDUCount = 0U; - m_rfPDUBits = 0U; - m_rfState = RS_RF_LISTENING; - m_rfDataFrames = 0U; - } - } - - if (m_rfState == RS_RF_DATA) { - m_rfPDUCount++; - - unsigned int bitLength = ((m_rfDataFrames + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_SYNC_LENGTH_BITS + P25_NID_LENGTH_BITS; - - if (m_rfPDUBits >= bitLength) { - unsigned int offset = P25_SYNC_LENGTH_BYTES + P25_NID_LENGTH_BYTES; - - // Regenerate the PDU header - CP25Trellis trellis; - unsigned char header[P25_PDU_HEADER_LENGTH_BYTES]; - trellis.decode12(m_rfPDU + offset, header); - trellis.encode12(header, m_rfPDU + offset); - offset += P25_PDU_FEC_LENGTH_BITS; - - // Regenerate the PDU data - for (unsigned int i = 0U; i < m_rfDataFrames; i++) { - unsigned char data[P25_PDU_CONFIRMED_LENGTH_BYTES]; - - bool valid = trellis.decode34(m_rfPDU + offset, data); - if (valid) { - trellis.encode34(data, m_rfPDU + offset); - } else { - valid = trellis.decode12(m_rfPDU + offset, data); - if (valid) - trellis.encode12(data, m_rfPDU + offset); - } - - offset += P25_PDU_FEC_LENGTH_BITS; - } - - unsigned char pdu[1024U]; - - // Add the data - unsigned int newBitLength = CP25Utils::encode(m_rfPDU, pdu + 2U, bitLength); - unsigned int newByteLength = newBitLength / 8U; - if ((newBitLength % 8U) > 0U) - newByteLength++; - - // Regenerate Sync - CSync::addP25Sync(pdu + 2U); - - // Regenerate NID - m_nid.encode(pdu + 2U, P25_DUID_PDU); - - // Add busy bits - addBusyBits(pdu + 2U, newBitLength, false, true); - - if (m_duplex) { - pdu[0U] = TAG_DATA; - pdu[1U] = 0x00U; - writeQueueRF(pdu, newByteLength + 2U); - } - - LogMessage("P25, ended RF data transmission"); - m_display->clearP25(); - - m_rfPDUCount = 0U; - m_rfPDUBits = 0U; - m_rfState = RS_RF_LISTENING; - m_rfDataFrames = 0U; - } - - return true; - } - } - - return false; -} - -unsigned int CP25Control::readModem(unsigned char* data) -{ - assert(data != NULL); - - if (m_queue.isEmpty()) - return 0U; - - unsigned char len = 0U; - m_queue.getData(&len, 1U); - - m_queue.getData(data, len); - - return len; -} - -void CP25Control::writeNetwork() -{ - unsigned char data[100U]; - unsigned int length = m_network->read(data, 100U); - if (length == 0U) - return; - - if (m_rfState != RS_RF_LISTENING && m_netState == RS_NET_IDLE) - return; - - m_networkWatchdog.start(); - - switch (data[0U]) { - case 0x62U: - ::memcpy(m_netLDU1 + 0U, data, 22U); - checkNetLDU2(); - break; - case 0x63U: - ::memcpy(m_netLDU1 + 25U, data, 14U); - checkNetLDU2(); - break; - case 0x64U: - ::memcpy(m_netLDU1 + 50U, data, 17U); - checkNetLDU2(); - break; - case 0x65U: - ::memcpy(m_netLDU1 + 75U, data, 17U); - checkNetLDU2(); - break; - case 0x66U: - ::memcpy(m_netLDU1 + 100U, data, 17U); - checkNetLDU2(); - break; - case 0x67U: - ::memcpy(m_netLDU1 + 125U, data, 17U); - checkNetLDU2(); - break; - case 0x68U: - ::memcpy(m_netLDU1 + 150U, data, 17U); - checkNetLDU2(); - break; - case 0x69U: - ::memcpy(m_netLDU1 + 175U, data, 17U); - checkNetLDU2(); - break; - case 0x6AU: - ::memcpy(m_netLDU1 + 200U, data, 16U); - checkNetLDU2(); - if (m_netState != RS_NET_IDLE) - createNetLDU1(); - break; - case 0x6BU: - ::memcpy(m_netLDU2 + 0U, data, 22U); - checkNetLDU1(); - break; - case 0x6CU: - ::memcpy(m_netLDU2 + 25U, data, 14U); - checkNetLDU1(); - break; - case 0x6DU: - ::memcpy(m_netLDU2 + 50U, data, 17U); - checkNetLDU1(); - break; - case 0x6EU: - ::memcpy(m_netLDU2 + 75U, data, 17U); - checkNetLDU1(); - break; - case 0x6FU: - ::memcpy(m_netLDU2 + 100U, data, 17U); - checkNetLDU1(); - break; - case 0x70U: - ::memcpy(m_netLDU2 + 125U, data, 17U); - checkNetLDU1(); - break; - case 0x71U: - ::memcpy(m_netLDU2 + 150U, data, 17U); - checkNetLDU1(); - break; - case 0x72U: - ::memcpy(m_netLDU2 + 175U, data, 17U); - checkNetLDU1(); - break; - case 0x73U: - ::memcpy(m_netLDU2 + 200U, data, 16U); - if (m_netState == RS_NET_IDLE) { - createNetHeader(); - createNetLDU1(); - } else { - checkNetLDU1(); - } - createNetLDU2(); - break; - case 0x80U: - createNetTerminator(); - break; - default: - break; - } -} - -void CP25Control::clock(unsigned int ms) -{ - if (m_network != NULL) - writeNetwork(); - - m_rfTimeout.clock(ms); - m_netTimeout.clock(ms); - - if (m_netState == RS_NET_AUDIO) { - m_networkWatchdog.clock(ms); - - if (m_networkWatchdog.hasExpired()) { - LogMessage("P25, network watchdog has expired, %.1f seconds, %u%% packet loss", float(m_netFrames) / 50.0F, (m_netLost * 100U) / m_netFrames); - m_display->clearP25(); - m_networkWatchdog.stop(); - m_netState = RS_NET_IDLE; - m_netData.reset(); - m_netTimeout.stop(); - } - } -} - -void CP25Control::writeQueueRF(const unsigned char* data, unsigned int length) -{ - assert(data != NULL); - - if (m_rfTimeout.isRunning() && m_rfTimeout.hasExpired()) - return; - - unsigned int space = m_queue.freeSpace(); - if (space < (length + 1U)) { - LogError("P25, overflow in the P25 RF queue"); - return; - } - - unsigned char len = length; - m_queue.addData(&len, 1U); - - m_queue.addData(data, len); -} - -void CP25Control::writeQueueNet(const unsigned char* data, unsigned int length) -{ - assert(data != NULL); - - if (m_netTimeout.isRunning() && m_netTimeout.hasExpired()) - return; - - unsigned int space = m_queue.freeSpace(); - if (space < (length + 1U)) { - LogError("P25, overflow in the P25 RF queue"); - return; - } - - unsigned char len = length; - m_queue.addData(&len, 1U); - - m_queue.addData(data, len); -} - -void CP25Control::writeNetwork(const unsigned char *data, unsigned char type, bool end) -{ - assert(data != NULL); - - if (m_network == NULL) - return; - - if (m_rfTimeout.isRunning() && m_rfTimeout.hasExpired()) - return; - - switch (type) - { - case P25_DUID_LDU1: - m_network->writeLDU1(data, m_rfData, m_rfLSD, end); - break; - case P25_DUID_LDU2: - m_network->writeLDU2(data, m_rfData, m_rfLSD, end); - break; - default: - break; - } -} - -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); - - for (unsigned int ss0Pos = P25_SS0_START; ss0Pos < length; ss0Pos += P25_SS_INCREMENT) { - unsigned int ss1Pos = ss0Pos + 1U; - WRITE_BIT(data, ss0Pos, b1); - WRITE_BIT(data, ss1Pos, b2); - } -} - -void CP25Control::checkNetLDU1() -{ - if (m_netState == RS_NET_IDLE) - return; - - // Check for an unflushed LDU1 - if (m_netLDU1[0U] != 0x00U || m_netLDU1[25U] != 0x00U || m_netLDU1[50U] != 0x00U || - m_netLDU1[75U] != 0x00U || m_netLDU1[100U] != 0x00U || m_netLDU1[125U] != 0x00U || - m_netLDU1[150U] != 0x00U || m_netLDU1[175U] != 0x00U || m_netLDU1[200U] != 0x00U) - createNetLDU1(); -} - -void CP25Control::checkNetLDU2() -{ - if (m_netState == RS_NET_IDLE) - return; - - // Check for an unflushed LDU1 - if (m_netLDU2[0U] != 0x00U || m_netLDU2[25U] != 0x00U || m_netLDU2[50U] != 0x00U || - m_netLDU2[75U] != 0x00U || m_netLDU2[100U] != 0x00U || m_netLDU2[125U] != 0x00U || - m_netLDU2[150U] != 0x00U || m_netLDU2[175U] != 0x00U || m_netLDU2[200U] != 0x00U) - createNetLDU2(); -} - -void CP25Control::insertMissingAudio(unsigned char* data) -{ - if (data[0U] == 0x00U) { - ::memcpy(data + 10U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 10U, 11U); - } - - if (data[25U] == 0x00U) { - ::memcpy(data + 26U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 26U, 11U); - } - - if (data[50U] == 0x00U) { - ::memcpy(data + 55U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 55U, 11U); - } - - if (data[75U] == 0x00U) { - ::memcpy(data + 80U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 80U, 11U); - } - - if (data[100U] == 0x00U) { - ::memcpy(data + 105U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 105U, 11U); - } - - if (data[125U] == 0x00U) { - ::memcpy(data + 130U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 130U, 11U); - } - - if (data[150U] == 0x00U) { - ::memcpy(data + 155U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 155U, 11U); - } - - if (data[175U] == 0x00U) { - ::memcpy(data + 180U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 180U, 11U); - } - - if (data[200U] == 0x00U) { - ::memcpy(data + 204U, m_lastIMBE, 11U); - m_netLost++; - } else { - ::memcpy(m_lastIMBE, data + 204U, 11U); - } -} - -void CP25Control::createRFHeader() -{ - unsigned char buffer[P25_HDR_FRAME_LENGTH_BYTES + 2U]; - ::memset(buffer, 0x00U, P25_HDR_FRAME_LENGTH_BYTES + 2U); - - buffer[0U] = TAG_HEADER; - buffer[1U] = 0x00U; - - // Add the sync - CSync::addP25Sync(buffer + 2U); - - // Add the NID - m_nid.encode(buffer + 2U, P25_DUID_HEADER); - - // Add the dummy header - m_rfData.encodeHeader(buffer + 2U); - - // Add busy bits - addBusyBits(buffer + 2U, P25_HDR_FRAME_LENGTH_BITS, false, true); - - m_rfFrames = 0U; - m_rfErrs = 0U; - m_rfBits = 1U; - m_rfTimeout.start(); - m_lastDUID = P25_DUID_HEADER; - ::memset(m_rfLDU, 0x00U, P25_LDU_FRAME_LENGTH_BYTES); - -#if defined(DUMP_P25) - openFile(); - writeFile(buffer + 2U, buffer - 2U); -#endif - - if (m_duplex) { - buffer[0U] = TAG_HEADER; - buffer[1U] = 0x00U; - writeQueueRF(buffer, P25_HDR_FRAME_LENGTH_BYTES + 2U); - } -} - -void CP25Control::createNetHeader() -{ - 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]; - - unsigned char algId = m_netLDU2[126U]; - unsigned int kId = (m_netLDU2[127U] << 8) + m_netLDU2[128U]; - - 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.reset(); - 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(); - m_netFrames = 0U; - m_netLost = 0U; - - unsigned char buffer[P25_HDR_FRAME_LENGTH_BYTES + 2U]; - ::memset(buffer, 0x00U, P25_HDR_FRAME_LENGTH_BYTES + 2U); - - buffer[0U] = TAG_HEADER; - buffer[1U] = 0x00U; - - // Add the sync - CSync::addP25Sync(buffer + 2U); - - // Add the NID - m_nid.encode(buffer + 2U, P25_DUID_HEADER); - - // Add the dummy header - m_netData.encodeHeader(buffer + 2U); - - // Add busy bits - if (m_remoteGateway) - addBusyBits(buffer + 2U, P25_HDR_FRAME_LENGTH_BITS, false, false); - else - addBusyBits(buffer + 2U, P25_HDR_FRAME_LENGTH_BITS, false, true); - - writeQueueNet(buffer, P25_HDR_FRAME_LENGTH_BYTES + 2U); -} - -void CP25Control::createNetLDU1() -{ - insertMissingAudio(m_netLDU1); - - unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES + 2U]; - ::memset(buffer, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 2U); - - buffer[0U] = TAG_DATA; - buffer[1U] = 0x00U; - - // Add the sync - CSync::addP25Sync(buffer + 2U); - - // Add the NID - m_nid.encode(buffer + 2U, P25_DUID_LDU1); - - // Add the LDU1 data - m_netData.encodeLDU1(buffer + 2U); - - // Add the Audio - m_audio.encode(buffer + 2U, m_netLDU1 + 10U, 0U); - m_audio.encode(buffer + 2U, m_netLDU1 + 26U, 1U); - m_audio.encode(buffer + 2U, m_netLDU1 + 55U, 2U); - m_audio.encode(buffer + 2U, m_netLDU1 + 80U, 3U); - m_audio.encode(buffer + 2U, m_netLDU1 + 105U, 4U); - m_audio.encode(buffer + 2U, m_netLDU1 + 130U, 5U); - m_audio.encode(buffer + 2U, m_netLDU1 + 155U, 6U); - m_audio.encode(buffer + 2U, m_netLDU1 + 180U, 7U); - m_audio.encode(buffer + 2U, m_netLDU1 + 204U, 8U); - - // Add the Low Speed Data - m_netLSD.setLSD1(m_netLDU1[201U]); - m_netLSD.setLSD2(m_netLDU1[202U]); - m_netLSD.encode(buffer + 2U); - - // Add busy bits - if (m_remoteGateway) - addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, false); - else - addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); - - writeQueueNet(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); - - ::memset(m_netLDU1, 0x00U, 9U * 25U); - - m_netFrames += 9U; -} - -void CP25Control::createNetLDU2() -{ - insertMissingAudio(m_netLDU2); - - unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES + 2U]; - ::memset(buffer, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 2U); - - buffer[0U] = TAG_DATA; - buffer[1U] = 0x00U; - - // Add the sync - CSync::addP25Sync(buffer + 2U); - - // Add the NID - m_nid.encode(buffer + 2U, P25_DUID_LDU2); - - // Add the dummy LDU2 data - m_netData.encodeLDU2(buffer + 2U); - - // Add the Audio - m_audio.encode(buffer + 2U, m_netLDU2 + 10U, 0U); - m_audio.encode(buffer + 2U, m_netLDU2 + 26U, 1U); - m_audio.encode(buffer + 2U, m_netLDU2 + 55U, 2U); - m_audio.encode(buffer + 2U, m_netLDU2 + 80U, 3U); - m_audio.encode(buffer + 2U, m_netLDU2 + 105U, 4U); - m_audio.encode(buffer + 2U, m_netLDU2 + 130U, 5U); - m_audio.encode(buffer + 2U, m_netLDU2 + 155U, 6U); - m_audio.encode(buffer + 2U, m_netLDU2 + 180U, 7U); - m_audio.encode(buffer + 2U, m_netLDU2 + 204U, 8U); - - // Add the Low Speed Data - m_netLSD.setLSD1(m_netLDU2[201U]); - m_netLSD.setLSD2(m_netLDU2[202U]); - m_netLSD.encode(buffer + 2U); - - // Add busy bits - if (m_remoteGateway) - addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, false); - else - addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); - - writeQueueNet(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); - - ::memset(m_netLDU2, 0x00U, 9U * 25U); - - m_netFrames += 9U; -} - -void CP25Control::createNetTerminator() -{ - unsigned char buffer[P25_TERM_FRAME_LENGTH_BYTES + 2U]; - ::memset(buffer, 0x00U, P25_TERM_FRAME_LENGTH_BYTES + 2U); - - buffer[0U] = TAG_EOT; - buffer[1U] = 0x00U; - - // Add the sync - CSync::addP25Sync(buffer + 2U); - - // Add the NID - m_nid.encode(buffer + 2U, P25_DUID_TERM); - - // Add busy bits - if (m_remoteGateway) - addBusyBits(buffer + 2U, P25_TERM_FRAME_LENGTH_BITS, false, false); - else - addBusyBits(buffer + 2U, P25_TERM_FRAME_LENGTH_BITS, false, true); - - writeQueueNet(buffer, P25_TERM_FRAME_LENGTH_BYTES + 2U); - - LogMessage("P25, network end of transmission, %.1f seconds, %u%% packet loss", float(m_netFrames) / 50.0F, (m_netLost * 100U) / m_netFrames); - - m_display->clearP25(); - m_netTimeout.stop(); - m_networkWatchdog.stop(); - m_netData.reset(); - m_netState = RS_NET_IDLE; -} - -bool CP25Control::openFile() -{ - if (m_fp != NULL) - return true; - - time_t t; - ::time(&t); - - struct tm* tm = ::localtime(&t); - - char name[100U]; - ::sprintf(name, "P25_%04d%02d%02d_%02d%02d%02d.ambe", tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec); - - m_fp = ::fopen(name, "wb"); - if (m_fp == NULL) - return false; - - ::fwrite("P25", 1U, 3U, m_fp); - - return true; -} - -bool CP25Control::writeFile(const unsigned char* data, unsigned char length) -{ - if (m_fp == NULL) - return false; - - ::fwrite(&length, 1U, 1U, m_fp); - - ::fwrite(data, 1U, length, m_fp); - - return true; -} - -void CP25Control::closeFile() -{ - if (m_fp != NULL) { - ::fclose(m_fp); - m_fp = NULL; - } -} +/* +* 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 +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "P25Control.h" +#include "P25Defines.h" +#include "P25Trellis.h" +#include "P25Utils.h" +#include "Utils.h" +#include "Sync.h" +#include "CRC.h" +#include "Log.h" + +#include +#include +#include +#include + +// #define DUMP_P25 + +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]) + +CP25Control::CP25Control(unsigned int nac, unsigned int id, bool selfOnly, bool uidOverride, CP25Network* network, CDisplay* display, unsigned int timeout, bool duplex, CDMRLookup* lookup, bool remoteGateway, CRSSIInterpolator* rssiMapper) : +m_nac(nac), +m_id(id), +m_selfOnly(selfOnly), +m_uidOverride(uidOverride), +m_remoteGateway(remoteGateway), +m_network(network), +m_display(display), +m_duplex(duplex), +m_lookup(lookup), +m_queue(1000U, "P25 Control"), +m_rfState(RS_RF_LISTENING), +m_netState(RS_NET_IDLE), +m_rfTimeout(1000U, timeout), +m_netTimeout(1000U, timeout), +m_networkWatchdog(1000U, 0U, 1500U), +m_rfFrames(0U), +m_rfBits(0U), +m_rfErrs(0U), +m_netFrames(0U), +m_netLost(0U), +m_rfDataFrames(0U), +m_nid(nac), +m_lastDUID(P25_DUID_TERM), +m_audio(), +m_rfData(), +m_netData(), +m_rfLSD(), +m_netLSD(), +m_netLDU1(NULL), +m_netLDU2(NULL), +m_lastIMBE(NULL), +m_rfLDU(NULL), +m_rfPDU(NULL), +m_rfPDUCount(0U), +m_rfPDUBits(0U), +m_rssiMapper(rssiMapper), +m_rssi(0U), +m_maxRSSI(0U), +m_minRSSI(0U), +m_aveRSSI(0U), +m_rssiCount(0U), +m_fp(NULL) +{ + assert(display != NULL); + assert(lookup != NULL); + assert(rssiMapper != NULL); + + m_netLDU1 = new unsigned char[9U * 25U]; + m_netLDU2 = new unsigned char[9U * 25U]; + + ::memset(m_netLDU1, 0x00U, 9U * 25U); + ::memset(m_netLDU2, 0x00U, 9U * 25U); + + m_lastIMBE = new unsigned char[11U]; + ::memcpy(m_lastIMBE, P25_NULL_IMBE, 11U); + + m_rfLDU = new unsigned char[P25_LDU_FRAME_LENGTH_BYTES]; + ::memset(m_rfLDU, 0x00U, P25_LDU_FRAME_LENGTH_BYTES); + + m_rfPDU = new unsigned char[P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U]; + ::memset(m_rfPDU, 0x00U, P25_MAX_PDU_COUNT * P25_LDU_FRAME_LENGTH_BYTES + 2U); +} + +CP25Control::~CP25Control() +{ + delete[] m_netLDU1; + delete[] m_netLDU2; + delete[] m_lastIMBE; + delete[] m_rfLDU; + delete[] m_rfPDU; +} + +bool CP25Control::writeModem(unsigned char* data, unsigned int len) +{ + assert(data != NULL); + + bool sync = data[1U] == 0x01U; + + if (data[0U] == TAG_LOST && m_rfState == RS_RF_AUDIO) { + if (m_rssi != 0U) + LogMessage("P25, transmission lost, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount); + else + LogMessage("P25, transmission lost, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits)); + + if (m_netState == RS_NET_IDLE) + m_display->clearP25(); + + writeNetwork(m_rfLDU, m_lastDUID, true); + writeNetwork(data + 2U, P25_DUID_TERM, true); + m_rfState = RS_RF_LISTENING; + m_rfTimeout.stop(); + m_rfData.reset(); +#if defined(DUMP_P25) + closeFile(); +#endif + return false; + } + + if (data[0U] == TAG_LOST && m_rfState == RS_RF_DATA) { + if (m_netState == RS_NET_IDLE) + m_display->clearP25(); + + m_rfState = RS_RF_LISTENING; + m_rfPDUCount = 0U; + m_rfPDUBits = 0U; +#if defined(DUMP_P25) + closeFile(); +#endif + return false; + } + + if (data[0U] == TAG_LOST) { + m_rfState = RS_RF_LISTENING; + return false; + } + + if (!sync && m_rfState == RS_RF_LISTENING) + return false; + + // Decode the NID + bool valid = m_nid.decode(data + 2U); + + if (m_rfState == RS_RF_LISTENING && !valid) + return false; + + unsigned char duid = m_nid.getDUID(); + if (!valid) { + switch (m_lastDUID) { + case P25_DUID_HEADER: + case P25_DUID_LDU2: + duid = P25_DUID_LDU1; + break; + case P25_DUID_LDU1: + duid = P25_DUID_LDU2; + break; + case P25_DUID_PDU: + duid = P25_DUID_PDU; + break; + case P25_DUID_TSDU: + duid = P25_DUID_TSDU; + break; + default: + break; + } + } + + // Have we got RSSI bytes on the end of a P25 LDU? + if (len == (P25_LDU_FRAME_LENGTH_BYTES + 4U)) { + uint16_t raw = 0U; + raw |= (data[218U] << 8) & 0xFF00U; + raw |= (data[219U] << 0) & 0x00FFU; + + // Convert the raw RSSI to dBm + int rssi = m_rssiMapper->interpolate(raw); + if (rssi != 0) + LogDebug("P25, raw RSSI: %u, reported RSSI: %d dBm", raw, rssi); + + // RSSI is always reported as positive + m_rssi = (rssi >= 0) ? rssi : -rssi; + + if (m_rssi > m_minRSSI) + m_minRSSI = m_rssi; + if (m_rssi < m_maxRSSI) + m_maxRSSI = m_rssi; + + m_aveRSSI += m_rssi; + m_rssiCount++; + } + + if (duid == P25_DUID_LDU1) { + if (m_rfState == RS_RF_LISTENING) { + m_rfData.reset(); + bool ret = m_rfData.decodeLDU1(data + 2U); + if (!ret) { + m_lastDUID = duid; + return false; + } + + unsigned int srcId = m_rfData.getSrcId(); + + if (m_selfOnly) { + if (m_id > 99999999U) { // Check that the Config DMR-ID is bigger than 8 digits + if (srcId != m_id / 100U) + return false; + } + + else if (m_id > 9999999U) { // Check that the Config DMR-ID is bigger than 7 digits + if (srcId != m_id / 10U) + return false; + } + + else if (srcId != m_id) { // All other cases + return false; + } + } + + if (!m_uidOverride) { + bool found = m_lookup->exists(srcId); + if (!found) + return false; + } + + 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 voice 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; + + m_minRSSI = m_rssi; + m_maxRSSI = m_rssi; + m_aveRSSI = m_rssi; + m_rssiCount = 1U; + + createRFHeader(); + writeNetwork(data + 2U, P25_DUID_HEADER, false); + } else if (m_rfState == RS_RF_AUDIO) { + writeNetwork(m_rfLDU, m_lastDUID, false); + } + + if (m_rfState == RS_RF_AUDIO) { + // Regenerate Sync + CSync::addP25Sync(data + 2U); + + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_LDU1); + + // Regenerate LDU1 Data + m_rfData.encodeLDU1(data + 2U); + + // Regenerate the Low Speed Data + m_rfLSD.process(data + 2U); + + // Regenerate Audio + unsigned int errors = m_audio.process(data + 2U); + LogDebug("P25, LDU1 audio, errs: %u/1233 (%.1f%%)", errors, float(errors) / 12.33F); + + m_display->writeP25BER(float(errors) / 12.33F); + + m_rfBits += 1233U; + m_rfErrs += errors; + m_rfFrames++; + m_lastDUID = duid; + + // Add busy bits + addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + +#if defined(DUMP_P25) + writeFile(data + 2U, len - 2U); +#endif + + ::memcpy(m_rfLDU, data + 2U, P25_LDU_FRAME_LENGTH_BYTES); + + if (m_duplex) { + data[0U] = TAG_DATA; + data[1U] = 0x00U; + writeQueueRF(data, P25_LDU_FRAME_LENGTH_BYTES + 2U); + } + + m_display->writeP25RSSI(m_rssi); + + return true; + } + } else if (duid == P25_DUID_LDU2) { + if (m_rfState == RS_RF_AUDIO) { + writeNetwork(m_rfLDU, m_lastDUID, false); + + // Regenerate Sync + CSync::addP25Sync(data + 2U); + + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_LDU2); + + // Add the dummy LDU2 data + m_rfData.encodeLDU2(data + 2U); + + // Regenerate the Low Speed Data + m_rfLSD.process(data + 2U); + + // Regenerate Audio + unsigned int errors = m_audio.process(data + 2U); + LogDebug("P25, LDU2 audio, errs: %u/1233 (%.1f%%)", errors, float(errors) / 12.33F); + + m_display->writeP25BER(float(errors) / 12.33F); + + m_rfBits += 1233U; + m_rfErrs += errors; + m_rfFrames++; + m_lastDUID = duid; + + // Add busy bits + addBusyBits(data + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + +#if defined(DUMP_P25) + writeFile(data + 2U, len - 2U); +#endif + + ::memcpy(m_rfLDU, data + 2U, P25_LDU_FRAME_LENGTH_BYTES); + + if (m_duplex) { + data[0U] = TAG_DATA; + data[1U] = 0x00U; + writeQueueRF(data, P25_LDU_FRAME_LENGTH_BYTES + 2U); + } + + m_display->writeP25RSSI(m_rssi); + + 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); + + ::memset(data + 2U, 0x00U, P25_TERM_FRAME_LENGTH_BYTES); + + // Regenerate Sync + CSync::addP25Sync(data + 2U); + + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_TERM); + + // Add busy bits + addBusyBits(data + 2U, P25_TERM_FRAME_LENGTH_BITS, false, true); + + m_rfState = RS_RF_LISTENING; + m_rfTimeout.stop(); + m_rfData.reset(); + m_lastDUID = duid; + + if (m_rssi != 0U) + LogMessage("P25, received RF end of voice transmission, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount); + else + LogMessage("P25, received RF end of voice transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 5.56F, float(m_rfErrs * 100U) / float(m_rfBits)); + + m_display->clearP25(); + +#if defined(DUMP_P25) + closeFile(); +#endif + + writeNetwork(data + 2U, P25_DUID_TERM, true); + + if (m_duplex) { + data[0U] = TAG_EOT; + data[1U] = 0x00U; + writeQueueRF(data, P25_TERM_FRAME_LENGTH_BYTES + 2U); + } + } + } else if (duid == P25_DUID_PDU) { + if (m_rfState != RS_RF_DATA) { + m_rfPDUCount = 0U; + m_rfPDUBits = 0U; + m_rfState = RS_RF_DATA; + m_rfDataFrames = 0U; + } + + unsigned int start = m_rfPDUCount * P25_LDU_FRAME_LENGTH_BITS; + + unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES]; + unsigned int bits = CP25Utils::decode(data + 2U, buffer, start, start + P25_LDU_FRAME_LENGTH_BITS); + + for (unsigned int i = 0U; i < bits; i++, m_rfPDUBits++) { + bool b = READ_BIT(buffer, i); + WRITE_BIT(m_rfPDU, m_rfPDUBits, b); + } + + if (m_rfPDUCount == 0U) { + CP25Trellis trellis; + unsigned char header[P25_PDU_HEADER_LENGTH_BYTES]; + bool valid = trellis.decode12(m_rfPDU + P25_SYNC_LENGTH_BYTES + P25_NID_LENGTH_BYTES, header); + if (valid) + valid = CCRC::checkCCITT162(header, P25_PDU_HEADER_LENGTH_BYTES); + + if (valid) { + unsigned int llId = (header[3U] << 16) + (header[4U] << 8) + header[5U]; + unsigned int sap = header[1U] & 0x3FU; + m_rfDataFrames = header[6U] & 0x7FU; + + LogMessage("P25, received RF data transmission for Local Link Id %u, SAP %u, %u blocks", llId, sap, m_rfDataFrames); + } else { + m_rfPDUCount = 0U; + m_rfPDUBits = 0U; + m_rfState = RS_RF_LISTENING; + m_rfDataFrames = 0U; + } + } + + if (m_rfState == RS_RF_DATA) { + m_rfPDUCount++; + + unsigned int bitLength = ((m_rfDataFrames + 1U) * P25_PDU_FEC_LENGTH_BITS) + P25_SYNC_LENGTH_BITS + P25_NID_LENGTH_BITS; + + if (m_rfPDUBits >= bitLength) { + unsigned int offset = P25_SYNC_LENGTH_BYTES + P25_NID_LENGTH_BYTES; + + // Regenerate the PDU header + CP25Trellis trellis; + unsigned char header[P25_PDU_HEADER_LENGTH_BYTES]; + trellis.decode12(m_rfPDU + offset, header); + trellis.encode12(header, m_rfPDU + offset); + offset += P25_PDU_FEC_LENGTH_BITS; + + // Regenerate the PDU data + for (unsigned int i = 0U; i < m_rfDataFrames; i++) { + unsigned char data[P25_PDU_CONFIRMED_LENGTH_BYTES]; + + bool valid = trellis.decode34(m_rfPDU + offset, data); + if (valid) { + trellis.encode34(data, m_rfPDU + offset); + } else { + valid = trellis.decode12(m_rfPDU + offset, data); + if (valid) + trellis.encode12(data, m_rfPDU + offset); + } + + offset += P25_PDU_FEC_LENGTH_BITS; + } + + unsigned char pdu[1024U]; + + // Add the data + unsigned int newBitLength = CP25Utils::encode(m_rfPDU, pdu + 2U, bitLength); + unsigned int newByteLength = newBitLength / 8U; + if ((newBitLength % 8U) > 0U) + newByteLength++; + + // Regenerate Sync + CSync::addP25Sync(pdu + 2U); + + // Regenerate NID + m_nid.encode(pdu + 2U, P25_DUID_PDU); + + // Add busy bits + addBusyBits(pdu + 2U, newBitLength, false, true); + + if (m_duplex) { + pdu[0U] = TAG_DATA; + pdu[1U] = 0x00U; + writeQueueRF(pdu, newByteLength + 2U); + } + + LogMessage("P25, ended RF data transmission"); + m_display->clearP25(); + + m_rfPDUCount = 0U; + m_rfPDUBits = 0U; + m_rfState = RS_RF_LISTENING; + m_rfDataFrames = 0U; + } + + return true; + } + } + + return false; +} + +unsigned int CP25Control::readModem(unsigned char* data) +{ + assert(data != NULL); + + if (m_queue.isEmpty()) + return 0U; + + unsigned char len = 0U; + m_queue.getData(&len, 1U); + + m_queue.getData(data, len); + + return len; +} + +void CP25Control::writeNetwork() +{ + unsigned char data[100U]; + unsigned int length = m_network->read(data, 100U); + if (length == 0U) + return; + + if (m_rfState != RS_RF_LISTENING && m_netState == RS_NET_IDLE) + return; + + m_networkWatchdog.start(); + + switch (data[0U]) { + case 0x62U: + ::memcpy(m_netLDU1 + 0U, data, 22U); + checkNetLDU2(); + break; + case 0x63U: + ::memcpy(m_netLDU1 + 25U, data, 14U); + checkNetLDU2(); + break; + case 0x64U: + ::memcpy(m_netLDU1 + 50U, data, 17U); + checkNetLDU2(); + break; + case 0x65U: + ::memcpy(m_netLDU1 + 75U, data, 17U); + checkNetLDU2(); + break; + case 0x66U: + ::memcpy(m_netLDU1 + 100U, data, 17U); + checkNetLDU2(); + break; + case 0x67U: + ::memcpy(m_netLDU1 + 125U, data, 17U); + checkNetLDU2(); + break; + case 0x68U: + ::memcpy(m_netLDU1 + 150U, data, 17U); + checkNetLDU2(); + break; + case 0x69U: + ::memcpy(m_netLDU1 + 175U, data, 17U); + checkNetLDU2(); + break; + case 0x6AU: + ::memcpy(m_netLDU1 + 200U, data, 16U); + checkNetLDU2(); + if (m_netState != RS_NET_IDLE) + createNetLDU1(); + break; + case 0x6BU: + ::memcpy(m_netLDU2 + 0U, data, 22U); + checkNetLDU1(); + break; + case 0x6CU: + ::memcpy(m_netLDU2 + 25U, data, 14U); + checkNetLDU1(); + break; + case 0x6DU: + ::memcpy(m_netLDU2 + 50U, data, 17U); + checkNetLDU1(); + break; + case 0x6EU: + ::memcpy(m_netLDU2 + 75U, data, 17U); + checkNetLDU1(); + break; + case 0x6FU: + ::memcpy(m_netLDU2 + 100U, data, 17U); + checkNetLDU1(); + break; + case 0x70U: + ::memcpy(m_netLDU2 + 125U, data, 17U); + checkNetLDU1(); + break; + case 0x71U: + ::memcpy(m_netLDU2 + 150U, data, 17U); + checkNetLDU1(); + break; + case 0x72U: + ::memcpy(m_netLDU2 + 175U, data, 17U); + checkNetLDU1(); + break; + case 0x73U: + ::memcpy(m_netLDU2 + 200U, data, 16U); + if (m_netState == RS_NET_IDLE) { + createNetHeader(); + createNetLDU1(); + } else { + checkNetLDU1(); + } + createNetLDU2(); + break; + case 0x80U: + createNetTerminator(); + break; + default: + break; + } +} + +void CP25Control::clock(unsigned int ms) +{ + if (m_network != NULL) + writeNetwork(); + + m_rfTimeout.clock(ms); + m_netTimeout.clock(ms); + + if (m_netState == RS_NET_AUDIO) { + m_networkWatchdog.clock(ms); + + if (m_networkWatchdog.hasExpired()) { + LogMessage("P25, network watchdog has expired, %.1f seconds, %u%% packet loss", float(m_netFrames) / 50.0F, (m_netLost * 100U) / m_netFrames); + m_display->clearP25(); + m_networkWatchdog.stop(); + m_netState = RS_NET_IDLE; + m_netData.reset(); + m_netTimeout.stop(); + } + } +} + +void CP25Control::writeQueueRF(const unsigned char* data, unsigned int length) +{ + assert(data != NULL); + + if (m_rfTimeout.isRunning() && m_rfTimeout.hasExpired()) + return; + + unsigned int space = m_queue.freeSpace(); + if (space < (length + 1U)) { + LogError("P25, overflow in the P25 RF queue"); + return; + } + + unsigned char len = length; + m_queue.addData(&len, 1U); + + m_queue.addData(data, len); +} + +void CP25Control::writeQueueNet(const unsigned char* data, unsigned int length) +{ + assert(data != NULL); + + if (m_netTimeout.isRunning() && m_netTimeout.hasExpired()) + return; + + unsigned int space = m_queue.freeSpace(); + if (space < (length + 1U)) { + LogError("P25, overflow in the P25 RF queue"); + return; + } + + unsigned char len = length; + m_queue.addData(&len, 1U); + + m_queue.addData(data, len); +} + +void CP25Control::writeNetwork(const unsigned char *data, unsigned char type, bool end) +{ + assert(data != NULL); + + if (m_network == NULL) + return; + + if (m_rfTimeout.isRunning() && m_rfTimeout.hasExpired()) + return; + + switch (type) + { + case P25_DUID_LDU1: + m_network->writeLDU1(data, m_rfData, m_rfLSD, end); + break; + case P25_DUID_LDU2: + m_network->writeLDU2(data, m_rfData, m_rfLSD, end); + break; + default: + break; + } +} + +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); + + for (unsigned int ss0Pos = P25_SS0_START; ss0Pos < length; ss0Pos += P25_SS_INCREMENT) { + unsigned int ss1Pos = ss0Pos + 1U; + WRITE_BIT(data, ss0Pos, b1); + WRITE_BIT(data, ss1Pos, b2); + } +} + +void CP25Control::checkNetLDU1() +{ + if (m_netState == RS_NET_IDLE) + return; + + // Check for an unflushed LDU1 + if (m_netLDU1[0U] != 0x00U || m_netLDU1[25U] != 0x00U || m_netLDU1[50U] != 0x00U || + m_netLDU1[75U] != 0x00U || m_netLDU1[100U] != 0x00U || m_netLDU1[125U] != 0x00U || + m_netLDU1[150U] != 0x00U || m_netLDU1[175U] != 0x00U || m_netLDU1[200U] != 0x00U) + createNetLDU1(); +} + +void CP25Control::checkNetLDU2() +{ + if (m_netState == RS_NET_IDLE) + return; + + // Check for an unflushed LDU1 + if (m_netLDU2[0U] != 0x00U || m_netLDU2[25U] != 0x00U || m_netLDU2[50U] != 0x00U || + m_netLDU2[75U] != 0x00U || m_netLDU2[100U] != 0x00U || m_netLDU2[125U] != 0x00U || + m_netLDU2[150U] != 0x00U || m_netLDU2[175U] != 0x00U || m_netLDU2[200U] != 0x00U) + createNetLDU2(); +} + +void CP25Control::insertMissingAudio(unsigned char* data) +{ + if (data[0U] == 0x00U) { + ::memcpy(data + 10U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 10U, 11U); + } + + if (data[25U] == 0x00U) { + ::memcpy(data + 26U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 26U, 11U); + } + + if (data[50U] == 0x00U) { + ::memcpy(data + 55U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 55U, 11U); + } + + if (data[75U] == 0x00U) { + ::memcpy(data + 80U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 80U, 11U); + } + + if (data[100U] == 0x00U) { + ::memcpy(data + 105U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 105U, 11U); + } + + if (data[125U] == 0x00U) { + ::memcpy(data + 130U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 130U, 11U); + } + + if (data[150U] == 0x00U) { + ::memcpy(data + 155U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 155U, 11U); + } + + if (data[175U] == 0x00U) { + ::memcpy(data + 180U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 180U, 11U); + } + + if (data[200U] == 0x00U) { + ::memcpy(data + 204U, m_lastIMBE, 11U); + m_netLost++; + } else { + ::memcpy(m_lastIMBE, data + 204U, 11U); + } +} + +void CP25Control::createRFHeader() +{ + unsigned char buffer[P25_HDR_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_HDR_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = TAG_HEADER; + buffer[1U] = 0x00U; + + // Add the sync + CSync::addP25Sync(buffer + 2U); + + // Add the NID + m_nid.encode(buffer + 2U, P25_DUID_HEADER); + + // Add the dummy header + m_rfData.encodeHeader(buffer + 2U); + + // Add busy bits + addBusyBits(buffer + 2U, P25_HDR_FRAME_LENGTH_BITS, false, true); + + m_rfFrames = 0U; + m_rfErrs = 0U; + m_rfBits = 1U; + m_rfTimeout.start(); + m_lastDUID = P25_DUID_HEADER; + ::memset(m_rfLDU, 0x00U, P25_LDU_FRAME_LENGTH_BYTES); + +#if defined(DUMP_P25) + openFile(); + writeFile(buffer + 2U, buffer - 2U); +#endif + + if (m_duplex) { + buffer[0U] = TAG_HEADER; + buffer[1U] = 0x00U; + writeQueueRF(buffer, P25_HDR_FRAME_LENGTH_BYTES + 2U); + } +} + +void CP25Control::createNetHeader() +{ + 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]; + + unsigned char algId = m_netLDU2[126U]; + unsigned int kId = (m_netLDU2[127U] << 8) + m_netLDU2[128U]; + + 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.reset(); + 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(); + m_netFrames = 0U; + m_netLost = 0U; + + unsigned char buffer[P25_HDR_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_HDR_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = TAG_HEADER; + buffer[1U] = 0x00U; + + // Add the sync + CSync::addP25Sync(buffer + 2U); + + // Add the NID + m_nid.encode(buffer + 2U, P25_DUID_HEADER); + + // Add the dummy header + m_netData.encodeHeader(buffer + 2U); + + // Add busy bits + if (m_remoteGateway) + addBusyBits(buffer + 2U, P25_HDR_FRAME_LENGTH_BITS, false, false); + else + addBusyBits(buffer + 2U, P25_HDR_FRAME_LENGTH_BITS, false, true); + + writeQueueNet(buffer, P25_HDR_FRAME_LENGTH_BYTES + 2U); +} + +void CP25Control::createNetLDU1() +{ + insertMissingAudio(m_netLDU1); + + unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = TAG_DATA; + buffer[1U] = 0x00U; + + // Add the sync + CSync::addP25Sync(buffer + 2U); + + // Add the NID + m_nid.encode(buffer + 2U, P25_DUID_LDU1); + + // Add the LDU1 data + m_netData.encodeLDU1(buffer + 2U); + + // Add the Audio + m_audio.encode(buffer + 2U, m_netLDU1 + 10U, 0U); + m_audio.encode(buffer + 2U, m_netLDU1 + 26U, 1U); + m_audio.encode(buffer + 2U, m_netLDU1 + 55U, 2U); + m_audio.encode(buffer + 2U, m_netLDU1 + 80U, 3U); + m_audio.encode(buffer + 2U, m_netLDU1 + 105U, 4U); + m_audio.encode(buffer + 2U, m_netLDU1 + 130U, 5U); + m_audio.encode(buffer + 2U, m_netLDU1 + 155U, 6U); + m_audio.encode(buffer + 2U, m_netLDU1 + 180U, 7U); + m_audio.encode(buffer + 2U, m_netLDU1 + 204U, 8U); + + // Add the Low Speed Data + m_netLSD.setLSD1(m_netLDU1[201U]); + m_netLSD.setLSD2(m_netLDU1[202U]); + m_netLSD.encode(buffer + 2U); + + // Add busy bits + if (m_remoteGateway) + addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, false); + else + addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + + writeQueueNet(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + ::memset(m_netLDU1, 0x00U, 9U * 25U); + + m_netFrames += 9U; +} + +void CP25Control::createNetLDU2() +{ + insertMissingAudio(m_netLDU2); + + unsigned char buffer[P25_LDU_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = TAG_DATA; + buffer[1U] = 0x00U; + + // Add the sync + CSync::addP25Sync(buffer + 2U); + + // Add the NID + m_nid.encode(buffer + 2U, P25_DUID_LDU2); + + // Add the dummy LDU2 data + m_netData.encodeLDU2(buffer + 2U); + + // Add the Audio + m_audio.encode(buffer + 2U, m_netLDU2 + 10U, 0U); + m_audio.encode(buffer + 2U, m_netLDU2 + 26U, 1U); + m_audio.encode(buffer + 2U, m_netLDU2 + 55U, 2U); + m_audio.encode(buffer + 2U, m_netLDU2 + 80U, 3U); + m_audio.encode(buffer + 2U, m_netLDU2 + 105U, 4U); + m_audio.encode(buffer + 2U, m_netLDU2 + 130U, 5U); + m_audio.encode(buffer + 2U, m_netLDU2 + 155U, 6U); + m_audio.encode(buffer + 2U, m_netLDU2 + 180U, 7U); + m_audio.encode(buffer + 2U, m_netLDU2 + 204U, 8U); + + // Add the Low Speed Data + m_netLSD.setLSD1(m_netLDU2[201U]); + m_netLSD.setLSD2(m_netLDU2[202U]); + m_netLSD.encode(buffer + 2U); + + // Add busy bits + if (m_remoteGateway) + addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, false); + else + addBusyBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, false, true); + + writeQueueNet(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + ::memset(m_netLDU2, 0x00U, 9U * 25U); + + m_netFrames += 9U; +} + +void CP25Control::createNetTerminator() +{ + unsigned char buffer[P25_TERM_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_TERM_FRAME_LENGTH_BYTES + 2U); + + buffer[0U] = TAG_EOT; + buffer[1U] = 0x00U; + + // Add the sync + CSync::addP25Sync(buffer + 2U); + + // Add the NID + m_nid.encode(buffer + 2U, P25_DUID_TERM); + + // Add busy bits + if (m_remoteGateway) + addBusyBits(buffer + 2U, P25_TERM_FRAME_LENGTH_BITS, false, false); + else + addBusyBits(buffer + 2U, P25_TERM_FRAME_LENGTH_BITS, false, true); + + writeQueueNet(buffer, P25_TERM_FRAME_LENGTH_BYTES + 2U); + + LogMessage("P25, network end of transmission, %.1f seconds, %u%% packet loss", float(m_netFrames) / 50.0F, (m_netLost * 100U) / m_netFrames); + + m_display->clearP25(); + m_netTimeout.stop(); + m_networkWatchdog.stop(); + m_netData.reset(); + m_netState = RS_NET_IDLE; +} + +bool CP25Control::openFile() +{ + if (m_fp != NULL) + return true; + + time_t t; + ::time(&t); + + struct tm* tm = ::localtime(&t); + + char name[100U]; + ::sprintf(name, "P25_%04d%02d%02d_%02d%02d%02d.ambe", tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec); + + m_fp = ::fopen(name, "wb"); + if (m_fp == NULL) + return false; + + ::fwrite("P25", 1U, 3U, m_fp); + + return true; +} + +bool CP25Control::writeFile(const unsigned char* data, unsigned char length) +{ + if (m_fp == NULL) + return false; + + ::fwrite(&length, 1U, 1U, m_fp); + + ::fwrite(data, 1U, length, m_fp); + + return true; +} + +void CP25Control::closeFile() +{ + if (m_fp != NULL) { + ::fclose(m_fp); + m_fp = NULL; + } +} diff --git a/P25Control.h b/P25Control.h index 934bdb5..fa069d3 100644 --- a/P25Control.h +++ b/P25Control.h @@ -1,118 +1,118 @@ -/* -* 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 -* the Free Software Foundation; either version 2 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software -* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. -*/ - -#if !defined(P25Control_H) -#define P25Control_H - -#include "RSSIInterpolator.h" -#include "P25LowSpeedData.h" -#include "RingBuffer.h" -#include "P25Network.h" -#include "DMRLookup.h" -#include "P25Audio.h" -#include "Defines.h" -#include "Display.h" -#include "P25Data.h" -#include "P25NID.h" -#include "Modem.h" -#include "Timer.h" - -#include - -class CP25Control { -public: - CP25Control(unsigned int nac, unsigned int id, bool selfOly, bool uidOverride, CP25Network* network, CDisplay* display, unsigned int timeout, bool duplex, CDMRLookup* lookup, bool remoteGateway, CRSSIInterpolator* rssiMapper); - ~CP25Control(); - - bool writeModem(unsigned char* data, unsigned int len); - - unsigned int readModem(unsigned char* data); - - void clock(unsigned int ms); - -private: - unsigned int m_nac; - unsigned int m_id; - bool m_selfOnly; - bool m_uidOverride; - bool m_remoteGateway; - CP25Network* m_network; - CDisplay* m_display; - bool m_duplex; - CDMRLookup* m_lookup; - CRingBuffer m_queue; - RPT_RF_STATE m_rfState; - RPT_NET_STATE m_netState; - CTimer m_rfTimeout; - CTimer m_netTimeout; - CTimer m_networkWatchdog; - unsigned int m_rfFrames; - unsigned int m_rfBits; - unsigned int m_rfErrs; - unsigned int m_netFrames; - unsigned int m_netLost; - unsigned int m_rfDataFrames; - CP25NID m_nid; - unsigned char m_lastDUID; - CP25Audio m_audio; - CP25Data m_rfData; - CP25Data m_netData; - CP25LowSpeedData m_rfLSD; - CP25LowSpeedData m_netLSD; - unsigned char* m_netLDU1; - unsigned char* m_netLDU2; - unsigned char* m_lastIMBE; - unsigned char* m_rfLDU; - unsigned char* m_rfPDU; - unsigned int m_rfPDUCount; - unsigned int m_rfPDUBits; - CRSSIInterpolator* m_rssiMapper; - unsigned char m_rssi; - unsigned char m_maxRSSI; - unsigned char m_minRSSI; - unsigned int m_aveRSSI; - unsigned int m_rssiCount; - FILE* m_fp; - - void writeQueueRF(const unsigned char* data, unsigned int length); - void writeQueueNet(const unsigned char* data, unsigned int length); - 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(); - void checkNetLDU2(); - - void insertMissingAudio(unsigned char* data); - - void createRFHeader(); - - void createNetHeader(); - void createNetLDU1(); - void createNetLDU2(); - void createNetTerminator(); - - bool openFile(); - bool writeFile(const unsigned char* data, unsigned char length); - void closeFile(); -}; - -#endif +/* +* 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 +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#if !defined(P25Control_H) +#define P25Control_H + +#include "RSSIInterpolator.h" +#include "P25LowSpeedData.h" +#include "RingBuffer.h" +#include "P25Network.h" +#include "DMRLookup.h" +#include "P25Audio.h" +#include "Defines.h" +#include "Display.h" +#include "P25Data.h" +#include "P25NID.h" +#include "Modem.h" +#include "Timer.h" + +#include + +class CP25Control { +public: + CP25Control(unsigned int nac, unsigned int id, bool selfOly, bool uidOverride, CP25Network* network, CDisplay* display, unsigned int timeout, bool duplex, CDMRLookup* lookup, bool remoteGateway, CRSSIInterpolator* rssiMapper); + ~CP25Control(); + + bool writeModem(unsigned char* data, unsigned int len); + + unsigned int readModem(unsigned char* data); + + void clock(unsigned int ms); + +private: + unsigned int m_nac; + unsigned int m_id; + bool m_selfOnly; + bool m_uidOverride; + bool m_remoteGateway; + CP25Network* m_network; + CDisplay* m_display; + bool m_duplex; + CDMRLookup* m_lookup; + CRingBuffer m_queue; + RPT_RF_STATE m_rfState; + RPT_NET_STATE m_netState; + CTimer m_rfTimeout; + CTimer m_netTimeout; + CTimer m_networkWatchdog; + unsigned int m_rfFrames; + unsigned int m_rfBits; + unsigned int m_rfErrs; + unsigned int m_netFrames; + unsigned int m_netLost; + unsigned int m_rfDataFrames; + CP25NID m_nid; + unsigned char m_lastDUID; + CP25Audio m_audio; + CP25Data m_rfData; + CP25Data m_netData; + CP25LowSpeedData m_rfLSD; + CP25LowSpeedData m_netLSD; + unsigned char* m_netLDU1; + unsigned char* m_netLDU2; + unsigned char* m_lastIMBE; + unsigned char* m_rfLDU; + unsigned char* m_rfPDU; + unsigned int m_rfPDUCount; + unsigned int m_rfPDUBits; + CRSSIInterpolator* m_rssiMapper; + unsigned char m_rssi; + unsigned char m_maxRSSI; + unsigned char m_minRSSI; + unsigned int m_aveRSSI; + unsigned int m_rssiCount; + FILE* m_fp; + + void writeQueueRF(const unsigned char* data, unsigned int length); + void writeQueueNet(const unsigned char* data, unsigned int length); + 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(); + void checkNetLDU2(); + + void insertMissingAudio(unsigned char* data); + + void createRFHeader(); + + void createNetHeader(); + void createNetLDU1(); + void createNetLDU2(); + void createNetTerminator(); + + bool openFile(); + bool writeFile(const unsigned char* data, unsigned char length); + void closeFile(); +}; + +#endif diff --git a/P25Data.cpp b/P25Data.cpp index 8987736..618fdc2 100644 --- a/P25Data.cpp +++ b/P25Data.cpp @@ -1,460 +1,469 @@ -/* -* 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 -* the Free Software Foundation; either version 2 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software -* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. -*/ - -#include "P25Data.h" -#include "P25Defines.h" -#include "P25Utils.h" -#include "CRC.h" -#include "Hamming.h" -#include "Utils.h" -#include "Log.h" - -#include -#include -#include - -const unsigned char DUMMY_HEADER[] = { - 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, - 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x08U, 0xDCU, 0x60U, - 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x02U, 0x93U, 0xE7U, 0x73U, 0x77U, 0x57U, 0xD6U, 0xD3U, 0xCFU, 0x77U, - 0xEEU, 0x82U, 0x93U, 0xE2U, 0x2FU, 0xF3U, 0xD5U, 0xF5U, 0xBEU, 0xBCU, 0x54U, 0x0DU, 0x9CU, 0x29U, 0x3EU, 0x46U, - 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_mi(NULL), -m_mfId(0U), -m_algId(0x80U), -m_kId(0U), -m_lcf(0x00U), -m_emergency(false), -m_srcId(0U), -m_dstId(0U), -m_serviceType(0U), -m_rs241213(), -m_trellis() -{ - m_mi = new unsigned char[P25_MI_LENGTH_BYTES]; -} - -CP25Data::~CP25Data() -{ - delete[] m_mi; -} - -void CP25Data::encodeHeader(unsigned char* data) -{ - assert(data != NULL); - - CP25Utils::encode(DUMMY_HEADER, data, 114U, 780U); -} - -bool CP25Data::decodeLDU1(const unsigned char* data) -{ - assert(data != NULL); - - unsigned char rs[18U]; - - unsigned char raw[5U]; - CP25Utils::decode(data, raw, 410U, 452U); - decodeLDUHamming(raw, rs + 0U); - - CP25Utils::decode(data, raw, 600U, 640U); - decodeLDUHamming(raw, rs + 3U); - - CP25Utils::decode(data, raw, 788U, 830U); - decodeLDUHamming(raw, rs + 6U); - - CP25Utils::decode(data, raw, 978U, 1020U); - decodeLDUHamming(raw, rs + 9U); - - CP25Utils::decode(data, raw, 1168U, 1208U); - decodeLDUHamming(raw, rs + 12U); - - CP25Utils::decode(data, raw, 1356U, 1398U); - decodeLDUHamming(raw, rs + 15U); - - try { - bool ret = m_rs241213.decode(rs); - if (!ret) - return false; - } catch (...) { - CUtils::dump(2U, "P25, RS carshed with input data", rs, 18U); - return false; - } - - unsigned int srcId = (rs[6U] << 16) + (rs[7U] << 8) + rs[8U]; - - switch (rs[0U]) { - case P25_LCF_GROUP: - m_emergency = (rs[2U] & 0x80U) == 0x80U; - m_dstId = (rs[4U] << 8) + rs[5U]; - m_srcId = srcId; - break; - case P25_LCF_PRIVATE: - m_emergency = false; - m_dstId = (rs[3U] << 16) + (rs[4U] << 8) + rs[5U]; - m_srcId = srcId; - break; - default: - return false; - } - - m_lcf = rs[0U]; - m_mfId = rs[1U]; - - return true; -} - -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); - - 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::encodeLDU2(unsigned char* data) -{ - assert(data != NULL); - - unsigned char raw[5U]; - encodeLDUHamming(raw, DUMMY_LDU2 + 0U); - CP25Utils::encode(raw, data, 410U, 452U); - - encodeLDUHamming(raw, DUMMY_LDU2 + 3U); - CP25Utils::encode(raw, data, 600U, 640U); - - encodeLDUHamming(raw, DUMMY_LDU2 + 6U); - CP25Utils::encode(raw, data, 788U, 830U); - - encodeLDUHamming(raw, DUMMY_LDU2 + 9U); - CP25Utils::encode(raw, data, 978U, 1020U); - - encodeLDUHamming(raw, DUMMY_LDU2 + 12U); - CP25Utils::encode(raw, data, 1168U, 1208U); - - encodeLDUHamming(raw, DUMMY_LDU2 + 15U); - 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); - - ::memcpy(m_mi, mi, P25_MI_LENGTH_BYTES); -} - -void CP25Data::getMI(unsigned char* mi) const -{ - assert(mi != NULL); - - ::memcpy(mi, m_mi, P25_MI_LENGTH_BYTES); -} - -void CP25Data::setMFId(unsigned char id) -{ - m_mfId = id; -} - -unsigned char CP25Data::getMFId() const -{ - return m_mfId; -} - -void CP25Data::setAlgId(unsigned char id) -{ - m_algId = id; -} - -unsigned char CP25Data::getAlgId() const -{ - 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() -{ - ::memset(m_mi, 0x00U, P25_MI_LENGTH_BYTES); - - m_algId = 0x80U; - m_kId = 0x0000U; - m_lcf = P25_LCF_GROUP; - m_mfId = 0x00U; - m_srcId = 0U; - m_dstId = 0U; - m_emergency = false; -} - -void CP25Data::decodeLDUHamming(const unsigned char* data, unsigned char* raw) -{ - unsigned int n = 0U; - unsigned int m = 0U; - for (unsigned int i = 0U; i < 4U; i++) { - bool hamming[10U]; - - for (unsigned int j = 0U; j < 10U; j++) { - hamming[j] = READ_BIT(data, n); - n++; - } - - CHamming::decode1063(hamming); - - for (unsigned int j = 0U; j < 6U; j++) { - WRITE_BIT(raw, m, hamming[j]); - m++; - } - } -} - -void CP25Data::encodeLDUHamming(unsigned char* data, const unsigned char* raw) -{ - unsigned int n = 0U; - unsigned int m = 0U; - for (unsigned int i = 0U; i < 4U; i++) { - bool hamming[10U]; - - for (unsigned int j = 0U; j < 6U; j++) { - hamming[j] = READ_BIT(raw, m); - m++; - } - - CHamming::encode1063(hamming); - - for (unsigned int j = 0U; j < 10U; j++) { - WRITE_BIT(data, n, hamming[j]); - n++; - } - } -} +/* +* 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 +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "P25Data.h" +#include "P25Defines.h" +#include "P25Utils.h" +#include "CRC.h" +#include "Hamming.h" +#include "Utils.h" +#include "Log.h" + +#include +#include +#include + +const unsigned char DUMMY_HEADER[] = { + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x08U, 0xDCU, 0x60U, + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x02U, 0x93U, 0xE7U, 0x73U, 0x77U, 0x57U, 0xD6U, 0xD3U, 0xCFU, 0x77U, + 0xEEU, 0x82U, 0x93U, 0xE2U, 0x2FU, 0xF3U, 0xD5U, 0xF5U, 0xBEU, 0xBCU, 0x54U, 0x0DU, 0x9CU, 0x29U, 0x3EU, 0x46U, + 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_mi(NULL), +m_mfId(0U), +m_algId(0x80U), +m_kId(0U), +m_lcf(0x00U), +m_emergency(false), +m_srcId(0U), +m_dstId(0U), +m_rs241213(), +m_trellis() +{ + m_mi = new unsigned char[P25_MI_LENGTH_BYTES]; +} + +CP25Data::~CP25Data() +{ + delete[] m_mi; +} + +void CP25Data::encodeHeader(unsigned char* data) +{ + assert(data != NULL); + + CP25Utils::encode(DUMMY_HEADER, data, 114U, 780U); +} + +bool CP25Data::decodeLDU1(const unsigned char* data) +{ + assert(data != NULL); + + unsigned char rs[18U]; + + unsigned char raw[5U]; + CP25Utils::decode(data, raw, 410U, 452U); + decodeLDUHamming(raw, rs + 0U); + + CP25Utils::decode(data, raw, 600U, 640U); + decodeLDUHamming(raw, rs + 3U); + + CP25Utils::decode(data, raw, 788U, 830U); + decodeLDUHamming(raw, rs + 6U); + + CP25Utils::decode(data, raw, 978U, 1020U); + decodeLDUHamming(raw, rs + 9U); + + CP25Utils::decode(data, raw, 1168U, 1208U); + decodeLDUHamming(raw, rs + 12U); + + CP25Utils::decode(data, raw, 1356U, 1398U); + decodeLDUHamming(raw, rs + 15U); + + try { + bool ret = m_rs241213.decode(rs); + if (!ret) + return false; + } catch (...) { + CUtils::dump(2U, "P25, RS carshed with input data", rs, 18U); + return false; + } + + unsigned int srcId = (rs[6U] << 16) + (rs[7U] << 8) + rs[8U]; + + switch (rs[0U]) { + case P25_LCF_GROUP: + m_emergency = (rs[2U] & 0x80U) == 0x80U; + m_dstId = (rs[4U] << 8) + rs[5U]; + m_srcId = srcId; + break; + case P25_LCF_PRIVATE: + m_emergency = false; + m_dstId = (rs[3U] << 16) + (rs[4U] << 8) + rs[5U]; + m_srcId = srcId; + break; + default: + return false; + } + + m_lcf = rs[0U]; + m_mfId = rs[1U]; + + return true; +} + +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); + + 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::encodeLDU2(unsigned char* data) +{ + assert(data != NULL); + + unsigned char raw[5U]; + encodeLDUHamming(raw, DUMMY_LDU2 + 0U); + CP25Utils::encode(raw, data, 410U, 452U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 3U); + CP25Utils::encode(raw, data, 600U, 640U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 6U); + CP25Utils::encode(raw, data, 788U, 830U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 9U); + CP25Utils::encode(raw, data, 978U, 1020U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 12U); + CP25Utils::encode(raw, data, 1168U, 1208U); + + encodeLDUHamming(raw, DUMMY_LDU2 + 15U); + 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); + + ::memcpy(m_mi, mi, P25_MI_LENGTH_BYTES); +} + +void CP25Data::getMI(unsigned char* mi) const +{ + assert(mi != NULL); + + ::memcpy(mi, m_mi, P25_MI_LENGTH_BYTES); +} + +void CP25Data::setMFId(unsigned char id) +{ + m_mfId = id; +} + +unsigned char CP25Data::getMFId() const +{ + return m_mfId; +} + +void CP25Data::setAlgId(unsigned char id) +{ + m_algId = id; +} + +unsigned char CP25Data::getAlgId() const +{ + 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::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); + + m_algId = 0x80U; + m_kId = 0x0000U; + m_lcf = P25_LCF_GROUP; + m_mfId = 0x00U; + m_srcId = 0U; + m_dstId = 0U; + m_emergency = false; +} + +void CP25Data::decodeLDUHamming(const unsigned char* data, unsigned char* raw) +{ + unsigned int n = 0U; + unsigned int m = 0U; + for (unsigned int i = 0U; i < 4U; i++) { + bool hamming[10U]; + + for (unsigned int j = 0U; j < 10U; j++) { + hamming[j] = READ_BIT(data, n); + n++; + } + + CHamming::decode1063(hamming); + + for (unsigned int j = 0U; j < 6U; j++) { + WRITE_BIT(raw, m, hamming[j]); + m++; + } + } +} + +void CP25Data::encodeLDUHamming(unsigned char* data, const unsigned char* raw) +{ + unsigned int n = 0U; + unsigned int m = 0U; + for (unsigned int i = 0U; i < 4U; i++) { + bool hamming[10U]; + + for (unsigned int j = 0U; j < 6U; j++) { + hamming[j] = READ_BIT(raw, m); + m++; + } + + CHamming::encode1063(hamming); + + for (unsigned int j = 0U; j < 10U; j++) { + WRITE_BIT(data, n, hamming[j]); + n++; + } + } +} diff --git a/P25Data.h b/P25Data.h index 40ade19..b2727ca 100644 --- a/P25Data.h +++ b/P25Data.h @@ -1,87 +1,87 @@ -/* -* 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 -* the Free Software Foundation; either version 2 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software -* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. -*/ - -#if !defined(P25Data_H) -#define P25Data_H - -#include "RS241213.h" -#include "P25Trellis.h" - -class CP25Data { -public: - CP25Data(); - ~CP25Data(); - - void encodeHeader(unsigned char* data); - - bool decodeLDU1(const unsigned char* data); - void encodeLDU1(unsigned char* data); - - 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; - - void setMFId(unsigned char id); - unsigned char getMFId() 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 setServiceType(unsigned char type); - unsigned char getServiceType() const; - - void reset(); - -private: - 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; - 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); -}; - -#endif +/* +* 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 +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#if !defined(P25Data_H) +#define P25Data_H + +#include "RS241213.h" +#include "P25Trellis.h" + +class CP25Data { +public: + CP25Data(); + ~CP25Data(); + + void encodeHeader(unsigned char* data); + + bool decodeLDU1(const unsigned char* data); + void encodeLDU1(unsigned char* data); + + 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; + + void setMFId(unsigned char id); + unsigned char getMFId() 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 setServiceType(unsigned char type); + unsigned char getServiceType() const; + + void reset(); + +private: + 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; + 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); +}; + +#endif diff --git a/P25Defines.h b/P25Defines.h index 5a61c27..6ceda75 100644 --- a/P25Defines.h +++ b/P25Defines.h @@ -1,78 +1,78 @@ -/* - * 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 - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -#if !defined(P25DEFINES_H) -#define P25DEFINES_H - -const unsigned int P25_HDR_FRAME_LENGTH_BYTES = 99U; -const unsigned int P25_HDR_FRAME_LENGTH_BITS = P25_HDR_FRAME_LENGTH_BYTES * 8U; - -const unsigned int P25_LDU_FRAME_LENGTH_BYTES = 216U; -const unsigned int P25_LDU_FRAME_LENGTH_BITS = P25_LDU_FRAME_LENGTH_BYTES * 8U; - -const unsigned int P25_TERM_FRAME_LENGTH_BYTES = 18U; -const unsigned int P25_TERM_FRAME_LENGTH_BITS = P25_TERM_FRAME_LENGTH_BYTES * 8U; - -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; - -const unsigned int P25_NID_LENGTH_BYTES = 8U; -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_MAX_PDU_COUNT = 10U; - -const unsigned int P25_PDU_HEADER_LENGTH_BYTES = 12U; -const unsigned int P25_PDU_CONFIRMED_LENGTH_BYTES = 18U; -const unsigned int P25_PDU_UNCONFIRMED_LENGTH_BYTES = 12U; - -const unsigned int P25_PDU_FEC_LENGTH_BYTES = 24U; -const unsigned int P25_PDU_FEC_LENGTH_BITS = P25_PDU_FEC_LENGTH_BYTES * 8U; - -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; - -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; - -const unsigned char P25_NULL_IMBE[] = {0x04U, 0x0CU, 0xFDU, 0x7BU, 0xFBU, 0x7DU, 0xF2U, 0x7BU, 0x3DU, 0x9EU, 0x45U}; - -#endif +/* + * 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 + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(P25DEFINES_H) +#define P25DEFINES_H + +const unsigned int P25_HDR_FRAME_LENGTH_BYTES = 99U; +const unsigned int P25_HDR_FRAME_LENGTH_BITS = P25_HDR_FRAME_LENGTH_BYTES * 8U; + +const unsigned int P25_LDU_FRAME_LENGTH_BYTES = 216U; +const unsigned int P25_LDU_FRAME_LENGTH_BITS = P25_LDU_FRAME_LENGTH_BYTES * 8U; + +const unsigned int P25_TERM_FRAME_LENGTH_BYTES = 18U; +const unsigned int P25_TERM_FRAME_LENGTH_BITS = P25_TERM_FRAME_LENGTH_BYTES * 8U; + +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; + +const unsigned int P25_NID_LENGTH_BYTES = 8U; +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_MAX_PDU_COUNT = 10U; + +const unsigned int P25_PDU_HEADER_LENGTH_BYTES = 12U; +const unsigned int P25_PDU_CONFIRMED_LENGTH_BYTES = 18U; +const unsigned int P25_PDU_UNCONFIRMED_LENGTH_BYTES = 12U; + +const unsigned int P25_PDU_FEC_LENGTH_BYTES = 24U; +const unsigned int P25_PDU_FEC_LENGTH_BITS = P25_PDU_FEC_LENGTH_BYTES * 8U; + +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; + +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; + +const unsigned char P25_NULL_IMBE[] = {0x04U, 0x0CU, 0xFDU, 0x7BU, 0xFBU, 0x7DU, 0xF2U, 0x7BU, 0x3DU, 0x9EU, 0x45U}; + +#endif From e903c1edea36e943d7a8febfc6d292d5b4ef098d Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Mon, 14 May 2018 10:45:49 -0400 Subject: [PATCH 3/5] Fix whitespace and tabbing. Fix missing closing brace. --- P25Control.cpp | 131 ++++++++++++++++++++++++------------------------- P25Data.h | 12 ++--- 2 files changed, 71 insertions(+), 72 deletions(-) diff --git a/P25Control.cpp b/P25Control.cpp index bb63230..007a681 100644 --- a/P25Control.cpp +++ b/P25Control.cpp @@ -350,86 +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) { + } 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; - } + 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); - unsigned int srcId = m_rfData.getSrcId(); - unsigned int dstId = m_rfData.getDstId(); + // Regenerate NID + m_nid.encode(data + 2U, P25_DUID_TSDU); - unsigned char data[P25_TSDU_FRAME_LENGTH_BYTES + 2U]; + // Regenerate TDULC Data + m_rfData.encodeTSDU(data + 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); + // 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 NID + m_nid.encode(data + 2U, P25_DUID_TSDU); - // Regenerate TDULC Data - m_rfData.encodeTSDU(data + 2U); + // Regenerate TDULC Data + m_rfData.encodeTSDU(data + 2U); - // Add busy bits - addBusyBits(data + 2U, P25_TSDU_FRAME_LENGTH_BITS, true, false); + // 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); + // 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; + 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); + writeQueueRF(data, P25_TSDU_FRAME_LENGTH_BYTES + 2U); + } + break; + default: + LogMessage("P25, recieved RF TSDU transmission, unhandled LCF $%02X", m_rfData.getLCF()); + break; + } - // 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; - } + 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); diff --git a/P25Data.h b/P25Data.h index b2727ca..1b9eec9 100644 --- a/P25Data.h +++ b/P25Data.h @@ -35,8 +35,8 @@ public: void encodeLDU2(unsigned char* data); - bool decodeTSDU(const unsigned char* data); - void encodeTSDU(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; @@ -62,8 +62,8 @@ public: void setDstId(unsigned int id); unsigned int getDstId() const; - void setServiceType(unsigned char type); - unsigned char getServiceType() const; + void setServiceType(unsigned char type); + unsigned char getServiceType() const; void reset(); @@ -76,9 +76,9 @@ private: bool m_emergency; unsigned int m_srcId; unsigned int m_dstId; - unsigned char m_serviceType; + unsigned char m_serviceType; CRS241213 m_rs241213; - CP25Trellis m_trellis; + CP25Trellis m_trellis; void decodeLDUHamming(const unsigned char* raw, unsigned char* data); void encodeLDUHamming(unsigned char* data, const unsigned char* raw); From 62dd3c420e26fc4eaf9356765ef468c0cff42429 Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Mon, 14 May 2018 10:49:03 -0400 Subject: [PATCH 4/5] Once again -- fix my poor whitespace formatting. --- P25Control.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/P25Control.cpp b/P25Control.cpp index 007a681..22e200a 100644 --- a/P25Control.cpp +++ b/P25Control.cpp @@ -400,8 +400,8 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len) 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 Sync + CSync::addP25Sync(data + 2U); // Regenerate NID m_nid.encode(data + 2U, P25_DUID_TSDU); From 074919ea5c900d3b0598e8703041208609e16af9 Mon Sep 17 00:00:00 2001 From: Bryan Biedenkapp Date: Mon, 14 May 2018 12:00:06 -0400 Subject: [PATCH 5/5] Oops! Forgot the P25 NID changes for the TSDU datatype. --- P25NID.cpp | 17 +++++++++++++++++ P25NID.h | 2 ++ 2 files changed, 19 insertions(+) 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; };