/* * Copyright (C) 2026 OpenGD77 contributors * * Building an outgoing DMR text message: TMS payload, IP/UDP wrapper, message CRC, * data header and the rate 1/2 blocks that carry it. * * This is a literal translation of a reference implementation that was verified on a PC * against a known-good packet produced by the server side, byte for byte (including both * IP/UDP checksums and the message CRC-32). * * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer * in the documentation and/or other materials provided with the distribution. * * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * 4. Use of this source code or binary releases for commercial purposes is strictly forbidden. This includes, without limitation, * incorporation in a commercial product or incorporation into a product or project which allows commercial use. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include #include "functions/sms.h" #define TMS_HEADER_OVERHEAD 8U // flag + 0x00 + seq + the five marker bytes #define IP_HEADER_LENGTH 20U #define UDP_HEADER_LENGTH 8U #define TMS_PORT 0x0FA7U static const uint8_t TMS_MARKER[5] = { 0x04, 0x0D, 0x00, 0x0A, 0x00 }; /* * ETSI / MOTOTRBO message CRC-32: bytes are swapped within each 16 bit pair, then an * MSB-first CRC32 (poly 0x04C11DB7, init 0, no final xor) is run over the result. */ static uint32_t smsMessageCRC32(const uint8_t *data, uint16_t length) { uint32_t crc = 0; for (uint16_t i = 0; i < length; i++) { // consume the bytes in swapped-pair order without copying the buffer uint16_t idx = ((i & 1U) ? (i - 1U) : (i + 1U)); uint8_t b = ((idx < length) ? data[idx] : 0x00); crc ^= ((uint32_t)b << 24); for (uint8_t k = 0; k < 8; k++) { crc = ((crc & 0x80000000U) ? ((crc << 1) ^ 0x04C11DB7U) : (crc << 1)); } } return crc; } static uint16_t onesComplementSum(const uint8_t *data, uint16_t length, uint32_t initial) { uint32_t sum = initial; for (uint16_t i = 0; (i + 1U) < length; i += 2U) { sum += (uint32_t)((data[i] << 8) | data[i + 1U]); } if (length & 1U) { sum += (uint32_t)(data[length - 1U] << 8); } while (sum >> 16) { sum = (sum & 0xFFFFU) + (sum >> 16); } return (uint16_t)(~sum); } static uint16_t smsCRC16CCITT(const uint8_t *data, uint16_t length) { uint16_t crc = 0; for (uint16_t i = 0; i < length; i++) { crc ^= ((uint16_t)data[i] << 8); for (uint8_t k = 0; k < 8; k++) { crc = ((crc & 0x8000U) ? ((crc << 1) ^ 0x1021U) : (crc << 1)); } } return crc; } uint16_t smsBuildPacket(uint8_t *out, uint16_t outSize, uint32_t srcId, uint32_t dstId, bool isGroup, const char *text, uint8_t flag, uint8_t seq, uint8_t *padOut) { uint16_t textLen = (uint16_t)strlen(text); uint16_t tmsLen = (uint16_t)(TMS_HEADER_OVERHEAD + (2U * textLen)); uint16_t udpLen = (uint16_t)(UDP_HEADER_LENGTH + 2U + tmsLen);// 2 = the TMS length field uint16_t ipLen = (uint16_t)(IP_HEADER_LENGTH + udpLen); uint32_t srcIp = (0x0CU << 24) | (srcId & 0x00FFFFFFU); uint32_t dstIp = (((uint32_t)(isGroup ? 0xE1U : 0x0CU)) << 24) | (dstId & 0x00FFFFFFU); uint16_t pos = 0; if ((uint16_t)(ipLen + 12U + 4U) > outSize) { return 0;// would not fit } // ---- IP header (checksum filled in afterwards) ---- out[pos++] = 0x45; out[pos++] = 0x00; out[pos++] = (uint8_t)(ipLen >> 8); out[pos++] = (uint8_t)(ipLen & 0xFF); out[pos++] = 0x00; out[pos++] = 0x01; // identification out[pos++] = 0x00; out[pos++] = 0x00; out[pos++] = (isGroup ? 0x01 : 0x40); // TTL out[pos++] = 0x11; // UDP out[pos++] = 0x00; // checksum placeholder out[pos++] = 0x00; out[pos++] = (uint8_t)(srcIp >> 24); out[pos++] = (uint8_t)(srcIp >> 16); out[pos++] = (uint8_t)(srcIp >> 8); out[pos++] = (uint8_t)(srcIp); out[pos++] = (uint8_t)(dstIp >> 24); out[pos++] = (uint8_t)(dstIp >> 16); out[pos++] = (uint8_t)(dstIp >> 8); out[pos++] = (uint8_t)(dstIp); uint16_t ipChecksum = onesComplementSum(out, IP_HEADER_LENGTH, 0); out[10] = (uint8_t)(ipChecksum >> 8); out[11] = (uint8_t)(ipChecksum & 0xFF); // ---- UDP header + payload ---- uint16_t udpStart = pos; out[pos++] = (uint8_t)(TMS_PORT >> 8); out[pos++] = (uint8_t)(TMS_PORT & 0xFF); out[pos++] = (uint8_t)(TMS_PORT >> 8); out[pos++] = (uint8_t)(TMS_PORT & 0xFF); out[pos++] = (uint8_t)(udpLen >> 8); out[pos++] = (uint8_t)(udpLen & 0xFF); out[pos++] = 0x00; // checksum placeholder out[pos++] = 0x00; out[pos++] = (uint8_t)(tmsLen >> 8); out[pos++] = (uint8_t)(tmsLen & 0xFF); out[pos++] = flag; out[pos++] = 0x00; out[pos++] = seq; memcpy(&out[pos], TMS_MARKER, sizeof(TMS_MARKER)); pos += sizeof(TMS_MARKER); for (uint16_t i = 0; i < textLen; i++) { out[pos++] = (uint8_t)text[i]; // UTF-16 little endian, ASCII only for now out[pos++] = 0x00; } // UDP checksum covers a pseudo header plus the whole UDP segment { uint8_t pseudo[12]; pseudo[0] = (uint8_t)(srcIp >> 24); pseudo[1] = (uint8_t)(srcIp >> 16); pseudo[2] = (uint8_t)(srcIp >> 8); pseudo[3] = (uint8_t)(srcIp); pseudo[4] = (uint8_t)(dstIp >> 24); pseudo[5] = (uint8_t)(dstIp >> 16); pseudo[6] = (uint8_t)(dstIp >> 8); pseudo[7] = (uint8_t)(dstIp); pseudo[8] = 0x00; pseudo[9] = 0x11; pseudo[10] = (uint8_t)(udpLen >> 8); pseudo[11] = (uint8_t)(udpLen & 0xFF); uint32_t sum = 0; for (uint8_t i = 0; i < sizeof(pseudo); i += 2U) { sum += (uint32_t)((pseudo[i] << 8) | pseudo[i + 1U]); } uint16_t udpChecksum = onesComplementSum(&out[udpStart], udpLen, sum); out[udpStart + 6U] = (uint8_t)(udpChecksum >> 8); out[udpStart + 7U] = (uint8_t)(udpChecksum & 0xFF); } // ---- pad to a multiple of 12, then append the message CRC as its own tail ---- uint8_t pad = (uint8_t)((12U - (pos % 12U)) % 12U); for (uint8_t i = 0; i < pad; i++) { out[pos++] = 0x00; } uint32_t crc = smsMessageCRC32(out, pos); out[pos++] = (uint8_t)(crc & 0xFF); // little endian out[pos++] = (uint8_t)(crc >> 8); out[pos++] = (uint8_t)(crc >> 16); out[pos++] = (uint8_t)(crc >> 24); if (padOut != NULL) { *padOut = pad; } return pos; } void smsBuildDataHeader(uint8_t *header, uint32_t srcId, uint32_t dstId, bool isGroup, uint8_t blocks, uint8_t pad) { memset(header, 0, 12); header[0] = (uint8_t)((isGroup ? 0x80U : 0x00U) | 0x02U);// DPF = 2, unconfirmed header[1] = (uint8_t)(0x40U | (pad & 0x0FU)); // SAP = 4 (IP based data) header[2] = (uint8_t)(dstId >> 16); header[3] = (uint8_t)(dstId >> 8); header[4] = (uint8_t)(dstId); header[5] = (uint8_t)(srcId >> 16); header[6] = (uint8_t)(srcId >> 8); header[7] = (uint8_t)(srcId); header[8] = (uint8_t)(0x80U | (blocks & 0x7FU)); header[9] = 0x10; uint16_t crc = (uint16_t)(smsCRC16CCITT(header, 10) ^ 0xCCCCU); header[10] = (uint8_t)(crc >> 8); header[11] = (uint8_t)(crc & 0xFF); } void smsBuildPreamble(uint8_t *preamble, uint32_t srcId, uint32_t dstId, bool isGroup, uint8_t countdown) { memset(preamble, 0, 12); preamble[0] = 0xBD; // Preamble CSBK, LB set preamble[1] = 0x00; preamble[2] = (uint8_t)(isGroup ? 0x80U : 0x00U); // data content preamble preamble[3] = countdown; preamble[4] = (uint8_t)(dstId >> 16); preamble[5] = (uint8_t)(dstId >> 8); preamble[6] = (uint8_t)(dstId); preamble[7] = (uint8_t)(srcId >> 16); preamble[8] = (uint8_t)(srcId >> 8); preamble[9] = (uint8_t)(srcId); uint16_t crc = (uint16_t)(smsCRC16CCITT(preamble, 10) ^ 0xA5A5U); preamble[10] = (uint8_t)(crc >> 8); preamble[11] = (uint8_t)(crc & 0xFF); }