Follow the ETSI standard for DMO operation.

DMRDMOTX.cpp

@@ -41,6 +41,21 @@ const q15_t DMR_LEVELB[] = { 213,  213,  213,  213,  213};
 const q15_t DMR_LEVELC[] = {-213, -213, -213, -213, -213};
 const q15_t DMR_LEVELD[] = {-640, -640, -640, -640, -640};
 
+const uint8_t CACH_INTERLEAVE[] =
+      {1U,   2U,  3U,  5U,  6U,  7U,  9U, 10U, 11U, 13U, 15U, 16U, 17U, 19U, 20U, 21U, 23U,
+       25U, 26U, 27U, 29U, 30U, 31U, 33U, 34U, 35U, 37U, 39U, 40U, 41U, 43U, 44U, 45U, 47U,
+       49U, 50U, 51U, 53U, 54U, 55U, 57U, 58U, 59U, 61U, 63U, 64U, 65U, 67U, 68U, 69U, 71U,
+       73U, 74U, 75U, 77U, 78U, 79U, 81U, 82U, 83U, 85U, 87U, 88U, 89U, 91U, 92U, 93U, 95U};
+
+const uint8_t EMPTY_SHORT_LC[] =
+      {0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U};
+
+const uint8_t BIT_MASK_TABLE[] = {0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U};
+
+#define WRITE_BIT1(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7])
+#define READ_BIT1(p,i)    (p[(i)>>3] & BIT_MASK_TABLE[(i)&7])
+
+const uint8_t DMR_SYNC = 0x77U;
 
 CDMRDMOTX::CDMRDMOTX() :
 m_fifo(),
@@ -49,7 +64,8 @@ m_modState(),
 m_poBuffer(),
 m_poLen(0U),
 m_poPtr(0U),
-m_txDelay(240U)       // 200ms
+m_txDelay(240U),      // 200ms
+m_cachPtr(0U)
 {
   ::memset(m_modState, 0x00U, 70U * sizeof(q15_t));
 
@@ -63,13 +79,17 @@ void CDMRDMOTX::process()
   if (m_poLen == 0U && m_fifo.getData() > 0U) {
     if (!m_tx) {
       for (uint16_t i = 0U; i < m_txDelay; i++)
-        m_poBuffer[m_poLen++] = 0x00U;
+        m_poBuffer[i] = DMR_SYNC;
+
+      m_poLen = m_txDelay;
     } else {
-      for (unsigned int i = 0U; i < 72U; i++)
+      createCACH(m_poBuffer + 0U,  0U);
-        m_poBuffer[m_poLen++] = 0x00U;
+      createCACH(m_poBuffer + 36U, 1U);
 
       for (unsigned int i = 0U; i < DMR_FRAME_LENGTH_BYTES; i++)
-        m_poBuffer[i] = m_fifo.get();
+        m_poBuffer[i + 3U] = m_poBuffer[i + 39U] = m_fifo.get();
+
+      m_poLen = 72U;
     }
 
     m_poPtr = 0U;
@@ -149,3 +169,34 @@ void CDMRDMOTX::setTXDelay(uint8_t delay)
   m_txDelay = 240U + uint16_t(delay) * 12U;        // 200ms + tx delay
 }
 
+void CDMRDMOTX::createCACH(uint8_t* buffer, uint8_t slotIndex)
+{
+  if (m_cachPtr >= 12U)
+    m_cachPtr = 0U;
+
+  ::memcpy(buffer, EMPTY_SHORT_LC + m_cachPtr, 3U);
+
+  bool at  = true;
+  bool tc  = slotIndex == 1U;
+  bool ls0 = true;            // For 1 and 2
+  bool ls1 = true;
+
+  if (m_cachPtr == 0U)          // For 0
+    ls1 = false;
+  else if (m_cachPtr == 9U)     // For 3
+    ls0 = false;
+
+  bool h0 = at ^ tc ^ ls1;
+  bool h1 = tc ^ ls1 ^ ls0;
+  bool h2 = at ^ tc       ^ ls0;
+
+  buffer[0U] |= at ? 0x80U : 0x00U;
+  buffer[0U] |= tc ? 0x08U : 0x00U;
+  buffer[1U] |= ls1 ? 0x80U : 0x00U;
+  buffer[1U] |= ls0 ? 0x08U : 0x00U;
+  buffer[1U] |= h0 ? 0x02U : 0x00U;
+  buffer[2U] |= h1 ? 0x20U : 0x00U;
+  buffer[2U] |= h2 ? 0x02U : 0x00U;
+
+  m_cachPtr += 3U;
+}

DMRDMOTX.h

@@ -41,11 +41,13 @@ private:
   CSerialRB            m_fifo;
   arm_fir_instance_q15 m_modFilter;
   q15_t                m_modState[70U];    // NoTaps + BlockSize - 1, 42 + 20 - 1 plus some spare
-  uint8_t              m_poBuffer[1200U];
+  uint8_t              m_poBuffer[800U];
   uint16_t             m_poLen;
   uint16_t             m_poPtr;
   uint32_t             m_txDelay;
+  uint8_t              m_cachPtr;
 
+  void createCACH(uint8_t* buffer, uint8_t slotIndex);
   void writeByte(uint8_t c);
 };