Switch to direct use of librtlsdr and PulseAudio

Substantial rework to replace the spawning of rtl_fm and aplay with direct
usage of librtlsdr and PulseAudio in a multi-threaded model. This is required
due to changes in AGL application execution that prevent spawned processes from
exiting, resulting in the plugin hanging on frequency changes or stopping.

The rework has been accomplished by refactoring the source of rtl_fm.c into a
reusable form and connecting it to the RtlFmRadioTunerControl class used to
implement the functionality exposed by the RtlFmRadioPlugin class. The idea for
reusing the source code in rtl_fm.c in this way is inspired by the older
qml_radio_plugin codebase, but a new refactor of rtl_fm.c was done to keep more
of its filtering functionality and ensure behavior consistent with the previous
implementation. The files radio_output.{h,cpp} are adapted from qml_radio_plugin
with some additional modifications.

Other changes include:
- The files in the convenience subdirectory have been copied from the librtldr
  source tree to reduce the effort of importing rtl_fm.c.
- The COPYING file containing the GPL license has been copied from the
  librtlsdr source tree to accompany rtl_fm.c and the convenience/* files.
- The recently added AM band support has been removed as the USB DVB adapters
  are incapable of receiving AM without significant tweaking, and a single
  adapter would be unable to do both AM and FM at the same time. The plugin
  now explicitly reports that it only supports FM.
- The list of known stations to act as an ersatz seeking implementation has
  been removed, as the updated higher-level QML application no longer exposes
  seeking. Adding this functionality back in would be straightforward if it
  becomes required again. There is also some code in rtl_fm.c that could possibly
  be adapted into a proper signal strength detection scheme in the future if
  that is desired.
- A Qt QSettings file is used to store the FM band plan information to
  allow using the specific frequency ranges for North America versus Japan.
  The band plan can be changed by modifying the "fmbandplan" entry in the
  QSettings .conf file to either "US" or "JP". The location of the .conf file
  can be one of:

  $HOME/.config/AGL/qtmultimedia-rtlfm-radio-plugin.conf
  $HOME/.config/AGL.conf
  /etc/xdg/AGL/qtmultimedia-rtlfm-radio-plugin.conf
  /etc/xdg/AGL.conf

Note that some debugging output has been left in place in the start and stop
methods to facilitate debugging of the higher-level QML application.  They will
be removed once that is complete.

Change-Id: I1d92c74eb24b24cb5416dd531b599645d1287295
Signed-off-by: Scott Murray <scott.murray@konsulko.com>

1 files changed, 1117 insertions, 0 deletions

diff --git a/rtl_fm.c b/rtl_fm.c
new file mode 100644
index 0000000..dcf28ce
--- /dev/null
+++ b/rtl_fm.c
@@ -0,0 +1,1117 @@
+/*
+ * rtl-sdr, turns your Realtek RTL2832 based DVB dongle into a SDR receiver
+ * Copyright (C) 2012 by Steve Markgraf <steve@steve-m.de>
+ * Copyright (C) 2012 by Hoernchen <la@tfc-server.de>
+ * Copyright (C) 2012 by Kyle Keen <keenerd@gmail.com>
+ * Copyright (C) 2013 by Elias Oenal <EliasOenal@gmail.com>
+ * Copyright (C) 2016 by Scott Murray <scott.murray@konsulko.com>
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * Note that this version replaces the standalone main() with separate
+ * init/start/stop API calls to allow building into another application.
+ * Other than removing the separate controller thread and adding an output
+ * function callback, other changes have been kept to a minimum to
+ * potentially allow using other rtl_fm features by modifying rtl_fm_init.
+ *
+ * December 2016, Scott Murray <scott.murray@konsulko.com>
+ */
+
+/*
+ * written because people could not do real time
+ * FM demod on Atom hardware with GNU radio
+ * based on rtl_sdr.c and rtl_tcp.c
+ *
+ * lots of locks, but that is okay
+ * (no many-to-many locks)
+ *
+ * todo:
+ *       sanity checks
+ *       scale squelch to other input parameters
+ *       test all the demodulations
+ *       pad output on hop
+ *       frequency ranges could be stored better
+ *       scaled AM demod amplification
+ *       auto-hop after time limit
+ *       peak detector to tune onto stronger signals
+ *       fifo for active hop frequency
+ *       clips
+ *       noise squelch
+ *       merge stereo patch
+ *       merge soft agc patch
+ *       merge udp patch
+ *       testmode to detect overruns
+ *       watchdog to reset bad dongle
+ *       fix oversampling
+ */
+
+#include <errno.h>
+#include <signal.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <math.h>
+#include <pthread.h>
+
+#include "rtl-sdr.h"
+#include "rtl_fm.h"
+#include "convenience/convenience.h"
+
+#define DEFAULT_SAMPLE_RATE		24000
+#define DEFAULT_BUF_LENGTH		RTL_FM_DEFAULT_BUF_LENGTH
+#define MAXIMUM_OVERSAMPLE		RTL_FM_MAXIMUM_OVERSAMPLE
+#define MAXIMUM_BUF_LENGTH		RTL_FM_MAXIMUM_BUF_LENGTH
+#define AUTO_GAIN			-100
+#define BUFFER_DUMP			4096
+
+#define FREQUENCIES_LIMIT		1000
+
+static volatile int do_exit = 0;
+static int lcm_post[17] = {1,1,1,3,1,5,3,7,1,9,5,11,3,13,7,15,1};
+static int ACTUAL_BUF_LENGTH;
+
+static int *atan_lut = NULL;
+static int atan_lut_size = 131072; /* 512 KB */
+static int atan_lut_coef = 8;
+
+struct dongle_state
+{
+	int      exit_flag;
+	pthread_t thread;
+	rtlsdr_dev_t *dev;
+	int      dev_index;
+	uint32_t freq;
+	uint32_t rate;
+	int      gain;
+	uint16_t buf16[MAXIMUM_BUF_LENGTH];
+	uint32_t buf_len;
+	int      ppm_error;
+	int      offset_tuning;
+	int      direct_sampling;
+	int      mute;
+	struct demod_state *demod_target;
+};
+
+struct demod_state
+{
+	int      exit_flag;
+	pthread_t thread;
+	int16_t  lowpassed[MAXIMUM_BUF_LENGTH];
+	int      lp_len;
+	int16_t  lp_i_hist[10][6];
+	int16_t  lp_q_hist[10][6];
+	int16_t  result[MAXIMUM_BUF_LENGTH];
+	int16_t  droop_i_hist[9];
+	int16_t  droop_q_hist[9];
+	int      result_len;
+	int      rate_in;
+	int      rate_out;
+	int      rate_out2;
+	int      now_r, now_j;
+	int      pre_r, pre_j;
+	int      prev_index;
+	int      downsample;    /* min 1, max 256 */
+	int      post_downsample;
+	int      output_scale;
+	int      squelch_level, conseq_squelch, squelch_hits, terminate_on_squelch;
+	int      downsample_passes;
+	int      comp_fir_size;
+	int      custom_atan;
+	int      deemph, deemph_a;
+	int      now_lpr;
+	int      prev_lpr_index;
+	int      dc_block, dc_avg;
+	void     (*mode_demod)(struct demod_state*);
+	pthread_rwlock_t rw;
+	pthread_cond_t ready;
+	pthread_mutex_t ready_m;
+	struct output_state *output_target;
+};
+
+struct output_state
+{
+	int      exit_flag;
+	pthread_t thread;
+	rtl_fm_output_fn_t output_fn;
+	void     *output_fn_data;
+	int16_t  result[MAXIMUM_BUF_LENGTH];
+	int      result_len;
+	int      rate;
+	pthread_rwlock_t rw;
+	pthread_cond_t ready;
+	pthread_mutex_t ready_m;
+};
+
+struct controller_state
+{
+	int      exit_flag;
+	pthread_t thread;
+	uint32_t freqs[FREQUENCIES_LIMIT];
+	int      freq_len;
+	int      freq_now;
+	int      edge;
+	int      wb_mode;
+	pthread_cond_t hop;
+	pthread_mutex_t hop_m;
+};
+
+// multiple of these, eventually
+struct dongle_state dongle;
+struct demod_state demod;
+struct output_state output;
+struct controller_state controller;
+
+#if 0
+static void sighandler(int signum)
+{
+	fprintf(stderr, "Signal caught, exiting!\n");
+	do_exit = 1;
+	rtlsdr_cancel_async(dongle.dev);
+}
+#endif
+
+/* more cond dumbness */
+#define safe_cond_signal(n, m) pthread_mutex_lock(m); pthread_cond_signal(n); pthread_mutex_unlock(m)
+#define safe_cond_wait(n, m) pthread_mutex_lock(m); pthread_cond_wait(n, m); pthread_mutex_unlock(m)
+
+/* {length, coef, coef, coef}  and scaled by 2^15
+   for now, only length 9, optimal way to get +85% bandwidth */
+#define CIC_TABLE_MAX 10
+int cic_9_tables[][10] = {
+	{0,},
+	{9, -156,  -97, 2798, -15489, 61019, -15489, 2798,  -97, -156},
+	{9, -128, -568, 5593, -24125, 74126, -24125, 5593, -568, -128},
+	{9, -129, -639, 6187, -26281, 77511, -26281, 6187, -639, -129},
+	{9, -122, -612, 6082, -26353, 77818, -26353, 6082, -612, -122},
+	{9, -120, -602, 6015, -26269, 77757, -26269, 6015, -602, -120},
+	{9, -120, -582, 5951, -26128, 77542, -26128, 5951, -582, -120},
+	{9, -119, -580, 5931, -26094, 77505, -26094, 5931, -580, -119},
+	{9, -119, -578, 5921, -26077, 77484, -26077, 5921, -578, -119},
+	{9, -119, -577, 5917, -26067, 77473, -26067, 5917, -577, -119},
+	{9, -199, -362, 5303, -25505, 77489, -25505, 5303, -362, -199},
+};
+
+void rotate_90(unsigned char *buf, uint32_t len)
+/* 90 rotation is 1+0j, 0+1j, -1+0j, 0-1j
+   or [0, 1, -3, 2, -4, -5, 7, -6] */
+{
+	uint32_t i;
+	unsigned char tmp;
+	for (i=0; i<len; i+=8) {
+		/* uint8_t negation = 255 - x */
+		tmp = 255 - buf[i+3];
+		buf[i+3] = buf[i+2];
+		buf[i+2] = tmp;
+
+		buf[i+4] = 255 - buf[i+4];
+		buf[i+5] = 255 - buf[i+5];
+
+		tmp = 255 - buf[i+6];
+		buf[i+6] = buf[i+7];
+		buf[i+7] = tmp;
+	}
+}
+
+void low_pass(struct demod_state *d)
+/* simple square window FIR */
+{
+	int i=0, i2=0;
+	while (i < d->lp_len) {
+		d->now_r += d->lowpassed[i];
+		d->now_j += d->lowpassed[i+1];
+		i += 2;
+		d->prev_index++;
+		if (d->prev_index < d->downsample) {
+			continue;
+		}
+		d->lowpassed[i2]   = d->now_r; // * d->output_scale;
+		d->lowpassed[i2+1] = d->now_j; // * d->output_scale;
+		d->prev_index = 0;
+		d->now_r = 0;
+		d->now_j = 0;
+		i2 += 2;
+	}
+	d->lp_len = i2;
+}
+
+int low_pass_simple(int16_t *signal2, int len, int step)
+// no wrap around, length must be multiple of step
+{
+	int i, i2, sum;
+	for(i=0; i < len; i+=step) {
+		sum = 0;
+		for(i2=0; i2<step; i2++) {
+			sum += (int)signal2[i + i2];
+		}
+		//signal2[i/step] = (int16_t)(sum / step);
+		signal2[i/step] = (int16_t)(sum);
+	}
+	signal2[i/step + 1] = signal2[i/step];
+	return len / step;
+}
+
+void low_pass_real(struct demod_state *s)
+/* simple square window FIR */
+// add support for upsampling?
+{
+	int i=0, i2=0;
+	int fast = (int)s->rate_out;
+	int slow = s->rate_out2;
+	while (i < s->result_len) {
+		s->now_lpr += s->result[i];
+		i++;
+		s->prev_lpr_index += slow;
+		if (s->prev_lpr_index < fast) {
+			continue;
+		}
+		s->result[i2] = (int16_t)(s->now_lpr / (fast/slow));
+		s->prev_lpr_index -= fast;
+		s->now_lpr = 0;
+		i2 += 1;
+	}
+	s->result_len = i2;
+}
+
+void fifth_order(int16_t *data, int length, int16_t *hist)
+/* for half of interleaved data */
+{
+	int i;
+	int16_t a, b, c, d, e, f;
+	a = hist[1];
+	b = hist[2];
+	c = hist[3];
+	d = hist[4];
+	e = hist[5];
+	f = data[0];
+	/* a downsample should improve resolution, so don't fully shift */
+	data[0] = (a + (b+e)*5 + (c+d)*10 + f) >> 4;
+	for (i=4; i<length; i+=4) {
+		a = c;
+		b = d;
+		c = e;
+		d = f;
+		e = data[i-2];
+		f = data[i];
+		data[i/2] = (a + (b+e)*5 + (c+d)*10 + f) >> 4;
+	}
+	/* archive */
+	hist[0] = a;
+	hist[1] = b;
+	hist[2] = c;
+	hist[3] = d;
+	hist[4] = e;
+	hist[5] = f;
+}
+
+void generic_fir(int16_t *data, int length, int *fir, int16_t *hist)
+/* Okay, not at all generic.  Assumes length 9, fix that eventually. */
+{
+	int d, temp, sum;
+	for (d=0; d<length; d+=2) {
+		temp = data[d];
+		sum = 0;
+		sum += (hist[0] + hist[8]) * fir[1];
+		sum += (hist[1] + hist[7]) * fir[2];
+		sum += (hist[2] + hist[6]) * fir[3];
+		sum += (hist[3] + hist[5]) * fir[4];
+		sum +=            hist[4]  * fir[5];
+		data[d] = sum >> 15 ;
+		hist[0] = hist[1];
+		hist[1] = hist[2];
+		hist[2] = hist[3];
+		hist[3] = hist[4];
+		hist[4] = hist[5];
+		hist[5] = hist[6];
+		hist[6] = hist[7];
+		hist[7] = hist[8];
+		hist[8] = temp;
+	}
+}
+
+/* define our own complex math ops
+   because ARMv5 has no hardware float */
+
+void multiply(int ar, int aj, int br, int bj, int *cr, int *cj)
+{
+	*cr = ar*br - aj*bj;
+	*cj = aj*br + ar*bj;
+}
+
+int polar_discriminant(int ar, int aj, int br, int bj)
+{
+	int cr, cj;
+	double angle;
+	multiply(ar, aj, br, -bj, &cr, &cj);
+	angle = atan2((double)cj, (double)cr);
+	return (int)(angle / 3.14159 * (1<<14));
+}
+
+int fast_atan2(int y, int x)
+/* pre scaled for int16 */
+{
+	int yabs, angle;
+	int pi4=(1<<12), pi34=3*(1<<12);  // note pi = 1<<14
+	if (x==0 && y==0) {
+		return 0;
+	}
+	yabs = y;
+	if (yabs < 0) {
+		yabs = -yabs;
+	}
+	if (x >= 0) {
+		angle = pi4  - pi4 * (x-yabs) / (x+yabs);
+	} else {
+		angle = pi34 - pi4 * (x+yabs) / (yabs-x);
+	}
+	if (y < 0) {
+		return -angle;
+	}
+	return angle;
+}
+
+int polar_disc_fast(int ar, int aj, int br, int bj)
+{
+	int cr, cj;
+	multiply(ar, aj, br, -bj, &cr, &cj);
+	return fast_atan2(cj, cr);
+}
+
+int atan_lut_init(void)
+{
+	int i = 0;
+
+	atan_lut = malloc(atan_lut_size * sizeof(int));
+
+	for (i = 0; i < atan_lut_size; i++) {
+		atan_lut[i] = (int) (atan((double) i / (1<<atan_lut_coef)) / 3.14159 * (1<<14));
+	}
+
+	return 0;
+}
+
+int polar_disc_lut(int ar, int aj, int br, int bj)
+{
+	int cr, cj, x, x_abs;
+
+	multiply(ar, aj, br, -bj, &cr, &cj);
+
+	/* special cases */
+	if (cr == 0 || cj == 0) {
+		if (cr == 0 && cj == 0)
+			{return 0;}
+		if (cr == 0 && cj > 0)
+			{return 1 << 13;}
+		if (cr == 0 && cj < 0)
+			{return -(1 << 13);}
+		if (cj == 0 && cr > 0)
+			{return 0;}
+		if (cj == 0 && cr < 0)
+			{return 1 << 14;}
+	}
+
+	/* real range -32768 - 32768 use 64x range -> absolute maximum: 2097152 */
+	x = (cj << atan_lut_coef) / cr;
+	x_abs = abs(x);
+
+	if (x_abs >= atan_lut_size) {
+		/* we can use linear range, but it is not necessary */
+		return (cj > 0) ? 1<<13 : -1<<13;
+	}
+
+	if (x > 0) {
+		return (cj > 0) ? atan_lut[x] : atan_lut[x] - (1<<14);
+	} else {
+		return (cj > 0) ? (1<<14) - atan_lut[-x] : -atan_lut[-x];
+	}
+
+	return 0;
+}
+
+void fm_demod(struct demod_state *fm)
+{
+	int i, pcm;
+	int16_t *lp = fm->lowpassed;
+	pcm = polar_discriminant(lp[0], lp[1],
+		fm->pre_r, fm->pre_j);
+	fm->result[0] = (int16_t)pcm;
+	for (i = 2; i < (fm->lp_len-1); i += 2) {
+		switch (fm->custom_atan) {
+		case 0:
+			pcm = polar_discriminant(lp[i], lp[i+1],
+				lp[i-2], lp[i-1]);
+			break;
+		case 1:
+			pcm = polar_disc_fast(lp[i], lp[i+1],
+				lp[i-2], lp[i-1]);
+			break;
+		case 2:
+			pcm = polar_disc_lut(lp[i], lp[i+1],
+				lp[i-2], lp[i-1]);
+			break;
+		}
+		fm->result[i/2] = (int16_t)pcm;
+	}
+	fm->pre_r = lp[fm->lp_len - 2];
+	fm->pre_j = lp[fm->lp_len - 1];
+	fm->result_len = fm->lp_len/2;
+}
+
+void am_demod(struct demod_state *fm)
+// todo, fix this extreme laziness
+{
+	int i, pcm;
+	int16_t *lp = fm->lowpassed;
+	int16_t *r  = fm->result;
+	for (i = 0; i < fm->lp_len; i += 2) {
+		// hypot uses floats but won't overflow
+		//r[i/2] = (int16_t)hypot(lp[i], lp[i+1]);
+		pcm = lp[i] * lp[i];
+		pcm += lp[i+1] * lp[i+1];
+		r[i/2] = (int16_t)sqrt(pcm) * fm->output_scale;
+	}
+	fm->result_len = fm->lp_len/2;
+	// lowpass? (3khz)  highpass?  (dc)
+}
+
+void usb_demod(struct demod_state *fm)
+{
+	int i, pcm;
+	int16_t *lp = fm->lowpassed;
+	int16_t *r  = fm->result;
+	for (i = 0; i < fm->lp_len; i += 2) {
+		pcm = lp[i] + lp[i+1];
+		r[i/2] = (int16_t)pcm * fm->output_scale;
+	}
+	fm->result_len = fm->lp_len/2;
+}
+
+void lsb_demod(struct demod_state *fm)
+{
+	int i, pcm;
+	int16_t *lp = fm->lowpassed;
+	int16_t *r  = fm->result;
+	for (i = 0; i < fm->lp_len; i += 2) {
+		pcm = lp[i] - lp[i+1];
+		r[i/2] = (int16_t)pcm * fm->output_scale;
+	}
+	fm->result_len = fm->lp_len/2;
+}
+
+void raw_demod(struct demod_state *fm)
+{
+	int i;
+	for (i = 0; i < fm->lp_len; i++) {
+		fm->result[i] = (int16_t)fm->lowpassed[i];
+	}
+	fm->result_len = fm->lp_len;
+}
+
+void deemph_filter(struct demod_state *fm)
+{
+	static int avg;  // cheating...
+	int i, d;
+	// de-emph IIR
+	// avg = avg * (1 - alpha) + sample * alpha;
+	for (i = 0; i < fm->result_len; i++) {
+		d = fm->result[i] - avg;
+		if (d > 0) {
+			avg += (d + fm->deemph_a/2) / fm->deemph_a;
+		} else {
+			avg += (d - fm->deemph_a/2) / fm->deemph_a;
+		}
+		fm->result[i] = (int16_t)avg;
+	}
+}
+
+void dc_block_filter(struct demod_state *fm)
+{
+	int i, avg;
+	int64_t sum = 0;
+	for (i=0; i < fm->result_len; i++) {
+		sum += fm->result[i];
+	}
+	avg = sum / fm->result_len;
+	avg = (avg + fm->dc_avg * 9) / 10;
+	for (i=0; i < fm->result_len; i++) {
+		fm->result[i] -= avg;
+	}
+	fm->dc_avg = avg;
+}
+
+int mad(int16_t *samples, int len, int step)
+/* mean average deviation */
+{
+	int i=0, sum=0, ave=0;
+	if (len == 0)
+		{return 0;}
+	for (i=0; i<len; i+=step) {
+		sum += samples[i];
+	}
+	ave = sum / (len * step);
+	sum = 0;
+	for (i=0; i<len; i+=step) {
+		sum += abs(samples[i] - ave);
+	}
+	return sum / (len / step);
+}
+
+int rms(int16_t *samples, int len, int step)
+/* largely lifted from rtl_power */
+{
+	int i;
+	long p, t, s;
+	double dc, err;
+
+	p = t = 0L;
+	for (i=0; i<len; i+=step) {
+		s = (long)samples[i];
+		t += s;
+		p += s * s;
+	}
+	/* correct for dc offset in squares */
+	dc = (double)(t*step) / (double)len;
+	err = t * 2 * dc - dc * dc * len;
+
+	return (int)sqrt((p-err) / len);
+}
+
+void arbitrary_upsample(int16_t *buf1, int16_t *buf2, int len1, int len2)
+/* linear interpolation, len1 < len2 */
+{
+	int i = 1;
+	int j = 0;
+	int tick = 0;
+	double frac;  // use integers...
+	while (j < len2) {
+		frac = (double)tick / (double)len2;
+		buf2[j] = (int16_t)(buf1[i-1]*(1-frac) + buf1[i]*frac);
+		j++;
+		tick += len1;
+		if (tick > len2) {
+			tick -= len2;
+			i++;
+		}
+		if (i >= len1) {
+			i = len1 - 1;
+			tick = len2;
+		}
+	}
+}
+
+void arbitrary_downsample(int16_t *buf1, int16_t *buf2, int len1, int len2)
+/* fractional boxcar lowpass, len1 > len2 */
+{
+	int i = 1;
+	int j = 0;
+	int tick = 0;
+	double remainder = 0;
+	double frac;  // use integers...
+	buf2[0] = 0;
+	while (j < len2) {
+		frac = 1.0;
+		if ((tick + len2) > len1) {
+			frac = (double)(len1 - tick) / (double)len2;}
+		buf2[j] += (int16_t)((double)buf1[i] * frac + remainder);
+		remainder = (double)buf1[i] * (1.0-frac);
+		tick += len2;
+		i++;
+		if (tick > len1) {
+			j++;
+			buf2[j] = 0;
+			tick -= len1;
+		}
+		if (i >= len1) {
+			i = len1 - 1;
+			tick = len1;
+		}
+	}
+	for (j=0; j<len2; j++) {
+		buf2[j] = buf2[j] * len2 / len1;}
+}
+
+void arbitrary_resample(int16_t *buf1, int16_t *buf2, int len1, int len2)
+/* up to you to calculate lengths and make sure it does not go OOB
+ * okay for buffers to overlap, if you are downsampling */
+{
+	if (len1 < len2) {
+		arbitrary_upsample(buf1, buf2, len1, len2);
+	} else {
+		arbitrary_downsample(buf1, buf2, len1, len2);
+	}
+}
+
+void full_demod(struct demod_state *d)
+{
+	int i, ds_p;
+	int sr = 0;
+	ds_p = d->downsample_passes;
+	if (ds_p) {
+		for (i=0; i < ds_p; i++) {
+			fifth_order(d->lowpassed,   (d->lp_len >> i), d->lp_i_hist[i]);
+			fifth_order(d->lowpassed+1, (d->lp_len >> i) - 1, d->lp_q_hist[i]);
+		}
+		d->lp_len = d->lp_len >> ds_p;
+		/* droop compensation */
+		if (d->comp_fir_size == 9 && ds_p <= CIC_TABLE_MAX) {
+			generic_fir(d->lowpassed, d->lp_len,
+				cic_9_tables[ds_p], d->droop_i_hist);
+			generic_fir(d->lowpassed+1, d->lp_len-1,
+				cic_9_tables[ds_p], d->droop_q_hist);
+		}
+	} else {
+		low_pass(d);
+	}
+	/* power squelch */
+	if (d->squelch_level) {
+		sr = rms(d->lowpassed, d->lp_len, 1);
+		if (sr < d->squelch_level) {
+			d->squelch_hits++;
+			for (i=0; i<d->lp_len; i++) {
+				d->lowpassed[i] = 0;
+			}
+		} else {
+			d->squelch_hits = 0;}
+	}
+	d->mode_demod(d);  /* lowpassed -> result */
+	if (d->mode_demod == &raw_demod) {
+		return;
+	}
+	/* todo, fm noise squelch */
+	// use nicer filter here too?
+	if (d->post_downsample > 1) {
+		d->result_len = low_pass_simple(d->result, d->result_len, d->post_downsample);}
+	if (d->deemph) {
+		deemph_filter(d);}
+	if (d->dc_block) {
+		dc_block_filter(d);}
+	if (d->rate_out2 > 0) {
+		low_pass_real(d);
+		//arbitrary_resample(d->result, d->result, d->result_len, d->result_len * d->rate_out2 / d->rate_out);
+	}
+}
+
+static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx)
+{
+	int i;
+	struct dongle_state *s = ctx;
+	struct demod_state *d = s->demod_target;
+
+	if (do_exit) {
+		return;}
+	if (!ctx) {
+		return;}
+	if (s->mute) {
+		for (i=0; i<s->mute; i++) {
+			buf[i] = 127;}
+		s->mute = 0;
+	}
+	if (!s->offset_tuning) {
+		rotate_90(buf, len);}
+	for (i=0; i<(int)len; i++) {
+		s->buf16[i] = (int16_t)buf[i] - 127;}
+	pthread_rwlock_wrlock(&d->rw);
+	memcpy(d->lowpassed, s->buf16, 2*len);
+	d->lp_len = len;
+	pthread_rwlock_unlock(&d->rw);
+	safe_cond_signal(&d->ready, &d->ready_m);
+}
+
+static void *dongle_thread_fn(void *arg)
+{
+	struct dongle_state *s = arg;
+	fprintf(stderr, "dongle_thread_fn running\n");
+	rtlsdr_read_async(s->dev, rtlsdr_callback, s, 0, s->buf_len);
+	fprintf(stderr, "dongle_thread_fn exited!\n");
+	return 0;
+}
+
+static void *demod_thread_fn(void *arg)
+{
+	struct demod_state *d = arg;
+	struct output_state *o = d->output_target;
+	fprintf(stderr, "demod_thread_fn running\n");
+	while (!do_exit) {
+		safe_cond_wait(&d->ready, &d->ready_m);
+		pthread_rwlock_wrlock(&d->rw);
+		full_demod(d);
+		pthread_rwlock_unlock(&d->rw);
+		if (d->exit_flag) {
+			do_exit = 1;
+		}
+		if (d->squelch_level && d->squelch_hits > d->conseq_squelch) {
+			d->squelch_hits = d->conseq_squelch + 1;  /* hair trigger */
+			safe_cond_signal(&controller.hop, &controller.hop_m);
+			continue;
+		}
+		pthread_rwlock_wrlock(&o->rw);
+		memcpy(o->result, d->result, 2*d->result_len);
+		o->result_len = d->result_len;
+		pthread_rwlock_unlock(&o->rw);
+		safe_cond_signal(&o->ready, &o->ready_m);
+	}
+	fprintf(stderr, "demod_thread_fn exited!\n");
+	return 0;
+}
+
+static void *output_thread_fn(void *arg)
+{
+	struct output_state *s = arg;
+	fprintf(stderr, "output_thread_fn running\n");
+	while (!do_exit) {
+		// use timedwait and pad out under runs
+		safe_cond_wait(&s->ready, &s->ready_m);
+		pthread_rwlock_rdlock(&s->rw);
+		if(s->output_fn) {
+			s->output_fn(s->result, s->result_len, s->output_fn_data);
+		}
+		pthread_rwlock_unlock(&s->rw);
+	}
+	fprintf(stderr, "output_thread_fn exited!\n");
+	return 0;
+}
+
+static void optimal_settings(int freq, int rate)
+{
+	// giant ball of hacks
+	// seems unable to do a single pass, 2:1
+	int capture_freq, capture_rate;
+	struct dongle_state *d = &dongle;
+	struct demod_state *dm = &demod;
+	struct controller_state *cs = &controller;
+	dm->downsample = (1000000 / dm->rate_in) + 1;
+	if (dm->downsample_passes) {
+		dm->downsample_passes = (int)log2(dm->downsample) + 1;
+		dm->downsample = 1 << dm->downsample_passes;
+	}
+	capture_freq = freq;
+	capture_rate = dm->downsample * dm->rate_in;
+	if (!d->offset_tuning) {
+		capture_freq = freq + capture_rate/4;}
+	capture_freq += cs->edge * dm->rate_in / 2;
+	dm->output_scale = (1<<15) / (128 * dm->downsample);
+	if (dm->output_scale < 1) {
+		dm->output_scale = 1;}
+	if (dm->mode_demod == &fm_demod) {
+		dm->output_scale = 1;}
+	d->freq = (uint32_t)capture_freq;
+	d->rate = (uint32_t)capture_rate;
+}
+
+
+void frequency_range(struct controller_state *s, char *arg)
+{
+	char *start, *stop, *step;
+	int i;
+	start = arg;
+	stop = strchr(start, ':') + 1;
+	stop[-1] = '\0';
+	step = strchr(stop, ':') + 1;
+	step[-1] = '\0';
+	for(i=(int)atofs(start); i<=(int)atofs(stop); i+=(int)atofs(step))
+	{
+		s->freqs[s->freq_len] = (uint32_t)i;
+		s->freq_len++;
+		if (s->freq_len >= FREQUENCIES_LIMIT) {
+			break;}
+	}
+	stop[-1] = ':';
+	step[-1] = ':';
+}
+
+void dongle_init(struct dongle_state *s)
+{
+	s->rate = DEFAULT_SAMPLE_RATE;
+	s->gain = AUTO_GAIN; // tenths of a dB
+	s->mute = 0;
+	s->direct_sampling = 0;
+	s->offset_tuning = 0;
+	s->demod_target = &demod;
+}
+
+void demod_init(struct demod_state *s)
+{
+	s->rate_in = DEFAULT_SAMPLE_RATE;
+	s->rate_out = DEFAULT_SAMPLE_RATE;
+	s->squelch_level = 0;
+	s->conseq_squelch = 10;
+	s->terminate_on_squelch = 0;
+	s->squelch_hits = 11;
+	s->downsample_passes = 0;
+	s->comp_fir_size = 0;
+	s->prev_index = 0;
+	s->post_downsample = 1;  // once this works, default = 4
+	s->custom_atan = 0;
+	s->deemph = 0;
+	s->rate_out2 = -1;  // flag for disabled
+	s->mode_demod = &fm_demod;
+	s->pre_j = s->pre_r = s->now_r = s->now_j = 0;
+	s->prev_lpr_index = 0;
+	s->deemph_a = 0;
+	s->now_lpr = 0;
+	s->dc_block = 0;
+	s->dc_avg = 0;
+	pthread_rwlock_init(&s->rw, NULL);
+	pthread_cond_init(&s->ready, NULL);
+	pthread_mutex_init(&s->ready_m, NULL);
+	s->output_target = &output;
+}
+
+void demod_cleanup(struct demod_state *s)
+{
+	pthread_rwlock_destroy(&s->rw);
+	pthread_cond_destroy(&s->ready);
+	pthread_mutex_destroy(&s->ready_m);
+}
+
+void output_init(struct output_state *s)
+{
+	s->rate = DEFAULT_SAMPLE_RATE;
+	s->output_fn = NULL;
+	s->output_fn_data = NULL;
+	pthread_rwlock_init(&s->rw, NULL);
+	pthread_cond_init(&s->ready, NULL);
+	pthread_mutex_init(&s->ready_m, NULL);
+}
+
+void output_cleanup(struct output_state *s)
+{
+	pthread_rwlock_destroy(&s->rw);
+	pthread_cond_destroy(&s->ready);
+	pthread_mutex_destroy(&s->ready_m);
+}
+
+void controller_init(struct controller_state *s)
+{
+	s->freqs[0] = 100000000;
+	s->freq_len = 0;
+	s->edge = 0;
+	s->wb_mode = 0;
+	pthread_cond_init(&s->hop, NULL);
+	pthread_mutex_init(&s->hop_m, NULL);
+}
+
+void controller_cleanup(struct controller_state *s)
+{
+	pthread_cond_destroy(&s->hop);
+	pthread_mutex_destroy(&s->hop_m);
+}
+
+void sanity_checks(void)
+{
+	if (controller.freq_len == 0) {
+		fprintf(stderr, "Please specify a frequency.\n");
+		exit(1);
+	}
+
+	if (controller.freq_len >= FREQUENCIES_LIMIT) {
+		fprintf(stderr, "Too many channels, maximum %i.\n", FREQUENCIES_LIMIT);
+		exit(1);
+	}
+
+	if (controller.freq_len > 1 && demod.squelch_level == 0) {
+		fprintf(stderr, "Please specify a squelch level.  Required for scanning multiple frequencies.\n");
+		exit(1);
+	}
+
+}
+
+int rtl_fm_init(uint32_t freq,
+		uint32_t sample_rate,
+		uint32_t resample_rate,
+		rtl_fm_output_fn_t output_fn,
+		void *output_fn_data)
+{
+	int r = 0;
+
+	dongle_init(&dongle);
+	demod_init(&demod);
+	output_init(&output);
+	controller_init(&controller);
+
+	/*
+	 * Simulate the effects of command line arguments:
+	 *
+	 * -W wbfm -s <sample rate> -r <resample rate>
+	 */
+
+	/* Set initial frequency */
+	controller.freqs[0] = freq;
+	controller.freq_len++;
+
+	/* Set mode to wbfm */
+	controller.wb_mode = 1;
+	demod.mode_demod = &fm_demod;
+	demod.rate_in = 170000;
+	demod.rate_out = 170000;
+	demod.rate_out2 = 32000;
+	demod.custom_atan = 1;
+	//demod.post_downsample = 4;
+	demod.deemph = 1;
+	demod.squelch_level = 0;
+
+	/* Set sample rate */
+	demod.rate_in = sample_rate;
+	demod.rate_out = sample_rate;
+
+	/* Set resample rate */
+	output.rate = (int) resample_rate;
+	demod.rate_out2 = (int) resample_rate;
+
+	/* Set output function pointer */
+	if(output_fn) {
+		output.output_fn = output_fn;
+		output.output_fn_data = output_fn_data;
+	}
+
+	/* quadruple sample_rate to limit to Δθ to ±π/2 */
+	demod.rate_in *= demod.post_downsample;
+
+	if (!output.rate) {
+		output.rate = demod.rate_out;
+	}
+
+	sanity_checks();
+
+	if (controller.freq_len > 1) {
+		demod.terminate_on_squelch = 0;
+	}
+
+	ACTUAL_BUF_LENGTH = lcm_post[demod.post_downsample] * DEFAULT_BUF_LENGTH;
+
+	dongle.dev_index = verbose_device_search("0");
+	if (dongle.dev_index < 0) {
+		return -1;
+	}
+
+	r = rtlsdr_open(&dongle.dev, (uint32_t)dongle.dev_index);
+	if (r < 0) {
+		fprintf(stderr, "Failed to open rtlsdr device #%d.\n", dongle.dev_index);
+		return r;
+	}
+
+	if (demod.deemph) {
+		demod.deemph_a = (int)round(1.0/((1.0-exp(-1.0/(demod.rate_out * 75e-6)))));
+	}
+
+	/* Set the tuner gain */
+	if (dongle.gain == AUTO_GAIN) {
+		verbose_auto_gain(dongle.dev);
+	} else {
+		dongle.gain = nearest_gain(dongle.dev, dongle.gain);
+		verbose_gain_set(dongle.dev, dongle.gain);
+	}
+
+	verbose_ppm_set(dongle.dev, dongle.ppm_error);
+
+	//r = rtlsdr_set_testmode(dongle.dev, 1);
+
+	return r;
+}
+
+void rtl_fm_start(void)
+{
+	int i;
+	struct controller_state *s = &controller;
+
+	/*
+	 * A bunch of the following is pulled from the controller_thread_fn,
+	 * which has been removed.
+	 */
+
+	/* Reset endpoint before we start reading from it (mandatory) */
+	verbose_reset_buffer(dongle.dev);
+
+	if (s->wb_mode) {
+		for (i=0; i < s->freq_len; i++) {
+			s->freqs[i] += 16000;}
+	}
+
+	/* set up primary channel */
+	optimal_settings(s->freqs[0], demod.rate_in);
+	if (dongle.direct_sampling) {
+		verbose_direct_sampling(dongle.dev, 1);}
+	if (dongle.offset_tuning) {
+		verbose_offset_tuning(dongle.dev);}
+
+	/* Set the frequency */
+	verbose_set_frequency(dongle.dev, dongle.freq);
+	fprintf(stderr, "Oversampling input by: %ix.\n", demod.downsample);
+	fprintf(stderr, "Oversampling output by: %ix.\n", demod.post_downsample);
+	fprintf(stderr, "Buffer size: %0.2fms\n",
+		1000 * 0.5 * (float)ACTUAL_BUF_LENGTH / (float)dongle.rate);
+
+	/* Set the sample rate */
+	verbose_set_sample_rate(dongle.dev, dongle.rate);
+	fprintf(stderr, "Output at %u Hz.\n", demod.rate_in/demod.post_downsample);
+	usleep(100000);
+
+	pthread_create(&output.thread, NULL, output_thread_fn, (void *)(&output));
+	pthread_create(&demod.thread, NULL, demod_thread_fn, (void *)(&demod));
+	pthread_create(&dongle.thread, NULL, dongle_thread_fn, (void *)(&dongle));
+}
+
+void rtl_fm_set_freq(uint32_t freq)
+{
+	int i;
+	struct controller_state *s = &controller;
+	struct demod_state *dm = &demod;
+
+	s->freqs[0] = freq;
+	s->freq_len = 1;
+
+	if (s->wb_mode) {
+		for (i=0; i < s->freq_len; i++) {
+			s->freqs[i] += 16000;}
+	}
+
+	optimal_settings(s->freqs[0], demod.rate_in);
+	if (dongle.offset_tuning) {
+		verbose_offset_tuning(dongle.dev);}
+	rtlsdr_set_center_freq(dongle.dev, dongle.freq);
+
+	// It does not look like refreshing the sample rate is desirable
+	// (e.g. the scanning code in the removed controller thread function
+	// did not do it), and behavior seemed a bit less robust with it
+	// present.  However, I am leaving this here as a reminder to revisit
+	// via some more testing.
+	//rtlsdr_set_sample_rate(dongle.dev, dongle.rate);
+
+	// This triggers a mute during the frequency change
+	dongle.mute = BUFFER_DUMP;
+}
+
+void rtl_fm_stop(void)
+{
+	rtlsdr_cancel_async(dongle.dev);
+	pthread_join(dongle.thread, NULL);
+	safe_cond_signal(&demod.ready, &demod.ready_m);
+	pthread_join(demod.thread, NULL);
+	safe_cond_signal(&output.ready, &output.ready_m);
+	pthread_join(output.thread, NULL);
+}
+
+void rtl_fm_cleanup(void)
+{
+	//dongle_cleanup(&dongle);
+	demod_cleanup(&demod);
+	output_cleanup(&output);
+	controller_cleanup(&controller);
+
+	rtlsdr_close(dongle.dev);
+}
+
+// vim: tabstop=8:softtabstop=8:shiftwidth=8:noexpandtab