opengd77-uv380-sms/firmware/application/source/functions/smsSend.c

/*
 * 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 <string.h>
#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);
}