Add the P25 transmitter.

Globals.h

@@ -55,6 +55,7 @@ enum MMDVM_STATE {
 #include "YSFRX.h"
 #include "YSFTX.h"
 #include "P25RX.h"
+#include "P25TX.h"
 #include "CalDStarRX.h"
 #include "CalDStarTX.h"
 #include "CalDMR.h"
@@ -102,6 +103,7 @@ extern CYSFRX ysfRX;
 extern CYSFTX ysfTX;
 
 extern CP25RX p25RX;
+extern CP25TX p25TX;
 
 extern CCalDStarRX calDStarRX;
 extern CCalDStarTX calDStarTX;

IO.cpp

@@ -129,6 +129,7 @@ m_rxLevel(128 * 128),
 m_dstarTXLevel(128 * 128),
 m_dmrTXLevel(128 * 128),
 m_ysfTXLevel(128 * 128),
+m_p25TXLevel(128 * 128),
 m_ledCount(0U),
 m_ledValue(true),
 m_dcd(false),
@@ -439,6 +440,9 @@ void CIO::write(MMDVM_STATE mode, q15_t* samples, uint16_t length, const uint8_t
     case STATE_YSF:
       txLevel = m_ysfTXLevel;
       break;
+    case STATE_P25:
+      txLevel = m_p25TXLevel;
+      break;
     default:
       txLevel = m_dstarTXLevel;
       break;
@@ -532,7 +536,7 @@ switch (m_modemState) {
 #endif
 }
 
-void CIO::setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rxLevel, uint8_t dstarTXLevel, uint8_t dmrTXLevel, uint8_t ysfTXLevel)
+void CIO::setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rxLevel, uint8_t dstarTXLevel, uint8_t dmrTXLevel, uint8_t ysfTXLevel, uint8_t p25TXLevel)
 {
   m_pttInvert = pttInvert;
 
@@ -540,6 +544,7 @@ void CIO::setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rx
   m_dstarTXLevel = q15_t(dstarTXLevel * 128);
   m_dmrTXLevel   = q15_t(dmrTXLevel * 128);
   m_ysfTXLevel   = q15_t(ysfTXLevel * 128);
+  m_p25TXLevel   = q15_t(p25TXLevel * 128);
   
   if (rxInvert)
     m_rxLevel = -m_rxLevel;
@@ -548,6 +553,7 @@ void CIO::setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rx
     m_dstarTXLevel = -m_dstarTXLevel;
     m_dmrTXLevel   = -m_dmrTXLevel;
     m_ysfTXLevel   = -m_ysfTXLevel;
+    m_p25TXLevel   = -m_p25TXLevel;
   }
 }
 

IO.h

@@ -41,7 +41,7 @@ public:
   
   void interrupt();
 
-  void setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rxLevel, uint8_t dstarTXLevel, uint8_t dmrTXLevel, uint8_t ysfTXLevel);
+  void setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rxLevel, uint8_t dstarTXLevel, uint8_t dmrTXLevel, uint8_t ysfTXLevel, uint8_t p25TXLevel);
 
   void getOverflow(bool& adcOverflow, bool& dacOverflow);
 
@@ -84,6 +84,7 @@ private:
   q15_t                m_dstarTXLevel;
   q15_t                m_dmrTXLevel;
   q15_t                m_ysfTXLevel;
+  q15_t                m_p25TXLevel;
 
   uint32_t             m_ledCount;
   bool                 m_ledValue;

MMDVM.cpp

@@ -52,6 +52,7 @@ CYSFRX     ysfRX;
 CYSFTX     ysfTX;
 
 CP25RX     p25RX;
+CP25TX     p25TX;
 
 CCalDStarRX calDStarRX;
 CCalDStarTX calDStarTX;
@@ -87,6 +88,9 @@ void loop()
   if (m_ysfEnable && m_modemState == STATE_YSF)
     ysfTX.process();
 
+  if (m_p25Enable && m_modemState == STATE_P25)
+    p25TX.process();
+
   if (m_modemState == STATE_DSTARCAL)
     calDStarTX.process();
 

P25TX.cpp

@@ -0,0 +1,180 @@
+/*
+ *   Copyright (C) 2016 by Jonathan Naylor G4KLX
+ *
+ *   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 2 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, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+// #define WANT_DEBUG
+
+#include "Config.h"
+#include "Globals.h"
+#include "P25TX.h"
+
+#include "P25Defines.h"
+
+#if defined(WIDE_C4FSK_FILTERS_TX)
+// Generated using rcosdesign(0.2, 4, 5, 'sqrt') in MATLAB
+static q15_t P25_C4FSK_FILTER[] = {688, -680, -2158, -3060, -2724, -775, 2684, 7041, 11310, 14425, 15565, 14425,
+                                   11310, 7041, 2684, -775, -2724, -3060, -2158, -680, 688, 0};
+const uint16_t P25_C4FSK_FILTER_LEN = 22U;
+#else
+// Generated using rcosdesign(0.2, 8, 5, 'sqrt') in MATLAB
+static q15_t P25_C4FSK_FILTER[] = {401, 104, -340, -731, -847, -553, 112, 909, 1472, 1450, 683, -675, -2144, -3040, -2706, -770, 2667, 6995,
+                                   11237, 14331, 15464, 14331, 11237, 6995, 2667, -770, -2706, -3040, -2144, -675, 683, 1450, 1472, 909, 112,
+                                   -553, -847, -731, -340, 104, 401, 0};
+const uint16_t P25_C4FSK_FILTER_LEN = 42U;
+#endif
+
+// XXX These will need setting correctly
+const q15_t P25_LEVELA[] = { 809,  809,  809,  809,  809};
+const q15_t P25_LEVELB[] = { 269,  269,  269,  269,  269};
+const q15_t P25_LEVELC[] = {-269, -269, -269, -269, -269};
+const q15_t P25_LEVELD[] = {-809, -809, -809, -809, -809};
+
+const uint8_t P25_START_SYNC = 0x77U;
+
+CP25TX::CP25TX() :
+m_buffer(1500U),
+m_modFilter(),
+m_modState(),
+m_poBuffer(),
+m_poLen(0U),
+m_poPtr(0U),
+m_txDelay(240U),      // 200ms
+m_count(0U)
+{
+  ::memset(m_modState, 0x00U, 70U * sizeof(q15_t));
+
+  m_modFilter.numTaps = P25_C4FSK_FILTER_LEN;
+  m_modFilter.pState  = m_modState;
+  m_modFilter.pCoeffs = P25_C4FSK_FILTER;
+}
+
+void CP25TX::process()
+{
+  if (m_buffer.getData() == 0U && m_poLen == 0U)
+    return;
+
+  if (m_poLen == 0U) {
+    if (!m_tx) {
+      m_count = 0U;
+
+      for (uint16_t i = 0U; i < m_txDelay; i++)
+        m_poBuffer[m_poLen++] = P25_START_SYNC;
+    } else {
+      uint8_t length = m_buffer.get();
+      for (uint8_t i = 0U; i < length; i++) {
+        uint8_t c = m_buffer.get();
+        m_poBuffer[m_poLen++] = c;
+      }
+    }
+
+    m_poPtr = 0U;
+  }
+
+  if (m_poLen > 0U) {
+    uint16_t space = io.getSpace();
+    
+    while (space > (4U * P25_RADIO_SYMBOL_LENGTH)) {
+      uint8_t c = m_poBuffer[m_poPtr++];
+      writeByte(c);
+
+      space -= 4U * P25_RADIO_SYMBOL_LENGTH;
+      
+      if (m_poPtr >= m_poLen) {
+        m_poPtr = 0U;
+        m_poLen = 0U;
+        return;
+      }
+    }
+  }
+}
+
+uint8_t CP25TX::writeData(const uint8_t* data, uint8_t length)
+{
+  if (length < (P25_HDR_FRAME_LENGTH_BYTES + 1U))
+    return 4U;
+
+  uint16_t space = m_buffer.getSpace();
+  if (space < length)
+    return 5U;
+
+  m_buffer.put(length - 1U);
+  for (uint8_t i = 0U; i < (length - 1U); i++)
+    m_buffer.put(data[i + 1U]);
+
+  return 0U;
+}
+
+void CP25TX::writeByte(uint8_t c)
+{
+  q15_t inBuffer[P25_RADIO_SYMBOL_LENGTH * 4U + 1U];
+  q15_t outBuffer[P25_RADIO_SYMBOL_LENGTH * 4U + 1U];
+
+  const uint8_t MASK = 0xC0U;
+
+  q15_t* p = inBuffer;
+  for (uint8_t i = 0U; i < 4U; i++, c <<= 2, p += P25_RADIO_SYMBOL_LENGTH) {
+    switch (c & MASK) {
+      case 0xC0U:
+        ::memcpy(p, P25_LEVELA, P25_RADIO_SYMBOL_LENGTH * sizeof(q15_t));
+        break;
+      case 0x80U:
+        ::memcpy(p, P25_LEVELB, P25_RADIO_SYMBOL_LENGTH * sizeof(q15_t));
+        break;
+      case 0x00U:
+        ::memcpy(p, P25_LEVELC, P25_RADIO_SYMBOL_LENGTH * sizeof(q15_t));
+        break;
+      default:
+        ::memcpy(p, P25_LEVELD, P25_RADIO_SYMBOL_LENGTH * sizeof(q15_t));
+        break;
+    }
+  }
+
+  uint16_t blockSize = P25_RADIO_SYMBOL_LENGTH * 4U;
+
+  // Handle the case of the oscillator not being accurate enough
+  if (m_sampleCount > 0U) {
+    m_count += P25_RADIO_SYMBOL_LENGTH * 4U;
+
+    if (m_count >= m_sampleCount) {
+      if (m_sampleInsert) {
+        inBuffer[P25_RADIO_SYMBOL_LENGTH * 4U] = inBuffer[P25_RADIO_SYMBOL_LENGTH * 4U - 1U];
+        blockSize++;
+      } else {
+        blockSize--;
+      }
+
+      m_count -= m_sampleCount;
+    }
+  }
+
+  ::arm_fir_fast_q15(&m_modFilter, inBuffer, outBuffer, blockSize);
+
+  // Inverse sinc lowpass filter needed here
+
+  io.write(STATE_P25, outBuffer, blockSize);
+}
+
+void CP25TX::setTXDelay(uint8_t delay)
+{
+  m_txDelay = 600U + uint16_t(delay) * 12U;        // 500ms + tx delay
+}
+
+uint16_t CP25TX::getSpace() const
+{
+  return m_buffer.getSpace() / P25_LDU_FRAME_LENGTH_BYTES;
+}
+

P25TX.h

@@ -0,0 +1,52 @@
+/*
+ *   Copyright (C) 2016 by Jonathan Naylor G4KLX
+ *
+ *   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 2 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, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#if !defined(P25TX_H)
+#define  P25TX_H
+
+#include "Config.h"
+
+#include "SerialRB.h"
+
+class CP25TX {
+public:
+  CP25TX();
+
+  uint8_t writeData(const uint8_t* data, uint8_t length);
+
+  void process();
+
+  void setTXDelay(uint8_t delay);
+
+  uint16_t getSpace() const;
+
+private:
+  CSerialRB            m_buffer;
+  arm_fir_instance_q15 m_modFilter;
+  q15_t                m_modState[70U];    // NoTaps + BlockSize - 1, 42 + 20 - 1 plus some spare
+  uint8_t              m_poBuffer[920U];
+  uint16_t             m_poLen;
+  uint16_t             m_poPtr;
+  uint16_t             m_txDelay;
+  uint32_t             m_count;
+
+  void writeByte(uint8_t c);
+};
+
+#endif
+

SerialPort.cpp

@@ -176,7 +176,10 @@ void CSerialPort::getStatus()
   else
     reply[9U] = 0U;
 
-  reply[10U] = 0U;  // P25
+  if (m_p25Enable)
+    reply[10U] = p25TX.getSpace();
+  else
+    reply[10U] = 0U;
 
   write(reply, 11);
 }
@@ -202,7 +205,7 @@ void CSerialPort::getVersion()
 
 uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length)
 {
-  if (length < 12U)
+  if (length < 13U)
     return 4U;
 
   bool rxInvert  = (data[0U] & 0x01U) == 0x01U;
@@ -255,6 +258,7 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length)
   uint8_t dstarTXLevel = data[9U];
   uint8_t dmrTXLevel   = data[10U];
   uint8_t ysfTXLevel   = data[11U];
+  uint8_t p25TXLevel   = data[12U];
 
   m_modemState  = modemState;
 
@@ -266,6 +270,7 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length)
 
   dstarTX.setTXDelay(txDelay);
   ysfTX.setTXDelay(txDelay);
+  p25TX.setTXDelay(txDelay);
   dmrDMOTX.setTXDelay(txDelay);
 
   dmrTX.setColorCode(colorCode);
@@ -274,7 +279,7 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length)
   dmrDMORX.setColorCode(colorCode);
   dmrIdleRX.setColorCode(colorCode);
 
-  io.setParameters(rxInvert, txInvert, pttInvert, rxLevel, dstarTXLevel, dmrTXLevel, ysfTXLevel);
+  io.setParameters(rxInvert, txInvert, pttInvert, rxLevel, dstarTXLevel, dmrTXLevel, ysfTXLevel, p25TXLevel);
 
   io.start();
 
@@ -314,6 +319,7 @@ void CSerialPort::setMode(MMDVM_STATE modemState)
       DEBUG1("Mode set to DMR");
       dstarRX.reset();
       ysfRX.reset();
+      p25RX.reset();
       cwIdTX.reset();
       break;
     case STATE_DSTAR:
@@ -322,6 +328,7 @@ void CSerialPort::setMode(MMDVM_STATE modemState)
       dmrDMORX.reset();
       dmrRX.reset();
       ysfRX.reset();
+      p25RX.reset();
       cwIdTX.reset();
       break;
     case STATE_YSF:
@@ -330,6 +337,7 @@ void CSerialPort::setMode(MMDVM_STATE modemState)
       dmrDMORX.reset();
       dmrRX.reset();
       dstarRX.reset();
+      p25RX.reset();
       cwIdTX.reset();
       break;
     case STATE_P25:
@@ -348,6 +356,7 @@ void CSerialPort::setMode(MMDVM_STATE modemState)
       dmrRX.reset();
       dstarRX.reset();
       ysfRX.reset();
+      p25RX.reset();
       cwIdTX.reset();
       break;
     case STATE_DMRCAL:
@@ -357,6 +366,7 @@ void CSerialPort::setMode(MMDVM_STATE modemState)
       dmrRX.reset();
       dstarRX.reset();
       ysfRX.reset();
+      p25RX.reset();
       cwIdTX.reset();
       break;
     default:
@@ -403,7 +413,7 @@ void CSerialPort::process()
       if (m_ptr == 2U)
         m_len = m_buffer[1U];
 
-      if (m_ptr == 3U && m_len > 130U) {
+      if (m_ptr == 3U && m_len > 230U) {
         sendNAK(3U);
         m_ptr = 0U;
         m_len = 0U;
@@ -594,6 +604,34 @@ void CSerialPort::process()
             }
             break;
 
+          case MMDVM_P25_HDR:
+            if (m_p25Enable) {
+              if (m_modemState == STATE_IDLE || m_modemState == STATE_P25)
+                err = p25TX.writeData(m_buffer + 3U, m_len - 3U);
+            }
+            if (err == 0U) {
+              if (m_modemState == STATE_IDLE)
+                setMode(STATE_P25);
+            } else {
+              DEBUG2("Received invalid P25 header", err);
+              sendNAK(err);
+            }
+            break;
+
+          case MMDVM_P25_LDU:
+            if (m_p25Enable) {
+              if (m_modemState == STATE_IDLE || m_modemState == STATE_P25)
+                err = p25TX.writeData(m_buffer + 3U, m_len - 3U);
+            }
+            if (err == 0U) {
+              if (m_modemState == STATE_IDLE)
+                setMode(STATE_P25);
+            } else {
+              DEBUG2("Received invalid P25 LDU", err);
+              sendNAK(err);
+            }
+            break;
+
           default:
             // Handle this, send a NAK back
             sendNAK(1U);