Add Wires-X connection confirmation function for DMR2YSF

DMR2YSF/DMR2YSF.cpp

@@ -33,6 +33,8 @@
 const unsigned char dt1_temp[] = {0x31, 0x22, 0x62, 0x5F, 0x29, 0x00, 0x00, 0x00, 0x00, 0x00};
 const unsigned char dt2_temp[] = {0x00, 0x00, 0x00, 0x00, 0x6C, 0x20, 0x1C, 0x20, 0x03, 0x08};
 
+const unsigned char CONN_RESP[] = {0x5DU, 0x41U, 0x5FU, 0x26U};
+
 #define DMR_FRAME_PER       55U
 #define YSF_FRAME_PER       90U
 
@@ -108,14 +110,21 @@ m_dmrinfo(false),
 m_config(NULL),
 m_configLen(0U)
 {
+	m_ysfFrame = new unsigned char[200U];
+	m_dmrFrame = new unsigned char[50U];
+	m_config   = new unsigned char[400U];
+	m_command  = new unsigned char[300U];
+
 	::memset(m_ysfFrame, 0U, 200U);
 	::memset(m_dmrFrame, 0U, 50U);
-
-	m_config = new unsigned char[400U];
 }
 
 CDMR2YSF::~CDMR2YSF()
 {
+	delete[] m_ysfFrame;
+	delete[] m_dmrFrame;
+	delete[] m_config;
+	delete[] m_command;
 }
 
 int CDMR2YSF::run()
@@ -288,26 +297,32 @@ int CDMR2YSF::run()
 			if (valid) {
 				unsigned char fi = fich.getFI();
 				unsigned char dt = fich.getDT();
+				unsigned char fn = fich.getFN();
+				unsigned char ft = fich.getFT();
 
-				if ((::memcmp(buffer, "YSFD", 4U) == 0U) && (dt == YSF_DT_VD_MODE2)) {
+				if (::memcmp(buffer, "YSFD", 4U) == 0U) {
-					CYSFPayload ysfPayload;
+					processWiresX(buffer + 35U, fi, dt, fn, ft);
 
-					if (fi == YSF_FI_HEADER) {
+					if (dt == YSF_DT_VD_MODE2) {
-						if (ysfPayload.processHeaderData(buffer + 35U)) {
+						CYSFPayload ysfPayload;
-							std::string ysfSrc = ysfPayload.getSource();
+
-							std::string ysfDst = ysfPayload.getDest();
+						if (fi == YSF_FI_HEADER) {
-							LogMessage("Received YSF Header: Src: %s Dst: %s", ysfSrc.c_str(), ysfDst.c_str());
+							if (ysfPayload.processHeaderData(buffer + 35U)) {
-							m_srcid = findYSFID(ysfSrc, true);
+								std::string ysfSrc = ysfPayload.getSource();
-							m_conv.putYSFHeader();
+								std::string ysfDst = ysfPayload.getDest();
+								LogMessage("Received YSF Header: Src: %s Dst: %s", ysfSrc.c_str(), ysfDst.c_str());
+								m_srcid = findYSFID(ysfSrc, true);
+								m_conv.putYSFHeader();
+								m_ysfFrames = 0U;
+							}
+						} else if (fi == YSF_FI_TERMINATOR) {
+							LogMessage("YSF received end of voice transmission, %.1f seconds", float(m_ysfFrames) / 10.0F);
+							m_conv.putYSFEOT();
 							m_ysfFrames = 0U;
+						} else if (fi == YSF_FI_COMMUNICATIONS) {
+							m_conv.putYSF(buffer + 35U);
+							m_ysfFrames++;
 						}
-					} else if (fi == YSF_FI_TERMINATOR) {
-						LogMessage("YSF received end of voice transmission, %.1f seconds", float(m_ysfFrames) / 10.0F);
-						m_conv.putYSFEOT();
-						m_ysfFrames = 0U;
-					} else if (fi == YSF_FI_COMMUNICATIONS) {
-						m_conv.putYSF(buffer + 35U);
-						m_ysfFrames++;
 					}
 				}
 			}
@@ -847,6 +862,59 @@ void CDMR2YSF::sendYSFDisc()
 	LogMessage("Sending YSF disconnect command");
 }
 
+void CDMR2YSF::processWiresX(const unsigned char* data, unsigned char fi, unsigned char dt, unsigned char fn, unsigned char ft)
+{
+	assert(data != NULL);
+
+	if (dt != YSF_DT_DATA_FR_MODE)
+		return;
+
+	if (fi != YSF_FI_COMMUNICATIONS)
+		return;
+
+	CYSFPayload payload;
+
+	if (fn == 0U)
+		return;
+
+	if (fn == 1U) {
+		bool valid = payload.readDataFRModeData2(data, m_command + 0U);
+		if (!valid)
+			return;
+	} else {
+		bool valid = payload.readDataFRModeData1(data, m_command + (fn - 2U) * 40U + 20U);
+		if (!valid)
+			return;
+
+		valid = payload.readDataFRModeData2(data, m_command + (fn - 2U) * 40U + 40U);
+		if (!valid)
+			return;
+	}
+
+	if (fn == ft) {
+		bool valid = false;
+		// Find the end marker
+		for (unsigned int i = (fn - 1U) * 40U + 20U; i > 0U; i--) {
+			if (m_command[i] == 0x03U) {
+				unsigned char crc = CCRC::addCRC(m_command, i + 1U);
+				if (crc == m_command[i + 1U])
+					valid = true;
+				break;
+			}
+		}
+
+		if (!valid)
+			return;
+
+		if (::memcmp(m_command + 1U, CONN_RESP, 4U) == 0) {
+			LogMessage("Reflector connected OK");
+			return;
+		}
+	}
+
+	return;
+}
+
 bool CDMR2YSF::createMMDVM()
 {
 	std::string rptAddress   = m_conf.getDMRRptAddress();

DMR2YSF/DMR2YSF.h

@@ -68,8 +68,8 @@ private:
 	std::string      m_netDst;
 	std::string      m_ysfSrc;
 	unsigned char    m_dmrLastDT;
-	unsigned char    m_ysfFrame[200U];
+	unsigned char*   m_ysfFrame;
-	unsigned char    m_dmrFrame[50U];
+	unsigned char*   m_dmrFrame;
 	unsigned int     m_dmrFrames;
 	unsigned int     m_ysfFrames;
 	CDMREmbeddedData m_EmbeddedLC;
@@ -77,11 +77,13 @@ private:
 	bool             m_dmrinfo;
 	unsigned char*   m_config;
 	unsigned int     m_configLen;
+	unsigned char*   m_command;
 
 	unsigned int findYSFID(std::string cs, bool showdst);
 	std::string getSrcYSF(const unsigned char* source);
 	void sendYSFConn(unsigned int id);
 	void sendYSFDisc();
+	void processWiresX(const unsigned char* data, unsigned char fi, unsigned char dt, unsigned char fn, unsigned char ft);
 	bool createMMDVM();
 
 };