diff --git a/src/common/p25/dfsi/frames/BlockHeader.cpp b/src/common/p25/dfsi/frames/BlockHeader.cpp index 01e56f29..5385ed37 100644 --- a/src/common/p25/dfsi/frames/BlockHeader.cpp +++ b/src/common/p25/dfsi/frames/BlockHeader.cpp @@ -4,7 +4,7 @@ * GPLv2 Open Source. Use is subject to license terms. * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. * - * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * Copyright (C) 2024-2025 Bryan Biedenkapp, N2PLL * */ #include "common/p25/dfsi/frames/BlockHeader.h" @@ -26,8 +26,10 @@ using namespace p25::dfsi::frames; /* Initializes a instance of the BlockHeader class. */ BlockHeader::BlockHeader() : - m_payloadType(false), - m_blockLength(BlockType::UNDEFINED) + m_payloadType(true), + m_blockType(BlockType::UNDEFINED), + m_timestampOffset(0U), + m_blockLength(0U) { /* stub */ } @@ -35,8 +37,10 @@ BlockHeader::BlockHeader() : /* Initializes a instance of the BlockHeader class. */ BlockHeader::BlockHeader(uint8_t* data, bool verbose) : - m_payloadType(false), - m_blockLength(BlockType::UNDEFINED) + m_payloadType(true), + m_blockType(BlockType::UNDEFINED), + m_timestampOffset(0U), + m_blockLength(0U) { decode(data, verbose); } diff --git a/src/common/p25/dfsi/frames/BlockHeader.h b/src/common/p25/dfsi/frames/BlockHeader.h index b56c54e3..9d47a6e7 100644 --- a/src/common/p25/dfsi/frames/BlockHeader.h +++ b/src/common/p25/dfsi/frames/BlockHeader.h @@ -4,7 +4,7 @@ * GPLv2 Open Source. Use is subject to license terms. * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. * - * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * Copyright (C) 2024-2025 Bryan Biedenkapp, N2PLL * */ /** @@ -53,8 +53,8 @@ namespace p25 */ class HOST_SW_API BlockHeader { public: - static const uint8_t LENGTH = 1; - static const uint8_t VERBOSE_LENGTH = 4; + static const uint8_t LENGTH = 1U; + static const uint8_t VERBOSE_LENGTH = 4U; /** * @brief Initializes a copy instance of the BlockHeader class. diff --git a/src/common/p25/dfsi/frames/ControlOctet.h b/src/common/p25/dfsi/frames/ControlOctet.h index 308ec2e4..3395408b 100644 --- a/src/common/p25/dfsi/frames/ControlOctet.h +++ b/src/common/p25/dfsi/frames/ControlOctet.h @@ -4,7 +4,7 @@ * GPLv2 Open Source. Use is subject to license terms. * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. * - * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * Copyright (C) 2024-2025 Bryan Biedenkapp, N2PLL * */ /** @@ -45,7 +45,7 @@ namespace p25 */ class HOST_SW_API ControlOctet { public: - static const uint8_t LENGTH = 1; + static const uint8_t LENGTH = 1U; /** * @brief Initializes a copy instance of the ControlOctet class. diff --git a/src/common/p25/dfsi/frames/FullRateVoice.cpp b/src/common/p25/dfsi/frames/FullRateVoice.cpp index f8b1ed56..8ea60eab 100644 --- a/src/common/p25/dfsi/frames/FullRateVoice.cpp +++ b/src/common/p25/dfsi/frames/FullRateVoice.cpp @@ -4,7 +4,7 @@ * GPLv2 Open Source. Use is subject to license terms. * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. * - * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * Copyright (C) 2024-2025 Bryan Biedenkapp, N2PLL * */ #include "common/p25/dfsi/frames/FullRateVoice.h" @@ -95,7 +95,15 @@ bool FullRateVoice::decode(const uint8_t* data) // CAI 9 and 10 are 3 bytes of additional data not 4 ::memcpy(additionalData, data + 14U, ADDITIONAL_LENGTH - 1U); } else { - ::memcpy(additionalData, data + 14U, ADDITIONAL_LENGTH); + uint8_t buffer[ADDITIONAL_LENGTH - 1U]; + ::memset(buffer, 0x00U, ADDITIONAL_LENGTH - 1U); + ::memcpy(buffer, data + 14U, ADDITIONAL_LENGTH - 1U); + buffer[2U] &= 0xC0U; // mask low bits + + uint32_t offset = 0; + for (uint8_t i = 0; i < ADDITIONAL_LENGTH - 1U; i++, offset += 6) { + Utils::hex2Bin(additionalData[i], buffer, offset); + } } } else { if (additionalData != nullptr) @@ -128,7 +136,16 @@ void FullRateVoice::encode(uint8_t* data) // CAI 9 and 10 are 3 bytes of additional data not 4 ::memcpy(data + 14U, additionalData, ADDITIONAL_LENGTH - 1U); } else { - ::memcpy(data + 14U, additionalData, ADDITIONAL_LENGTH); + uint8_t buffer[ADDITIONAL_LENGTH - 1U]; + ::memset(buffer, 0x00U, ADDITIONAL_LENGTH - 1U); + ::memcpy(buffer, additionalData, ADDITIONAL_LENGTH - 1U); + + uint32_t offset = 0; + for (uint8_t i = 0; i < ADDITIONAL_LENGTH - 1U; i++, offset += 6) { + buffer[i] = Utils::bin2Hex(additionalData, offset); + } + + ::memcpy(data + 14U, buffer, ADDITIONAL_LENGTH); } } } diff --git a/src/common/p25/dfsi/frames/FullRateVoice.h b/src/common/p25/dfsi/frames/FullRateVoice.h index a3696bb0..afde4e83 100644 --- a/src/common/p25/dfsi/frames/FullRateVoice.h +++ b/src/common/p25/dfsi/frames/FullRateVoice.h @@ -4,7 +4,7 @@ * GPLv2 Open Source. Use is subject to license terms. * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. * - * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * Copyright (C) 2024-2025 Bryan Biedenkapp, N2PLL * */ /** @@ -133,10 +133,10 @@ namespace p25 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ * | IMBE 8 | IMBE 9 | IMBE 10 | IMBE 11 | * +=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+ - * | Et | Er |M|L|E| E1 |SF | B | Link Ctrl | Link Ctrl | - * | | | | |4| | | | | | + * | Et | Er |M|L|E| E1 |SF | B | Link Ctrl | Link Ctrl | Link | + * | | | | |4| | | | | | | * +=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+ - * | Link Ctrl |R| Status | + * |Ctr|R| Status | Rsvd | * +=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+ * * CAI Frames 12 - 17. @@ -150,10 +150,10 @@ namespace p25 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ * | IMBE 8 | IMBE 9 | IMBE 10 | IMBE 11 | * +=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+ - * | Et | Er |M|L|E| E1 |SF | B | Enc Sync | Enc Sync | - * | | | | |4| | | | | | + * | Et | Er |M|L|E| E1 |SF | B | Enc Sync | Enc Sync | Enc | + * | | | | |4| | | | | | | * +=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+ - * | Enc Sync |R| Status | + * |Syn|R| Status | Rsvd | * +=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+ * * CAI Frames 9 and 10. @@ -177,9 +177,9 @@ namespace p25 */ class HOST_SW_API FullRateVoice { public: - static const uint8_t LENGTH = 18; - static const uint8_t ADDITIONAL_LENGTH = 4; - static const uint8_t IMBE_BUF_LEN = 11; + static const uint8_t LENGTH = 18U; + static const uint8_t ADDITIONAL_LENGTH = 4U; + static const uint8_t IMBE_BUF_LEN = 11U; /** * @brief Initializes a copy instance of the FullRateVoice class. diff --git a/src/common/p25/dfsi/frames/MotFullRateVoice.h b/src/common/p25/dfsi/frames/MotFullRateVoice.h index 1def57cf..111a0bbd 100644 --- a/src/common/p25/dfsi/frames/MotFullRateVoice.h +++ b/src/common/p25/dfsi/frames/MotFullRateVoice.h @@ -55,9 +55,9 @@ namespace p25 */ class HOST_SW_API MotFullRateVoice { public: - static const uint8_t LENGTH = 17; - static const uint8_t SHORTENED_LENGTH = 14; - static const uint8_t ADDITIONAL_LENGTH = 4; + static const uint8_t LENGTH = 17U; + static const uint8_t SHORTENED_LENGTH = 14U; + static const uint8_t ADDITIONAL_LENGTH = 4U; /** * @brief Initializes a copy instance of the MotFullRateVoice class. diff --git a/src/common/p25/dfsi/frames/MotPDUFrame.h b/src/common/p25/dfsi/frames/MotPDUFrame.h index 4280d15a..792143a6 100644 --- a/src/common/p25/dfsi/frames/MotPDUFrame.h +++ b/src/common/p25/dfsi/frames/MotPDUFrame.h @@ -55,7 +55,7 @@ namespace p25 */ class HOST_SW_API MotPDUFrame { public: - static const uint8_t LENGTH = 20; + static const uint8_t LENGTH = 20U; /** * @brief Initializes a copy instance of the MotPDUFrame class. diff --git a/src/common/p25/dfsi/frames/MotStartOfStream.h b/src/common/p25/dfsi/frames/MotStartOfStream.h index 73e898d2..4afcab92 100644 --- a/src/common/p25/dfsi/frames/MotStartOfStream.h +++ b/src/common/p25/dfsi/frames/MotStartOfStream.h @@ -50,8 +50,8 @@ namespace p25 */ class HOST_SW_API MotStartOfStream { public: - static const uint8_t LENGTH = 10; - static const uint8_t FIXED_MARKER = 0x02; + static const uint8_t LENGTH = 10U; + static const uint8_t FIXED_MARKER = 0x02U; /** * @brief Initializes a copy instance of the MotStartOfStream class. diff --git a/src/common/p25/dfsi/frames/MotStartVoiceFrame.h b/src/common/p25/dfsi/frames/MotStartVoiceFrame.h index 0c7d3713..acaede19 100644 --- a/src/common/p25/dfsi/frames/MotStartVoiceFrame.h +++ b/src/common/p25/dfsi/frames/MotStartVoiceFrame.h @@ -58,7 +58,7 @@ namespace p25 */ class HOST_SW_API MotStartVoiceFrame { public: - static const uint8_t LENGTH = 22; + static const uint8_t LENGTH = 22U; /** * @brief Initializes a copy instance of the MotStartVoiceFrame class. diff --git a/src/common/p25/dfsi/frames/MotTSBKFrame.h b/src/common/p25/dfsi/frames/MotTSBKFrame.h index 15db4872..654f2640 100644 --- a/src/common/p25/dfsi/frames/MotTSBKFrame.h +++ b/src/common/p25/dfsi/frames/MotTSBKFrame.h @@ -57,7 +57,7 @@ namespace p25 */ class HOST_SW_API MotTSBKFrame { public: - static const uint8_t LENGTH = 24; + static const uint8_t LENGTH = 24U; /** * @brief Initializes a copy instance of the MotTSBKFrame class. diff --git a/src/common/p25/dfsi/frames/MotVoiceHeader1.h b/src/common/p25/dfsi/frames/MotVoiceHeader1.h index 37cdc3cd..049572bd 100644 --- a/src/common/p25/dfsi/frames/MotVoiceHeader1.h +++ b/src/common/p25/dfsi/frames/MotVoiceHeader1.h @@ -61,8 +61,8 @@ namespace p25 */ class HOST_SW_API MotVoiceHeader1 { public: - static const uint8_t LENGTH = 30; - static const uint8_t HCW_LENGTH = 21; + static const uint8_t LENGTH = 30U; + static const uint8_t HCW_LENGTH = 21U; /** * @brief Initializes a copy instance of the MotVoiceHeader1 class. diff --git a/src/common/p25/dfsi/frames/MotVoiceHeader2.h b/src/common/p25/dfsi/frames/MotVoiceHeader2.h index 1fa6615a..07af5245 100644 --- a/src/common/p25/dfsi/frames/MotVoiceHeader2.h +++ b/src/common/p25/dfsi/frames/MotVoiceHeader2.h @@ -57,8 +57,8 @@ namespace p25 */ class HOST_SW_API MotVoiceHeader2 { public: - static const uint8_t LENGTH = 22; - static const uint8_t HCW_LENGTH = 20; + static const uint8_t LENGTH = 22U; + static const uint8_t HCW_LENGTH = 20U; /** * @brief Initializes a copy instance of the MotVoiceHeader2 class. diff --git a/src/common/p25/dfsi/frames/StartOfStream.h b/src/common/p25/dfsi/frames/StartOfStream.h index 11c6fbac..06972926 100644 --- a/src/common/p25/dfsi/frames/StartOfStream.h +++ b/src/common/p25/dfsi/frames/StartOfStream.h @@ -4,7 +4,7 @@ * GPLv2 Open Source. Use is subject to license terms. * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. * - * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * Copyright (C) 2024-2025 Bryan Biedenkapp, N2PLL * */ /** @@ -45,7 +45,7 @@ namespace p25 */ class HOST_SW_API StartOfStream { public: - static const uint8_t LENGTH = 4; + static const uint8_t LENGTH = 4U; /** * @brief Initializes a copy instance of the StartOfStream class. diff --git a/src/common/p25/lc/LC.cpp b/src/common/p25/lc/LC.cpp index 98e35693..574df473 100644 --- a/src/common/p25/lc/LC.cpp +++ b/src/common/p25/lc/LC.cpp @@ -96,14 +96,17 @@ LC& LC::operator=(const LC& data) /* Decode a header data unit. */ -bool LC::decodeHDU(const uint8_t* data) +bool LC::decodeHDU(const uint8_t* data, bool rawOnly) { assert(data != nullptr); // deinterleave uint8_t rs[P25_HDU_LENGTH_BYTES + 1U]; uint8_t raw[P25_HDU_LENGTH_BYTES + 1U]; - P25Utils::decode(data, raw, 114U, 780U); + if (rawOnly) + ::memcpy(raw, data, P25_HDU_LENGTH_BYTES); + else + P25Utils::decode(data, raw, 114U, 780U); // decode Golay (18,6,8) FEC decodeHDUGolay(raw, rs); @@ -167,7 +170,7 @@ bool LC::decodeHDU(const uint8_t* data) /* Encode a header data unit. */ -void LC::encodeHDU(uint8_t* data) +void LC::encodeHDU(uint8_t* data, bool rawOnly) { assert(data != nullptr); assert(m_mi != nullptr); @@ -202,6 +205,11 @@ void LC::encodeHDU(uint8_t* data) // encode Golay (18,6,8) FEC encodeHDUGolay(raw, rs); + if (rawOnly) { + ::memcpy(data, raw, P25_HDU_LENGTH_BYTES); + return; + } + // interleave P25Utils::encode(raw, data, 114U, 780U); diff --git a/src/common/p25/lc/LC.h b/src/common/p25/lc/LC.h index 1b854af5..bab14773 100644 --- a/src/common/p25/lc/LC.h +++ b/src/common/p25/lc/LC.h @@ -73,14 +73,16 @@ namespace p25 /** * @brief Decode a header data unit. * @param[in] data Buffer containing the HDU to decode. + * @param rawOnly Flag indicating only the raw bytes of the LC should be decoded. * @returns True, if HDU decoded, otherwise false. */ - bool decodeHDU(const uint8_t* data); + bool decodeHDU(const uint8_t* data, bool rawOnly = false); /** * @brief Encode a header data unit. * @param[out] data Buffer to encode an HDU. + * @param rawOnly Flag indicating only the raw bytes of the LC should be encoded. */ - void encodeHDU(uint8_t* data); + void encodeHDU(uint8_t* data, bool rawOnly = false); /** * @brief Decode a logical link data unit 1. diff --git a/src/host/modem/ModemV24.cpp b/src/host/modem/ModemV24.cpp index bea2a2fe..f5f34744 100644 --- a/src/host/modem/ModemV24.cpp +++ b/src/host/modem/ModemV24.cpp @@ -4,7 +4,7 @@ * GPLv2 Open Source. Use is subject to license terms. * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. * - * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * Copyright (C) 2024-2024 Bryan Biedenkapp, N2PLL * Copyright (C) 2024 Patrick McDonnell, W3AXL * */ @@ -56,7 +56,8 @@ ModemV24::ModemV24(port::IModemPort* port, bool duplex, uint32_t p25QueueSize, u m_callTimeout(200U), m_jitter(jitter), m_lastP25Tx(0U), - m_rs() + m_rs(), + m_useTIAFormat(false) { m_v24Connected = false; // defaulted to false for V.24 modems @@ -89,6 +90,13 @@ void ModemV24::setP25NAC(uint32_t nac) m_nid = new NID(nac); } +/* Helper to set the TIA-102 format DFSI frame flag. */ + +void ModemV24::setTIAFormat(bool set) +{ + m_useTIAFormat = set; +} + /* Opens connection to the air interface modem. */ bool ModemV24::open() @@ -127,7 +135,10 @@ bool ModemV24::open() m_error = false; - LogMessage(LOG_MODEM, "Modem Ready [Direct Mode]"); + if (m_useTIAFormat) + LogMessage(LOG_MODEM, "Modem Ready [Direct Mode / TIA-102]"); + else + LogMessage(LOG_MODEM, "Modem Ready [Direct Mode / V.24]"); return true; } @@ -182,7 +193,10 @@ void ModemV24::clock(uint32_t ms) std::lock_guard lock(m_p25ReadLock); // convert data from V.24/DFSI formatting to TIA-102 air formatting - convertToAir(m_buffer + (cmdOffset + 1U), m_length - (cmdOffset + 1U)); + if (m_useTIAFormat) + convertToAirTIA(m_buffer + (cmdOffset + 1U), m_length - (cmdOffset + 1U)); + else + convertToAir(m_buffer + (cmdOffset + 1U), m_length - (cmdOffset + 1U)); } } break; @@ -429,7 +443,10 @@ int ModemV24::write(const uint8_t* data, uint32_t length) ::memset(buffer, 0x00U, length); ::memcpy(buffer, data + 2U, length); - convertFromAir(buffer, length); + if (m_useTIAFormat) + convertFromAirTIA(buffer, length); + else + convertFromAir(buffer, length); return length; } else { return Modem::write(data, length); @@ -943,8 +960,9 @@ void ModemV24::convertToAir(const uint8_t *data, uint32_t length) } } break; + default: - break; + break; } // increment our voice frame counter @@ -1077,235 +1095,1155 @@ void ModemV24::convertToAir(const uint8_t *data, uint32_t length) } } -/* Helper to add a V.24 data frame to the P25 TX queue with the proper timestamp and formatting */ +/* Internal helper to convert from TIA-102 DFSI to TIA-102 air interface. */ -void ModemV24::queueP25Frame(uint8_t* data, uint16_t len, SERIAL_TX_TYPE msgType) +void ModemV24::convertToAirTIA(const uint8_t *data, uint32_t length) { assert(data != nullptr); - assert(len > 0U); - - if (m_debug) - LogDebug(LOG_MODEM, "ModemV24::queueP25Frame() msgType = $%02X", msgType); - if (m_trace) - Utils::dump(1U, "ModemV24::queueP25Frame() data", data, len); - - // get current time in ms - uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - - // timestamp for this message (in ms) - uint64_t msgTime = 0U; - - // if this is our first message, timestamp is just now + the jitter buffer offset in ms - if (m_lastP25Tx == 0U) { - msgTime = now + m_jitter; - - // if the message type requests no jitter delay -- just set the message time to now - if (msgType == STT_NON_IMBE_NO_JITTER) - msgTime = now; - } - // if we had a message before this, calculate the new timestamp dynamically - else { - // if the last message occurred longer than our jitter buffer delay, we restart the sequence and calculate the same as above - if ((int64_t)(now - m_lastP25Tx) > m_jitter) { - msgTime = now + m_jitter; - } - // otherwise, we time out messages as required by the message type - else { - if (msgType == STT_IMBE) { - // IMBEs must go out at 20ms intervals - msgTime = m_lastP25Tx + 20U; - } else { - // Otherwise we don't care, we use 5ms since that's the theoretical minimum time a 9600 baud message can take - msgTime = m_lastP25Tx + 5U; - } - } - } - - len += 4U; - - // convert 16-bit length to 2 bytes - uint8_t length[2U]; - if (len > 255U) - length[0U] = (len >> 8U) & 0xFFU; - else - length[0U] = 0x00U; - length[1U] = len & 0xFFU; - - m_txP25Queue.addData(length, 2U); - - // add the data tag - uint8_t tag = TAG_DATA; - m_txP25Queue.addData(&tag, 1U); - - // convert 64-bit timestamp to 8 bytes and add - uint8_t tsBytes[8U]; - assert(sizeof msgTime == 8U); - ::memcpy(tsBytes, &msgTime, 8U); - m_txP25Queue.addData(tsBytes, 8U); - - // add the DVM start byte, length byte, CMD byte, and padding 0 - uint8_t header[4U]; - header[0U] = DVM_SHORT_FRAME_START; - header[1U] = len & 0xFFU; - header[2U] = CMD_P25_DATA; - header[3U] = 0x00U; - m_txP25Queue.addData(header, 4U); - - // add the data - m_txP25Queue.addData(data, len - 4U); - - // update the last message time - m_lastP25Tx = msgTime; -} - -/* Send a start of stream sequence (HDU, etc) to the connected serial V.24 device */ - -void ModemV24::startOfStream(const p25::lc::LC& control) -{ - m_txCallInProgress = true; - - MotStartOfStream start = MotStartOfStream(); - start.setStartStop(StartStopFlag::START); - start.setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + assert(length > 0U); - // create buffer for bytes and encode - uint8_t startBuf[start.LENGTH]; - ::memset(startBuf, 0x00U, start.LENGTH); - start.encode(startBuf); + uint8_t buffer[P25_PDU_FRAME_LENGTH_BYTES + 2U]; + ::memset(buffer, 0x00U, P25_PDU_FRAME_LENGTH_BYTES + 2U); - if (m_trace) - Utils::dump(1U, "ModemV24::startOfStream() MotStartOfStream", startBuf, MotStartOfStream::LENGTH); + // get the DFSI data (skip the 0x00 padded byte at the start) + UInt8Array __dfsiData = std::make_unique(length - 1U); + uint8_t* dfsiData = __dfsiData.get(); + ::memset(dfsiData, 0x00U, length - 1U); + ::memcpy(dfsiData, data + 1U, length - 1U); - queueP25Frame(startBuf, MotStartOfStream::LENGTH, STT_NON_IMBE); + if (m_debug) + Utils::dump("DFSI RX data from board", dfsiData, length - 1U); - uint8_t mi[MI_LENGTH_BYTES]; - ::memset(mi, 0x00U, MI_LENGTH_BYTES); - control.getMI(mi); + ControlOctet ctrl = ControlOctet(); + ctrl.decode(dfsiData); - uint8_t vhdr[DFSI_VHDR_LEN]; - ::memset(vhdr, 0x00U, DFSI_VHDR_LEN); + uint8_t blockCnt = ctrl.getBlockHeaderCnt(); - ::memcpy(vhdr, mi, MI_LENGTH_BYTES); + // iterate through blocks + uint8_t hdrOffs = 1U, dataOffs = blockCnt; + for (uint8_t i = 0U; i < blockCnt; i++) { + BlockHeader hdr = BlockHeader(); + hdr.decode(dfsiData + hdrOffs); - vhdr[9U] = control.getMFId(); - vhdr[10U] = control.getAlgId(); - __SET_UINT16B(control.getKId(), vhdr, 11U); - __SET_UINT16B(control.getDstId(), vhdr, 13U); + BlockType::E blockType = hdr.getBlockType(); + switch (blockType) { + case BlockType::START_OF_STREAM: + { + StartOfStream start = StartOfStream(); + start.decode(dfsiData + dataOffs); - // perform RS encoding - m_rs.encode362017(vhdr); + uint16_t nac = start.getNID() & 0xFFFU; - // convert the binary bytes to hex bytes - uint8_t raw[DFSI_VHDR_RAW_LEN]; - uint32_t offset = 0; - for (uint8_t i = 0; i < DFSI_VHDR_RAW_LEN; i++, offset += 6) { - raw[i] = Utils::bin2Hex(vhdr, offset); - } + // bryanb: maybe compare the NACs? - // prepare VHDR1 - MotVoiceHeader1 vhdr1 = MotVoiceHeader1(); - vhdr1.startOfStream->setStartStop(StartStopFlag::START); - vhdr1.startOfStream->setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); - vhdr1.setICW(m_diu ? ICWFlag::DIU : ICWFlag::QUANTAR); + dataOffs += StartOfStream::LENGTH; + } + break; + case BlockType::END_OF_STREAM: + { + dataOffs += 1U; - ::memcpy(vhdr1.header, raw, 8U); - ::memcpy(vhdr1.header + 9U, raw + 8U, 8U); - ::memcpy(vhdr1.header + 18U, raw + 16U, 2U); + // generate Sync + Sync::addP25Sync(buffer + 2U); - // encode VHDR1 and send - uint8_t vhdr1Buf[vhdr1.LENGTH]; - ::memset(vhdr1Buf, 0x00U, vhdr1.LENGTH); - vhdr1.encode(vhdr1Buf); + // generate NID + m_nid->encode(buffer + 2U, DUID::TDU); - if (m_trace) - Utils::dump(1U, "ModemV24::startOfStream() MotVoiceHeader1", vhdr1Buf, MotVoiceHeader1::LENGTH); + // add status bits + P25Utils::setStatusBitsAllIdle(buffer + 2U, P25_TDU_FRAME_LENGTH_BITS); - queueP25Frame(vhdr1Buf, MotVoiceHeader1::LENGTH, STT_NON_IMBE); + buffer[0U] = modem::TAG_DATA; + buffer[1U] = 0x01U; + storeConvertedRx(buffer, P25_TDU_FRAME_LENGTH_BITS + 2U); + } + break; - // prepare VHDR2 - MotVoiceHeader2 vhdr2 = MotVoiceHeader2(); - ::memcpy(vhdr2.header, raw + 18U, 8U); - ::memcpy(vhdr2.header + 9U, raw + 26U, 8U); - ::memcpy(vhdr2.header + 18U, raw + 34U, 2U); + case BlockType::VOICE_HEADER_P1: + { + // copy to call data VHDR1 + ::memset(m_rxCall->VHDR1, 0x00U, 18U); + ::memcpy(m_rxCall->VHDR1, dfsiData + dataOffs + 1U, 18U); - // encode VHDR2 and send - uint8_t vhdr2Buf[vhdr2.LENGTH]; - ::memset(vhdr2Buf, 0x00U, vhdr2.LENGTH); - vhdr2.encode(vhdr2Buf); + dataOffs += 19U; // 18 Golay + Block Type Marker + } + break; + case BlockType::VOICE_HEADER_P2: + { + // copy to call data VHDR2 + ::memset(m_rxCall->VHDR2, 0x00U, 18U); + ::memcpy(m_rxCall->VHDR2, dfsiData + dataOffs + 1U, 18U); - if (m_trace) - Utils::dump(1U, "ModemV24::startOfStream() MotVoiceHeader2", vhdr2Buf, MotVoiceHeader2::LENGTH); + dataOffs += 19U; // 18 Golay + Block Type Marker - queueP25Frame(vhdr2Buf, MotVoiceHeader2::LENGTH, STT_NON_IMBE); -} + // buffer for raw VHDR data + uint8_t raw[DFSI_VHDR_RAW_LEN]; + ::memset(raw, 0x00U, DFSI_VHDR_RAW_LEN); -/* Send an end of stream sequence (TDU, etc) to the connected serial V.24 device */ + ::memcpy(raw, m_rxCall->VHDR1, 18U); + ::memcpy(raw + 18U, m_rxCall->VHDR2, 18U); -void ModemV24::endOfStream() -{ - MotStartOfStream end = MotStartOfStream(); - end.setStartStop(StartStopFlag::STOP); + // buffer for decoded VHDR data + uint8_t vhdr[DFSI_VHDR_RAW_LEN]; - // create buffer and encode - uint8_t endBuf[MotStartOfStream::LENGTH]; - ::memset(endBuf, 0x00U, MotStartOfStream::LENGTH); - end.encode(endBuf); + uint32_t offset = 0U; + for (uint32_t i = 0; i < DFSI_VHDR_RAW_LEN; i++, offset += 6) + Utils::hex2Bin(raw[i], vhdr, offset); - if (m_trace) - Utils::dump(1U, "ModemV24::endOfStream() MotStartOfStream", endBuf, MotStartOfStream::LENGTH); + // try to decode the RS data + try { + lc::LC lc = lc::LC(); + if (!lc.decodeHDU(raw, true)) { + LogError(LOG_MODEM, "V.24/DFSI traffic failed to decode RS (36,20,17) FEC"); + } else { + // late entry? + if (!m_rxCallInProgress) { + m_rxCallInProgress = true; + m_rxCall->resetCallData(); + if (m_debug) + LogDebug(LOG_MODEM, "V24 RX VHDR late entry, resetting call data"); + } - queueP25Frame(endBuf, MotStartOfStream::LENGTH, STT_NON_IMBE); + uint8_t mi[MI_LENGTH_BYTES]; + lc.getMI(mi); - m_txCallInProgress = false; -} + ::memcpy(m_rxCall->MI, mi, MI_LENGTH_BYTES); -/* Internal helper to convert from TIA-102 air interface to V.24/DFSI. */ + m_rxCall->mfId = lc.getMFId(); + m_rxCall->algoId = lc.getAlgId(); + m_rxCall->kId = lc.getKId(); + m_rxCall->dstId = lc.getDstId(); -void ModemV24::convertFromAir(uint8_t* data, uint32_t length) -{ - assert(data != nullptr); - assert(length > 0U); + if (m_debug) { + LogDebug(LOG_MODEM, "P25, VHDR algId = $%02X, kId = $%04X, dstId = $%04X", m_rxCall->algoId, m_rxCall->kId, m_rxCall->dstId); + } - if (m_trace) - Utils::dump(1U, "ModemV24::convertFromAir() data", data, length); + // generate Sync + Sync::addP25Sync(buffer + 2U); - uint8_t ldu[9U * 25U]; - ::memset(ldu, 0x00U, 9 * 25U); + // generate NID + m_nid->encode(buffer + 2U, DUID::HDU); - // decode the NID - bool valid = m_nid->decode(data + 2U); - if (!valid) - return; + // generate HDU + lc.encodeHDU(buffer + 2U); - DUID::E duid = m_nid->getDUID(); + // add status bits + P25Utils::addStatusBits(buffer + 2U, P25_HDU_FRAME_LENGTH_BITS, true, false); - // handle individual DUIDs - lc::LC lc = lc::LC(); - data::LowSpeedData lsd = data::LowSpeedData(); - switch (duid) { - case DUID::HDU: - { - bool ret = lc.decodeHDU(data + 2U); - if (!ret) { - LogWarning(LOG_MODEM, P25_HDU_STR ", undecodable LC"); + buffer[0U] = modem::TAG_DATA; + buffer[1U] = 0x01U; + storeConvertedRx(buffer, P25_HDU_FRAME_LENGTH_BYTES + 2U); + } + } + catch (...) { + LogError(LOG_MODEM, "V.24/DFSI RX traffic got exception while trying to decode RS data for VHDR"); } - - startOfStream(lc); } break; - case DUID::LDU1: + + case BlockType::FULL_RATE_VOICE: { - bool ret = lc.decodeLDU1(data + 2U, true); - if (!ret) { - LogWarning(LOG_MODEM, P25_LDU1_STR ", undecodable LC"); - return; - } - - lsd.process(data + 2U); + FullRateVoice voice = FullRateVoice(); + voice.decode(dfsiData + dataOffs); - // late entry? - if (!m_txCallInProgress) { - startOfStream(lc); + DFSIFrameType::E frameType = voice.getFrameType(); + switch (frameType) { + case DFSIFrameType::LDU1_VOICE1: + { + ::memcpy(m_rxCall->netLDU1 + 10U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU1_VOICE2: + { + ::memcpy(m_rxCall->netLDU1 + 26U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU1_VOICE3: + { + ::memcpy(m_rxCall->netLDU1 + 55U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + m_rxCall->lco = voice.additionalData[0U]; + m_rxCall->mfId = voice.additionalData[1U]; + m_rxCall->serviceOptions = voice.additionalData[2U]; + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC3 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU1_VOICE4: + { + ::memcpy(m_rxCall->netLDU1 + 80U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + m_rxCall->dstId = __GET_UINT16(voice.additionalData, 0U); + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC4 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU1_VOICE5: + { + ::memcpy(m_rxCall->netLDU1 + 105U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + m_rxCall->srcId = __GET_UINT16(voice.additionalData, 0U); + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC5 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU1_VOICE6: + { + ::memcpy(m_rxCall->netLDU1 + 130U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU1_VOICE7: + { + ::memcpy(m_rxCall->netLDU1 + 155U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU1_VOICE8: + { + ::memcpy(m_rxCall->netLDU1 + 180U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU1_VOICE9: + { + ::memcpy(m_rxCall->netLDU1 + 204U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + m_rxCall->lsd1 = voice.additionalData[0U]; + m_rxCall->lsd2 = voice.additionalData[1U]; + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC9 traffic missing metadata"); + } + } + break; + + case DFSIFrameType::LDU2_VOICE10: + { + ::memcpy(m_rxCall->netLDU2 + 10U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU2_VOICE11: + { + ::memcpy(m_rxCall->netLDU2 + 26U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU2_VOICE12: + { + ::memcpy(m_rxCall->netLDU2 + 55U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + ::memcpy(m_rxCall->MI, voice.additionalData, 3U); + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC12 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU2_VOICE13: + { + ::memcpy(m_rxCall->netLDU2 + 80U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + ::memcpy(m_rxCall->MI + 3U, voice.additionalData, 3U); + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC13 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU2_VOICE14: + { + ::memcpy(m_rxCall->netLDU2 + 105U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + ::memcpy(m_rxCall->MI + 6U, voice.additionalData, 3U); + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC14 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU2_VOICE15: + { + ::memcpy(m_rxCall->netLDU2 + 130U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + m_rxCall->algoId = voice.additionalData[0U]; + m_rxCall->kId = __GET_UINT16B(voice.additionalData, 1U); + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC15 traffic missing metadata"); + } + } + break; + case DFSIFrameType::LDU2_VOICE16: + { + ::memcpy(m_rxCall->netLDU2 + 155U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU2_VOICE17: + { + ::memcpy(m_rxCall->netLDU2 + 180U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + } + break; + case DFSIFrameType::LDU2_VOICE18: + { + ::memcpy(m_rxCall->netLDU2 + 204U, voice.imbeData, RAW_IMBE_LENGTH_BYTES); + if (voice.additionalData != nullptr) { + m_rxCall->lsd1 = voice.additionalData[0U]; + m_rxCall->lsd2 = voice.additionalData[1U]; + } else { + LogWarning(LOG_MODEM, "V.24/DFSI VC18 traffic missing metadata"); + } + } + break; + + default: + break; + } + + // increment our voice frame counter + m_rxCall->n++; + } + break; + + default: + break; + } + + hdrOffs += BlockHeader::LENGTH; + } + + m_rxLastFrameTime = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + + // encode LDU1 if ready + if (m_rxCall->n == 9U) { + lc::LC lc = lc::LC(); + lc.setLCO(m_rxCall->lco); + lc.setMFId(m_rxCall->mfId); + + if (lc.isStandardMFId()) { + lc.setSrcId(m_rxCall->srcId); + lc.setDstId(m_rxCall->dstId); + } else { + uint8_t rsBuffer[P25_LDU_LC_FEC_LENGTH_BYTES]; + ::memset(rsBuffer, 0x00U, P25_LDU_LC_FEC_LENGTH_BYTES); + + rsBuffer[0U] = m_rxCall->lco; + rsBuffer[1U] = m_rxCall->mfId; + rsBuffer[2U] = m_rxCall->serviceOptions; + rsBuffer[3U] = (m_rxCall->dstId >> 16) & 0xFFU; + rsBuffer[4U] = (m_rxCall->dstId >> 8) & 0xFFU; + rsBuffer[5U] = (m_rxCall->dstId >> 0) & 0xFFU; + rsBuffer[6U] = (m_rxCall->srcId >> 16) & 0xFFU; + rsBuffer[7U] = (m_rxCall->srcId >> 8) & 0xFFU; + rsBuffer[8U] = (m_rxCall->srcId >> 0) & 0xFFU; + + // combine bytes into ulong64_t (8 byte) value + ulong64_t rsValue = 0U; + rsValue = rsBuffer[1U]; + rsValue = (rsValue << 8) + rsBuffer[2U]; + rsValue = (rsValue << 8) + rsBuffer[3U]; + rsValue = (rsValue << 8) + rsBuffer[4U]; + rsValue = (rsValue << 8) + rsBuffer[5U]; + rsValue = (rsValue << 8) + rsBuffer[6U]; + rsValue = (rsValue << 8) + rsBuffer[7U]; + rsValue = (rsValue << 8) + rsBuffer[8U]; + + lc.setRS(rsValue); + } + + bool emergency = ((m_rxCall->serviceOptions & 0xFFU) & 0x80U) == 0x80U; // Emergency Flag + bool encryption = ((m_rxCall->serviceOptions & 0xFFU) & 0x40U) == 0x40U; // Encryption Flag + uint8_t priority = ((m_rxCall->serviceOptions & 0xFFU) & 0x07U); // Priority + lc.setEmergency(emergency); + lc.setEncrypted(encryption); + lc.setPriority(priority); + + data::LowSpeedData lsd = data::LowSpeedData(); + lsd.setLSD1(m_rxCall->lsd1); + lsd.setLSD2(m_rxCall->lsd2); + + // generate Sync + Sync::addP25Sync(buffer + 2U); + + // generate NID + m_nid->encode(buffer + 2U, DUID::LDU1); + + // generate LDU1 Data + lc.encodeLDU1(buffer + 2U); + + // generate Low Speed Data + lsd.process(buffer + 2U); + + // generate audio + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 10U, 0U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 26U, 1U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 55U, 2U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 80U, 3U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 105U, 4U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 130U, 5U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 155U, 6U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 180U, 7U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU1 + 204U, 8U); + + // add status bits + P25Utils::addStatusBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, true, false); + + buffer[0U] = modem::TAG_DATA; + buffer[1U] = 0x01U; + storeConvertedRx(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); + } + + // encode LDU2 if ready + if (m_rxCall->n == 18U) { + lc::LC lc = lc::LC(); + lc.setMI(m_rxCall->MI); + lc.setAlgId(m_rxCall->algoId); + lc.setKId(m_rxCall->kId); + + data::LowSpeedData lsd = data::LowSpeedData(); + lsd.setLSD1(m_rxCall->lsd1); + lsd.setLSD2(m_rxCall->lsd2); + + // generate Sync + Sync::addP25Sync(buffer + 2U); + + // generate NID + m_nid->encode(buffer + 2U, DUID::LDU2); + + // generate LDU2 data + lc.encodeLDU2(buffer + 2U); + + // generate Low Speed Data + lsd.process(buffer + 2U); + + // generate audio + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 10U, 0U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 26U, 1U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 55U, 2U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 80U, 3U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 105U, 4U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 130U, 5U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 155U, 6U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 180U, 7U); + m_audio.encode(buffer + 2U, m_rxCall->netLDU2 + 204U, 8U); + + // add status bits + P25Utils::addStatusBits(buffer + 2U, P25_LDU_FRAME_LENGTH_BITS, true, false); + + buffer[0U] = modem::TAG_DATA; + buffer[1U] = 0x01U; + storeConvertedRx(buffer, P25_LDU_FRAME_LENGTH_BYTES + 2U); + + m_rxCall->n = 0; + } +} + +/* Helper to add a V.24 data frame to the P25 TX queue with the proper timestamp and formatting */ + +void ModemV24::queueP25Frame(uint8_t* data, uint16_t len, SERIAL_TX_TYPE msgType) +{ + assert(data != nullptr); + assert(len > 0U); + + if (m_debug) + LogDebug(LOG_MODEM, "ModemV24::queueP25Frame() msgType = $%02X", msgType); + if (m_trace) + Utils::dump(1U, "ModemV24::queueP25Frame() data", data, len); + + // get current time in ms + uint64_t now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + + // timestamp for this message (in ms) + uint64_t msgTime = 0U; + + // if this is our first message, timestamp is just now + the jitter buffer offset in ms + if (m_lastP25Tx == 0U) { + msgTime = now + m_jitter; + + // if the message type requests no jitter delay -- just set the message time to now + if (msgType == STT_NON_IMBE_NO_JITTER) + msgTime = now; + } + // if we had a message before this, calculate the new timestamp dynamically + else { + // if the last message occurred longer than our jitter buffer delay, we restart the sequence and calculate the same as above + if ((int64_t)(now - m_lastP25Tx) > m_jitter) { + msgTime = now + m_jitter; + } + // otherwise, we time out messages as required by the message type + else { + if (msgType == STT_IMBE) { + // IMBEs must go out at 20ms intervals + msgTime = m_lastP25Tx + 20U; + } else { + // Otherwise we don't care, we use 5ms since that's the theoretical minimum time a 9600 baud message can take + msgTime = m_lastP25Tx + 5U; + } + } + } + + len += 4U; + + // convert 16-bit length to 2 bytes + uint8_t length[2U]; + if (len > 255U) + length[0U] = (len >> 8U) & 0xFFU; + else + length[0U] = 0x00U; + length[1U] = len & 0xFFU; + + m_txP25Queue.addData(length, 2U); + + // add the data tag + uint8_t tag = TAG_DATA; + m_txP25Queue.addData(&tag, 1U); + + // convert 64-bit timestamp to 8 bytes and add + uint8_t tsBytes[8U]; + assert(sizeof msgTime == 8U); + ::memcpy(tsBytes, &msgTime, 8U); + m_txP25Queue.addData(tsBytes, 8U); + + // add the DVM start byte, length byte, CMD byte, and padding 0 + uint8_t header[4U]; + header[0U] = DVM_SHORT_FRAME_START; + header[1U] = len & 0xFFU; + header[2U] = CMD_P25_DATA; + header[3U] = 0x00U; + m_txP25Queue.addData(header, 4U); + + // add the data + m_txP25Queue.addData(data, len - 4U); + + // update the last message time + m_lastP25Tx = msgTime; +} + +/* Send a start of stream sequence (HDU, etc) to the connected serial V.24 device */ + +void ModemV24::startOfStream(const p25::lc::LC& control) +{ + m_txCallInProgress = true; + + MotStartOfStream start = MotStartOfStream(); + start.setStartStop(StartStopFlag::START); + start.setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + + // create buffer for bytes and encode + uint8_t startBuf[start.LENGTH]; + ::memset(startBuf, 0x00U, start.LENGTH); + start.encode(startBuf); + + if (m_trace) + Utils::dump(1U, "ModemV24::startOfStream() MotStartOfStream", startBuf, MotStartOfStream::LENGTH); + + queueP25Frame(startBuf, MotStartOfStream::LENGTH, STT_NON_IMBE); + + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + control.getMI(mi); + + uint8_t vhdr[DFSI_VHDR_LEN]; + ::memset(vhdr, 0x00U, DFSI_VHDR_LEN); + + ::memcpy(vhdr, mi, MI_LENGTH_BYTES); + + vhdr[9U] = control.getMFId(); + vhdr[10U] = control.getAlgId(); + __SET_UINT16B(control.getKId(), vhdr, 11U); + __SET_UINT16B(control.getDstId(), vhdr, 13U); + + // perform RS encoding + m_rs.encode362017(vhdr); + + // convert the binary bytes to hex bytes + uint8_t raw[DFSI_VHDR_RAW_LEN]; + uint32_t offset = 0; + for (uint8_t i = 0; i < DFSI_VHDR_RAW_LEN; i++, offset += 6) { + raw[i] = Utils::bin2Hex(vhdr, offset); + } + + // prepare VHDR1 + MotVoiceHeader1 vhdr1 = MotVoiceHeader1(); + vhdr1.startOfStream->setStartStop(StartStopFlag::START); + vhdr1.startOfStream->setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + vhdr1.setICW(m_diu ? ICWFlag::DIU : ICWFlag::QUANTAR); + + ::memcpy(vhdr1.header, raw, 8U); + ::memcpy(vhdr1.header + 9U, raw + 8U, 8U); + ::memcpy(vhdr1.header + 18U, raw + 16U, 2U); + + // encode VHDR1 and send + uint8_t vhdr1Buf[vhdr1.LENGTH]; + ::memset(vhdr1Buf, 0x00U, vhdr1.LENGTH); + vhdr1.encode(vhdr1Buf); + + if (m_trace) + Utils::dump(1U, "ModemV24::startOfStream() MotVoiceHeader1", vhdr1Buf, MotVoiceHeader1::LENGTH); + + queueP25Frame(vhdr1Buf, MotVoiceHeader1::LENGTH, STT_NON_IMBE); + + // prepare VHDR2 + MotVoiceHeader2 vhdr2 = MotVoiceHeader2(); + ::memcpy(vhdr2.header, raw + 18U, 8U); + ::memcpy(vhdr2.header + 9U, raw + 26U, 8U); + ::memcpy(vhdr2.header + 18U, raw + 34U, 2U); + + // encode VHDR2 and send + uint8_t vhdr2Buf[vhdr2.LENGTH]; + ::memset(vhdr2Buf, 0x00U, vhdr2.LENGTH); + vhdr2.encode(vhdr2Buf); + + if (m_trace) + Utils::dump(1U, "ModemV24::startOfStream() MotVoiceHeader2", vhdr2Buf, MotVoiceHeader2::LENGTH); + + queueP25Frame(vhdr2Buf, MotVoiceHeader2::LENGTH, STT_NON_IMBE); +} + +/* Send an end of stream sequence (TDU, etc) to the connected serial V.24 device */ + +void ModemV24::endOfStream() +{ + MotStartOfStream end = MotStartOfStream(); + end.setStartStop(StartStopFlag::STOP); + + // create buffer and encode + uint8_t endBuf[MotStartOfStream::LENGTH]; + ::memset(endBuf, 0x00U, MotStartOfStream::LENGTH); + end.encode(endBuf); + + if (m_trace) + Utils::dump(1U, "ModemV24::endOfStream() MotStartOfStream", endBuf, MotStartOfStream::LENGTH); + + queueP25Frame(endBuf, MotStartOfStream::LENGTH, STT_NON_IMBE); + + m_txCallInProgress = false; +} + +/* Helper to generate the NID value. */ + +uint16_t ModemV24::generateNID(DUID::E duid) +{ + uint8_t nid[2U]; + ::memset(nid, 0x00U, 2U); + + nid[0U] = (m_p25NAC >> 4) & 0xFFU; + nid[1U] = (m_p25NAC << 4) & 0xF0U; + nid[1U] |= duid; + + return __GET_UINT16B(nid, 0U); +} + +/* Send a start of stream sequence (HDU, etc) to the connected UDP TIA-102 device. */ + +void ModemV24::startOfStreamTIA(const p25::lc::LC& control) +{ + m_txCallInProgress = true; + + p25::lc::LC lc = p25::lc::LC(control); + + uint16_t length = 0U; + uint8_t buffer[P25_HDU_LENGTH_BYTES]; + ::memset(buffer, 0x00U, P25_HDU_LENGTH_BYTES); + + // generate control octet + ControlOctet ctrl = ControlOctet(); + ctrl.setBlockHeaderCnt(1U); + ctrl.encode(buffer); + length += ControlOctet::LENGTH; + + // generate block header + BlockHeader hdr = BlockHeader(); + hdr.setBlockType(BlockType::START_OF_STREAM); + hdr.encode(buffer + 1U); + length += BlockHeader::LENGTH; + + // generate start of stream + StartOfStream start = StartOfStream(); + start.setNID(generateNID()); + start.encode(buffer + 2U); + length += StartOfStream::LENGTH; + + if (m_trace) + Utils::dump(1U, "ModemV24::startOfStreamTIA() StartOfStream", buffer, length); + + queueP25Frame(buffer, length, STT_NON_IMBE); + + ::memset(buffer, 0x00U, P25_HDU_LENGTH_BYTES); + length = 0U; + + // generate control octet + ctrl.setBlockHeaderCnt(2U); + ctrl.encode(buffer); + length += ControlOctet::LENGTH; + + // generate block header 1 + hdr.setBlockType(BlockType::VOICE_HEADER_P1); + hdr.encode(buffer + 1U); + length += BlockHeader::LENGTH; + hdr.setBlockType(BlockType::START_OF_STREAM); + hdr.encode(buffer + 2U); + length += BlockHeader::LENGTH; + + // generate voice header 1 + uint8_t hdu[P25_HDU_LENGTH_BYTES]; + ::memset(hdu, 0x00U, P25_HDU_LENGTH_BYTES); + lc.encodeHDU(hdu, true); + + // convert the binary bytes to hex bytes + uint8_t raw[DFSI_VHDR_RAW_LEN]; + uint32_t offset = 0; + for (uint8_t i = 0; i < DFSI_VHDR_RAW_LEN; i++, offset += 6) { + raw[i] = Utils::bin2Hex(hdu, offset); + } + + // prepare VHDR1 + buffer[3U] = DFSIFrameType::MOT_VHDR_1; + ::memcpy(buffer + 4U, raw, 18U); + length += 19U; // 18 Golay + Block Type Marker + + start.encode(buffer + length); + length += StartOfStream::LENGTH; + + if (m_trace) + Utils::dump(1U, "ModemV24::startOfStreamTIA() VoiceHeader1", buffer, length); + + queueP25Frame(buffer, length, STT_NON_IMBE); + + ::memset(buffer, 0x00U, P25_HDU_LENGTH_BYTES); + length = 0U; + + // generate control octet + ctrl.setBlockHeaderCnt(2U); + ctrl.encode(buffer); + length += ControlOctet::LENGTH; + + // generate block header 1 + hdr.setBlockType(BlockType::VOICE_HEADER_P2); + hdr.encode(buffer + 1U); + length += BlockHeader::LENGTH; + hdr.setBlockType(BlockType::START_OF_STREAM); + hdr.encode(buffer + 2U); + length += BlockHeader::LENGTH; + + // prepare VHDR2 + buffer[3U] = DFSIFrameType::MOT_VHDR_2; + ::memcpy(buffer + 4U, raw + 18U, 18U); + length += 19U; // 18 Golay + Block Type Marker + + start.encode(buffer + length); + length += StartOfStream::LENGTH; + + if (m_trace) + Utils::dump(1U, "ModemV24::startOfStreamTIA() VoiceHeader2", buffer, length); + + queueP25Frame(buffer, length, STT_NON_IMBE); +} + +/* Send an end of stream sequence (TDU, etc) to the connected UDP TIA-102 device. */ + +void ModemV24::endOfStreamTIA() +{ + uint16_t length = 0U; + uint8_t buffer[2U]; + ::memset(buffer, 0x00U, 2U); + + // generate control octet + ControlOctet ctrl = ControlOctet(); + ctrl.setBlockHeaderCnt(1U); + ctrl.encode(buffer); + length += ControlOctet::LENGTH; + + // generate block header + BlockHeader hdr = BlockHeader(); + hdr.setBlockType(BlockType::END_OF_STREAM); + hdr.encode(buffer + 1U); + length += BlockHeader::LENGTH; + + if (m_trace) + Utils::dump(1U, "ModemV24::endOfStreamTIA() EndOfStream", buffer, length); + + queueP25Frame(buffer, length, STT_NON_IMBE); + + m_txCallInProgress = false; +} + +/* Internal helper to convert from TIA-102 air interface to V.24/DFSI. */ + +void ModemV24::convertFromAir(uint8_t* data, uint32_t length) +{ + assert(data != nullptr); + assert(length > 0U); + + if (m_trace) + Utils::dump(1U, "ModemV24::convertFromAir() data", data, length); + + uint8_t ldu[9U * 25U]; + ::memset(ldu, 0x00U, 9 * 25U); + + // decode the NID + bool valid = m_nid->decode(data + 2U); + if (!valid) + return; + + DUID::E duid = m_nid->getDUID(); + + // handle individual DUIDs + lc::LC lc = lc::LC(); + data::LowSpeedData lsd = data::LowSpeedData(); + switch (duid) { + case DUID::HDU: + { + bool ret = lc.decodeHDU(data + 2U); + if (!ret) { + LogWarning(LOG_MODEM, P25_HDU_STR ", undecodable LC"); + } + + startOfStream(lc); + } + break; + case DUID::LDU1: + { + bool ret = lc.decodeLDU1(data + 2U, true); + if (!ret) { + LogWarning(LOG_MODEM, P25_LDU1_STR ", undecodable LC"); + return; + } + + lsd.process(data + 2U); + + // late entry? + if (!m_txCallInProgress) { + startOfStream(lc); + if (m_debug) + LogDebug(LOG_MODEM, "V24 TX VHDR late entry, resetting TX call data"); + } + + // generate audio + m_audio.decode(data + 2U, ldu + 10U, 0U); + m_audio.decode(data + 2U, ldu + 26U, 1U); + m_audio.decode(data + 2U, ldu + 55U, 2U); + m_audio.decode(data + 2U, ldu + 80U, 3U); + m_audio.decode(data + 2U, ldu + 105U, 4U); + m_audio.decode(data + 2U, ldu + 130U, 5U); + m_audio.decode(data + 2U, ldu + 155U, 6U); + m_audio.decode(data + 2U, ldu + 180U, 7U); + m_audio.decode(data + 2U, ldu + 204U, 8U); + } + break; + case DUID::LDU2: + { + bool ret = lc.decodeLDU2(data + 2U); + if (!ret) { + LogWarning(LOG_MODEM, P25_LDU2_STR ", undecodable LC"); + return; + } + + lsd.process(data + 2U); + + // generate audio + m_audio.decode(data + 2U, ldu + 10U, 0U); + m_audio.decode(data + 2U, ldu + 26U, 1U); + m_audio.decode(data + 2U, ldu + 55U, 2U); + m_audio.decode(data + 2U, ldu + 80U, 3U); + m_audio.decode(data + 2U, ldu + 105U, 4U); + m_audio.decode(data + 2U, ldu + 130U, 5U); + m_audio.decode(data + 2U, ldu + 155U, 6U); + m_audio.decode(data + 2U, ldu + 180U, 7U); + m_audio.decode(data + 2U, ldu + 204U, 8U); + } + break; + + case DUID::TDU: + case DUID::TDULC: + if (m_txCallInProgress) + endOfStream(); + break; + + case DUID::PDU: + break; + + case DUID::TSDU: + { + lc::tsbk::OSP_TSBK_RAW tsbk = lc::tsbk::OSP_TSBK_RAW(); + if (!tsbk.decode(data + 2U)) { + LogWarning(LOG_MODEM, P25_TSDU_STR ", undecodable LC"); + return; + } + + MotStartOfStream startOfStream = MotStartOfStream(); + startOfStream.setStartStop(StartStopFlag::START); + startOfStream.setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + startOfStream.setStreamType(StreamTypeFlag::TSBK); + + // create buffer and encode + uint8_t startBuf[MotStartOfStream::LENGTH]; + ::memset(startBuf, 0x00U, MotStartOfStream::LENGTH); + startOfStream.encode(startBuf); + + queueP25Frame(startBuf, MotStartOfStream::LENGTH, STT_NON_IMBE_NO_JITTER); + + MotTSBKFrame tf = MotTSBKFrame(); + tf.startOfStream->setStartStop(StartStopFlag::START); + tf.startOfStream->setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + tf.startOfStream->setStreamType(StreamTypeFlag::TSBK); + delete[] tf.tsbkData; + + tf.tsbkData = new uint8_t[P25_TSBK_LENGTH_BYTES]; + ::memset(tf.tsbkData, 0x00U, P25_TSBK_LENGTH_BYTES); + ::memcpy(tf.tsbkData, tsbk.getDecodedRaw(), P25_TSBK_LENGTH_BYTES); + + // create buffer and encode + uint8_t tsbkBuf[MotTSBKFrame::LENGTH]; + ::memset(tsbkBuf, 0x00U, MotTSBKFrame::LENGTH); + tf.encode(tsbkBuf); + + if (m_trace) + Utils::dump(1U, "ModemV24::convertFromAir() MotTSBKFrame", tsbkBuf, MotTSBKFrame::LENGTH); + + queueP25Frame(tsbkBuf, MotTSBKFrame::LENGTH, STT_NON_IMBE_NO_JITTER); + + MotStartOfStream endOfStream = MotStartOfStream(); + endOfStream.setStartStop(StartStopFlag::STOP); + endOfStream.setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + endOfStream.setStreamType(StreamTypeFlag::TSBK); + + // create buffer and encode + uint8_t endBuf[MotStartOfStream::LENGTH]; + ::memset(endBuf, 0x00U, MotStartOfStream::LENGTH); + endOfStream.encode(endBuf); + + queueP25Frame(endBuf, MotStartOfStream::LENGTH, STT_NON_IMBE_NO_JITTER); + queueP25Frame(endBuf, MotStartOfStream::LENGTH, STT_NON_IMBE_NO_JITTER); + } + break; + + default: + break; + } + + if (duid == DUID::LDU1 || duid == DUID::LDU2) { + uint8_t rs[P25_LDU_LC_FEC_LENGTH_BYTES]; + ::memset(rs, 0x00U, P25_LDU_LC_FEC_LENGTH_BYTES); + + if (duid == DUID::LDU1) { + rs[0U] = lc.getLCO(); // LCO + + // split ulong64_t (8 byte) value into bytes + rs[1U] = (uint8_t)((lc.getRS() >> 56) & 0xFFU); + rs[2U] = (uint8_t)((lc.getRS() >> 48) & 0xFFU); + rs[3U] = (uint8_t)((lc.getRS() >> 40) & 0xFFU); + rs[4U] = (uint8_t)((lc.getRS() >> 32) & 0xFFU); + rs[5U] = (uint8_t)((lc.getRS() >> 24) & 0xFFU); + rs[6U] = (uint8_t)((lc.getRS() >> 16) & 0xFFU); + rs[7U] = (uint8_t)((lc.getRS() >> 8) & 0xFFU); + rs[8U] = (uint8_t)((lc.getRS() >> 0) & 0xFFU); + + // encode RS (24,12,13) FEC + m_rs.encode241213(rs); + } else { + // generate MI data + uint8_t mi[MI_LENGTH_BYTES]; + ::memset(mi, 0x00U, MI_LENGTH_BYTES); + lc.getMI(mi); + + for (uint32_t i = 0; i < MI_LENGTH_BYTES; i++) + rs[i] = mi[i]; // Message Indicator + + rs[9U] = lc.getAlgId(); // Algorithm ID + rs[10U] = (lc.getKId() >> 8) & 0xFFU; // Key ID + rs[11U] = (lc.getKId() >> 0) & 0xFFU; // ... + + // encode RS (24,16,9) FEC + m_rs.encode24169(rs); + } + + for (int n = 0; n < 9; n++) { + uint8_t* buffer = nullptr; + uint16_t bufferSize = 0; + MotFullRateVoice voice = MotFullRateVoice(); + + switch (n) { + case 0: // VOICE1/10 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE1 : DFSIFrameType::LDU2_VOICE10); + + MotStartVoiceFrame svf = MotStartVoiceFrame(); + svf.startOfStream->setStartStop(StartStopFlag::START); + svf.startOfStream->setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); + svf.fullRateVoice->setFrameType(voice.getFrameType()); + svf.fullRateVoice->setSource(m_diu ? SourceFlag::DIU : SourceFlag::QUANTAR); + svf.setICW(m_diu ? ICWFlag::DIU : ICWFlag::QUANTAR); + + ::memcpy(svf.fullRateVoice->imbeData, ldu + 10U, RAW_IMBE_LENGTH_BYTES); + + buffer = new uint8_t[MotStartVoiceFrame::LENGTH]; + ::memset(buffer, 0x00U, MotStartVoiceFrame::LENGTH); + svf.encode(buffer); + bufferSize = MotStartVoiceFrame::LENGTH; + } + break; + case 1: // VOICE2/11 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE2 : DFSIFrameType::LDU2_VOICE11); + voice.setSource(m_diu ? SourceFlag::DIU : SourceFlag::QUANTAR); + ::memcpy(voice.imbeData, ldu + 26U, RAW_IMBE_LENGTH_BYTES); + } + break; + case 2: // VOICE3/12 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE3 : DFSIFrameType::LDU2_VOICE12); + ::memcpy(voice.imbeData, ldu + 55U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + // copy additional data + voice.additionalData[0U] = rs[0U]; + voice.additionalData[1U] = rs[1U]; + voice.additionalData[2U] = rs[2U]; + } + break; + case 3: // VOICE4/13 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE4 : DFSIFrameType::LDU2_VOICE13); + ::memcpy(voice.imbeData, ldu + 80U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + // copy additional data + voice.additionalData[0U] = rs[3U]; + voice.additionalData[1U] = rs[4U]; + voice.additionalData[2U] = rs[5U]; + } + break; + case 4: // VOICE5/14 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE5 : DFSIFrameType::LDU2_VOICE14); + ::memcpy(voice.imbeData, ldu + 105U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + voice.additionalData[0U] = rs[6U]; + voice.additionalData[1U] = rs[7U]; + voice.additionalData[2U] = rs[8U]; + } + break; + case 5: // VOICE6/15 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE6 : DFSIFrameType::LDU2_VOICE15); + ::memcpy(voice.imbeData, ldu + 130U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + voice.additionalData[0U] = rs[9U]; + voice.additionalData[1U] = rs[10U]; + voice.additionalData[2U] = rs[11U]; + } + break; + case 6: // VOICE7/16 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE7 : DFSIFrameType::LDU2_VOICE16); + ::memcpy(voice.imbeData, ldu + 155U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + voice.additionalData[0U] = rs[12U]; + voice.additionalData[1U] = rs[13U]; + voice.additionalData[2U] = rs[14U]; + } + break; + case 7: // VOICE8/17 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE8 : DFSIFrameType::LDU2_VOICE17); + ::memcpy(voice.imbeData, ldu + 180U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + voice.additionalData[0U] = rs[15U]; + voice.additionalData[1U] = rs[16U]; + voice.additionalData[2U] = rs[17U]; + } + break; + case 8: // VOICE9/18 + { + voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE9 : DFSIFrameType::LDU2_VOICE18); + ::memcpy(voice.imbeData, ldu + 204U, RAW_IMBE_LENGTH_BYTES); + + voice.additionalData = new uint8_t[voice.ADDITIONAL_LENGTH]; + ::memset(voice.additionalData, 0x00U, voice.ADDITIONAL_LENGTH); + + voice.additionalData[0U] = lsd.getLSD1(); + voice.additionalData[1U] = lsd.getLSD2(); + } + break; + } + + // For n=0 (VHDR1/10) case we create the buffer in the switch, for all other frame types we do that here + if (n != 0) { + buffer = new uint8_t[voice.size()]; + ::memset(buffer, 0x00U, voice.size()); + voice.encode(buffer); + bufferSize = voice.size(); + } + + if (buffer != nullptr) { + if (m_trace) { + Utils::dump("ModemV24::convertFromAir() Encoded V.24 Voice Frame Data", buffer, bufferSize); + } + + queueP25Frame(buffer, bufferSize, STT_IMBE); + delete[] buffer; + } + } + } +} + +/* Internal helper to convert from TIA-102 air interface to TIA-102 DFSI. */ + +void ModemV24::convertFromAirTIA(uint8_t* data, uint32_t length) +{ + assert(data != nullptr); + assert(length > 0U); + + if (m_trace) + Utils::dump(1U, "ModemV24::convertFromAirTIA() data", data, length); + + uint8_t ldu[9U * 25U]; + ::memset(ldu, 0x00U, 9 * 25U); + + // decode the NID + bool valid = m_nid->decode(data + 2U); + if (!valid) + return; + + DUID::E duid = m_nid->getDUID(); + + // handle individual DUIDs + lc::LC lc = lc::LC(); + data::LowSpeedData lsd = data::LowSpeedData(); + switch (duid) { + case DUID::HDU: + { + bool ret = lc.decodeHDU(data + 2U); + if (!ret) { + LogWarning(LOG_MODEM, P25_HDU_STR ", undecodable LC"); + } + + startOfStreamTIA(lc); + } + break; + case DUID::LDU1: + { + bool ret = lc.decodeLDU1(data + 2U, true); + if (!ret) { + LogWarning(LOG_MODEM, P25_LDU1_STR ", undecodable LC"); + return; + } + + lsd.process(data + 2U); + + // late entry? + if (!m_txCallInProgress) { + startOfStreamTIA(lc); if (m_debug) LogDebug(LOG_MODEM, "V24 TX VHDR late entry, resetting TX call data"); } @@ -1348,66 +2286,14 @@ void ModemV24::convertFromAir(uint8_t* data, uint32_t length) case DUID::TDU: case DUID::TDULC: if (m_txCallInProgress) - endOfStream(); + endOfStreamTIA(); break; case DUID::PDU: break; case DUID::TSDU: - { - lc::tsbk::OSP_TSBK_RAW tsbk = lc::tsbk::OSP_TSBK_RAW(); - if (!tsbk.decode(data + 2U)) { - LogWarning(LOG_MODEM, P25_TSDU_STR ", undecodable LC"); - return; - } - - MotStartOfStream startOfStream = MotStartOfStream(); - startOfStream.setStartStop(StartStopFlag::START); - startOfStream.setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); - startOfStream.setStreamType(StreamTypeFlag::TSBK); - - // create buffer and encode - uint8_t startBuf[MotStartOfStream::LENGTH]; - ::memset(startBuf, 0x00U, MotStartOfStream::LENGTH); - startOfStream.encode(startBuf); - - queueP25Frame(startBuf, MotStartOfStream::LENGTH, STT_NON_IMBE_NO_JITTER); - - MotTSBKFrame tf = MotTSBKFrame(); - tf.startOfStream->setStartStop(StartStopFlag::START); - tf.startOfStream->setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); - tf.startOfStream->setStreamType(StreamTypeFlag::TSBK); - delete[] tf.tsbkData; - - tf.tsbkData = new uint8_t[P25_TSBK_LENGTH_BYTES]; - ::memset(tf.tsbkData, 0x00U, P25_TSBK_LENGTH_BYTES); - ::memcpy(tf.tsbkData, tsbk.getDecodedRaw(), P25_TSBK_LENGTH_BYTES); - - // create buffer and encode - uint8_t tsbkBuf[MotTSBKFrame::LENGTH]; - ::memset(tsbkBuf, 0x00U, MotTSBKFrame::LENGTH); - tf.encode(tsbkBuf); - - if (m_trace) - Utils::dump(1U, "ModemV24::convertFromAir() MotTSBKFrame", tsbkBuf, MotTSBKFrame::LENGTH); - - queueP25Frame(tsbkBuf, MotTSBKFrame::LENGTH, STT_NON_IMBE_NO_JITTER); - - MotStartOfStream endOfStream = MotStartOfStream(); - endOfStream.setStartStop(StartStopFlag::STOP); - endOfStream.setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); - endOfStream.setStreamType(StreamTypeFlag::TSBK); - - // create buffer and encode - uint8_t endBuf[MotStartOfStream::LENGTH]; - ::memset(endBuf, 0x00U, MotStartOfStream::LENGTH); - endOfStream.encode(endBuf); - - queueP25Frame(endBuf, MotStartOfStream::LENGTH, STT_NON_IMBE_NO_JITTER); - queueP25Frame(endBuf, MotStartOfStream::LENGTH, STT_NON_IMBE_NO_JITTER); - } - break; + break; default: break; @@ -1452,32 +2338,18 @@ void ModemV24::convertFromAir(uint8_t* data, uint32_t length) for (int n = 0; n < 9; n++) { uint8_t* buffer = nullptr; uint16_t bufferSize = 0; - MotFullRateVoice voice = MotFullRateVoice(); + FullRateVoice voice = FullRateVoice(); switch (n) { case 0: // VOICE1/10 { voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE1 : DFSIFrameType::LDU2_VOICE10); - - MotStartVoiceFrame svf = MotStartVoiceFrame(); - svf.startOfStream->setStartStop(StartStopFlag::START); - svf.startOfStream->setRT(m_rtrt ? RTFlag::ENABLED : RTFlag::DISABLED); - svf.fullRateVoice->setFrameType(voice.getFrameType()); - svf.fullRateVoice->setSource(m_diu ? SourceFlag::DIU : SourceFlag::QUANTAR); - svf.setICW(m_diu ? ICWFlag::DIU : ICWFlag::QUANTAR); - - ::memcpy(svf.fullRateVoice->imbeData, ldu + 10U, RAW_IMBE_LENGTH_BYTES); - - buffer = new uint8_t[MotStartVoiceFrame::LENGTH]; - ::memset(buffer, 0x00U, MotStartVoiceFrame::LENGTH); - svf.encode(buffer); - bufferSize = MotStartVoiceFrame::LENGTH; + ::memcpy(voice.imbeData, ldu + 10U, RAW_IMBE_LENGTH_BYTES); } break; case 1: // VOICE2/11 { voice.setFrameType((duid == DUID::LDU1) ? DFSIFrameType::LDU1_VOICE2 : DFSIFrameType::LDU2_VOICE11); - voice.setSource(m_diu ? SourceFlag::DIU : SourceFlag::QUANTAR); ::memcpy(voice.imbeData, ldu + 26U, RAW_IMBE_LENGTH_BYTES); } break; @@ -1577,15 +2449,39 @@ void ModemV24::convertFromAir(uint8_t* data, uint32_t length) // For n=0 (VHDR1/10) case we create the buffer in the switch, for all other frame types we do that here if (n != 0) { - buffer = new uint8_t[voice.size()]; - ::memset(buffer, 0x00U, voice.size()); - voice.encode(buffer); - bufferSize = voice.size(); + buffer = new uint8_t[P25_PDU_FRAME_LENGTH_BYTES]; + ::memset(buffer, 0x00U, P25_PDU_FRAME_LENGTH_BYTES); + + // generate control octet + ControlOctet ctrl = ControlOctet(); + ctrl.setBlockHeaderCnt(2U); + ctrl.encode(buffer); + bufferSize += ControlOctet::LENGTH; + + // generate block header + BlockHeader hdr = BlockHeader(); + hdr.setBlockType(BlockType::FULL_RATE_VOICE); + hdr.encode(buffer + 1U); + bufferSize += BlockHeader::LENGTH; + + // generate block header + hdr.setBlockType(BlockType::START_OF_STREAM); + hdr.encode(buffer + 2U); + bufferSize += BlockHeader::LENGTH; + + voice.encode(buffer + bufferSize); + bufferSize += FullRateVoice::LENGTH; + + // generate start of stream + StartOfStream start = StartOfStream(); + start.setNID(generateNID()); + start.encode(buffer + bufferSize); + bufferSize += StartOfStream::LENGTH; } if (buffer != nullptr) { if (m_trace) { - Utils::dump("ModemV24::convertFromAir() Encoded V.24 Voice Frame Data", buffer, bufferSize); + Utils::dump("ModemV24::convertFromAirTIA() Encoded V.24 Voice Frame Data", buffer, bufferSize); } queueP25Frame(buffer, bufferSize, STT_IMBE); diff --git a/src/host/modem/ModemV24.h b/src/host/modem/ModemV24.h index 71a126ed..bd78e475 100644 --- a/src/host/modem/ModemV24.h +++ b/src/host/modem/ModemV24.h @@ -4,7 +4,7 @@ * GPLv2 Open Source. Use is subject to license terms. * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. * - * Copyright (C) 2024 Bryan Biedenkapp, N2PLL + * Copyright (C) 2024-2025 Bryan Biedenkapp, N2PLL * */ /** @@ -261,6 +261,12 @@ namespace modem */ void setP25NAC(uint32_t nac) override; + /** + * @brief Helper to set the TIA-102 format DFSI frame flag. + * @param set + */ + void setTIAFormat(bool set); + /** * @brief Opens connection to the air interface modem. * @returns bool True, if connection to modem is made, otherwise false. @@ -316,6 +322,8 @@ namespace modem edac::RS634717 m_rs; + bool m_useTIAFormat; + /** * @brief Helper to write data from the P25 Tx queue to the serial interface. * @return int Actual number of bytes written to the serial interface. @@ -340,6 +348,12 @@ namespace modem * @param length Length of buffer. */ void convertToAir(const uint8_t *data, uint32_t length); + /** + * @brief Internal helper to convert from TIA-102 DFSI to TIA-102 air interface. + * @param data Buffer containing data to convert. + * @param length Length of buffer. + */ + void convertToAirTIA(const uint8_t *data, uint32_t length); /** * @brief Helper to add a V.24 data frame to the P25 Tx queue with the proper timestamp and formatting. @@ -359,12 +373,35 @@ namespace modem */ void endOfStream(); + /** + * @brief Helper to generate the NID value. + * @param duid P25 DUID. + * @returns uint16_t P25 NID. + */ + uint16_t generateNID(P25DEF::DUID::E duid = P25DEF::DUID::LDU1); + + /** + * @brief Send a start of stream sequence (HDU, etc) to the connected UDP TIA-102 device. + * @param[in] control Instance of p25::lc::LC containing link control data. + */ + void startOfStreamTIA(const p25::lc::LC& control); + /** + * @brief Send an end of stream sequence (TDU, etc) to the connected UDP TIA-102 device. + */ + void endOfStreamTIA(); + /** * @brief Internal helper to convert from TIA-102 air interface to V.24/DFSI. * @param data Buffer containing data to convert. * @param length Length of buffer. */ void convertFromAir(uint8_t* data, uint32_t length); + /** + * @brief Internal helper to convert from TIA-102 air interface to TIA-102 DFSI. + * @param data Buffer containing data to convert. + * @param length Length of buffer. + */ + void convertFromAirTIA(uint8_t* data, uint32_t length); }; } // namespace modem