DroidStar/dmrcodec.cpp

/*
	Copyright (C) 2019-2021 Doug McLain

	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 3 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, see <https://www.gnu.org/licenses/>.
*/

#include <iostream>
#include <cstring>
#include "dmrcodec.h"
#include "cgolay2087.h"
#include "crs129.h"
#include "SHA256.h"
#include "CRCenc.h"

#define DEBUG

const unsigned char MMDVM_DMR_DATA1   = 0x18U;
const unsigned char MMDVM_DMR_LOST1   = 0x19U;
const unsigned char MMDVM_DMR_DATA2   = 0x1AU;
const unsigned char MMDVM_DMR_LOST2   = 0x1BU;
const unsigned char MMDVM_DMR_SHORTLC = 0x1CU;
const unsigned char MMDVM_DMR_START   = 0x1DU;
const unsigned char MMDVM_DMR_ABORT   = 0x1EU;

const uint32_t ENCODING_TABLE_1676[] =
	{0x0000U, 0x0273U, 0x04E5U, 0x0696U, 0x09C9U, 0x0BBAU, 0x0D2CU, 0x0F5FU, 0x11E2U, 0x1391U, 0x1507U, 0x1774U,
	 0x182BU, 0x1A58U, 0x1CCEU, 0x1EBDU, 0x21B7U, 0x23C4U, 0x2552U, 0x2721U, 0x287EU, 0x2A0DU, 0x2C9BU, 0x2EE8U,
	 0x3055U, 0x3226U, 0x34B0U, 0x36C3U, 0x399CU, 0x3BEFU, 0x3D79U, 0x3F0AU, 0x411EU, 0x436DU, 0x45FBU, 0x4788U,
	 0x48D7U, 0x4AA4U, 0x4C32U, 0x4E41U, 0x50FCU, 0x528FU, 0x5419U, 0x566AU, 0x5935U, 0x5B46U, 0x5DD0U, 0x5FA3U,
	 0x60A9U, 0x62DAU, 0x644CU, 0x663FU, 0x6960U, 0x6B13U, 0x6D85U, 0x6FF6U, 0x714BU, 0x7338U, 0x75AEU, 0x77DDU,
	 0x7882U, 0x7AF1U, 0x7C67U, 0x7E14U, 0x804FU, 0x823CU, 0x84AAU, 0x86D9U, 0x8986U, 0x8BF5U, 0x8D63U, 0x8F10U,
	 0x91ADU, 0x93DEU, 0x9548U, 0x973BU, 0x9864U, 0x9A17U, 0x9C81U, 0x9EF2U, 0xA1F8U, 0xA38BU, 0xA51DU, 0xA76EU,
	 0xA831U, 0xAA42U, 0xACD4U, 0xAEA7U, 0xB01AU, 0xB269U, 0xB4FFU, 0xB68CU, 0xB9D3U, 0xBBA0U, 0xBD36U, 0xBF45U,
	 0xC151U, 0xC322U, 0xC5B4U, 0xC7C7U, 0xC898U, 0xCAEBU, 0xCC7DU, 0xCE0EU, 0xD0B3U, 0xD2C0U, 0xD456U, 0xD625U,
	 0xD97AU, 0xDB09U, 0xDD9FU, 0xDFECU, 0xE0E6U, 0xE295U, 0xE403U, 0xE670U, 0xE92FU, 0xEB5CU, 0xEDCAU, 0xEFB9U,
	 0xF104U, 0xF377U, 0xF5E1U, 0xF792U, 0xF8CDU, 0xFABEU, 0xFC28U, 0xFE5BU};

DMRCodec::DMRCodec(QString callsign, uint32_t dmrid, uint8_t essid, QString password, QString lat, QString lon, QString location, QString desc, QString freq, QString url, QString swid, QString pkid, QString options, uint32_t dstid, QString host, uint32_t port, bool ipv6, QString vocoder, QString modem, QString audioin, QString audioout) :
	Codec(callsign, 0, NULL, host, port, ipv6, vocoder, modem, audioin, audioout),
	m_dmrid(dmrid),
	m_password(password),
	m_lat(lat),
	m_lon(lon),
	m_location(location),
	m_desc(desc),
	m_freq(freq),
	m_url(url),
	m_swid(swid),
	m_pkid(pkid),
	m_txdstid(dstid),
	m_pc(false),
	m_options(options)
{
	m_dmrcnt = 0;
	m_flco = FLCO(0);

	if (essid){
		m_essid = m_dmrid * 100 + (essid-1);
	}
	else{
		m_essid = m_dmrid;
	}
}

DMRCodec::~DMRCodec()
{
}

void DMRCodec::process_udp()
{
	QByteArray buf;
	QByteArray in;
	QByteArray out;
	QHostAddress sender;
	quint16 senderPort;
	CSHA256 sha256;
	char buffer[400U];

	buf.resize(m_udp->pendingDatagramSize());
	m_udp->readDatagram(buf.data(), buf.size(), &sender, &senderPort);
#ifdef DEBUG
	fprintf(stderr, "RECV: ");
	for(int i = 0; i < buf.size(); ++i){
		fprintf(stderr, "%02x ", (unsigned char)buf.data()[i]);
	}
	fprintf(stderr, "\n");
	fflush(stderr);
#endif
	if((m_modeinfo.status != CONNECTED_RW) && (::memcmp(buf.data() + 3, "NAK", 3U) == 0)){
		//m_udp->disconnect();
		//m_udp->close();
		//delete m_udp;
		m_modeinfo.status = DISCONNECTED;
	}
	if((m_modeinfo.status != CONNECTED_RW) && (::memcmp(buf.data(), "MSTCL", 5U) == 0)){
		//m_udp->disconnect();
		//m_udp->close();
		//delete m_udp;
		m_modeinfo.status = CLOSED;
	}
	if((m_modeinfo.status != CONNECTED_RW) && (::memcmp(buf.data(), "RPTACK", 6U) == 0)){
		switch(m_modeinfo.status){
		case CONNECTING:
			m_modeinfo.status = DMR_AUTH;
			in.append(buf[6]);
			in.append(buf[7]);
			in.append(buf[8]);
			in.append(buf[9]);
			in.append(m_password.toUtf8());

			out.clear();
			out.resize(40);
			out[0] = 'R';
			out[1] = 'P';
			out[2] = 'T';
			out[3] = 'K';
			out[4] = (m_essid >> 24) & 0xff;
			out[5] = (m_essid >> 16) & 0xff;
			out[6] = (m_essid >> 8) & 0xff;
			out[7] = (m_essid >> 0) & 0xff;
			sha256.buffer((unsigned char *)in.data(), (unsigned int)(m_password.size() + sizeof(uint32_t)), (unsigned char *)out.data() + 8U);
			break;
		case DMR_AUTH:
			out.clear();
			buffer[0] = 'R';
			buffer[1] = 'P';
			buffer[2] = 'T';
			buffer[3] = 'C';
			buffer[4] = (m_essid >> 24) & 0xff;
			buffer[5] = (m_essid >> 16) & 0xff;
			buffer[6] = (m_essid >> 8) & 0xff;
			buffer[7] = (m_essid >> 0) & 0xff;

			m_modeinfo.status = DMR_CONF;
			char latitude[20U];
			char longitude[20U];

			sprintf(latitude, "%08f", m_lat.toFloat());
			sprintf(longitude, "%09f", m_lon.toFloat());

			char *p;
			if((p = strchr(latitude, ',')) != NULL){
				*p = '.';
			}
			if((p = strchr(longitude, ',')) != NULL){
				*p = '.';
			}
			::sprintf(buffer + 8U, "%-8.8s%09u%09u%02u%02u%8.8s%9.9s%03d%-20.20s%-19.19s%c%-124.124s%-40.40s%-40.40s", m_modeinfo.callsign.toStdString().c_str(),
					  m_freq.toUInt(), m_freq.toUInt(), 1, 1, latitude, longitude, 0, m_location.toStdString().c_str(), m_desc.toStdString().c_str(), '4',
					  m_url.toStdString().c_str(), m_swid.toStdString().c_str(), m_pkid.toStdString().c_str());
			out.append(buffer, 302);
			break;
		case DMR_CONF:
			setup_connection();
			if(m_options.size()){
				out.clear();
				out.append('R');
				out.append('P');
				out.append('T');
				out.append('O');
				out.append((m_essid >> 24) & 0xff);
				out.append((m_essid >> 16) & 0xff);
				out.append((m_essid >> 8) & 0xff);
				out.append((m_essid >> 0) & 0xff);
				out.append(m_options.toUtf8());
				//m_status = DMR_OPTS;
			}
			break;
		case DMR_OPTS:
			//setup_connection();
			break;
		default:
			break;
		}
		m_udp->writeDatagram(out, m_address, m_modeinfo.port);
	}
	if((buf.size() == 11) && (::memcmp(buf.data(), "MSTPONG", 7U) == 0)){
		m_modeinfo.count++;
	}
	if((buf.size() != 55) && ( (m_modeinfo.stream_state == STREAM_LOST) || (m_modeinfo.stream_state == STREAM_END) )){
		m_modeinfo.stream_state = STREAM_IDLE;
	}
	if((buf.size() == 55) &&
		(::memcmp(buf.data(), "DMRD", 4U) == 0) &&
		((uint8_t)buf.data()[15] & 0x20) &&
		(m_modeinfo.status == CONNECTED_RW))
	{
		m_rxwatchdog = 0;
		uint8_t t;
		if((uint8_t)buf.data()[15] & 0x02){
			qDebug() << "DMR RX EOT";
			m_modeinfo.stream_state = STREAM_END;
			m_modeinfo.ts = QDateTime::currentMSecsSinceEpoch();
			m_modeinfo.streamid = 0;
			t = 0x42;
		}
		else if((uint8_t)buf.data()[15] & 0x01){
			m_audio->start_playback();
			if(!m_rxtimer->isActive()){
				m_rxtimer->start(m_rxtimerint);
			}
			m_modeinfo.stream_state = STREAM_NEW;
			m_modeinfo.ts = QDateTime::currentMSecsSinceEpoch();
			m_modeinfo.srcid = (uint32_t)((buf.data()[5] << 16) | ((buf.data()[6] << 8) & 0xff00) | (buf.data()[7] & 0xff));
			m_modeinfo.dstid = (uint32_t)((buf.data()[8] << 16) | ((buf.data()[9] << 8) & 0xff00) | (buf.data()[10] & 0xff));
			m_modeinfo.gwid = (uint32_t)((buf.data()[11] << 24) | ((buf.data()[12] << 16) & 0xff0000) | ((buf.data()[13] << 8) & 0xff00) | (buf.data()[14] & 0xff));
			m_modeinfo.streamid = (uint32_t)((buf.data()[16] << 24) | ((buf.data()[17] << 16) & 0xff0000) | ((buf.data()[18] << 8) & 0xff00) | (buf.data()[19] & 0xff));
			m_modeinfo.frame_number = buf.data()[4];
			t = 0x41;
			qDebug() << "New DMR stream from " << m_modeinfo.srcid << " to " << m_modeinfo.dstid;
		}
		if(m_modem){
			m_rxmodemq.append(0xe0);
			m_rxmodemq.append(0x25);
			m_rxmodemq.append(MMDVM_DMR_DATA2);
			m_rxmodemq.append(t);

			for(int i = 0; i < 33; ++i){
				m_rxmodemq.append(buf.data()[20+i]);
			};
			//m_rxmodemq.append('\x00');
			//m_rxmodemq.append(0x2f);
		}
	}
	if((buf.size() == 55) &&
		(::memcmp(buf.data(), "DMRD", 4U) == 0) &&
		!((uint8_t)buf.data()[15] & 0x20) &&
		(m_modeinfo.status == CONNECTED_RW))
	{
		if(!m_tx && ( (m_modeinfo.stream_state == STREAM_LOST) || (m_modeinfo.stream_state == STREAM_END) || (m_modeinfo.stream_state == STREAM_IDLE) )){
			m_audio->start_playback();
			if(!m_rxtimer->isActive()){
				m_rxtimer->start(m_rxtimerint);
			}
			m_modeinfo.stream_state = STREAM_NEW;
		}
		else{
			m_modeinfo.stream_state = STREAMING;
		}
		m_rxwatchdog = 0;

		uint8_t dmrframe[33];
		uint8_t dmr3ambe[27];
		uint8_t dmrsync[7];
		// get the 33 bytes ambe
		memcpy(dmrframe, &(buf.data()[20]), 33);
		// extract the 3 ambe frames
		memcpy(dmr3ambe, dmrframe, 14);
		dmr3ambe[13] &= 0xF0;
		dmr3ambe[13] |= (dmrframe[19] & 0x0F);
		memcpy(&dmr3ambe[14], &dmrframe[20], 13);
		// extract sync
		dmrsync[0] = dmrframe[13] & 0x0F;
		::memcpy(&dmrsync[1], &dmrframe[14], 5);
		dmrsync[6] = dmrframe[19] & 0xF0;
		m_modeinfo.srcid =		(uint32_t)((buf.data()[5] << 16) | ((buf.data()[6] << 8) & 0xff00) | (buf.data()[7] & 0xff));
		m_modeinfo.dstid =		(uint32_t)((buf.data()[8] << 16) | ((buf.data()[9] << 8) & 0xff00) | (buf.data()[10] & 0xff));
		m_modeinfo.gwid =		(uint32_t)((buf.data()[11] << 24) | ((buf.data()[12] << 16) & 0xff0000) | ((buf.data()[13] << 8) & 0xff00) | (buf.data()[14] & 0xff));
		m_modeinfo.streamid =	(uint32_t)((buf.data()[16] << 24) | ((buf.data()[17] << 16) & 0xff0000) | ((buf.data()[18] << 8) & 0xff00) | (buf.data()[19] & 0xff));
		m_modeinfo.frame_number = buf.data()[4];

		if(m_modem){
			uint8_t t = ((uint8_t)buf.data()[15] & 0x0f);
			if(!t) t = 0x20;

			m_rxmodemq.append(0xe0);
			m_rxmodemq.append(0x25);
			m_rxmodemq.append(MMDVM_DMR_DATA2);
			m_rxmodemq.append(t);

			for(int i = 0; i < 33; ++i){
				m_rxmodemq.append(buf.data()[20+i]);
			}
		}

		for(int i = 0; i < 3; ++i){
			for(int j = 0; j < 9; ++j){
				m_rxcodecq.append(dmr3ambe[j + (9*i)]);
			}
		}
		//uint32_t id = (uint32_t)((buf.data()[5] << 16) | ((buf.data()[6] << 8) & 0xff00) | (buf.data()[7] & 0xff));
	}
	emit update(m_modeinfo);
#ifdef DEBUG
	if(out.size() > 0){
		fprintf(stderr, "SEND: ");
		for(int i = 0; i < out.size(); ++i){
			fprintf(stderr, "%02x ", (unsigned char)out.data()[i]);
		}
		fprintf(stderr, "\n");
		fflush(stderr);
	}
#endif
}

void DMRCodec::setup_connection()
{
	m_modeinfo.status = CONNECTED_RW;
	//m_mbeenc->set_gain_adjust(2.5);
	m_modeinfo.vocoder_loaded = load_vocoder_plugin();
	m_txtimer = new QTimer();
	connect(m_txtimer, SIGNAL(timeout()), this, SLOT(transmit()));
	m_rxtimer = new QTimer();
	connect(m_rxtimer, SIGNAL(timeout()), this, SLOT(process_rx_data()));
	m_ping_timer = new QTimer();
	connect(m_ping_timer, SIGNAL(timeout()), this, SLOT(send_ping()));
	m_ping_timer->start(5000);
	if(m_vocoder != ""){
		m_hwrx = true;
		m_hwtx = true;
		m_ambedev = new SerialAMBE("DMR");
		m_ambedev->connect_to_serial(m_vocoder);
		connect(m_ambedev, SIGNAL(data_ready()), this, SLOT(get_ambe()));
	}
	else{
		m_hwrx = false;
		m_hwtx = false;
	}
	if(m_modemport != ""){
		m_modem = new SerialModem("DMR");
		m_modem->set_modem_flags(m_rxInvert, m_txInvert, m_pttInvert, m_useCOSAsLockout, m_duplex);
		m_modem->set_modem_params(m_rxfreq, m_txfreq, m_txDelay, m_rxLevel, m_rfLevel, m_ysfTXHang, m_cwIdTXLevel, m_dstarTXLevel, m_dmrTXLevel, m_ysfTXLevel, m_p25TXLevel, m_nxdnTXLevel, m_pocsagTXLevel);
		m_modem->connect_to_serial(m_modemport);
		connect(m_modem, SIGNAL(modem_data_ready(QByteArray)), this, SLOT(process_modem_data(QByteArray)));
	}
	m_audio = new AudioEngine(m_audioin, m_audioout);
	m_audio->init();
}

void DMRCodec::hostname_lookup(QHostInfo i)
{
	if (!i.addresses().isEmpty()) {
		QByteArray out;
		out.append('R');
		out.append('P');
		out.append('T');
		out.append('L');
		out.append((m_essid >> 24) & 0xff);
		out.append((m_essid >> 16) & 0xff);
		out.append((m_essid >> 8) & 0xff);
		out.append((m_essid >> 0) & 0xff);
		m_address = i.addresses().first();
		m_udp = new QUdpSocket(this);
		connect(m_udp, SIGNAL(readyRead()), this, SLOT(process_udp()));
		m_udp->writeDatagram(out, m_address, m_modeinfo.port);
#ifdef DEBUG
		fprintf(stderr, "CONN: ");
		for(int i = 0; i < out.size(); ++i){
			fprintf(stderr, "%02x ", (unsigned char)out.data()[i]);
		}
		fprintf(stderr, "\n");
		fflush(stderr);
#endif
	}
}

void DMRCodec::send_ping()
{
	QByteArray out;
	char tag[] = { 'R','P','T','P','I','N','G' };
	out.append(tag, 7);
	out.append((m_essid >> 24) & 0xff);
	out.append((m_essid >> 16) & 0xff);
	out.append((m_essid >> 8) & 0xff);
	out.append((m_essid >> 0) & 0xff);
	m_udp->writeDatagram(out, m_address, m_modeinfo.port);
#ifdef DEBUG
	fprintf(stderr, "PING: ");
	for(int i = 0; i < out.size(); ++i){
		fprintf(stderr, "%02x ", (unsigned char)out.data()[i]);
	}
	fprintf(stderr, "\n");
	fflush(stderr);
#endif
}

void DMRCodec::send_disconnect()
{
	QByteArray out;
	out.append('R');
	out.append('P');
	out.append('T');
	out.append('C');
	out.append('L');
	out.append((m_essid >> 24) & 0xff);
	out.append((m_essid >> 16) & 0xff);
	out.append((m_essid >> 8) & 0xff);
	out.append((m_essid >> 0) & 0xff);
	m_udp->writeDatagram(out, m_address, m_modeinfo.port);
#ifdef DEBUG
	fprintf(stderr, "SEND: ");
	for(int i = 0; i < out.size(); ++i){
		fprintf(stderr, "%02x ", (unsigned char)out.data()[i]);
	}
	fprintf(stderr, "\n");
	fflush(stderr);
#endif
}

void DMRCodec::process_modem_data(QByteArray d)
{
	QByteArray txdata;
	uint8_t lcData[12U];

	uint8_t *p_frame = (uint8_t *)(d.data());
	m_dataType = p_frame[3U] & 0x0f;

	if ((p_frame[3U] & DMR_SYNC_DATA) == DMR_SYNC_DATA){
		if((m_dataType == DT_VOICE_LC_HEADER) && (m_modeinfo.stream_state == STREAM_IDLE)){
			m_modeinfo.stream_state = TRANSMITTING_MODEM;
			//qDebug() << "ids == " <<  m_txsrcid << ":" << m_txdstid;
		}
		else if(m_dataType == DT_TERMINATOR_WITH_LC){
			m_modeinfo.stream_state = STREAM_IDLE;
		}

		m_dmrcnt = 0;
		m_bptc.decode(p_frame + 4, lcData);
		m_txdstid = lcData[3U] << 16 | lcData[4U] << 8 | lcData[5U];
		m_txsrcid = lcData[6U] << 16 | lcData[7U] << 8 | lcData[8U];
		m_flco = FLCO(lcData[0U] & 0x3FU);
		build_frame();
		::memcpy(m_dmrFrame + 20U, p_frame + 4, 33U);
		txdata.append((char *)m_dmrFrame, 55);
		m_udp->writeDatagram(txdata, m_address, m_modeinfo.port);
	}
	else {
		m_dataType = (m_dmrcnt % 6U) ? DT_VOICE : DT_VOICE_SYNC;
		build_frame();
		::memcpy(m_dmrFrame + 20U, p_frame + 4, 33U);
		txdata.append((char *)m_dmrFrame, 55);
		m_udp->writeDatagram(txdata, m_address, m_modeinfo.port);
		++m_dmrcnt;
	}
#ifdef DEBUG
	fprintf(stderr, "SEND:%d: ", txdata.size());
	for(int i = 0; i < txdata.size(); ++i){
		fprintf(stderr, "%02x ", (unsigned char)txdata.data()[i]);
	}
	fprintf(stderr, "\n");
	fflush(stderr);
#endif
}

void DMRCodec::transmit()
{
	uint8_t ambe[72];
	int16_t pcm[160];

#ifdef USE_FLITE
	if(m_ttsid > 0){
		for(int i = 0; i < 160; ++i){
			if(m_ttscnt >= tts_audio->num_samples/2){
				pcm[i] = 0;
			}
			else{
				pcm[i] = tts_audio->samples[m_ttscnt*2] / 2;
				m_ttscnt++;
			}
		}
	}
#endif
	if(m_ttsid == 0){
		if(m_audio->read(pcm, 160)){
		}
		else{
			return;
		}
	}

	if(m_hwtx){
		m_ambedev->encode(pcm);
	}
	else{
		if(m_modeinfo.vocoder_loaded){
			m_mbevocoder->encode_2450x1150(pcm, ambe);
		}
		for(int i = 0; i < 9; ++i){
			m_txcodecq.append(ambe[i]);
		}
	}

	if(m_tx && (m_txcodecq.size() >= 27)){
		for(int i = 0; i < 27; ++i){
			m_ambe[i] = m_txcodecq.dequeue();
		}
		send_frame();
	}
	else if(m_tx == false){
		send_frame();
	}
}

void DMRCodec::send_frame()
{
	QByteArray txdata;
	if(m_pc){
		set_calltype(3);
	}
	else{
		set_calltype(0);
	}
	m_txsrcid = m_dmrid;
	if(m_tx){
		m_modeinfo.stream_state = TRANSMITTING;

		if(!m_dmrcnt){
			encode_header(DT_VOICE_LC_HEADER);
			m_txstreamid = static_cast<uint32_t>(::rand());
		}
		else{
			::memcpy(m_dmrFrame + 20U, m_ambe, 13U);
			m_dmrFrame[33U] = m_ambe[13U] & 0xF0U;
			m_dmrFrame[39U] = m_ambe[13U] & 0x0FU;
			::memcpy(m_dmrFrame + 40U, &m_ambe[14U], 13U);
			encode_data();
		}

		build_frame();
		txdata.append((char *)m_dmrFrame, 55);
		m_udp->writeDatagram(txdata, m_address, m_modeinfo.port);
		++m_dmrcnt;
/*
		if(!m_dmrcnt){
			for (int i = 0U; i < 3; i++) {
				m_dmrFrame[4U] = m_dmrcnt;
				txdata.append((char *)m_dmrFrame, 55);
				m_udp->writeDatagram(txdata, m_address, m_modeinfo.port);
				m_dmrcnt++;
			}

		}
		else{
			++m_dmrcnt;
			txdata.append((char *)m_dmrFrame, 55);
			m_udp->writeDatagram(txdata, m_address, m_modeinfo.port);
		}
*/
	}
	else{
		//fprintf(stderr, "DMR TX stopped\n");
		get_eot();
		build_frame();
		m_ttscnt = 0;
		txdata.append((char *)m_dmrFrame, 55);
		m_udp->writeDatagram(txdata, m_address, m_modeinfo.port);
		m_txtimer->stop();

		if(m_ttsid == 0){
			m_audio->stop_capture();
		}

		m_modeinfo.stream_state = STREAM_IDLE;
	}
	emit update_output_level(m_audio->level());
	emit update(m_modeinfo);
#ifdef DEBUG
	fprintf(stderr, "SEND:%d: ", txdata.size());
	for(int i = 0; i < txdata.size(); ++i){
		fprintf(stderr, "%02x ", (unsigned char)txdata.data()[i]);
	}
	fprintf(stderr, "\n");
	fflush(stderr);
#endif
}

unsigned char * DMRCodec::get_eot()
{
	encode_header(DT_TERMINATOR_WITH_LC);
	m_dmrcnt = 0;
	return m_dmrFrame;
}

void DMRCodec::build_frame()
{
	m_dmrFrame[0U]  = 'D';
	m_dmrFrame[1U]  = 'M';
	m_dmrFrame[2U]  = 'R';
	m_dmrFrame[3U]  = 'D';

	m_dmrFrame[5U]  = m_txsrcid >> 16;
	m_dmrFrame[6U]  = m_txsrcid >> 8;
	m_dmrFrame[7U]  = m_txsrcid >> 0;
	m_dmrFrame[8U]  = m_txdstid >> 16;
	m_dmrFrame[9U]  = m_txdstid >> 8;
	m_dmrFrame[10U] = m_txdstid >> 0;
	m_dmrFrame[11U]  = m_essid >> 24;
	m_dmrFrame[12U]  = m_essid >> 16;
	m_dmrFrame[13U]  = m_essid >> 8;
	m_dmrFrame[14U]  = m_essid >> 0;

	m_dmrFrame[15U] = (m_slot == 1U) ? 0x00U : 0x80U;
	m_dmrFrame[15U] |= (m_flco == FLCO_GROUP) ? 0x00U : 0x40U;

	if (m_dataType == DT_VOICE_SYNC) {
		m_dmrFrame[15U] |= 0x10U;
	} else if (m_dataType == DT_VOICE) {
		m_dmrFrame[15U] |= ((m_dmrcnt - 1) % 6U);
	} else {
		m_dmrFrame[15U] |= (0x20U | m_dataType);
	}

	m_dmrFrame[4U] = m_dmrcnt;
	::memcpy(m_dmrFrame + 16U, &m_txstreamid, 4U);

	m_dmrFrame[53U] = 0; //data.getBER();
	m_dmrFrame[54U] = 0; //data.getRSSI();

	m_modeinfo.srcid = m_txsrcid;
	m_modeinfo.dstid = m_txdstid;
	m_modeinfo.gwid = m_essid;
	m_modeinfo.frame_number = m_dmrcnt;
}

void DMRCodec::encode_header(uint8_t t)
{
	addDMRDataSync(m_dmrFrame+20, 0);
	m_dataType = t;
	full_lc_encode(m_dmrFrame+20, t);
}

void DMRCodec::encode_data()
{
	unsigned int n_dmr = (m_dmrcnt - 1) % 6U;

	if (!n_dmr) {
		m_dataType = DT_VOICE_SYNC;
		addDMRAudioSync(m_dmrFrame+20, 0);
		encode_embedded_data();
	}
	else {
		m_dataType = DT_VOICE;
		uint8_t lcss = get_embedded_data(m_dmrFrame+20, n_dmr);
		get_emb_data(m_dmrFrame+20, lcss);
	}
}

void DMRCodec::encode16114(bool* d)
{
	d[11] = d[0] ^ d[1] ^ d[2] ^ d[3] ^ d[5] ^ d[7] ^ d[8];
	d[12] = d[1] ^ d[2] ^ d[3] ^ d[4] ^ d[6] ^ d[8] ^ d[9];
	d[13] = d[2] ^ d[3] ^ d[4] ^ d[5] ^ d[7] ^ d[9] ^ d[10];
	d[14] = d[0] ^ d[1] ^ d[2] ^ d[4] ^ d[6] ^ d[7] ^ d[10];
	d[15] = d[0] ^ d[2] ^ d[5] ^ d[6] ^ d[8] ^ d[9] ^ d[10];
}

void DMRCodec::encode_qr1676(uint8_t* data)
{
	uint32_t value = (data[0U] >> 1) & 0x7FU;
	uint32_t cksum = ENCODING_TABLE_1676[value];

	data[0U] = cksum >> 8;
	data[1U] = cksum & 0xFFU;
}

void DMRCodec::get_emb_data(uint8_t* data, uint8_t lcss)
{
	uint8_t DMREMB[2U];
	DMREMB[0U]  = (m_colorcode << 4) & 0xF0U;
	//DMREMB[0U] |= m_PI ? 0x08U : 0x00U;
	DMREMB[0U] |= (lcss << 1) & 0x06U;
	DMREMB[1U]  = 0x00U;

	encode_qr1676(DMREMB);

	data[13U] = (data[13U] & 0xF0U) | ((DMREMB[0U] >> 4U) & 0x0FU);
	data[14U] = (data[14U] & 0x0FU) | ((DMREMB[0U] << 4U) & 0xF0U);
	data[18U] = (data[18U] & 0xF0U) | ((DMREMB[1U] >> 4U) & 0x0FU);
	data[19U] = (data[19U] & 0x0FU) | ((DMREMB[1U] << 4U) & 0xF0U);
}

uint8_t DMRCodec::get_embedded_data(uint8_t* data, uint8_t n)
{
	if (n >= 1U && n < 5U) {
		n--;

		bool bits[40U];
		::memset(bits, 0x00U, 40U * sizeof(bool));
		::memcpy(bits + 4U, m_raw + n * 32U, 32U * sizeof(bool));

		uint8_t bytes[5U];
		bitsToByteBE(bits + 0U,  bytes[0U]);
		bitsToByteBE(bits + 8U,  bytes[1U]);
		bitsToByteBE(bits + 16U, bytes[2U]);
		bitsToByteBE(bits + 24U, bytes[3U]);
		bitsToByteBE(bits + 32U, bytes[4U]);

		data[14U] = (data[14U] & 0xF0U) | (bytes[0U] & 0x0FU);
		data[15U] = bytes[1U];
		data[16U] = bytes[2U];
		data[17U] = bytes[3U];
		data[18U] = (data[18U] & 0x0FU) | (bytes[4U] & 0xF0U);

		switch (n) {
		case 0U:
			return 1U;
		case 3U:
			return 2U;
		default:
			return 3U;
		}
	} else {
		data[14U] &= 0xF0U;
		data[15U]  = 0x00U;
		data[16U]  = 0x00U;
		data[17U]  = 0x00U;
		data[18U] &= 0x0FU;

		return 0U;
	}
}

void DMRCodec::encode_embedded_data()
{
	uint32_t crc;
	lc_get_data(m_data);
	CCRC::encodeFiveBit(m_data, crc);

	bool data[128U];
	::memset(data, 0x00U, 128U * sizeof(bool));

	data[106U] = (crc & 0x01U) == 0x01U;
	data[90U]  = (crc & 0x02U) == 0x02U;
	data[74U]  = (crc & 0x04U) == 0x04U;
	data[58U]  = (crc & 0x08U) == 0x08U;
	data[42U]  = (crc & 0x10U) == 0x10U;

	uint32_t b = 0U;
	for (uint32_t a = 0U; a < 11U; a++, b++)
		data[a] = m_data[b];
	for (uint32_t a = 16U; a < 27U; a++, b++)
		data[a] = m_data[b];
	for (uint32_t a = 32U; a < 42U; a++, b++)
		data[a] = m_data[b];
	for (uint32_t a = 48U; a < 58U; a++, b++)
		data[a] = m_data[b];
	for (uint32_t a = 64U; a < 74U; a++, b++)
		data[a] = m_data[b];
	for (uint32_t a = 80U; a < 90U; a++, b++)
		data[a] = m_data[b];
	for (uint32_t a = 96U; a < 106U; a++, b++)
		data[a] = m_data[b];

	// Hamming (16,11,4) check each row except the last one
	for (uint32_t a = 0U; a < 112U; a += 16U)
		encode16114(data + a);

	// Add the parity bits for each column
	for (uint32_t a = 0U; a < 16U; a++)
		data[a + 112U] = data[a + 0U] ^ data[a + 16U] ^ data[a + 32U] ^ data[a + 48U] ^ data[a + 64U] ^ data[a + 80U] ^ data[a + 96U];

	// The data is packed downwards in columns
	b = 0U;
	for (uint32_t a = 0U; a < 128U; a++) {
		m_raw[a] = data[b];
		b += 16U;
		if (b > 127U)
			b -= 127U;
	}
}

void DMRCodec::bitsToByteBE(const bool* bits, uint8_t& byte)
{
	byte  = bits[0U] ? 0x80U : 0x00U;
	byte |= bits[1U] ? 0x40U : 0x00U;
	byte |= bits[2U] ? 0x20U : 0x00U;
	byte |= bits[3U] ? 0x10U : 0x00U;
	byte |= bits[4U] ? 0x08U : 0x00U;
	byte |= bits[5U] ? 0x04U : 0x00U;
	byte |= bits[6U] ? 0x02U : 0x00U;
	byte |= bits[7U] ? 0x01U : 0x00U;
}

void DMRCodec::byteToBitsBE(uint8_t byte, bool* bits)
{
	bits[0U] = (byte & 0x80U) == 0x80U;
	bits[1U] = (byte & 0x40U) == 0x40U;
	bits[2U] = (byte & 0x20U) == 0x20U;
	bits[3U] = (byte & 0x10U) == 0x10U;
	bits[4U] = (byte & 0x08U) == 0x08U;
	bits[5U] = (byte & 0x04U) == 0x04U;
	bits[6U] = (byte & 0x02U) == 0x02U;
	bits[7U] = (byte & 0x01U) == 0x01U;
}

void DMRCodec::lc_get_data(bool* bits)
{
	uint8_t bytes[9U];
	memset(bytes, 0, 9);
	lc_get_data(bytes);

	byteToBitsBE(bytes[0U], bits + 0U);
	byteToBitsBE(bytes[1U], bits + 8U);
	byteToBitsBE(bytes[2U], bits + 16U);
	byteToBitsBE(bytes[3U], bits + 24U);
	byteToBitsBE(bytes[4U], bits + 32U);
	byteToBitsBE(bytes[5U], bits + 40U);
	byteToBitsBE(bytes[6U], bits + 48U);
	byteToBitsBE(bytes[7U], bits + 56U);
	byteToBitsBE(bytes[8U], bits + 64U);
}

void DMRCodec::lc_get_data(uint8_t *bytes)
{
	bool pf, r;
	uint8_t fid, options;
	bytes[0U] = (uint8_t)m_flco;

	pf = (bytes[0U] & 0x80U) == 0x80U;
	r  = (bytes[0U] & 0x40U) == 0x40U;
	//m_flco = FLCO(bytes[0U] & 0x3FU);
	fid = bytes[1U];
	options = bytes[2U];

	//bytes[0U] = (uint8_t)m_flco;

	if (pf)
		bytes[0U] |= 0x80U;

	if (r)
		bytes[0U] |= 0x40U;

	bytes[1U] = fid;
	bytes[2U] = options;
	bytes[3U] = m_txdstid >> 16;
	bytes[4U] = m_txdstid >> 8;
	bytes[5U] = m_txdstid >> 0;
	bytes[6U] = m_dmrid >> 16;
	bytes[7U] = m_dmrid >> 8;
	bytes[8U] = m_dmrid >> 0;
}

void DMRCodec::full_lc_encode(uint8_t* data, uint8_t type)  // for header
{
	uint8_t lcData[12U];
	uint8_t parity[4U];
	::memset(lcData, 0, sizeof(lcData));
	lc_get_data(lcData);

	CRS129::encode(lcData, 9U, parity);

	switch (type) {
		case DT_VOICE_LC_HEADER:
			lcData[9U]  = parity[2U] ^ VOICE_LC_HEADER_CRC_MASK[0U];
			lcData[10U] = parity[1U] ^ VOICE_LC_HEADER_CRC_MASK[1U];
			lcData[11U] = parity[0U] ^ VOICE_LC_HEADER_CRC_MASK[2U];
			break;

		case DT_TERMINATOR_WITH_LC:
			lcData[9U]  = parity[2U] ^ TERMINATOR_WITH_LC_CRC_MASK[0U];
			lcData[10U] = parity[1U] ^ TERMINATOR_WITH_LC_CRC_MASK[1U];
			lcData[11U] = parity[0U] ^ TERMINATOR_WITH_LC_CRC_MASK[2U];
			break;

		default:
			return;
	}
	get_slot_data(data);
	m_bptc.encode(lcData, data);
}

void DMRCodec::get_slot_data(uint8_t* data)
{
	uint8_t DMRSlotType[3U];
	DMRSlotType[0U]  = (m_colorcode << 4) & 0xF0U;
	DMRSlotType[0U] |= (m_dataType  << 0) & 0x0FU;
	DMRSlotType[1U]  = 0x00U;
	DMRSlotType[2U]  = 0x00U;

	CGolay2087::encode(DMRSlotType);

	data[12U] = (data[12U] & 0xC0U) | ((DMRSlotType[0U] >> 2) & 0x3FU);
	data[13U] = (data[13U] & 0x0FU) | ((DMRSlotType[0U] << 6) & 0xC0U) | ((DMRSlotType[1U] >> 2) & 0x30U);
	data[19U] = (data[19U] & 0xF0U) | ((DMRSlotType[1U] >> 2) & 0x0FU);
	data[20U] = (data[20U] & 0x03U) | ((DMRSlotType[1U] << 6) & 0xC0U) | ((DMRSlotType[2U] >> 2) & 0x3CU);
}


void DMRCodec::addDMRDataSync(uint8_t* data, bool duplex)
{
	if (duplex) {
		for (uint32_t i = 0U; i < 7U; i++)
			data[i + 13U] = (data[i + 13U] & ~SYNC_MASK[i]) | BS_SOURCED_DATA_SYNC[i];
	} else {
		for (uint32_t i = 0U; i < 7U; i++)
			data[i + 13U] = (data[i + 13U] & ~SYNC_MASK[i]) | MS_SOURCED_DATA_SYNC[i];
	}
}

void DMRCodec::addDMRAudioSync(uint8_t* data, bool duplex)
{
	if (duplex) {
		for (uint32_t i = 0U; i < 7U; i++)
			data[i + 13U] = (data[i + 13U] & ~SYNC_MASK[i]) | BS_SOURCED_AUDIO_SYNC[i];
	} else {
		for (uint32_t i = 0U; i < 7U; i++)
			data[i + 13U] = (data[i + 13U] & ~SYNC_MASK[i]) | MS_SOURCED_AUDIO_SYNC[i];
	}
}

void DMRCodec::get_ambe()
{
	uint8_t ambe[9];

	if(m_ambedev->get_ambe(ambe)){
		for(int i = 0; i < 9; ++i){
			m_txcodecq.append(ambe[i]);
		}
	}
}

void DMRCodec::process_rx_data()
{
	int16_t pcm[160];
	uint8_t ambe[9];

	if(m_rxwatchdog++ > 100){
		qDebug() << "DMR RX stream timeout ";
		m_rxwatchdog = 0;
		m_modeinfo.stream_state = STREAM_LOST;
		m_modeinfo.ts = QDateTime::currentMSecsSinceEpoch();
		emit update(m_modeinfo);
		m_modeinfo.streamid = 0;
	}

	if(m_rxmodemq.size() > 2){
		QByteArray out;
		int s = m_rxmodemq[1];
		if((m_rxmodemq[0] == 0xe0) && (m_rxmodemq.size() >= s)){
			for(int i = 0; i < s; ++i){
				out.append(m_rxmodemq.dequeue());
			}
			m_modem->write(out);
		}
	}

	if((!m_tx) && (m_rxcodecq.size() > 8) ){
		for(int i = 0; i < 9; ++i){
			ambe[i] = m_rxcodecq.dequeue();
		}
		if(m_hwrx){
			m_ambedev->decode(ambe);

			if(m_ambedev->get_audio(pcm)){
				m_audio->write(pcm, 160);
				emit update_output_level(m_audio->level());
			}
		}
		else{
			if(m_modeinfo.vocoder_loaded){
				m_mbevocoder->decode_2450x1150(pcm, ambe);
			}
			else{
				memset(pcm, 0, 160 * sizeof(int16_t));
			}
			m_audio->write(pcm, 160);
			emit update_output_level(m_audio->level());
		}
	}
	else if ( (m_modeinfo.stream_state == STREAM_END) || (m_modeinfo.stream_state == STREAM_LOST) ){
		m_rxtimer->stop();
		m_audio->stop_playback();
		m_rxwatchdog = 0;
		m_modeinfo.streamid = 0;
		m_rxcodecq.clear();
		qDebug() << "DMR playback stopped";
		return;
	}
}