Merge raspine's ZeroMQ RC and UHD staticdelay

Merge raspine/master pull request, fix indentation, code style, and two
minor conflicts in:
        doc/example.ini
        src/DabMod.cpp

21 files changed, 757 insertions, 249 deletions

diff --git a/src/Buffer.cpp b/src/Buffer.cpp
index 7cbbe57..aa0ef4c 100644
--- a/src/Buffer.cpp
+++ b/src/Buffer.cpp
@@ -26,7 +26,7 @@
 #include <stdlib.h>
 #include <string.h>
 #include <malloc.h>
-#ifdef HAVE_DECL__MM_MALLOC
+#if HAVE_DECL__MM_MALLOC
 #   include <mm_malloc.h>
 #else
 #   define memalign(a, b)   malloc(b)
@@ -73,8 +73,10 @@ void Buffer::setLength(size_t len)
         /* Align to 32-byte boundary for AVX. */
         data = memalign(32, len);
 
-        memcpy(data, tmp, this->len);
-        free(tmp);
+        if (tmp != NULL) {
+            memcpy(data, tmp, this->len);
+            free(tmp);
+        }
         size = len;
     }
     this->len = len;
@@ -97,14 +99,3 @@ void Buffer::appendData(const void *data, size_t len)
     }
 }
 
-
-size_t Buffer::getLength()
-{
-    return len;
-}
-
-
-void *Buffer::getData()
-{
-    return data;
-}
diff --git a/src/Buffer.h b/src/Buffer.h
index b1a5d93..4bb157b 100644
--- a/src/Buffer.h
+++ b/src/Buffer.h
@@ -29,28 +29,47 @@
 
 #include <unistd.h>
 
-
+/* Buffer is a container for a byte array, that is memcpy'ed
+ * on assignment and by the copy-constructor.
+ *
+ * The allocation/freeing of the data is handled internally.
+ */
 class Buffer {
-protected:
-    size_t len;
-    size_t size;
-    void *data;
+    protected:
+        /* Current length of the data in the Buffer */
+        size_t len;
 
-public:
-    Buffer(const Buffer& copy);
-    Buffer(size_t len = 0, const void *data = NULL);
-    ~Buffer();
+        /* Allocated size of the Buffer */
+        size_t size;
 
-    Buffer &operator=(const Buffer &copy);
-    Buffer &operator+=(const Buffer &copy);
+        /* Pointer to the data. Memory allocation is entirely
+         * handled by setLength.
+         */
+        void *data;
 
-    void setLength(size_t len);
-    void setData(const void *data, size_t len);
-    void appendData(const void *data, size_t len);
+    public:
+        Buffer(const Buffer& copy);
+        Buffer(size_t len = 0, const void *data = NULL);
+        ~Buffer();
 
-    size_t getLength();
-    void *getData();
-};
+        /* Resize the buffer, reallocate memory if needed */
+        void setLength(size_t len);
+
+        /* Replace the data in the Buffer by the new data given.
+         * Reallocates memory if needed.
+         */
+        void setData(const void *data, size_t len);
+        Buffer &operator=(const Buffer &copy);
 
+        /* Concatenate the current data with the new data given.
+         * Reallocates memory if needed.
+         */
+        void appendData(const void *data, size_t len);
+        Buffer &operator+=(const Buffer &copy);
+
+        size_t getLength() const { return len; }
+        void *getData() const { return data; }
+};
 
 #endif // BUFFER_H
+
diff --git a/src/DabMod.cpp b/src/DabMod.cpp
index ea6334f..4912bee 100644
--- a/src/DabMod.cpp
+++ b/src/DabMod.cpp
@@ -38,6 +38,7 @@
 #if defined(HAVE_OUTPUT_UHD)
 #   include "OutputUHD.h"
 #endif
+#include "OutputZeroMQ.h"
 #include "InputReader.h"
 #include "PcDebug.h"
 #include "TimestampDecoder.h"
@@ -56,11 +57,11 @@
 #include <stdexcept>
 #include <signal.h>
 
-#ifdef HAVE_NETINET_IN_H
+#if HAVE_NETINET_IN_H
 #   include <netinet/in.h>
 #endif
 
-#ifdef HAVE_DECL__MM_MALLOC
+#if HAVE_DECL__MM_MALLOC
 #   include <mm_malloc.h>
 #else
 #   define memalign(a, b)   malloc(b)
@@ -91,11 +92,11 @@ void printUsage(char* progName, FILE* out = stderr)
 #endif
             __DATE__, __TIME__);
     fprintf(out, "Usage with configuration file:\n");
-    fprintf(out, "\t%s -C config_file.ini\n\n", progName);
+    fprintf(out, "\t%s [-C] config_file.ini\n\n", progName);
 
     fprintf(out, "Usage with command line options:\n");
     fprintf(out, "\t%s"
-            " [input]"
+            " input"
             " (-f filename | -u uhddevice -F frequency) "
             " [-G txgain]"
             " [-o offset]"
@@ -137,8 +138,13 @@ void printVersion(FILE *out = stderr)
 {
     fprintf(out, "Welcome to %s %s, compiled at %s, %s\n\n",
             PACKAGE, VERSION, __DATE__, __TIME__);
-    fprintf(out, "ODR-DabMod is copyright (C) Her Majesty the Queen in Right of Canada,\n"
-            "    2009, 2010, 2011, 2012 Communications Research Centre (CRC).\n"
+    fprintf(out,
+            "    ODR-DabMod is copyright (C) Her Majesty the Queen in Right of Canada,\n"
+            "    2009, 2010, 2011, 2012 Communications Research Centre (CRC),\n"
+            "     and\n"
+            "    Copyright (C) 2014 Matthias P. Braendli, matthias.braendli@mpb.li\n"
+            "\n"
+            "    http://opendigitalradio.org\n"
             "\n"
             "    This program is available free of charge and is licensed to you on a\n"
             "    non-exclusive basis; you may not redistribute it.\n"
@@ -151,8 +157,10 @@ void printVersion(FILE *out = stderr)
             "    In no event shall CRC be LIABLE for any LOSS, DAMAGE or COST that may be\n"
             "    incurred in connection with the use of this software.\n"
             "\n"
+#if USE_KISS_FFT
             "ODR-DabMod makes use of the following open source packages:\n"
             "    Kiss FFT v1.2.9 (Revised BSD) - http://kissfft.sourceforge.net/\n"
+#endif
            );
 
 }
@@ -166,6 +174,7 @@ int main(int argc, char* argv[])
     std::string inputTransport = "file";
 
     std::string outputName;
+    int useZeroMQOutput = 0;
     int useFileOutput = 0;
     int useUHDOutput = 0;
 
@@ -257,7 +266,7 @@ int main(int argc, char* argv[])
             break;
         case 'G':
 #if defined(HAVE_OUTPUT_UHD)
-            outputuhd_conf.txgain = (int)strtol(optarg, NULL, 10);
+            outputuhd_conf.txgain = strtod(optarg, NULL);
 #endif
             break;
         case 'l':
@@ -330,11 +339,47 @@ int main(int argc, char* argv[])
 #endif
             << std::endl;
 
+    std::cerr << "Using FFT library " <<
+#if defined(USE_FFTW)
+        "FFTW" <<
+#endif
+#if defined(USE_KISS_FFT)
+        "Kiss FFT" <<
+#endif
+#if defined(USE_SIMD)
+        " (with fft_simd)" <<
+#endif
+        "\n";
+
+    std::cerr << "Compiled with features: " <<
+#if defined(HAVE_INPUT_ZEROMQ)
+        "input_zeromq " <<
+#endif
+#if defined(HAVE_OUTPUT_UHD)
+        "output_uhd " <<
+#endif
+#if defined(HAVE_OUTPUT_ZEROMQ)
+        "output_zeromq " <<
+#endif
+        "\n";
+
     if (use_configuration_file && use_configuration_cmdline) {
         fprintf(stderr, "Warning: configuration file and command line parameters are defined:\n\t"
                         "Command line parameters override settings in the configuration file !\n");
     }
 
+    // No argument given ? You can't be serious ! Show usage.
+    if (argc == 1) {
+        printUsage(argv[0]);
+        goto END_MAIN;
+    }
+
+    // If only one argument is given, interpret as configuration file name
+    if (argc == 2) {
+        use_configuration_file = true;
+        configuration_file = argv[1];
+    }
+
     if (use_configuration_file) {
         // First read parameters from the file
         using boost::property_tree::ptree;
@@ -415,7 +460,7 @@ int main(int argc, char* argv[])
         clockRate = pt.get("modulator.dac_clk_rate", (size_t)0);
         digitalgain = pt.get("modulator.digital_gain", digitalgain);
         outputRate = pt.get("modulator.rate", outputRate);
-        
+
         // FIR Filter parameters:
         if (pt.get("firfilter.enabled", 0) == 1) {
             try {
@@ -467,7 +512,7 @@ int main(int argc, char* argv[])
                     "setting type in [uhd] device is deprecated !\n";
             }
 
-            outputuhd_conf.txgain = pt.get("uhdoutput.txgain", 0);
+            outputuhd_conf.txgain = pt.get("uhdoutput.txgain", 0.0);
             outputuhd_conf.frequency = pt.get<double>("uhdoutput.frequency", 0);
             std::string chan = pt.get<std::string>("uhdoutput.channel", "");
 
@@ -547,6 +592,12 @@ int main(int argc, char* argv[])
             useUHDOutput = 1;
         }
 #endif
+#if defined(HAVE_OUTPUT_ZEROMQ)
+        else if (output_selected == "zmq") {
+            outputName = pt.get<std::string>("zmqoutput.listen");
+            useZeroMQOutput = 1;
+        }
+#endif
         else {
             std::cerr << "Error: Invalid output defined.\n";
             goto END_MAIN;
@@ -579,6 +630,7 @@ int main(int argc, char* argv[])
         outputuhd_conf.muteNoTimestamps = (pt.get("delaymanagement.mutenotimestamps", 0) == 1);
 #endif
     }
+
     if (rcs.get_no_controllers() == 0) {
         logger.level(warn) << "No Remote-Control started";
         rcs.add_controller(new RemoteControllerDummy());
@@ -600,7 +652,7 @@ int main(int argc, char* argv[])
     }
 
     // Setting ETI input filename
-    if (inputName == "") {
+    if (use_configuration_cmdline && inputName == "") {
         if (optind < argc) {
             inputName = argv[optind++];
 
@@ -616,7 +668,7 @@ int main(int argc, char* argv[])
     }
 
     // Checking unused arguments
-    if (optind != argc) {
+    if (use_configuration_cmdline && optind != argc) {
         fprintf(stderr, "Invalid arguments:");
         while (optind != argc) {
             fprintf(stderr, " %s", argv[optind++]);
@@ -628,7 +680,7 @@ int main(int argc, char* argv[])
         goto END_MAIN;
     }
 
-    if (!useFileOutput && !useUHDOutput) {
+    if (!useFileOutput && !useUHDOutput && !useZeroMQOutput) {
         logger.level(error) << "Output not specified";
         fprintf(stderr, "Must specify output !");
         goto END_MAIN;
@@ -639,8 +691,12 @@ int main(int argc, char* argv[])
     fprintf(stderr, "  Type: %s\n", inputTransport.c_str());
     fprintf(stderr, "  Source: %s\n", inputName.c_str());
     fprintf(stderr, "Output\n");
+
+    if (useFileOutput) {
+        fprintf(stderr, "  Name: %s\n", outputName.c_str());
+    }
 #if defined(HAVE_OUTPUT_UHD)
-    if (useUHDOutput) {
+    else if (useUHDOutput) {
         fprintf(stderr, " UHD\n"
                         "  Device: %s\n"
                         "  Type: %s\n"
@@ -649,12 +705,13 @@ int main(int argc, char* argv[])
                 outputuhd_conf.usrpType.c_str(),
                 outputuhd_conf.masterClockRate);
     }
-    else if (useFileOutput) {
-#else
-    if (useFileOutput) {
 #endif
-        fprintf(stderr, "  Name: %s\n", outputName.c_str());
+    else if (useZeroMQOutput) {
+        fprintf(stderr, " ZeroMQ\n"
+                        "  Listening on: %s\n",
+                        outputName.c_str());
     }
+
     fprintf(stderr, "  Sampling rate: ");
     if (outputRate > 1000) {
         if (outputRate > 1000000) {
@@ -683,7 +740,13 @@ int main(int argc, char* argv[])
         ret = -1;
         goto END_MAIN;
 #else
-        inputZeroMQReader.Open(inputName.substr(4));
+        // The URL might start with zmq+tcp://
+        if (inputName.substr(0, 4) == "zmq+") {
+            inputZeroMQReader.Open(inputName.substr(4));
+        }
+        else {
+            inputZeroMQReader.Open(inputName);
+        }
         inputReader = &inputZeroMQReader;
 #endif
     }
@@ -723,6 +786,14 @@ int main(int argc, char* argv[])
         }
     }
 #endif
+#if defined(HAVE_OUTPUT_ZEROMQ)
+    else if (useZeroMQOutput) {
+        /* We normalise the same way as for the UHD output */
+        normalise = 1.0f/50000.0f;
+
+        output = new OutputZeroMQ(outputName);
+    }
+#endif
 
     flowgraph = new Flowgraph();
     data.setLength(6144);
diff --git a/src/EtiReader.cpp b/src/EtiReader.cpp
index b61481c..fe54f55 100644
--- a/src/EtiReader.cpp
+++ b/src/EtiReader.cpp
@@ -2,8 +2,10 @@
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011 Her Majesty
    the Queen in Right of Canada (Communications Research Center Canada)
 
-   Includes modifications for which no copyright is claimed
-   2012, Matthias P. Braendli, matthias.braendli@mpb.li
+   Copyright (C) 2014
+   Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
  */
 /*
    This file is part of ODR-DabMod.
@@ -269,18 +271,17 @@ int EtiReader::process(Buffer* dataIn)
             }
             break;
         default:
-    //        throw std::runtime_error("Invalid state!");
+            // throw std::runtime_error("Invalid state!");
             PDEBUG("Invalid state (%i)!", state);
             input_size = 0;
         }
     }
-    
+
     // Update timestamps
     myTimestampDecoder.updateTimestampEti(eti_fc.FP & 0x3,
-            eti_eoh.MNSC, 
-            getPPSOffset());
+            eti_eoh.MNSC, getPPSOffset(), eti_fc.FCT);
 
-    if (getFCT() % 125 == 0) //every 3 seconds is fine enough
+    if (eti_fc.FCT % 125 == 0) //every 3 seconds is fine enough
     {
         myTimestampDecoder.updateModulatorOffset();
     }
@@ -296,17 +297,15 @@ bool EtiReader::sourceContainsTimestamp()
 
 double EtiReader::getPPSOffset()
 {
-    if (!sourceContainsTimestamp())
+    if (!sourceContainsTimestamp()) {
+        //fprintf(stderr, "****** SOURCE NO TS\n");
         return 0.0;
+    }
 
     uint32_t timestamp = ntohl(eti_tist.TIST) & 0xFFFFFF;
-    //fprintf(stderr, "TIST 0x%x\n", timestamp);
+    //fprintf(stderr, "****** TIST 0x%x\n", timestamp);
     double pps = timestamp / 16384000.0; // seconds
 
     return pps;
 }
 
-uint32_t EtiReader::getFCT()
-{
-    return eti_fc.FCT;
-}
diff --git a/src/EtiReader.h b/src/EtiReader.h
index be12075..209b208 100644
--- a/src/EtiReader.h
+++ b/src/EtiReader.h
@@ -2,8 +2,10 @@
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011 Her Majesty
    the Queen in Right of Canada (Communications Research Center Canada)
 
-   Includes modifications for which no copyright is claimed
-   2012, Matthias P. Braendli, matthias.braendli@mpb.li
+   Copyright (C) 2014
+   Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
  */
 /*
    This file is part of ODR-DabMod.
@@ -60,9 +62,6 @@ public:
         myTimestampDecoder.calculateTimestamp(ts);
     }
 
-    /* Return the frame counter */
-    uint32_t getFCT();
-
     /* Returns true if we have valid time stamps in the ETI*/
     bool sourceContainsTimestamp();
 
diff --git a/src/Flowgraph.cpp b/src/Flowgraph.cpp
index 4e44f28..dd9c68b 100644
--- a/src/Flowgraph.cpp
+++ b/src/Flowgraph.cpp
@@ -23,10 +23,10 @@
 #include "PcDebug.h"
 
 
-#ifdef __ppc__
-#   define memalign(a, b)   malloc(b)
-#else // !__ppc__
+#if HAVE_DECL__MM_MALLOC
 #   include <mm_malloc.h>
+#else
+#   define memalign(a, b)   malloc(b)
 #endif
 #include <sys/types.h>
 #include <stdexcept>
diff --git a/src/GainControl.cpp b/src/GainControl.cpp
index 6304de3..03d8aa6 100644
--- a/src/GainControl.cpp
+++ b/src/GainControl.cpp
@@ -27,12 +27,20 @@
 
 #include "GainControl.h"
 #include "PcDebug.h"
-#include "kiss_fftsimd.h"
 
 #include <stdio.h>
 #include <stdexcept>
 #include <string>
 
+#ifdef __SSE__
+#  include <xmmintrin.h>
+union __u128 {
+    __m128 m;
+    float f[4];
+};
+#endif
+
+
 using namespace std;
 
 
@@ -224,13 +232,26 @@ __m128 GainControl::computeGainVar(const __m128* in, size_t sizeIn)
         __m128 i128 = _mm_set1_ps(sample + 1);
         __m128 q128 = _mm_div_ps(delta128, i128);
         mean128.m = _mm_add_ps(mean128.m, q128);
+
+        /*
+        tmp128.m = in[sample];
+        printf("S %zu, %.2f+%.2fj\t",
+                sample,
+                tmp128.f[0], tmp128.f[1]);
+        printf(": %.2f+%.2fj\n", mean128.f[0], mean128.f[1]);
+
+        printf("S %zu, %.2f+%.2fj\t",
+                sample,
+                tmp128.f[2], tmp128.f[3]);
+        printf(": %.2f+%.2fj\n", mean128.f[2], mean128.f[3]);
+        */
     }
 
     // Merging average
     tmp128.m = _mm_shuffle_ps(mean128.m, mean128.m, _MM_SHUFFLE(1, 0, 3, 2));
     mean128.m = _mm_add_ps(mean128.m, tmp128.m);
     mean128.m = _mm_mul_ps(mean128.m, _mm_set1_ps(0.5f));
-    PDEBUG("********** Mean:  %10f, %10f, %10f, %10f **********\n",
+    PDEBUG("********** Mean:  %10f + %10fj %10f + %10fj **********\n",
             mean128.f[0], mean128.f[1], mean128.f[2], mean128.f[3]);
 
     ////////////////////////////////////////////////////////////////////////
@@ -243,16 +264,32 @@ __m128 GainControl::computeGainVar(const __m128* in, size_t sizeIn)
         __m128 i128 = _mm_set1_ps(sample + 1);
         __m128 q128 = _mm_div_ps(delta128, i128);
         var128.m = _mm_add_ps(var128.m, q128);
+
+        /*
+        __u128 udiff128;  udiff128.m = diff128;
+        __u128 udelta128; udelta128.m = delta128;
+        for (int off=0; off<4; off+=2) {
+            printf("S %zu, %.2f+%.2fj\t",
+                    sample,
+                    udiff128.f[off], udiff128.f[1+off]);
+            printf(": %.2f+%.2fj\t", udelta128.f[off], udelta128.f[1+off]);
+            printf(": %.2f+%.2fj\n", var128.f[off], var128.f[1+off]);
+        }
+        */
+
     }
-    // Merging average
+    PDEBUG("********** Vars:  %10f + %10fj, %10f + %10fj **********\n",
+            var128.f[0], var128.f[1], var128.f[2], var128.f[3]);
+
+    // Merging standard deviations
     tmp128.m = _mm_shuffle_ps(var128.m, var128.m, _MM_SHUFFLE(1, 0, 3, 2));
     var128.m = _mm_add_ps(var128.m, tmp128.m);
     var128.m = _mm_mul_ps(var128.m, _mm_set1_ps(0.5f));
     var128.m = _mm_sqrt_ps(var128.m);
-    PDEBUG("********** Var:   %10f, %10f, %10f, %10f **********\n",
+    PDEBUG("********** Var:   %10f + %10fj, %10f + %10fj **********\n",
             var128.f[0], var128.f[1], var128.f[2], var128.f[3]);
     var128.m = _mm_mul_ps(var128.m, _mm_set1_ps(4.0f));
-    PDEBUG("********** 4*Var: %10f, %10f, %10f, %10f **********\n",
+    PDEBUG("********** 4*Var: %10f + %10fj, %10f + %10fj **********\n",
             var128.f[0], var128.f[1], var128.f[2], var128.f[3]);
 
     ////////////////////////////////////////////////////////////////////////////
@@ -333,7 +370,17 @@ float GainControl::computeGainVar(const complexf* in, size_t sizeIn)
 {
     float gain;
     complexf mean;
-    complexf var;
+
+    /* The variance calculation is a bit strange, because we
+     * emulate the exact same functionality as the SSE code,
+     * which is the most used one.
+     *
+     * TODO: verify that this actually corresponds to the
+     * gain mode suggested in EN 300 798 Clause 5.3 Numerical Range.
+     */
+    complexf var1;
+    complexf var2;
+
     static const float factor = 0x7fff;
 
     mean = complexf(0.0f, 0.0f);
@@ -343,25 +390,66 @@ float GainControl::computeGainVar(const complexf* in, size_t sizeIn)
         float i = sample + 1;
         complexf q = delta / i;
         mean = mean + q;
+
+        /*
+        printf("F %zu, %.2f+%.2fj\t",
+                sample,
+                in[sample].real(), in[sample].imag());
+
+        printf(": %.2f+%.2fj\n", mean.real(), mean.imag());
+        */
     }
 
-    PDEBUG("********** Mean:  %10f, %10f **********\n", mean.real(), mean.imag());
+    PDEBUG("********** Mean:  %10f + %10fj **********\n", mean.real(), mean.imag());
 
     ////////////////////////////////////////////////////////////////////////
     // Computing standard deviation
     ////////////////////////////////////////////////////////////////////////
-    var = complexf(0.0f, 0.0f);
+    var1 = complexf(0.0f, 0.0f);
+    var2 = complexf(0.0f, 0.0f);
     for (size_t sample = 0; sample < sizeIn; ++sample) {
         complexf diff  = in[sample] - mean;
-        complexf delta = complexf(diff.real() * diff.real(),
-                                  diff.imag() * diff.imag()) - var;
-        float    i = sample + 1;
-        complexf q = delta / i;
-        var = var + q;
+        complexf delta;
+        complexf q;
+
+        float    i = (sample/2) + 1;
+        if (sample % 2 == 0) {
+            delta = complexf(diff.real() * diff.real(),
+                             diff.imag() * diff.imag()) - var1;
+            q = delta / i;
+
+            var1 += q;
+        }
+        else {
+            delta = complexf(diff.real() * diff.real(),
+                             diff.imag() * diff.imag()) - var2;
+            q = delta / i;
+
+            var2 += q;
+        }
+
+        /*
+        printf("F %zu, %.2f+%.2fj\t",
+                sample,
+                diff.real(), diff.imag());
+
+        printf(": %.2f+%.2fj\t", delta.real(), delta.imag());
+        printf(": %.2f+%.2fj\t", var1.real(), var1.imag());
+        printf(": %.2f+%.2fj\n", var2.real(), var2.imag());
+        */
     }
-    PDEBUG("********** Var:   %10f, %10f **********\n", var.real(), var.imag());
+
+    PDEBUG("********** Vars:  %10f + %10fj, %10f + %10fj **********\n",
+                var1.real(), var1.imag(),
+                var2.real(), var2.imag());
+
+    // Merge standard deviations in the same way the SSE version does it
+    complexf tmpvar = (var1 + var2) * 0.5f;
+    complexf var(sqrt(tmpvar.real()), sqrt(tmpvar.imag()));
+    PDEBUG("********** Var:   %10f + %10fj **********\n", var.real(), var.imag());
+
     var = var * 4.0f;
-    PDEBUG("********** 4*Var: %10f, %10f **********\n", var.real(), var.imag());
+    PDEBUG("********** 4*Var: %10f + %10fj **********\n", var.real(), var.imag());
 
     ////////////////////////////////////////////////////////////////////////////
     // Computing gain
@@ -372,10 +460,10 @@ float GainControl::computeGainVar(const complexf* in, size_t sizeIn)
     }
     // Detect NULL
     if ((int)var.real() == 0) {
-        gain = factor / var.real();
+        gain = 1.0f;
     }
     else {
-        gain = 1.0f;
+        gain = factor / var.real();
     }
 
     return gain;
diff --git a/src/InputZeroMQReader.cpp b/src/InputZeroMQReader.cpp
index cf3f7aa..cfb56b2 100644
--- a/src/InputZeroMQReader.cpp
+++ b/src/InputZeroMQReader.cpp
@@ -3,8 +3,10 @@
    Her Majesty the Queen in Right of Canada (Communications Research
    Center Canada)
 
-   Copyrigth (C) 2013, 2014
+   Copyright (C) 2013, 2014
    Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
  */
 /*
    This file is part of ODR-DabMod.
diff --git a/src/Makefile.am b/src/Makefile.am
index 3e851c4..922ce52 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -22,89 +22,105 @@ else
 GITVERSION_FLAGS =
 endif
 
-if HAVE_INPUT_ZEROMQ_TEST
-ZMQ_LIBS    =-lzmq
+if HAVE_SSE
+SIMD_CFLAGS = -msse -msse2
 else
-ZMQ_LIBS    =
+SIMD_CFLAGS =
 endif
 
-if HAVE_OUTPUT_UHD_TEST
-UHD_SOURCES    =OutputUHD.cpp OutputUHD.h
-else
-UHD_SOURCES    =
-endif
+bin_PROGRAMS = odr-dabmod
 
+if USE_KISS_FFT
 FFT_DIR=$(top_builddir)/lib/kiss_fft129
 FFT_INC=-I$(FFT_DIR) -I$(FFT_DIR)/tools
-FFT_SRC=$(FFT_DIR)/kiss_fft.c $(FFT_DIR)/kiss_fft.h $(FFT_DIR)/tools/kiss_fftr.c $(FFT_DIR)/tools/kiss_fftr.h kiss_fftsimd.c kiss_fftsimd.h
+FFT_SRC=$(FFT_DIR)/kiss_fft.c \
+		$(FFT_DIR)/kiss_fft.h \
+		$(FFT_DIR)/tools/kiss_fftr.c \
+		$(FFT_DIR)/tools/kiss_fftr.h \
+		kiss_fftsimd.c \
+		kiss_fftsimd.h
 FFT_FLG=-ffast-math
 
 .PHONY: kiss_fft129 reed-solomon-4.0
 
-bin_PROGRAMS =  odr-dabmod
+DabModulator.cpp: $(FFT_DIR)
 
-DabModulator.cpp:   $(FFT_DIR)
+BUILT_SOURCES: $(FFT_DIR)
 
-BUILT_SOURCES:  $(FFT_DIR)
+FFT_LDADD=
 
 $(FFT_DIR):
 	if [ ! -e $(FFT_DIR) ]; then \
 		tar xzf $(top_srcdir)/lib/kiss_fft129.tar.gz -C $(top_builddir)/lib; \
 	fi
 
-odr_dabmod_CPPFLAGS = -Wall $(FFT_INC) $(FFT_FLG) -msse -msse2 $(GITVERSION_FLAGS)
-odr_dabmod_LDADD    = $(ZMQ_LIBS)
+else
+FFT_LDADD=
+FFT_DIR=
+FFT_INC=
+FFT_SRC=
+FFT_FLG=
+endif
+
+odr_dabmod_CPPFLAGS = -Wall \
+					  $(FFT_INC) $(FFT_FLG) $(SIMD_CFLAGS) $(GITVERSION_FLAGS)
+odr_dabmod_LDADD    = $(FFT_LDADD)
 odr_dabmod_SOURCES  = DabMod.cpp \
-                      PcDebug.h \
-                      porting.c porting.h \
-                      DabModulator.cpp DabModulator.h  \
-                      Buffer.cpp Buffer.h \
-                      ModCodec.cpp ModCodec.h \
-                      ModPlugin.cpp ModPlugin.h \
-                      ModFormat.cpp ModFormat.h \
-                      EtiReader.cpp EtiReader.h \
-                      Eti.cpp Eti.h \
-                      FicSource.cpp FicSource.h \
+					  PcDebug.h \
+					  porting.c porting.h \
+					  DabModulator.cpp DabModulator.h  \
+					  Buffer.cpp Buffer.h \
+					  ModCodec.cpp ModCodec.h \
+					  ModPlugin.cpp ModPlugin.h \
+					  ModFormat.cpp ModFormat.h \
+					  EtiReader.cpp EtiReader.h \
+					  Eti.cpp Eti.h \
+					  FicSource.cpp FicSource.h \
 					  FIRFilter.cpp FIRFilter.h \
-                      ModInput.cpp ModInput.h \
-                      PuncturingRule.cpp PuncturingRule.h \
-                      PuncturingEncoder.cpp PuncturingEncoder.h \
-                      SubchannelSource.cpp SubchannelSource.h \
-                      Flowgraph.cpp Flowgraph.h \
-                      GainControl.cpp GainControl.h \
-                      OutputMemory.cpp OutputMemory.h \
+					  ModInput.cpp ModInput.h \
+					  PuncturingRule.cpp PuncturingRule.h \
+					  PuncturingEncoder.cpp PuncturingEncoder.h \
+					  SubchannelSource.cpp SubchannelSource.h \
+					  Flowgraph.cpp Flowgraph.h \
+					  GainControl.cpp GainControl.h \
+					  OutputMemory.cpp OutputMemory.h \
+					  OutputZeroMQ.cpp OutputZeroMQ.h \
 					  TimestampDecoder.h TimestampDecoder.cpp \
-                      $(UHD_SOURCES) \
-                      ModOutput.cpp ModOutput.h \
-                      InputMemory.cpp InputMemory.h \
+					  OutputUHD.cpp OutputUHD.h \
+					  ModOutput.cpp ModOutput.h \
+					  InputMemory.cpp InputMemory.h \
 					  InputFileReader.cpp InputZeroMQReader.cpp InputReader.h \
-                      OutputFile.cpp OutputFile.h \
-                      FrameMultiplexer.cpp FrameMultiplexer.h \
-                      ModMux.cpp ModMux.h \
-                      PrbsGenerator.cpp PrbsGenerator.h \
-                      BlockPartitioner.cpp BlockPartitioner.h \
-                      QpskSymbolMapper.cpp QpskSymbolMapper.h \
-                      FrequencyInterleaver.cpp FrequencyInterleaver.h \
-                      PhaseReference.cpp PhaseReference.h \
-                      DifferentialModulator.cpp DifferentialModulator.h \
-                      NullSymbol.cpp NullSymbol.h \
-                      SignalMultiplexer.cpp SignalMultiplexer.h \
-                      CicEqualizer.cpp CicEqualizer.h \
-                      OfdmGenerator.cpp OfdmGenerator.h \
-                      GuardIntervalInserter.cpp GuardIntervalInserter.h \
-                      Resampler.cpp Resampler.h \
-                      ConvEncoder.cpp ConvEncoder.h \
-                      TimeInterleaver.cpp TimeInterleaver.h \
+					  OutputFile.cpp OutputFile.h \
+					  FrameMultiplexer.cpp FrameMultiplexer.h \
+					  ModMux.cpp ModMux.h \
+					  PrbsGenerator.cpp PrbsGenerator.h \
+					  BlockPartitioner.cpp BlockPartitioner.h \
+					  QpskSymbolMapper.cpp QpskSymbolMapper.h \
+					  FrequencyInterleaver.cpp FrequencyInterleaver.h \
+					  PhaseReference.cpp PhaseReference.h \
+					  DifferentialModulator.cpp DifferentialModulator.h \
+					  NullSymbol.cpp NullSymbol.h \
+					  SignalMultiplexer.cpp SignalMultiplexer.h \
+					  CicEqualizer.cpp CicEqualizer.h \
+					  OfdmGenerator.cpp OfdmGenerator.h \
+					  GuardIntervalInserter.cpp GuardIntervalInserter.h \
+					  Resampler.cpp Resampler.h \
+					  ConvEncoder.cpp ConvEncoder.h \
+					  TimeInterleaver.cpp TimeInterleaver.h \
 					  ThreadsafeQueue.h \
 					  Log.cpp Log.h \
 					  RemoteControl.cpp RemoteControl.h \
 					  zmq.hpp
 
-nodist_odr_dabmod_SOURCES	=$(FFT_SRC)
+nodist_odr_dabmod_SOURCES = $(FFT_SRC)
 
-dist_bin_SCRIPTS	=crc-dwap.py
+dist_bin_SCRIPTS = crc-dwap.py
 
-EXTRA_DIST	=kiss_fftsimd.c kiss_fftsimd.h
+if USE_KISS_FFT
+EXTRA_DIST = kiss_fftsimd.c kiss_fftsimd.h
 
 clean-local:
 	rm -rf $(FFT_DIR)
+
+endif
+
diff --git a/src/OfdmGenerator.cpp b/src/OfdmGenerator.cpp
index c7e8ebf..7044249 100644
--- a/src/OfdmGenerator.cpp
+++ b/src/OfdmGenerator.cpp
@@ -1,6 +1,11 @@
 /*
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011 Her Majesty
    the Queen in Right of Canada (Communications Research Center Canada)
+
+   Copyright (C) 2014
+   Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
  */
 /*
    This file is part of ODR-DabMod.
@@ -21,9 +26,15 @@
 
 #include "OfdmGenerator.h"
 #include "PcDebug.h"
-#include "kiss_fftsimd.h"
+#if USE_FFTW
+#  include "fftw3.h"
+#  define FFT_TYPE fftwf_complex
+#else
+#  include "kiss_fftsimd.h"
+#endif
 
 #include <stdio.h>
+#include <string.h>
 #include <stdexcept>
 #include <assert.h>
 #include <complex>
@@ -37,7 +48,11 @@ OfdmGenerator::OfdmGenerator(size_t nbSymbols,
     ModCodec(ModFormat(nbSymbols * nbCarriers * sizeof(FFT_TYPE)),
             ModFormat(nbSymbols * spacing * sizeof(FFT_TYPE))),
     myFftPlan(NULL),
+#if USE_FFTW
+    myFftIn(NULL), myFftOut(NULL),
+#else
     myFftBuffer(NULL),
+#endif
     myNbSymbols(nbSymbols),
     myNbCarriers(nbCarriers),
     mySpacing(spacing)
@@ -57,7 +72,8 @@ OfdmGenerator::OfdmGenerator(size_t nbSymbols,
         myNegDst = spacing - (nbCarriers / 2);
         myNegSrc = (nbCarriers + 1) / 2;
         myNegSize = nbCarriers / 2;
-    } else {
+    }
+    else {
         myPosDst = (nbCarriers & 1 ? 0 : 1);
         myPosSrc = nbCarriers / 2;
         myPosSize = (nbCarriers + 1) / 2;
@@ -77,8 +93,25 @@ OfdmGenerator::OfdmGenerator(size_t nbSymbols,
     PDEBUG("  myZeroDst: %u\n", myZeroDst);
     PDEBUG("  myZeroSize: %u\n", myZeroSize);
 
+#if USE_FFTW
+    const int N = mySpacing; // The size of the FFT
+    myFftIn = (FFT_TYPE*)fftwf_malloc(sizeof(FFT_TYPE) * N);
+    myFftOut = (FFT_TYPE*)fftwf_malloc(sizeof(FFT_TYPE) * N);
+    myFftPlan = fftwf_plan_dft_1d(N,
+            myFftIn, myFftOut,
+            FFTW_BACKWARD, FFTW_MEASURE);
+
+    if (sizeof(complexf) != sizeof(FFT_TYPE)) {
+        printf("sizeof(complexf) %zu\n", sizeof(complexf));
+        printf("sizeof(FFT_TYPE) %zu\n", sizeof(FFT_TYPE));
+        throw std::runtime_error(
+                "OfdmGenerator::process complexf size is not FFT_TYPE size!");
+    }
+#else
     myFftPlan = kiss_fft_alloc(mySpacing, 1, NULL, NULL);
     myFftBuffer = (FFT_TYPE*)memalign(16, mySpacing * sizeof(FFT_TYPE));
+#endif
+
 }
 
 
@@ -86,16 +119,32 @@ OfdmGenerator::~OfdmGenerator()
 {
     PDEBUG("OfdmGenerator::~OfdmGenerator() @ %p\n", this);
 
+#if USE_FFTW
+    if (myFftIn) {
+         fftwf_free(myFftIn);
+    }
+
+    if (myFftOut) {
+         fftwf_free(myFftOut);
+    }
+
+    if (myFftPlan) {
+        fftwf_destroy_plan(myFftPlan);
+    }
+
+#else
     if (myFftPlan != NULL) {
         kiss_fft_free(myFftPlan);
     }
+
     if (myFftBuffer != NULL) {
         free(myFftBuffer);
     }
+
     kiss_fft_cleanup();
+#endif
 }
 
-
 int OfdmGenerator::process(Buffer* const dataIn, Buffer* dataOut)
 {
     PDEBUG("OfdmGenerator::process(dataIn: %p, dataOut: %p)\n",
@@ -105,6 +154,7 @@ int OfdmGenerator::process(Buffer* const dataIn, Buffer* dataOut)
 
     FFT_TYPE* in = reinterpret_cast<FFT_TYPE*>(dataIn->getData());
     FFT_TYPE* out = reinterpret_cast<FFT_TYPE*>(dataOut->getData());
+
     size_t sizeIn = dataIn->getLength() / sizeof(complexf);
     size_t sizeOut = dataOut->getLength() / sizeof(complexf);
 
@@ -125,7 +175,27 @@ int OfdmGenerator::process(Buffer* const dataIn, Buffer* dataOut)
                 "OfdmGenerator::process output size not valid!");
     }
 
-#ifdef USE_SIMD
+#if USE_FFTW
+    // No SIMD/no-SIMD distinction, it's too early to optimize anything
+    for (size_t i = 0; i < myNbSymbols; ++i) {
+        myFftIn[0][0] = 0;
+        myFftIn[0][1] = 0;
+
+        bzero(&myFftIn[myZeroDst], myZeroSize * sizeof(FFT_TYPE));
+        memcpy(&myFftIn[myPosDst], &in[myPosSrc],
+                myPosSize * sizeof(FFT_TYPE));
+        memcpy(&myFftIn[myNegDst], &in[myNegSrc],
+                myNegSize * sizeof(FFT_TYPE));
+
+        fftwf_execute(myFftPlan);
+
+        memcpy(out, myFftOut, mySpacing * sizeof(FFT_TYPE));
+
+        in += myNbCarriers;
+        out += mySpacing;
+    }
+#else
+#  ifdef USE_SIMD
     for (size_t i = 0, j = 0; i < sizeIn; ) {
         // Pack 4 fft operations
         typedef struct {
@@ -154,7 +224,8 @@ int OfdmGenerator::process(Buffer* const dataIn, Buffer* dataOut)
                     dataBuffer[myNegDst + l].i[k] = cplxIn[i + myNegSrc + l].imag();
                 }
                 i += myNbCarriers;
-            } else {
+            }
+            else {
                 for (size_t l = 0; l < myNbCarriers; ++l) {
                     dataBuffer[l].r[k] = 0.0f;
                     dataBuffer[l].i[k] = 0.0f;
@@ -174,7 +245,7 @@ int OfdmGenerator::process(Buffer* const dataIn, Buffer* dataOut)
             }
         }
     }
-#else
+#  else
     for (size_t i = 0; i < myNbSymbols; ++i) {
         FFT_REAL(myFftBuffer[0]) = 0;
         FFT_IMAG(myFftBuffer[0]) = 0;
@@ -189,7 +260,9 @@ int OfdmGenerator::process(Buffer* const dataIn, Buffer* dataOut)
         in += myNbCarriers;
         out += mySpacing;
     }
+#  endif
 #endif
 
     return sizeOut;
 }
+
diff --git a/src/OfdmGenerator.h b/src/OfdmGenerator.h
index 5be2d03..ec9f14a 100644
--- a/src/OfdmGenerator.h
+++ b/src/OfdmGenerator.h
@@ -1,6 +1,11 @@
 /*
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011 Her Majesty
    the Queen in Right of Canada (Communications Research Center Canada)
+
+   Copyright (C) 2014
+   Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
  */
 /*
    This file is part of ODR-DabMod.
@@ -28,10 +33,14 @@
 
 #include "porting.h"
 #include "ModCodec.h"
-#include "kiss_fftsimd.h"
 
+#if USE_FFTW
+#  include "fftw3.h"
+#else
+#  include "kiss_fftsimd.h"
+#  include <kiss_fft.h>
+#endif
 
-#include <kiss_fft.h>
 #include <sys/types.h>
 
 
@@ -48,8 +57,13 @@ public:
     const char* name() { return "OfdmGenerator"; }
 
 protected:
+#if USE_FFTW
+    fftwf_plan myFftPlan;
+    fftwf_complex *myFftIn, *myFftOut;
+#else
     FFT_PLAN myFftPlan;
     FFT_TYPE *myFftBuffer;
+#endif
     size_t myNbSymbols;
     size_t myNbCarriers;
     size_t mySpacing;
@@ -63,5 +77,5 @@ protected:
     unsigned myZeroSize;
 };
 
-
 #endif // OFDM_GENERATOR_H
+
diff --git a/src/OutputUHD.cpp b/src/OutputUHD.cpp
index c7770fa..741731e 100644
--- a/src/OutputUHD.cpp
+++ b/src/OutputUHD.cpp
@@ -25,6 +25,9 @@
  */
 
 #include "OutputUHD.h"
+
+#ifdef HAVE_OUTPUT_UHD
+
 #include "PcDebug.h"
 #include "Log.h"
 #include "RemoteControl.h"
@@ -60,6 +63,9 @@ OutputUHD::OutputUHD(
     myMuting = 0; // is remote-controllable
     myStaticDelay = 0; // is remote-controllable
 
+#if FAKE_UHD
+    MDEBUG("OutputUHD:Using fake UHD output");
+#else
     std::stringstream device;
     device << myConf.device;
 
@@ -202,6 +208,7 @@ OutputUHD::OutputUHD(
 
     // preparing output thread worker data
     uwd.myUsrp = myUsrp;
+#endif
 
     uwd.frame0.ts.timestamp_valid = false;
     uwd.frame1.ts.timestamp_valid = false;
@@ -265,7 +272,15 @@ int OutputUHD::process(Buffer* dataIn, Buffer* dataOut)
 
         myEtiReader->calculateTimestamp(ts);
         uwd.frame0.ts = ts;
-        uwd.frame0.fct = myEtiReader->getFCT();
+
+        switch (myEtiReader->getMode()) {
+            case 1: uwd.fct_increment = 4; break;
+            case 2:
+            case 3: uwd.fct_increment = 1; break;
+            case 4: uwd.fct_increment = 2; break;
+            default: break;
+        }
+
 
         activebuffer = 1;
 
@@ -306,7 +321,6 @@ int OutputUHD::process(Buffer* dataIn, Buffer* dataOut)
             memcpy(&myDelayBuf[0], &pInData[uwd.bufsize - noByteDelay], noByteDelay);
 
             uwd.frame0.ts = ts;
-            uwd.frame0.fct = myEtiReader->getFCT();
         }
         else if (activebuffer == 1) {
             uint8_t *pTmp = (uint8_t*) uwd.frame1.buf;
@@ -318,7 +332,6 @@ int OutputUHD::process(Buffer* dataIn, Buffer* dataOut)
             memcpy(&myDelayBuf[0], &pInData[uwd.bufsize - noByteDelay], noByteDelay);
 
             uwd.frame1.ts = ts;
-            uwd.frame1.fct = myEtiReader->getFCT();
         }
 
         activebuffer = (activebuffer + 1) % 2;
@@ -347,9 +360,13 @@ void UHDWorker::process()
     // Transmit timeout
     const double timeout = 0.2;
 
+#if FAKE_UHD == 0
     uhd::stream_args_t stream_args("fc32"); //complex floats
     uhd::tx_streamer::sptr myTxStream = uwd->myUsrp->get_tx_stream(stream_args);
     size_t usrp_max_num_samps = myTxStream->get_max_num_samps();
+#else
+    size_t usrp_max_num_samps = 2048; // arbitrarily chosen
+#endif
 
     const complexf* in;
 
@@ -357,7 +374,10 @@ void UHDWorker::process()
     md.start_of_burst = false;
     md.end_of_burst = false;
 
+    int expected_next_fct = -1;
+
     while (running) {
+        bool fct_discontinuity = false;
         md.has_time_spec = false;
         md.time_spec = uhd::time_spec_t(0.0);
         num_acc_samps = 0;
@@ -387,6 +407,20 @@ void UHDWorker::process()
 
         sizeIn = uwd->bufsize / sizeof(complexf);
 
+        /* Verify that the FCT value is correct. If we miss one transmission
+         * frame we must interrupt UHD and resync to the timestamps
+         */
+        if (expected_next_fct != -1) {
+            if (expected_next_fct != (int)frame->ts.fct) {
+                uwd->logger->level(warn) <<
+                    "OutputUHD: Incorrect expect fct " << frame->ts.fct;
+
+                fct_discontinuity = true;
+            }
+        }
+
+        expected_next_fct = (frame->ts.fct + uwd->fct_increment) % 250;
+
         // Check for ref_lock
         if (uwd->check_refclk_loss)
         {
@@ -417,7 +451,7 @@ void UHDWorker::process()
                  * MNSC. We sleep through the frame.
                  */
                 uwd->logger->level(info) <<
-                    "OutputUHD: Throwing sample " << frame->fct <<
+                    "OutputUHD: Throwing sample " << frame->ts.fct <<
                     " away: incomplete timestamp " << tx_second <<
                     " + " << pps_offset;
                 usleep(20000); //TODO should this be TM-dependant ?
@@ -433,7 +467,7 @@ void UHDWorker::process()
                     "OutputUHD: Timestamp in the past! offset: " <<
                     md.time_spec.get_real_secs() - usrp_time <<
                     "  (" << usrp_time << ")"
-                    " frame " << frame->fct <<
+                    " frame " << frame->ts.fct <<
                     ", tx_second " << tx_second <<
                     ", pps " << pps_offset;
                 goto loopend; //skip the frame
@@ -458,7 +492,7 @@ void UHDWorker::process()
                         "OutputUHD (usrp time: %f): frame %d;"
                         "  tx_second %zu; pps %.9f\n",
                         usrp_time,
-                        frame->fct, tx_second, pps_offset);
+                        frame->ts.fct, tx_second, pps_offset);
             }
 
         }
@@ -469,12 +503,12 @@ void UHDWorker::process()
                 if (uwd->muting) {
                     uwd->logger->log(info,
                             "OutputUHD: Muting sample %d requested\n",
-                            frame->fct);
+                            frame->ts.fct);
                 }
                 else {
                     uwd->logger->log(info,
                             "OutputUHD: Muting sample %d : no timestamp\n",
-                            frame->fct);
+                            frame->ts.fct);
                 }
                 usleep(20000);
                 goto loopend;
@@ -482,20 +516,31 @@ void UHDWorker::process()
         }
 
         PDEBUG("UHDWorker::process:max_num_samps: %zu.\n",
-                myTxStream->get_max_num_samps());
+                usrp_max_num_samps);
 
         while (running && !uwd->muting && (num_acc_samps < sizeIn)) {
             size_t samps_to_send = std::min(sizeIn - num_acc_samps, usrp_max_num_samps);
 
             //ensure the the last packet has EOB set if the timestamps has been
             //refreshed and need to be reconsidered.
-            md.end_of_burst = (frame->ts.timestamp_refresh &&
-                    (samps_to_send <= usrp_max_num_samps));
-
+            //Also, if we saw that the FCT did not increment as expected, which
+            //could be due to a lost incoming packet.
+            md.end_of_burst = (
+                    uwd->sourceContainsTimestamp &&
+                    (frame->ts.timestamp_refresh || fct_discontinuity) &&
+                    samps_to_send <= usrp_max_num_samps );
+
+
+#if FAKE_UHD
+            // This is probably very approximate
+            usleep( (1000000 / uwd->sampleRate) * samps_to_send);
+            size_t num_tx_samps = samps_to_send;
+#else
             //send a single packet
             size_t num_tx_samps = myTxStream->send(
                     &in[num_acc_samps],
                     samps_to_send, md, timeout);
+#endif
 
             num_acc_samps += num_tx_samps;
 
@@ -540,6 +585,7 @@ void UHDWorker::process()
 #endif
             }
 
+#if FAKE_UHD == 0
             uhd::async_metadata_t async_md;
             if (uwd->myUsrp->get_device()->recv_async_msg(async_md, 0)) {
                 const char* uhd_async_message = "";
@@ -570,22 +616,12 @@ void UHDWorker::process()
 
                 if (failure) {
                     uwd->logger->level(alert) << "Near frame " <<
-                            frame->fct << ": Received Async UHD Message '" << 
+                            frame->ts.fct << ": Received Async UHD Message '" << 
                             uhd_async_message << "'";
 
                 }
             }
-
-            /*
-               bool got_async_burst_ack = false;
-            //loop through all messages for the ACK packet (may have underflow messages in queue)
-            while (not got_async_burst_ack and uwd->myUsrp->get_device()->recv_async_msg(async_md, 0.2)){
-            got_async_burst_ack = (async_md.event_code == uhd::async_metadata_t::EVENT_CODE_BURST_ACK);
-            }
-            //std::cerr << (got_async_burst_ack? "success" : "fail") << std::endl;
-            // */
-
-
+#endif
         }
 
         last_pps = pps_offset;
@@ -663,3 +699,4 @@ const string OutputUHD::get_parameter(const string& parameter) const
     return ss.str();
 }
 
+#endif // HAVE_OUTPUT_UHD
diff --git a/src/OutputUHD.h b/src/OutputUHD.h
index 90d9d1b..7eb6733 100644
--- a/src/OutputUHD.h
+++ b/src/OutputUHD.h
@@ -36,10 +36,14 @@ DESCRIPTION:
 #ifndef OUTPUT_UHD_H
 #define OUTPUT_UHD_H
 
+#define FAKE_UHD 0
+
 #ifdef HAVE_CONFIG_H
 #   include <config.h>
 #endif
 
+#ifdef HAVE_OUTPUT_UHD
+
 #include <uhd/utils/thread_priority.hpp>
 #include <uhd/utils/safe_main.hpp>
 #include <uhd/usrp/multi_usrp.hpp>
@@ -77,15 +81,14 @@ struct UHDWorkerFrameData {
 
     // Full timestamp
     struct frame_timestamp ts;
-
-    // Frame counter
-    uint32_t fct;
 };
 
 enum refclk_lock_loss_behaviour_t { CRASH, IGNORE };
 
 struct UHDWorkerData {
+#if FAKE_UHD == 0
     uhd::usrp::multi_usrp::sptr myUsrp;
+#endif
     unsigned sampleRate;
 
     // Double buffering between the two threads
@@ -117,7 +120,12 @@ struct UHDWorkerData {
 
     // The common logger
     Logger* logger;
-}; 
+
+    // What transmission mode we're using defines by how
+    // much the FCT should increment for each
+    // transmission frame.
+    int fct_increment;
+};
 
 
 class UHDWorker {
@@ -160,7 +168,7 @@ struct OutputUHDConfig {
     long masterClockRate;
     unsigned sampleRate;
     double frequency;
-    int txgain;
+    double txgain;
     bool enableSync;
     bool muteNoTimestamps;
 
@@ -229,6 +237,7 @@ class OutputUHD: public ModOutput, public RemoteControllable {
         size_t lastLen;
 };
 
+#endif // HAVE_OUTPUT_UHD
 
 #endif // OUTPUT_UHD_H
 
diff --git a/src/OutputZeroMQ.cpp b/src/OutputZeroMQ.cpp
new file mode 100644
index 0000000..0e759dd
--- /dev/null
+++ b/src/OutputZeroMQ.cpp
@@ -0,0 +1,67 @@
+/*
+   Copyright (C) 2007, 2008, 2009, 2010, 2011 Her Majesty the Queen in
+   Right of Canada (Communications Research Center Canada)
+
+   Copyright (C) 2014
+   Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
+ */
+/*
+   This file is part of ODR-DabMod.
+
+   ODR-DabMod 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.
+
+   ODR-DabMod 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 ODR-DabMod.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "OutputZeroMQ.h"
+#include "PcDebug.h"
+#include <stdexcept>
+#include <string.h>
+#include <sstream>
+
+#if defined(HAVE_OUTPUT_ZEROMQ)
+
+OutputZeroMQ::OutputZeroMQ(std::string endpoint, Buffer* dataOut)
+    : ModOutput(ModFormat(1), ModFormat(0)),
+    m_zmq_context(1),
+    m_zmq_pub_sock(m_zmq_context, ZMQ_PUB),
+    m_endpoint(endpoint)
+{
+    PDEBUG("OutputZeroMQ::OutputZeroMQ(%p) @ %p\n", dataOut, this);
+
+    std::stringstream ss;
+    ss << "OutputZeroMQ(" << m_endpoint << ")";
+    m_name = ss.str();
+
+    m_zmq_pub_sock.bind(m_endpoint.c_str());
+}
+
+OutputZeroMQ::~OutputZeroMQ()
+{
+    PDEBUG("OutputZeroMQ::~OutputZeroMQ() @ %p\n", this);
+}
+
+int OutputZeroMQ::process(Buffer* dataIn, Buffer* dataOut)
+{
+    PDEBUG("OutputZeroMQ::process"
+            "(dataIn: %p, dataOut: %p)\n",
+            dataIn, dataOut);
+
+    m_zmq_pub_sock.send(dataIn->getData(), dataIn->getLength());
+
+    return dataIn->getLength();
+}
+
+#endif // HAVE_OUTPUT_ZEROMQ_H
+
diff --git a/src/OutputZeroMQ.h b/src/OutputZeroMQ.h
new file mode 100644
index 0000000..1c48fe7
--- /dev/null
+++ b/src/OutputZeroMQ.h
@@ -0,0 +1,60 @@
+/*
+   Copyright (C) 2007, 2008, 2009, 2010, 2011 Her Majesty the Queen in
+   Right of Canada (Communications Research Center Canada)
+
+   Copyright (C) 2014
+   Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
+ */
+/*
+   This file is part of ODR-DabMod.
+
+   ODR-DabMod 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.
+
+   ODR-DabMod 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 ODR-DabMod.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef OUTPUT_ZEROMQ_H
+#define OUTPUT_ZEROMQ_H
+
+#ifdef HAVE_CONFIG_H
+#   include "config.h"
+#endif
+
+#if defined(HAVE_OUTPUT_ZEROMQ)
+
+#include "ModOutput.h"
+#include "zmq.hpp"
+
+class OutputZeroMQ : public ModOutput
+{
+    public:
+        OutputZeroMQ(std::string endpoint, Buffer* dataOut = NULL);
+        virtual ~OutputZeroMQ();
+        virtual int process(Buffer* dataIn, Buffer* dataOut);
+        const char* name() { return m_name.c_str(); }
+
+    protected:
+        zmq::context_t m_zmq_context; // handle for the zmq context
+        zmq::socket_t m_zmq_pub_sock; // handle for the zmq publisher socket
+
+        std::string m_endpoint;       // On which port to listen: e.g.
+                                      // tcp://*:58300
+
+        std::string m_name;
+};
+
+#endif // HAVE_OUTPUT_ZEROMQ_H
+
+#endif // OUTPUT_ZEROMQ_H
+
diff --git a/src/QpskSymbolMapper.cpp b/src/QpskSymbolMapper.cpp
index 2a8ebe9..05d847c 100644
--- a/src/QpskSymbolMapper.cpp
+++ b/src/QpskSymbolMapper.cpp
@@ -109,7 +109,6 @@ int QpskSymbolMapper::process(Buffer* const dataIn, Buffer* dataOut)
         inOffset += d_carriers / 8;
     }
 #else // !__SSE__
-#error "Code section not verified"
     const unsigned char* in = reinterpret_cast<const unsigned char*>(dataIn->getData());
     float* out = reinterpret_cast<float*>(dataOut->getData());
     if (dataIn->getLength() % (d_carriers / 4) != 0) {
@@ -142,7 +141,6 @@ int QpskSymbolMapper::process(Buffer* const dataIn, Buffer* dataOut)
     size_t inOffset = 0;
     size_t outOffset = 0;
     unsigned char tmp;
-    fprintf(stderr, "TODO: Validate QpskSymbolMapper::process without SSE\n");
     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;
@@ -150,15 +148,15 @@ int QpskSymbolMapper::process(Buffer* const dataIn, Buffer* dataOut)
             memcpy(&out[outOffset], symbols[tmp], sizeof(float) * 4);
             tmp =  (in[inOffset] & 0x30) >> 2;
             tmp |= (in[inOffset + (d_carriers / 8)] & 0x30) >> 4;
-            memcpy(&out[outOffset + 1], symbols[tmp], sizeof(float) * 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 + 2], symbols[tmp], sizeof(float) * 4);
+            memcpy(&out[outOffset + 8], symbols[tmp], sizeof(float) * 4);
             tmp =  (in[inOffset] & 0x03) << 2;
             tmp |= (in[inOffset + (d_carriers / 8)] & 0x03);
-            memcpy(&out[outOffset + 3], symbols[tmp], sizeof(float) * 4);
+            memcpy(&out[outOffset + 12], symbols[tmp], sizeof(float) * 4);
             ++inOffset;
-            outOffset += 4;
+            outOffset += 4*4;
         }
         inOffset += d_carriers / 8;
     }
@@ -166,3 +164,4 @@ int QpskSymbolMapper::process(Buffer* const dataIn, Buffer* dataOut)
 
     return 1;
 }
+
diff --git a/src/Resampler.cpp b/src/Resampler.cpp
index 773b9cc..cda4ff4 100644
--- a/src/Resampler.cpp
+++ b/src/Resampler.cpp
@@ -1,6 +1,11 @@
 /*
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011 Her Majesty
    the Queen in Right of Canada (Communications Research Center Canada)
+
+   Copyright (C) 2014
+   Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
  */
 /*
    This file is part of ODR-DabMod.
@@ -22,17 +27,16 @@
 #include "Resampler.h"
 #include "PcDebug.h"
 
-
-#ifdef __ppc__
-#   define memalign(a, b)   malloc(b)
-#else // !__ppc__
-#   include <mm_malloc.h>
-#endif
+#include <malloc.h>
 #include <sys/types.h>
 #include <string.h>
 #include <stdexcept>
 #include <assert.h>
 
+#if USE_FFTW
+#  define FFT_REAL(x) x[0]
+#  define FFT_IMAG(x) x[1]
+#endif
 
 unsigned gcd(unsigned a, unsigned b)
 {
@@ -59,6 +63,9 @@ Resampler::Resampler(size_t inputRate, size_t outputRate, size_t resolution) :
 {
     PDEBUG("Resampler::Resampler(%zu, %zu) @ %p\n", inputRate, outputRate, this);
 
+#if USE_FFTW
+    fprintf(stderr, "This software uses the FFTW library.\n\n");
+#else
     fprintf(stderr, "This software uses KISS FFT.\n\n");
     fprintf(stderr, "Copyright (c) 2003-2004 Mark Borgerding\n"
             "\n"
@@ -92,6 +99,7 @@ Resampler::Resampler(size_t inputRate, size_t outputRate, size_t resolution) :
             "OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY "
             "OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE "
             "POSSIBILITY OF SUCH DAMAGE.\n");
+#endif
 
     size_t divisor = gcd(inputRate, outputRate);
     L = outputRate / divisor;
@@ -119,6 +127,22 @@ Resampler::Resampler(size_t inputRate, size_t outputRate, size_t resolution) :
         PDEBUG("Window[%zu] = %f\n", i, myWindow[i]);
     }
 
+#if USE_FFTW
+    myFftIn = (FFT_TYPE*)fftwf_malloc(sizeof(FFT_TYPE) * myFftSizeIn);
+    myFront = (FFT_TYPE*)fftwf_malloc(sizeof(FFT_TYPE) * myFftSizeIn);
+    myFftPlan1 = fftwf_plan_dft_1d(myFftSizeIn,
+            myFftIn, myFront,
+            FFTW_FORWARD, FFTW_MEASURE);
+
+    myBack = (FFT_TYPE*)fftwf_malloc(sizeof(FFT_TYPE) * myFftSizeOut);
+    myFftOut = (FFT_TYPE*)fftwf_malloc(sizeof(FFT_TYPE) * myFftSizeOut);
+    myFftPlan2 = fftwf_plan_dft_1d(myFftSizeOut,
+            myBack, myFftOut,
+            FFTW_BACKWARD, FFTW_MEASURE);
+
+    myBufferIn = (complexf*)fftwf_malloc(sizeof(FFT_TYPE) * myFftSizeIn / 2);
+    myBufferOut = (complexf*)fftwf_malloc(sizeof(FFT_TYPE) * myFftSizeOut / 2);
+#else
     myFftIn = (FFT_TYPE*)memalign(16, myFftSizeIn * sizeof(FFT_TYPE));
     myFftOut = (FFT_TYPE*)memalign(16, myFftSizeOut * sizeof(FFT_TYPE));
     myBufferIn = (complexf*)memalign(16, myFftSizeIn / 2 * sizeof(FFT_TYPE));
@@ -127,6 +151,7 @@ Resampler::Resampler(size_t inputRate, size_t outputRate, size_t resolution) :
     myBack = (FFT_TYPE*)memalign(16, myFftSizeOut * sizeof(FFT_TYPE));
     myFftPlan1 = kiss_fft_alloc(myFftSizeIn, 0, NULL, NULL);
     myFftPlan2 = kiss_fft_alloc(myFftSizeOut, 1, NULL, NULL);
+#endif
 
     memset(myBufferIn, 0, myFftSizeIn / 2 * sizeof(FFT_TYPE));
     memset(myBufferOut, 0, myFftSizeOut / 2 * sizeof(FFT_TYPE));
@@ -137,34 +162,30 @@ Resampler::~Resampler()
 {
     PDEBUG("Resampler::~Resampler() @ %p\n", this);
 
-    if (myFftPlan1 != NULL) {
-        free(myFftPlan1);
-    }
-    if (myFftPlan2 != NULL) {
-        free(myFftPlan2);
-    }
-    if (myFftIn != NULL) {
-        free(myFftIn);
-    }
-    if (myFftOut != NULL) {
-        free(myFftOut);
-    }
-    if (myBufferIn != NULL) {
-        free(myBufferIn);
-    }
-    if (myBufferOut != NULL) {
-        free(myBufferOut);
-    }
-    if (myFront != NULL) {
-        free(myFront);
-    }
-    if (myBack != NULL) {
-        free(myBack);
-    }
-    if (myWindow != NULL) {
-        free(myWindow);
-    }
+#if USE_FFTW
+    if (myFftPlan1 != NULL) { fftwf_free(myFftPlan1); }
+    if (myFftPlan2 != NULL) { fftwf_free(myFftPlan2); }
+    if (myFftIn != NULL) { fftwf_free(myFftIn); }
+    if (myFftOut != NULL) { fftwf_free(myFftOut); }
+    if (myBufferIn != NULL) { fftwf_free(myBufferIn); }
+    if (myBufferOut != NULL) { fftwf_free(myBufferOut); }
+    if (myFront != NULL) { fftwf_free(myFront); }
+    if (myBack != NULL) { fftwf_free(myBack); }
+    if (myWindow != NULL) { fftwf_free(myWindow); }
+    fftwf_destroy_plan(myFftPlan1);
+    fftwf_destroy_plan(myFftPlan2);
+#else
+    if (myFftPlan1 != NULL) { free(myFftPlan1); }
+    if (myFftPlan2 != NULL) { free(myFftPlan2); }
+    if (myFftIn != NULL) { free(myFftIn); }
+    if (myFftOut != NULL) { free(myFftOut); }
+    if (myBufferIn != NULL) { free(myBufferIn); }
+    if (myBufferOut != NULL) { free(myBufferOut); }
+    if (myFront != NULL) { free(myFront); }
+    if (myBack != NULL) { free(myBack); }
+    if (myWindow != NULL) { free(myWindow); }
     kiss_fft_cleanup();
+#endif
 }
 
 
@@ -179,7 +200,7 @@ int Resampler::process(Buffer* const dataIn, Buffer* dataOut)
     FFT_TYPE* out = reinterpret_cast<FFT_TYPE*>(dataOut->getData());
     size_t sizeIn = dataIn->getLength() / sizeof(complexf);
 
-#ifdef USE_SIMD
+#if defined(USE_SIMD) && !USE_FFTW
     size_t sizeOut = dataOut->getLength() / sizeof(complexf);
 
     typedef struct {
@@ -263,7 +284,9 @@ int Resampler::process(Buffer* const dataIn, Buffer* dataOut)
             j += myFftSizeOut / 2;
         }
     }
-#else
+#endif
+
+#if USE_FFTW || (!defined(USE_SIMD))
     for (size_t i = 0, j = 0; i < sizeIn; i += myFftSizeIn / 2, j += myFftSizeOut / 2) {
         memcpy(myFftIn, myBufferIn, myFftSizeIn / 2 * sizeof(FFT_TYPE));
         memcpy(myFftIn + (myFftSizeIn / 2), in + i, myFftSizeIn / 2 * sizeof(FFT_TYPE));
@@ -273,7 +296,11 @@ int Resampler::process(Buffer* const dataIn, Buffer* dataOut)
             FFT_IMAG(myFftIn[k]) *= myWindow[k];
         }
 
+#if USE_FFTW
+        fftwf_execute(myFftPlan1);
+#else
         kiss_fft(myFftPlan1, myFftIn, myFront);
+#endif
 
         if (myFftSizeOut > myFftSizeIn) {
             memset(myBack, 0, myFftSizeOut * sizeof(FFT_TYPE));
@@ -304,7 +331,11 @@ int Resampler::process(Buffer* const dataIn, Buffer* dataOut)
             FFT_IMAG(myBack[k]) *= myFactor;
         }
 
+#if USE_FFTW
+        fftwf_execute(myFftPlan2);
+#else
         kiss_fft(myFftPlan2, myBack, myFftOut);
+#endif
 
         for (size_t k = 0; k < myFftSizeOut / 2; ++k) {
             FFT_REAL(out[j + k]) = myBufferOut[k].real() + FFT_REAL(myFftOut[k]);
@@ -316,3 +347,4 @@ int Resampler::process(Buffer* const dataIn, Buffer* dataOut)
 
     return 1;
 }
+
diff --git a/src/Resampler.h b/src/Resampler.h
index a19b14e..392d0a6 100644
--- a/src/Resampler.h
+++ b/src/Resampler.h
@@ -1,6 +1,11 @@
 /*
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011 Her Majesty
    the Queen in Right of Canada (Communications Research Center Canada)
+
+   Copyright (C) 2014
+   Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
  */
 /*
    This file is part of ODR-DabMod.
@@ -28,12 +33,19 @@
 
 #include "porting.h"
 #include "ModCodec.h"
-#include "kiss_fftsimd.h"
+#if USE_FFTW
+#  include <sys/types.h>
+#  include <fftw3.h>
 
+#  define FFT_TYPE fftwf_complex
+#  define FFT_PLAN fftwf_plan
 
-#include <sys/types.h>
-#include <kiss_fft.h>
-#include <tools/kiss_fftr.h>
+#else
+#  include "kiss_fftsimd.h"
+#  include <sys/types.h>
+#  include <kiss_fft.h>
+#  include <tools/kiss_fftr.h>
+#endif
 #include <complex>
 typedef std::complex<float> complexf;
 
@@ -68,5 +80,5 @@ protected:
     float myFactor;
 };
 
-
 #endif // RESAMPLER_H
+
diff --git a/src/TimestampDecoder.cpp b/src/TimestampDecoder.cpp
index d6c627f..96c84c0 100644
--- a/src/TimestampDecoder.cpp
+++ b/src/TimestampDecoder.cpp
@@ -2,8 +2,10 @@
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010 Her Majesty the
    Queen in Right of Canada (Communications Research Center Canada)
 
-   Includes modifications for which no copyright is claimed
-   2012, Matthias P. Braendli, matthias.braendli@mpb.li
+   Copyright (C) 2014
+   Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
  */
 /*
    This file is part of ODR-DabMod.
@@ -33,7 +35,7 @@
 #include "Eti.h"
 #include "Log.h"
 
-//#define MDEBUG(fmt, args...) fprintf (LOG, fmt , ## args) 
+//#define MDEBUG(fmt, args...) fprintf (LOG, "*****" fmt , ## args) 
 #define MDEBUG(fmt, args...) PDEBUG(fmt, ## args) 
 
 
@@ -45,10 +47,12 @@ void TimestampDecoder::calculateTimestamp(struct frame_timestamp& ts)
     ts_queued->timestamp_valid = full_timestamp_received_mnsc;
     ts_queued->timestamp_sec = time_secs;
     ts_queued->timestamp_pps_offset = time_pps;
+    ts_queued->fct = latestFCT;
 
     ts_queued->timestamp_refresh = offset_changed;
     offset_changed = false;
 
+    MDEBUG("time_secs=%d, time_pps=%f\n", time_secs, time_pps);
     *ts_queued += timestamp_offset;
 
     queue_timestamps.push(ts_queued);
@@ -65,6 +69,7 @@ void TimestampDecoder::calculateTimestamp(struct frame_timestamp& ts)
         ts.timestamp_sec = 0;
         ts.timestamp_pps_offset = 0;
         ts.timestamp_refresh = false;
+        ts.fct = 0;
     }
     else {
         //fprintf(stderr, ". %zu ", queue_timestamps.size());
@@ -86,7 +91,7 @@ void TimestampDecoder::calculateTimestamp(struct frame_timestamp& ts)
         delete ts_queued;
     }
 
-    PDEBUG("Timestamp queue size %zu, delay_calc %u\n",
+    MDEBUG("Timestamp queue size %zu, delay_calc %u\n",
             queue_timestamps.size(),
             modconfig.delay_calculation_pipeline_stages);
 
@@ -153,13 +158,14 @@ void TimestampDecoder::pushMNSCData(int framephase, uint16_t mnsc)
 
 void TimestampDecoder::updateTimestampSeconds(uint32_t secs)
 {
-    MDEBUG("TimestampDecoder::updateTimestampSeconds(%d)\n", secs);
     if (inhibit_second_update > 0)
     {
+        MDEBUG("TimestampDecoder::updateTimestampSeconds(%d) inhibit\n", secs);
         inhibit_second_update--;
     }
     else
     {
+        MDEBUG("TimestampDecoder::updateTimestampSeconds(%d) apply\n", secs);
         time_secs = secs;
     }
 }
@@ -179,13 +185,17 @@ void TimestampDecoder::updateTimestampPPS(double pps)
     }
 
     time_pps = pps;
-
 }
 
-void TimestampDecoder::updateTimestampEti(int framephase, uint16_t mnsc, double pps)
+void TimestampDecoder::updateTimestampEti(
+        int framephase,
+        uint16_t mnsc,
+        double pps,
+        uint32_t fct)
 {
     updateTimestampPPS(pps);
     pushMNSCData(framephase, mnsc);
+    latestFCT = fct;
 }
 
 
@@ -221,13 +231,16 @@ bool TimestampDecoder::updateModulatorOffset()
                 }
                 catch (bad_lexical_cast& e)
                 {
-                    myLogger.level(error) << "Error parsing timestamp offset from file '" << modconfig.offset_filename << "'";
+                    myLogger.level(error) <<
+                        "Error parsing timestamp offset from file '" <<
+                        modconfig.offset_filename << "'";
                     r = false;
                 }
             }
             else
             {
-                myLogger.level(error) << "Error reading from timestamp offset file: eof reached\n";
+                myLogger.level(error) <<
+                    "Error reading from timestamp offset file: eof reached\n";
                 r = false;
             }
             filestream.close();
@@ -244,7 +257,9 @@ bool TimestampDecoder::updateModulatorOffset()
             if (timestamp_offset != newoffset)
             {
                 timestamp_offset = newoffset;
-                myLogger.level(info) << "TimestampDecoder::updateTimestampOffset: new offset is " << timestamp_offset;
+                myLogger.level(info) <<
+                    "TimestampDecoder::updateTimestampOffset: new offset is " <<
+                    timestamp_offset;
                 offset_changed = true;
             }
 
@@ -256,3 +271,4 @@ bool TimestampDecoder::updateModulatorOffset()
         return false;
     }
 }
+
diff --git a/src/TimestampDecoder.h b/src/TimestampDecoder.h
index 52290ac..0c393e4 100644
--- a/src/TimestampDecoder.h
+++ b/src/TimestampDecoder.h
@@ -2,8 +2,10 @@
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010 Her Majesty the
    Queen in Right of Canada (Communications Research Center Canada)
 
-   Includes modifications for which no copyright is claimed
-   2012, Matthias P. Braendli, matthias.braendli@mpb.li
+   Copyright (C) 2014
+   Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
  */
 /*
    This file is part of ODR-DabMod.
@@ -52,6 +54,9 @@ struct modulator_offset_config
 
 struct frame_timestamp
 {
+    // Which frame count does this timestamp apply to
+    uint32_t fct;
+
     uint32_t timestamp_sec;
     double timestamp_pps_offset;
     bool timestamp_valid;
@@ -64,6 +69,7 @@ struct frame_timestamp
             this->timestamp_pps_offset = rhs.timestamp_pps_offset;
             this->timestamp_valid = rhs.timestamp_valid;
             this->timestamp_refresh = rhs.timestamp_refresh;
+            this->fct = rhs.fct;
         }
 
         return *this;
@@ -104,7 +110,7 @@ struct frame_timestamp
 /* This module decodes MNSC time information */
 class TimestampDecoder
 {
-    public: 
+    public:
         TimestampDecoder(
                 struct modulator_offset_config& config,
                 Logger& logger):
@@ -113,6 +119,7 @@ class TimestampDecoder
             inhibit_second_update = 0;
             time_pps = 0.0;
             time_secs = 0;
+            latestFCT = 0;
             enableDecode = false;
             full_timestamp_received_mnsc = false;
             gmtime_r(0, &temp_time);
@@ -129,7 +136,11 @@ class TimestampDecoder
         void calculateTimestamp(struct frame_timestamp& ts);
 
         /* Update timestamp data from data in ETI */
-        void updateTimestampEti(int framephase, uint16_t mnsc, double pps);
+        void updateTimestampEti(
+                int framephase,
+                uint16_t mnsc,
+                double pps,
+                uint32_t fct);
 
         /* Update the modulator timestamp offset according to the modconf
          */
@@ -147,7 +158,7 @@ class TimestampDecoder
          * the timestamp
          */
         void updateTimestampPPS(double pps);
-        
+
         /* Update the timestamp when a full set of MNSC data is
          * known. This function can be called at most every four
          * frames when the data is transferred using the MNSC.
@@ -156,6 +167,7 @@ class TimestampDecoder
 
         struct tm temp_time;
         uint32_t time_secs;
+        uint32_t latestFCT;
         double time_pps;
         double timestamp_offset;
         int inhibit_second_update;
diff --git a/src/kiss_fftsimd.h b/src/kiss_fftsimd.h
index 6d38dc7..90ee435 100644
--- a/src/kiss_fftsimd.h
+++ b/src/kiss_fftsimd.h
@@ -36,14 +36,6 @@
 #define FFT_IMAG(a) (a).i
 
 
-#ifdef __SSE__
-#include <xmmintrin.h>
-union __u128 {
-    __m128 m;
-    float f[4];
-};
-#endif
-
 
 #ifdef USE_SIMD