diff --git a/src/fm_mpx.c b/src/fm_mpx.c
index 7fa869c..0a67440 100644
--- a/src/fm_mpx.c
+++ b/src/fm_mpx.c
@@ -19,10 +19,14 @@
 
     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.
+    
+    fm_mpx.c: generates an FM multiplex signal containing RDS plus possibly
+    monaural or stereo audio.
 */
 
 #include <sndfile.h>
 #include <stdlib.h>
+#include <strings.h>
 #include <math.h>
 
 #include "rds.h"
@@ -34,8 +38,8 @@
 #define FIR_HALF_SIZE 30 
 #define FIR_SIZE (2*FIR_HALF_SIZE-1)
 
-#define LENGTH 114000
-// TODO: remove constant
+
+size_t length;
 
 // coefficients of the low-pass FIR filter
 float low_pass_fir[FIR_HALF_SIZE];
@@ -52,81 +56,112 @@ int phase_19 = 0;
 float downsample_factor;
 
 
-float rds_buffer[LENGTH] = {0};
-float audio_buffer[LENGTH] = {0};
+float *audio_buffer;
 int audio_index = 0;
 int audio_len = 0;
 float audio_pos;
-float out = 0;
-float alpha = .03;
 
-float fir_buffer[FIR_SIZE] = {0};
+float fir_buffer_mono[FIR_SIZE] = {0};
+float fir_buffer_stereo[FIR_SIZE] = {0};
 int fir_index = 0;
+int channels;
 
 SNDFILE *inf;
 
+
+
+float *alloc_empty_buffer(size_t length) {
+    float *p = malloc(length * sizeof(float));
+    if(p == NULL) return NULL;
     
+    bzero(p, length * sizeof(float));
+    
+    return p;
+}
     
 
-int fm_mpx_open(char *filename) {
-    // Open the input file
-    SF_INFO sfinfo;
-    if(! (inf = sf_open(filename, SFM_READ, &sfinfo))) {
-        fprintf(stderr, "Error: could not open input file %s.\n", filename) ;
-        return EXIT_FAILURE; // TODO better error code
-    }
-    
-    int in_samplerate = sfinfo.samplerate;
-    downsample_factor = 228000. / in_samplerate;
-    
-    printf("Input: %d Hz, upsampling factor: %.2f\n", in_samplerate, downsample_factor);
-    
-    
-    // Create the low-pass FIR filter
-    float cutoff_freq = 15000 * .8;
-    if(in_samplerate/2 < cutoff_freq) cutoff_freq = in_samplerate/2 * .8;
-    
-    
-    
-    low_pass_fir[FIR_HALF_SIZE-1] = 2 * cutoff_freq / 228000 /2;
-    // Here we divide this coefficient by two because it will be counted twice
-    // when applying the filter
+int fm_mpx_open(char *filename, size_t len) {
+    length = len;
 
-    // Only store half of the filter since it is symmetric
-    for(int i=1; i<FIR_HALF_SIZE; i++) {
-        low_pass_fir[FIR_HALF_SIZE-1-i] = 
-            sin(2 * PI * cutoff_freq * i / 228000) / (PI * i)      // sinc
-            * (.54 - .46 * cos(2*PI * (i+FIR_HALF_SIZE) / (2*FIR_HALF_SIZE)));
-                                                          // Hamming window
+    if(filename != NULL) {
+        // Open the input file
+        SF_INFO sfinfo;
+        if(! (inf = sf_open(filename, SFM_READ, &sfinfo))) {
+            fprintf(stderr, "Error: could not open input file %s.\n", filename) ;
+            return -1;
+        }
+        
+    
+        int in_samplerate = sfinfo.samplerate;
+        downsample_factor = 228000. / in_samplerate;
+    
+        printf("Input: %d Hz, upsampling factor: %.2f\n", in_samplerate, downsample_factor);
+
+        channels = sfinfo.channels;
+        if(channels > 1) {
+            printf("%d channels, generating stereo multiplex.\n", channels);
+        } else {
+            printf("1 channel, monophonic operation.\n");
+        }
+    
+    
+        // Create the low-pass FIR filter
+        float cutoff_freq = 15000 * .8;
+        if(in_samplerate/2 < cutoff_freq) cutoff_freq = in_samplerate/2 * .8;
+    
+    
+    
+        low_pass_fir[FIR_HALF_SIZE-1] = 2 * cutoff_freq / 228000 /2;
+        // Here we divide this coefficient by two because it will be counted twice
+        // when applying the filter
+
+        // Only store half of the filter since it is symmetric
+        for(int i=1; i<FIR_HALF_SIZE; i++) {
+            low_pass_fir[FIR_HALF_SIZE-1-i] = 
+                sin(2 * PI * cutoff_freq * i / 228000) / (PI * i)      // sinc
+                * (.54 - .46 * cos(2*PI * (i+FIR_HALF_SIZE) / (2*FIR_HALF_SIZE)));
+                                                              // Hamming window
+        }
+        printf("Created low-pass FIR filter for audio channels, with cutoff at %.1f Hz\n", cutoff_freq);
+    
+        /*
+        for(int i=0; i<FIR_HALF_SIZE; i++) {
+            printf("%.5f ", low_pass_fir[i]);
+        }
+        printf("\n");
+        */
+        
+        audio_pos = downsample_factor;
+        audio_buffer = alloc_empty_buffer(length * channels);
+        if(audio_buffer == NULL) return -1;
+
+    } // end if(filename != NULL)
+    else {
+        inf = NULL;
+        // inf == NULL indicates that there is no audio
     }
-    printf("Created low-pass FIR filter for audio channels, with cutoff at %.1f Hz\n", cutoff_freq);
     
-    /*
-    for(int i=0; i<FIR_HALF_SIZE; i++) {
-        printf("%.5f ", low_pass_fir[i]);
-    }
-    printf("\n");
-    */
-    
-    audio_pos = downsample_factor;
-    
-    return 0; // TODO
+    return 0;
 }
 
 
-int fm_mpx_get_samples(float *mpx_buffer) { // TODO accept length in argument
-    get_rds_samples(rds_buffer, LENGTH);
+// samples provided by this function are in 0..10: they need to be divided by
+// 10 after.
+int fm_mpx_get_samples(float *mpx_buffer) {
+    get_rds_samples(mpx_buffer, length);
 
-    for(int i=0; i<LENGTH; i++) {
+    if(inf  == NULL) return 0; // if there is no audio, stop here
+    
+    for(int i=0; i<length; i++) {
         if(audio_pos >= downsample_factor) {
             audio_pos -= downsample_factor;
             
             if(audio_len == 0) {
                 for(int j=0; j<2; j++) { // one retry
-                    audio_len = sf_read_float(inf, audio_buffer, LENGTH);
+                    audio_len = sf_read_float(inf, audio_buffer, length);
                     if (audio_len < 0) {
                         fprintf(stderr, "Error reading audio\n");
-                        exit(EXIT_FAILURE);
+                        return -1;
                     }
                     if(audio_len == 0) {
                         sf_seek(inf, 0, SEEK_SET);
@@ -136,49 +171,72 @@ int fm_mpx_get_samples(float *mpx_buffer) { // TODO accept length in argument
                 }
                 audio_index = 0;
             } else {
-                audio_index++;
-                audio_len--;
+                audio_index += channels;
+                audio_len -= channels;
             }
         }
 
-        // Apply FIR low-pass filter
-        fir_buffer[fir_index] = audio_buffer[audio_index];
+        
+        // First store the current sample(s) into the FIR filter's ring buffer
+        if(channels == 0) {
+            fir_buffer_mono[fir_index] = audio_buffer[audio_index];
+        } else {
+            // In stereo operation, generate sum and difference signals
+            fir_buffer_mono[fir_index] = 
+                audio_buffer[audio_index] + audio_buffer[audio_index+1];
+            fir_buffer_stereo[fir_index] = 
+                audio_buffer[audio_index] - audio_buffer[audio_index+1];
+        }
         fir_index++;
         if(fir_index >= FIR_SIZE) fir_index = 0;
         
+        // Now apply the FIR low-pass filter
         
         /* As the FIR filter is symmetric, we do not multiply all 
            the coefficients independently, but two-by-two, thus reducing
            the total number of multiplications by a factor of two
         */
-        out = 0;
+        float out_mono = 0;
+        float out_stereo = 0;
         int ifbi = fir_index;  // ifbi = increasing FIR Buffer Index
         int dfbi = fir_index;  // dfbi = decreasing FIR Buffer Index
         for(int fi=0; fi<FIR_HALF_SIZE; fi++) {  // fi = Filter Index
             dfbi--;
             if(dfbi < 0) dfbi = FIR_SIZE-1;
-            out += low_pass_fir[fi] * (fir_buffer[ifbi] + fir_buffer[dfbi]);
+            out_mono += 
+                low_pass_fir[fi] * 
+                    (fir_buffer_mono[ifbi] + fir_buffer_mono[dfbi]);
+            if(channels > 1) {
+                out_stereo += 
+                    low_pass_fir[fi] * 
+                        (fir_buffer_stereo[ifbi] + fir_buffer_stereo[dfbi]);
+            }
             ifbi++;
             if(ifbi >= FIR_SIZE) ifbi = 0;
         }
         // End of FIR filter
         
 
-        mpx_buffer[i] = (rds_buffer[i] + 
-            4*out /* + 
-            2 * carrier_38[phase_38] * out + 
-            .1*carrier_19[phase_19]*/) / 10;
-        
-        phase_19++;
-        phase_38++;
-        if(phase_19 >= 12) phase_19 = 0;
-        if(phase_38 >= 6) phase_38 = 0;
-        
+        mpx_buffer[i] = 
+            mpx_buffer[i] +    // RDS data samples are currently in mpx_buffer
+            4.05*out_mono;     // Unmodulated monophonic (or stereo-sum) signal
+            
+        if(channels>1) {
+            mpx_buffer[i] +=
+                4.05 * carrier_38[phase_38] * out_stereo + // Stereo difference signal
+                .09*carrier_19[phase_19];                  // Stereo pilot tone
+
+            phase_19++;
+            phase_38++;
+            if(phase_19 >= 12) phase_19 = 0;
+            if(phase_38 >= 6) phase_38 = 0;
+        }
+            
         audio_pos++;   
         
     }
     
-    return 0; // TODO
+    return 0;
 }
 
 
@@ -186,5 +244,8 @@ int fm_mpx_close() {
     if(sf_close(inf) ) {
         fprintf(stderr, "Error closing audio file");
     }
-    return 0; // TODO
+    
+    free(audio_buffer);
+    
+    return 0;
 }
\ No newline at end of file
diff --git a/src/fm_mpx.h b/src/fm_mpx.h
index c1cc120..81b61be 100644
--- a/src/fm_mpx.h
+++ b/src/fm_mpx.h
@@ -21,6 +21,6 @@
     along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-extern int fm_mpx_open(char *filename);
+extern int fm_mpx_open(char *filename, size_t len);
 extern int fm_mpx_get_samples(float *mpx_buffer);
 extern int fm_mpx_close();
\ No newline at end of file
diff --git a/src/rds_wav.c b/src/rds_wav.c
index f46ef9a..8d90a9f 100644
--- a/src/rds_wav.c
+++ b/src/rds_wav.c
@@ -36,17 +36,20 @@
 
 /* Simple test program */
 int main(int argc, char **argv) {
-    if(argc < 3) {
+    if(argc < 4) {
         fprintf(stderr, "Error: missing argument.\n");
-        fprintf(stderr, "Syntax: rds_wav <out.wav> <text>\n");
+        fprintf(stderr, "Syntax: rds_wav <in_file> <out.wav> <text>\n");
         return EXIT_FAILURE;
     }
     
     set_rds_pi(0x1234);
-    set_rds_ps(argv[2]);
-    set_rds_rt(argv[2]);
+    set_rds_ps(argv[3]);
+    set_rds_rt(argv[3]);
     
-    fm_mpx_open("sound_22050.wav");
+    if(fm_mpx_open(argv[1], LENGTH) != 0) {
+        printf("Could not setup FM mulitplex generator.\n");
+        return EXIT_FAILURE;
+    }
     
 
     
@@ -62,8 +65,8 @@ int main(int argc, char **argv) {
     sfinfo.seekable = 0;
     
     // Open the output file
-    if (! (outf = sf_open(argv[1], SFM_WRITE, &sfinfo))) {
-        fprintf(stderr, "Error: could not open output file %s.\n", argv[1]) ;
+    if (! (outf = sf_open(argv[2], SFM_WRITE, &sfinfo))) {
+        fprintf(stderr, "Error: could not open output file %s.\n", argv[2]) ;
         return EXIT_FAILURE;
     }
 
@@ -71,6 +74,11 @@ int main(int argc, char **argv) {
 
     for(int j=0; j<40; j++) {
         fm_mpx_get_samples(mpx_buffer);
+        
+        // scale samples
+        for(int i=0; i<LENGTH; i++) {
+            mpx_buffer[i] /= 10.;
+        }
 
         if(sf_write_float(outf, mpx_buffer, LENGTH) != LENGTH) {
             fprintf(stderr, "Error: writing to file %s.\n", argv[1]);
diff --git a/src/stereo_44100.wav b/src/stereo_44100.wav
new file mode 100644
index 0000000..1ddbbfc
Binary files /dev/null and b/src/stereo_44100.wav differ