Implement fixed-point symbols, FFT and file output

1 files changed, 162 insertions, 101 deletions

diff --git a/src/QpskSymbolMapper.cpp b/src/QpskSymbolMapper.cpp
index c9b01fb..c12ad80 100644
--- a/src/QpskSymbolMapper.cpp
+++ b/src/QpskSymbolMapper.cpp
@@ -31,9 +31,10 @@
 #include "QpskSymbolMapper.h"
 #include "PcDebug.h"
 
-QpskSymbolMapper::QpskSymbolMapper(size_t carriers) :
+QpskSymbolMapper::QpskSymbolMapper(size_t carriers, bool fixedPoint) :
     ModCodec(),
-    d_carriers(carriers) { }
+    m_fixedPoint(fixedPoint),
+    m_carriers(carriers) { }
 
 int QpskSymbolMapper::process(Buffer* const dataIn, Buffer* dataOut)
 {
@@ -41,112 +42,172 @@ int QpskSymbolMapper::process(Buffer* const dataIn, Buffer* dataOut)
             "(dataIn: %p, dataOut: %p)\n",
             dataIn, dataOut);
 
-    dataOut->setLength(dataIn->getLength() * 4 * 2 * sizeof(float));   // 4 output complex symbols per input byte
-#ifdef __SSE__
-    const uint8_t* in = reinterpret_cast<const uint8_t*>(dataIn->getData());
-    __m128* out = reinterpret_cast<__m128*>(dataOut->getData());
-
-    if (dataIn->getLength() % (d_carriers / 4) != 0) {
-        throw std::runtime_error(
-                "QpskSymbolMapper::process input size not valid: " +
-                std::to_string(dataIn->getLength()) +
-                "(input size) % (" + std::to_string(d_carriers) +
-                " (carriers) / 4) != 0");
-    }
+    // 4 output complex symbols per input byte
+
+    if (m_fixedPoint) {
+        dataOut->setLength(dataIn->getLength() * 4 * sizeof(complexfix));
+
+        using fixed_t = complexfix::value_type;
+
+        const uint8_t* in = reinterpret_cast<const uint8_t*>(dataIn->getData());
+        fixed_t* out = reinterpret_cast<fixed_t*>(dataOut->getData());
 
-    const static __m128 symbols[16] = {
-        _mm_setr_ps( M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2),
-        _mm_setr_ps( M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2),
-        _mm_setr_ps( M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2),
-        _mm_setr_ps( M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2),
-        _mm_setr_ps( M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2),
-        _mm_setr_ps( M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2),
-        _mm_setr_ps( M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2),
-        _mm_setr_ps( M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2),
-        _mm_setr_ps(-M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2),
-        _mm_setr_ps(-M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2),
-        _mm_setr_ps(-M_SQRT1_2,- M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2),
-        _mm_setr_ps(-M_SQRT1_2,- M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2),
-        _mm_setr_ps(-M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2),
-        _mm_setr_ps(-M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2),
-        _mm_setr_ps(-M_SQRT1_2,- M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2),
-        _mm_setr_ps(-M_SQRT1_2,- M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2)
-    };
-    size_t inOffset = 0;
-    size_t outOffset = 0;
-    uint8_t tmp = 0;
-    for (size_t i = 0; i < dataIn->getLength(); i += d_carriers / 4) {
-        for (size_t j = 0; j < d_carriers / 8; ++j) {
-            tmp =  (in[inOffset] & 0xc0) >> 4;
-            tmp |= (in[inOffset + (d_carriers / 8)] & 0xc0) >> 6;
-            out[outOffset] = symbols[tmp];
-            tmp =  (in[inOffset] & 0x30) >> 2;
-            tmp |= (in[inOffset + (d_carriers / 8)] & 0x30) >> 4;
-            out[outOffset + 1] = symbols[tmp];
-            tmp =  (in[inOffset] & 0x0c);
-            tmp |= (in[inOffset + (d_carriers / 8)] & 0x0c) >> 2;
-            out[outOffset + 2] = symbols[tmp];
-            tmp =  (in[inOffset] & 0x03) << 2;
-            tmp |= (in[inOffset + (d_carriers / 8)] & 0x03);
-            out[outOffset + 3] = symbols[tmp];
-            ++inOffset;
-            outOffset += 4;
+        if (dataIn->getLength() % (m_carriers / 4) != 0) {
+            throw std::runtime_error(
+                    "QpskSymbolMapper::process input size not valid!");
+        }
+
+        constexpr fixed_t v = static_cast<fixed_t>(M_SQRT1_2);
+
+        const static fixed_t symbols[16][4] = {
+            { v,  v,  v,  v},
+            { v,  v,  v, -v},
+            { v, -v,  v,  v},
+            { v, -v,  v, -v},
+            { v,  v, -v,  v},
+            { v,  v, -v, -v},
+            { v, -v, -v,  v},
+            { v, -v, -v, -v},
+            {-v,  v,  v,  v},
+            {-v,  v,  v, -v},
+            {-v, -v,  v,  v},
+            {-v, -v,  v, -v},
+            {-v,  v, -v,  v},
+            {-v,  v, -v, -v},
+            {-v, -v, -v,  v},
+            {-v, -v, -v, -v}
+        };
+        size_t inOffset = 0;
+        size_t outOffset = 0;
+        uint8_t tmp;
+        for (size_t i = 0; i < dataIn->getLength(); i += m_carriers / 4) {
+            for (size_t j = 0; j < m_carriers / 8; ++j) {
+                tmp =  (in[inOffset] & 0xc0) >> 4;
+                tmp |= (in[inOffset + (m_carriers / 8)] & 0xc0) >> 6;
+                memcpy(&out[outOffset], symbols[tmp], sizeof(fixed_t) * 4);
+                tmp =  (in[inOffset] & 0x30) >> 2;
+                tmp |= (in[inOffset + (m_carriers / 8)] & 0x30) >> 4;
+                memcpy(&out[outOffset + 4], symbols[tmp], sizeof(fixed_t) * 4);
+                tmp =  (in[inOffset] & 0x0c);
+                tmp |= (in[inOffset + (m_carriers / 8)] & 0x0c) >> 2;
+                memcpy(&out[outOffset + 8], symbols[tmp], sizeof(fixed_t) * 4);
+                tmp =  (in[inOffset] & 0x03) << 2;
+                tmp |= (in[inOffset + (m_carriers / 8)] & 0x03);
+                memcpy(&out[outOffset + 12], symbols[tmp], sizeof(fixed_t) * 4);
+                ++inOffset;
+                outOffset += 4*4;
+            }
+            inOffset += m_carriers / 8;
         }
-        inOffset += d_carriers / 8;
     }
+    else {
+        dataOut->setLength(dataIn->getLength() * 4 * sizeof(complexf));
+#ifdef __SSE__
+        const uint8_t* in = reinterpret_cast<const uint8_t*>(dataIn->getData());
+        __m128* out = reinterpret_cast<__m128*>(dataOut->getData());
+
+        if (dataIn->getLength() % (m_carriers / 4) != 0) {
+            throw std::runtime_error(
+                    "QpskSymbolMapper::process input size not valid: " +
+                    std::to_string(dataIn->getLength()) +
+                    "(input size) % (" + std::to_string(m_carriers) +
+                    " (carriers) / 4) != 0");
+        }
+
+        const static __m128 symbols[16] = {
+            _mm_setr_ps( M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2),
+            _mm_setr_ps( M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2),
+            _mm_setr_ps( M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2),
+            _mm_setr_ps( M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2),
+            _mm_setr_ps( M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2),
+            _mm_setr_ps( M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2),
+            _mm_setr_ps( M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2),
+            _mm_setr_ps( M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2),
+            _mm_setr_ps(-M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2),
+            _mm_setr_ps(-M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2),
+            _mm_setr_ps(-M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2),
+            _mm_setr_ps(-M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2),
+            _mm_setr_ps(-M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2),
+            _mm_setr_ps(-M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2),
+            _mm_setr_ps(-M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2),
+            _mm_setr_ps(-M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2)
+        };
+        size_t inOffset = 0;
+        size_t outOffset = 0;
+        uint8_t tmp = 0;
+        for (size_t i = 0; i < dataIn->getLength(); i += m_carriers / 4) {
+            for (size_t j = 0; j < m_carriers / 8; ++j) {
+                tmp =  (in[inOffset] & 0xc0) >> 4;
+                tmp |= (in[inOffset + (m_carriers / 8)] & 0xc0) >> 6;
+                out[outOffset] = symbols[tmp];
+                tmp =  (in[inOffset] & 0x30) >> 2;
+                tmp |= (in[inOffset + (m_carriers / 8)] & 0x30) >> 4;
+                out[outOffset + 1] = symbols[tmp];
+                tmp =  (in[inOffset] & 0x0c);
+                tmp |= (in[inOffset + (m_carriers / 8)] & 0x0c) >> 2;
+                out[outOffset + 2] = symbols[tmp];
+                tmp =  (in[inOffset] & 0x03) << 2;
+                tmp |= (in[inOffset + (m_carriers / 8)] & 0x03);
+                out[outOffset + 3] = symbols[tmp];
+                ++inOffset;
+                outOffset += 4;
+            }
+            inOffset += m_carriers / 8;
+        }
 #else // !__SSE__
-    const uint8_t* in = reinterpret_cast<const uint8_t*>(dataIn->getData());
-    float* out = reinterpret_cast<float*>(dataOut->getData());
-    if (dataIn->getLength() % (d_carriers / 4) != 0) {
-        throw std::runtime_error(
-                "QpskSymbolMapper::process input size not valid!");
-    }
-    if (dataOut->getLength() / sizeof(float) != dataIn->getLength() * 4 * 2) {    // 4 output complex symbols per input byte
-        throw std::runtime_error(
-                "QpskSymbolMapper::process output size not valid!");
-    }
+        const uint8_t* in = reinterpret_cast<const uint8_t*>(dataIn->getData());
+        float* out = reinterpret_cast<float*>(dataOut->getData());
+        if (dataIn->getLength() % (m_carriers / 4) != 0) {
+            throw std::runtime_error(
+                    "QpskSymbolMapper::process input size not valid!");
+        }
+        if (dataOut->getLength() / sizeof(float) != dataIn->getLength() * 4 * 2) {    // 4 output complex symbols per input byte
+            throw std::runtime_error(
+                    "QpskSymbolMapper::process output size not valid!");
+        }
 
-    const static float symbols[16][4] = {
-        { M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2},
-        { M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2},
-        { M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2},
-        { M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2},
-        { M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2},
-        { M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2},
-        { M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2},
-        { M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2},
-        {-M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2},
-        {-M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2},
-        {-M_SQRT1_2,- M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2},
-        {-M_SQRT1_2,- M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2},
-        {-M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2},
-        {-M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2},
-        {-M_SQRT1_2,- M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2},
-        {-M_SQRT1_2,- M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2}
-    };
-    size_t inOffset = 0;
-    size_t outOffset = 0;
-    uint8_t tmp;
-    for (size_t i = 0; i < dataIn->getLength(); i += d_carriers / 4) {
-        for (size_t j = 0; j < d_carriers / 8; ++j) {
-            tmp =  (in[inOffset] & 0xc0) >> 4;
-            tmp |= (in[inOffset + (d_carriers / 8)] & 0xc0) >> 6;
-            memcpy(&out[outOffset], symbols[tmp], sizeof(float) * 4);
-            tmp =  (in[inOffset] & 0x30) >> 2;
-            tmp |= (in[inOffset + (d_carriers / 8)] & 0x30) >> 4;
-            memcpy(&out[outOffset + 4], symbols[tmp], sizeof(float) * 4);
-            tmp =  (in[inOffset] & 0x0c);
-            tmp |= (in[inOffset + (d_carriers / 8)] & 0x0c) >> 2;
-            memcpy(&out[outOffset + 8], symbols[tmp], sizeof(float) * 4);
-            tmp =  (in[inOffset] & 0x03) << 2;
-            tmp |= (in[inOffset + (d_carriers / 8)] & 0x03);
-            memcpy(&out[outOffset + 12], symbols[tmp], sizeof(float) * 4);
-            ++inOffset;
-            outOffset += 4*4;
+        const static float symbols[16][4] = {
+            { M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2},
+            { M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2},
+            { M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2},
+            { M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2},
+            { M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2},
+            { M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2},
+            { M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2},
+            { M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2},
+            {-M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2},
+            {-M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2},
+            {-M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2,  M_SQRT1_2},
+            {-M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2},
+            {-M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2},
+            {-M_SQRT1_2,  M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2},
+            {-M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2,  M_SQRT1_2},
+            {-M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2, -M_SQRT1_2}
+        };
+        size_t inOffset = 0;
+        size_t outOffset = 0;
+        uint8_t tmp;
+        for (size_t i = 0; i < dataIn->getLength(); i += m_carriers / 4) {
+            for (size_t j = 0; j < m_carriers / 8; ++j) {
+                tmp =  (in[inOffset] & 0xc0) >> 4;
+                tmp |= (in[inOffset + (m_carriers / 8)] & 0xc0) >> 6;
+                memcpy(&out[outOffset], symbols[tmp], sizeof(float) * 4);
+                tmp =  (in[inOffset] & 0x30) >> 2;
+                tmp |= (in[inOffset + (m_carriers / 8)] & 0x30) >> 4;
+                memcpy(&out[outOffset + 4], symbols[tmp], sizeof(float) * 4);
+                tmp =  (in[inOffset] & 0x0c);
+                tmp |= (in[inOffset + (m_carriers / 8)] & 0x0c) >> 2;
+                memcpy(&out[outOffset + 8], symbols[tmp], sizeof(float) * 4);
+                tmp =  (in[inOffset] & 0x03) << 2;
+                tmp |= (in[inOffset + (m_carriers / 8)] & 0x03);
+                memcpy(&out[outOffset + 12], symbols[tmp], sizeof(float) * 4);
+                ++inOffset;
+                outOffset += 4*4;
+            }
+            inOffset += m_carriers / 8;
         }
-        inOffset += d_carriers / 8;
-    }
 #endif // __SSE__
+    }
 
     return 1;
 }