From b81e1e6d226fa5b20f0107f8e9d3faee9e2f7fed Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 09:38:52 -0500 Subject: [PATCH 01/48] =?UTF-8?q?rtl=5Fsdr=5Ffm:=20fix=20atan2/LUT=20discr?= =?UTF-8?q?iminator=20=E2=80=94=20use=20precise=20=CF=80,=20quadrant-corre?= =?UTF-8?q?ct=20LUT=20saturation,=20clamp=20=C2=B1=CF=80=20at=20wrap,=20an?= =?UTF-8?q?d=20mutex-protect=20LUT=20free.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 3 +- src/rtl_sdr_fm.cpp | 169 +++++++++++++++++++++++++++++++++------------ 2 files changed, 128 insertions(+), 44 deletions(-) diff --git a/.gitignore b/.gitignore index 3327894..e2a9a5c 100644 --- a/.gitignore +++ b/.gitignore @@ -9,4 +9,5 @@ examples *.ans *.patch *.zip -result \ No newline at end of file +result +.cache \ No newline at end of file diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index eefcb8b..7d81dec 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include #include @@ -43,9 +42,25 @@ 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 const double kPi = 3.14159265358979323846; + static int *atan_lut = NULL; static int atan_lut_size = 131072; /* 512 KB */ static int atan_lut_coef = 8; +static pthread_once_t atan_lut_once = PTHREAD_ONCE_INIT; +static pthread_mutex_t atan_lut_mutex = PTHREAD_MUTEX_INITIALIZER; + +static void atan_lut_once_init(void) +{ + int i; + atan_lut = static_cast(malloc(atan_lut_size * sizeof(int))); + if (atan_lut == NULL) { + return; + } + for (i = 0; i < atan_lut_size; i++) { + atan_lut[i] = (int) (atan((double) i / (1< @@ -106,6 +121,7 @@ struct demod_state int now_lpr; int prev_lpr_index; int dc_block, dc_avg; + int (*discriminator)(int, int, int, int); void (*mode_demod)(struct demod_state*); pthread_rwlock_t rw; pthread_cond_t ready; @@ -304,13 +320,20 @@ void multiply(int ar, int aj, int br, int bj, int *cr, int *cj) *cj = aj*br + ar*bj; } +/* 64-bit safe complex multiply to prevent overflow in discriminator math */ +static inline void multiply64(int ar, int aj, int br, int bj, int64_t *cr, int64_t *cj) +{ + *cr = (int64_t)ar * (int64_t)br - (int64_t)aj * (int64_t)bj; + *cj = (int64_t)aj * (int64_t)br + (int64_t)ar * (int64_t)bj; +} + int polar_discriminant(int ar, int aj, int br, int bj) { - int cr, cj; + int64_t cr, cj; double angle; - multiply(ar, aj, br, -bj, &cr, &cj); + multiply64(ar, aj, br, -bj, &cr, &cj); angle = atan2((double)cj, (double)cr); - return (int)(angle / 3.14159 * (1<<14)); + return (int)(angle / kPi * (1<<14)); } int fast_atan2(int y, int x) @@ -336,31 +359,79 @@ int fast_atan2(int y, int x) return angle; } +/* 64-bit safe version of fast atan2 to avoid overflow in intermediate math */ +int fast_atan2_64(int64_t y, int64_t x) +/* pre scaled for int16, returns angle scaled so that pi == 1<<14 */ +{ + int angle; + int pi4=(1<<12), pi34=3*(1<<12); /* note: pi = 1<<14 */ + int64_t yabs; + if (x == 0 && y == 0) { + return 0; + } + yabs = y; + if (yabs < 0) { + yabs = -yabs; + } + if (x >= 0) { + /* denominator (x + yabs) cannot be zero here unless x==y==0 handled above */ + angle = (int)(pi4 - ( (int64_t)pi4 * (x - yabs) ) / (x + yabs)); + } else { + /* denominator (yabs - x) > 0 */ + angle = (int)(pi34 - ( (int64_t)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); + int64_t cr, cj; + multiply64(ar, aj, br, -bj, &cr, &cj); + return fast_atan2_64(cj, cr); } int atan_lut_init(void) { - int i = 0; - - atan_lut = static_cast(malloc(atan_lut_size * sizeof(int))); - - for (i = 0; i < atan_lut_size; i++) { - atan_lut[i] = (int) (atan((double) i / (1< 0) {return 0;} if (cj == 0 && cr < 0) - {return 1 << 14;} + {return (1 << 14) - 1;} } /* real range -32768 - 32768 use 64x range -> absolute maximum: 2097152 */ - x = (cj << atan_lut_coef) / cr; - x_abs = abs(x); + x = ((int64_t)cj << atan_lut_coef) / cr; + x_abs = (x < 0) ? -x : 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_abs >= (int64_t)atan_lut_size) { + /* Preserve quadrant using both cr and cj signs */ + if (cr < 0) { + return (cj >= 0) ? ((1 << 14) - 1) : (-(1 << 14) + 1); + } else { + return (cj >= 0) ? (1 << 13) : -(1 << 13); + } } if (x > 0) { - return (cj > 0) ? atan_lut[x] : atan_lut[x] - (1<<14); + int val = (cj > 0) ? atan_lut[(int)x] : (atan_lut[(int)x] - (1<<14)); + if (val == (1 << 14)) { val = (1 << 14) - 1; } + if (val == -(1 << 14)) { val = -(1 << 14) + 1; } + return val; } else { - return (cj > 0) ? (1<<14) - atan_lut[-x] : -atan_lut[-x]; + int val = (cj > 0) ? ((1<<14) - atan_lut[(int)(-x)]) : (-atan_lut[(int)(-x)]); + if (val == (1 << 14)) { val = (1 << 14) - 1; } + if (val == -(1 << 14)) { val = -(1 << 14) + 1; } + return val; } return 0; @@ -398,24 +479,11 @@ 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); + /* Use selected discriminator from the very first sample */ + pcm = fm->discriminator(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; - } + pcm = fm->discriminator(lp[i], lp[i+1], lp[i-2], lp[i-1]); fm->result[i/2] = (int16_t)pcm; } fm->pre_r = lp[fm->lp_len - 2]; @@ -827,7 +895,7 @@ void demod_init_analog(struct demod_state *s) s->comp_fir_size = 0; s->prev_index = 0; s->post_downsample = 1; //1 -- once this works, default = 4 -- doesn't work on the official rtl-sdr source code either - s->custom_atan = 0; + s->custom_atan = 2; s->deemph = 1; // s->rate_out2 = rtl_bandwidth; // -1 flag for disabled -- this enables low_pass_real, seems to work okay s->mode_demod = &fm_demod; @@ -841,6 +909,10 @@ void demod_init_analog(struct demod_state *s) pthread_cond_init(&s->ready, NULL); pthread_mutex_init(&s->ready_m, NULL); s->output_target = &output; + if (s->custom_atan == 2 && atan_lut == NULL) { atan_lut_init(); } + /* set discriminator function pointer */ + s->discriminator = (s->custom_atan == 0) ? &polar_discriminant : + (s->custom_atan == 1) ? &polar_disc_fast : &polar_disc_lut; } void demod_init_ro2(struct demod_state *s) @@ -855,7 +927,7 @@ void demod_init_ro2(struct demod_state *s) s->comp_fir_size = 0; s->prev_index = 0; s->post_downsample = 1; //1 -- once this works, default = 4 -- doesn't work on the official rtl-sdr source code either - s->custom_atan = 0; + s->custom_atan = 2; s->deemph = 0; s->rate_out2 = rtl_bandwidth; // -1 flag for disabled -- this enables low_pass_real, seems to work okay s->mode_demod = &fm_demod; @@ -869,6 +941,10 @@ void demod_init_ro2(struct demod_state *s) pthread_cond_init(&s->ready, NULL); pthread_mutex_init(&s->ready_m, NULL); s->output_target = &output; + if (s->custom_atan == 2 && atan_lut == NULL) { atan_lut_init(); } + /* set discriminator function pointer */ + s->discriminator = (s->custom_atan == 0) ? &polar_discriminant : + (s->custom_atan == 1) ? &polar_disc_fast : &polar_disc_lut; } void demod_init(struct demod_state *s) @@ -883,7 +959,7 @@ void demod_init(struct demod_state *s) s->comp_fir_size = 0; s->prev_index = 0; s->post_downsample = 1; //1 -- once this works, default = 4 -- doesn't work on the official rtl-sdr source code either - s->custom_atan = 0; + s->custom_atan = 2; s->deemph = 0; s->rate_out2 = -1; // -1 flag for disabled -- this enables low_pass_real, seems to work okay s->mode_demod = &fm_demod; @@ -897,6 +973,10 @@ void demod_init(struct demod_state *s) pthread_cond_init(&s->ready, NULL); pthread_mutex_init(&s->ready_m, NULL); s->output_target = &output; + if (s->custom_atan == 2 && atan_lut == NULL) { atan_lut_init(); } + /* set discriminator function pointer */ + s->discriminator = (s->custom_atan == 0) ? &polar_discriminant : + (s->custom_atan == 1) ? &polar_disc_fast : &polar_disc_lut; } void demod_cleanup(struct demod_state *s) @@ -1123,6 +1203,9 @@ void cleanup_rtlsdr_stream() output_cleanup(&output); controller_cleanup(&controller); + /* free LUT memory if allocated */ + atan_lut_free(); + rtlsdr_close(dongle.dev); } From 3adcf5f4494e224da6484f908537ac399c87f5e2 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 12:21:47 -0500 Subject: [PATCH 02/48] rtl_sdr_fm: replace std::queue + locks with a single-producer single-consumer ring buffer --- src/rtl_sdr_fm.cpp | 166 +++++++++++++++++++++++++++++++++++++-------- 1 file changed, 137 insertions(+), 29 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 7d81dec..c197af1 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -26,7 +26,7 @@ #include #include #include -#include +#include #include #include "dsd.h" @@ -132,9 +132,13 @@ struct demod_state struct output_state { int rate; - std::queue queue; + int16_t *buffer; + size_t capacity; + std::atomic head; + std::atomic tail; pthread_rwlock_t rw; pthread_cond_t ready; + pthread_cond_t space; pthread_mutex_t ready_m; }; @@ -159,6 +163,111 @@ struct controller_state controller; #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) +/* ===================== + SPSC Ring Buffer (output path) + ===================== */ + +static inline size_t ring_used(const struct output_state *o) +{ + size_t h = o->head.load(); + size_t t = o->tail.load(); + if (h >= t) return h - t; + return o->capacity - (t - h); +} + +static inline size_t ring_free(const struct output_state *o) +{ + return (o->capacity - 1) - ring_used(o); +} + +static inline int ring_is_empty(const struct output_state *o) +{ + return o->head.load() == o->tail.load(); +} + +static inline void ring_clear(struct output_state *o) +{ + o->tail.store(0); + o->head.store(0); +} + +/* Write up to count samples, blocking until space is available. Signals data availability after writes. */ +static void ring_write(struct output_state *o, const int16_t *data, size_t count) +{ + while (count > 0 && !exitflag) { + size_t free_sp = ring_free(o); + if (free_sp == 0) { + /* Wait for space */ + safe_cond_wait(&o->space, &o->ready_m); + continue; + } + size_t write_now = (count < free_sp) ? count : free_sp; + size_t h = o->head.load(); + size_t first = o->capacity - h; + if (first > write_now) first = write_now; + memcpy(o->buffer + h, data, first * sizeof(int16_t)); + if (write_now > first) { + memcpy(o->buffer, data + first, (write_now - first) * sizeof(int16_t)); + h = write_now - first; + } else { + h += first; + if (h == o->capacity) h = 0; + } + o->head.store(h); + data += write_now; + count -= write_now; + } +} + +/* Same as ring_write but does not signal; caller decides when to signal */ +static void ring_write_no_signal(struct output_state *o, const int16_t *data, size_t count) +{ + while (count > 0 && !exitflag) { + size_t free_sp = ring_free(o); + if (free_sp == 0) { + safe_cond_wait(&o->space, &o->ready_m); + continue; + } + size_t write_now = (count < free_sp) ? count : free_sp; + size_t h = o->head.load(); + size_t first = o->capacity - h; + if (first > write_now) first = write_now; + memcpy(o->buffer + h, data, first * sizeof(int16_t)); + if (write_now > first) { + memcpy(o->buffer, data + first, (write_now - first) * sizeof(int16_t)); + h = write_now - first; + } else { + h += first; + if (h == o->capacity) h = 0; + } + o->head.store(h); + data += write_now; + count -= write_now; + } +} + +/* Read one sample, returns 0 on success, -1 on exit */ +static int ring_read_one(struct output_state *o, int16_t *out) +{ + while (ring_is_empty(o)) { + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_nsec += 10e6; /* 10ms */ + pthread_mutex_lock(&o->ready_m); + pthread_cond_timedwait(&o->ready, &o->ready_m, &ts); + pthread_mutex_unlock(&o->ready_m); + if (exitflag) return -1; + } + size_t t = o->tail.load(); + *out = o->buffer[t]; + t += 1; + if (t == o->capacity) t = 0; + o->tail.store(t); + /* Signal space available for producer */ + safe_cond_signal(&o->space, &o->ready_m); + return 0; +} + /* {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 @@ -656,13 +765,17 @@ static void *demod_thread_fn(void *arg) safe_cond_signal(&controller.hop, &controller.hop_m); continue; } - pthread_rwlock_wrlock(&o->rw); - for (int i = 0; i < d->result_len; i++) - { - for (int j=0; j < bandwidth_multiplier; j++){ - o->queue.push(d->result[i]);} + /* Write demod block to SPSC ring, duplicating per bandwidth_multiplier as before. + Wake consumer once per produced block. */ + if (bandwidth_multiplier <= 1) { + ring_write_no_signal(o, d->result, (size_t)d->result_len); + } else { + for (int i = 0; i < d->result_len; i++) { + for (int j = 0; j < bandwidth_multiplier; j++) { + ring_write_no_signal(o, &d->result[i], 1); + } + } } - pthread_rwlock_unlock(&o->rw); safe_cond_signal(&o->ready, &o->ready_m); } return 0; @@ -991,14 +1104,22 @@ void output_init(struct output_state *s) s->rate = rtl_bandwidth; pthread_rwlock_init(&s->rw, NULL); pthread_cond_init(&s->ready, NULL); + pthread_cond_init(&s->space, NULL); pthread_mutex_init(&s->ready_m, NULL); + /* Allocate SPSC ring buffer */ + s->capacity = (size_t)(MAXIMUM_BUF_LENGTH * 8); + s->buffer = static_cast(malloc(s->capacity * sizeof(int16_t))); + s->head.store(0); + s->tail.store(0); } void output_cleanup(struct output_state *s) { pthread_rwlock_destroy(&s->rw); pthread_cond_destroy(&s->ready); + pthread_cond_destroy(&s->space); pthread_mutex_destroy(&s->ready_m); + if (s->buffer) { free(s->buffer); s->buffer = NULL; } } void controller_init(struct controller_state *s) @@ -1234,25 +1355,11 @@ int get_rtlsdr_sample(int16_t *sample, dsd_opts * opts, dsd_state * state) verbose_ppm_set(dongle.dev, dongle.ppm_error); } - while (output.queue.empty()) - { - struct timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); - ts.tv_nsec += 10e6; - - pthread_mutex_lock(&output.ready_m); - pthread_cond_timedwait(&output.ready, &output.ready_m, &ts); - pthread_mutex_unlock(&output.ready_m); - - if (exitflag) - { - return -1; - } + int16_t tmp; + if (ring_read_one(&output, &tmp) < 0) { + return -1; } - pthread_rwlock_wrlock(&output.rw); - *sample = output.queue.front() * volume_multiplier; - output.queue.pop(); - pthread_rwlock_unlock(&output.rw); + *sample = tmp * volume_multiplier; return 0; } @@ -1295,7 +1402,8 @@ long int rtl_return_rms() //simple function to clear the rtl sample queue when tuning and during other events (ncurses menu open/close) void rtl_clean_queue() { - //insert method to clear the entire queue to prevent sample 'lag' - std::queue empty; //create an empty queue - std::swap( output.queue, empty ); //swap in empty queue to effectively zero out current queue + /* Clear the entire ring to prevent sample 'lag' */ + ring_clear(&output); + /* Wake producer waiting for space */ + safe_cond_signal(&output.space, &output.ready_m); } \ No newline at end of file From 3fe14f3b1d27d1e69b477f30c45b76488f4a67c9 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 13:30:32 -0500 Subject: [PATCH 03/48] rtl_sdr_fm: double-buffer the I/Q handoff and remove memcpy/locks between callback and demod --- src/rtl_sdr_fm.cpp | 54 ++++++++++++++++++++++++++++++++++------------ 1 file changed, 40 insertions(+), 14 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index c197af1..187e19a 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -83,7 +83,6 @@ struct dongle_state uint32_t freq; uint32_t rate; int gain; - uint16_t buf16[MAXIMUM_BUF_LENGTH]; uint32_t buf_len; int ppm_error; int offset_tuning; @@ -96,7 +95,12 @@ struct demod_state { int exit_flag; pthread_t thread; - int16_t lowpassed[MAXIMUM_BUF_LENGTH]; + int16_t *lowpassed; + /* Double-buffered input for callback→demod handoff */ + int16_t input_buffers[2][MAXIMUM_BUF_LENGTH]; + std::atomic write_buf_index; + std::atomic ready_buf_index; + std::atomic input_len[2]; int lp_len; int16_t lp_i_hist[10][6]; int16_t lp_q_hist[10][6]; @@ -123,7 +127,6 @@ struct demod_state int dc_block, dc_avg; int (*discriminator)(int, int, int, int); void (*mode_demod)(struct demod_state*); - pthread_rwlock_t rw; pthread_cond_t ready; pthread_mutex_t ready_m; struct output_state *output_target; @@ -732,12 +735,16 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) } if (!s->offset_tuning) { rotate_90(buf, len);} + /* Write directly into the current write buffer */ + int wb = d->write_buf_index.load(); + int16_t *dst = d->input_buffers[wb]; 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); + dst[i] = (int16_t)buf[i] - 127; + } + d->input_len[wb].store(len); + /* Flip buffers atomically: the buffer we just wrote becomes ready */ + d->ready_buf_index.store(wb); + d->write_buf_index.store(wb ^ 1); safe_cond_signal(&d->ready, &d->ready_m); } @@ -754,9 +761,11 @@ static void *demod_thread_fn(void *arg) struct output_state *o = d->output_target; while (!exitflag) { safe_cond_wait(&d->ready, &d->ready_m); - pthread_rwlock_wrlock(&d->rw); + /* Consume from last fully-written input buffer */ + int rb = d->ready_buf_index.load(); + d->lowpassed = d->input_buffers[rb]; + d->lp_len = (int)d->input_len[rb].load(); full_demod(d); - pthread_rwlock_unlock(&d->rw); if (d->exit_flag) { exitflag = 1; } @@ -1018,7 +1027,13 @@ void demod_init_analog(struct demod_state *s) s->now_lpr = 0; s->dc_block = 1; // s->dc_avg = 0; - pthread_rwlock_init(&s->rw, NULL); + /* Double-buffer init */ + s->write_buf_index.store(0); + s->ready_buf_index.store(1); + s->input_len[0].store(0); + s->input_len[1].store(0); + s->lowpassed = s->input_buffers[s->ready_buf_index.load()]; + s->lp_len = 0; pthread_cond_init(&s->ready, NULL); pthread_mutex_init(&s->ready_m, NULL); s->output_target = &output; @@ -1050,7 +1065,13 @@ void demod_init_ro2(struct demod_state *s) s->now_lpr = 0; s->dc_block = 1; //enabling by default, but offset tuning is also enabled, so center spike shouldn't be an issue s->dc_avg = 0; - pthread_rwlock_init(&s->rw, NULL); + /* Double-buffer init */ + s->write_buf_index.store(0); + s->ready_buf_index.store(1); + s->input_len[0].store(0); + s->input_len[1].store(0); + s->lowpassed = s->input_buffers[s->ready_buf_index.load()]; + s->lp_len = 0; pthread_cond_init(&s->ready, NULL); pthread_mutex_init(&s->ready_m, NULL); s->output_target = &output; @@ -1082,7 +1103,13 @@ void demod_init(struct demod_state *s) s->now_lpr = 0; s->dc_block = 1; //enabling by default, but offset tuning is also enabled, so center spike shouldn't be an issue s->dc_avg = 0; - pthread_rwlock_init(&s->rw, NULL); + /* Double-buffer init */ + s->write_buf_index.store(0); + s->ready_buf_index.store(1); + s->input_len[0].store(0); + s->input_len[1].store(0); + s->lowpassed = s->input_buffers[s->ready_buf_index.load()]; + s->lp_len = 0; pthread_cond_init(&s->ready, NULL); pthread_mutex_init(&s->ready_m, NULL); s->output_target = &output; @@ -1094,7 +1121,6 @@ void demod_init(struct demod_state *s) void demod_cleanup(struct demod_state *s) { - pthread_rwlock_destroy(&s->rw); pthread_cond_destroy(&s->ready); pthread_mutex_destroy(&s->ready_m); } From 03c6b28de37e15f91f89dcf063706d10f9ffe3c6 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 13:36:26 -0500 Subject: [PATCH 04/48] rtl_sdr_fm: initialize async buffer length and consider buffer count --- src/rtl_sdr_fm.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 187e19a..b70fc54 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -1297,6 +1297,8 @@ void open_rtlsdr_stream(dsd_opts *opts) if (controller.freq_len > 1) demod.terminate_on_squelch = 0; ACTUAL_BUF_LENGTH = lcm_post[demod.post_downsample] * DEFAULT_BUF_LENGTH; + /* Ensure async read uses a valid, explicit buffer length */ + dongle.buf_len = (uint32_t)ACTUAL_BUF_LENGTH; r = rtlsdr_open(&dongle.dev, (uint32_t)dongle.dev_index); if (r < 0) From cc200ea82c098e0f68deb388d9e43c75b0f5fb57 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 13:48:33 -0500 Subject: [PATCH 05/48] rtl_sdr: make RMS squelch sqrt-free --- src/rtl_sdr_fm.cpp | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index b70fc54..0d00207 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -671,7 +671,6 @@ long int rms(int16_t *samples, int len, int step) 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++) { @@ -689,16 +688,32 @@ void full_demod(struct demod_state *d) } else { low_pass(d); } - /* power squelch */ + /* power squelch (sqrt-free): compare mean power to squared threshold */ if (d->squelch_level) { - sr = rms(d->lowpassed, d->lp_len, 1); - if (sr < d->squelch_level) { + long int pwr = 0; + { + int j; + long p = 0L, t = 0L, s; + double dc, err; + for (j = 0; j < d->lp_len; j += 1) { + s = (long)d->lowpassed[j]; + t += s; + p += s * s; + } + dc = (double)t; /* step is 1 */ + err = t * 2 * dc - dc * dc * d->lp_len; + pwr = (long int)((p - err) / (d->lp_len ? d->lp_len : 1)); + if (pwr < 0) pwr = 0; + } + long int thr2 = (long int)d->squelch_level * (long int)d->squelch_level; + if (pwr < thr2) { d->squelch_hits++; for (i=0; ilp_len; i++) { d->lowpassed[i] = 0; } } else { - d->squelch_hits = 0;} + d->squelch_hits = 0; + } } d->mode_demod(d); /* lowpassed -> result */ if (d->mode_demod == &raw_demod) { From 62a9372d61e34d88df1e5e5b197730cfacf2f408 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 14:03:38 -0500 Subject: [PATCH 06/48] rtl_sdr: make async buffer params explicit and safe --- src/rtl_sdr_fm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 0d00207..ad4b267 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -766,7 +766,7 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) static void *dongle_thread_fn(void *arg) { struct dongle_state *s = static_cast(arg); - rtlsdr_read_async(s->dev, rtlsdr_callback, s, 0, s->buf_len); + rtlsdr_read_async(s->dev, rtlsdr_callback, s, 16, s->buf_len); return 0; } From c6d3e4cea040fd76734fa36b71b27bff577302e4 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 14:08:19 -0500 Subject: [PATCH 07/48] rtl_sdr_fm: removed the obsolete rwlock and lock sites --- src/rtl_sdr_fm.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index ad4b267..2657f64 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -139,7 +139,6 @@ struct output_state size_t capacity; std::atomic head; std::atomic tail; - pthread_rwlock_t rw; pthread_cond_t ready; pthread_cond_t space; pthread_mutex_t ready_m; @@ -1143,7 +1142,6 @@ void demod_cleanup(struct demod_state *s) void output_init(struct output_state *s) { s->rate = rtl_bandwidth; - pthread_rwlock_init(&s->rw, NULL); pthread_cond_init(&s->ready, NULL); pthread_cond_init(&s->space, NULL); pthread_mutex_init(&s->ready_m, NULL); @@ -1156,7 +1154,6 @@ void output_init(struct output_state *s) void output_cleanup(struct output_state *s) { - pthread_rwlock_destroy(&s->rw); pthread_cond_destroy(&s->ready); pthread_cond_destroy(&s->space); pthread_mutex_destroy(&s->ready_m); @@ -1420,9 +1417,7 @@ void rtl_dev_tune(dsd_opts * opts, long int frequency) if (r < 0) fprintf (stderr, " (WARNING: Failed to set Center Frequency %u). \n", dongle.freq); - pthread_rwlock_wrlock(&output.rw); //prevent possible segfault when cleaning the queue rtl_clean_queue(); - pthread_rwlock_unlock(&output.rw); //resume } From e700e418be51eaa6064cf2c89bc169822553d8ca Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 14:17:07 -0500 Subject: [PATCH 08/48] rtl_sdr_fm: make RMS squelch incremental and decimated --- src/rtl_sdr_fm.cpp | 67 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 51 insertions(+), 16 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 2657f64..f78aaa9 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -118,6 +118,11 @@ struct demod_state int post_downsample; int output_scale; int squelch_level, conseq_squelch, squelch_hits, terminate_on_squelch; + /* Incremental, decimated RMS squelch estimator (power-domain, sqrt-free) */ + int64_t squelch_running_power; + int squelch_decim_stride; + int squelch_decim_phase; + int squelch_window; int downsample_passes; int comp_fir_size; int custom_atan; @@ -689,23 +694,38 @@ void full_demod(struct demod_state *d) } /* power squelch (sqrt-free): compare mean power to squared threshold */ if (d->squelch_level) { - long int pwr = 0; - { - int j; - long p = 0L, t = 0L, s; - double dc, err; - for (j = 0; j < d->lp_len; j += 1) { - s = (long)d->lowpassed[j]; - t += s; - p += s * s; - } - dc = (double)t; /* step is 1 */ - err = t * 2 * dc - dc * dc * d->lp_len; - pwr = (long int)((p - err) / (d->lp_len ? d->lp_len : 1)); - if (pwr < 0) pwr = 0; + /* Decimated block power estimate (no DC correction; EMA smooths) */ + int stride = (d->squelch_decim_stride > 0) ? d->squelch_decim_stride : 16; + int phase = d->squelch_decim_phase; + int64_t p = 0; + int count = 0; + for (int j = phase; j < d->lp_len; j += stride) { + int64_t s2 = (int64_t)d->lowpassed[j]; + p += s2 * s2; + count++; } - long int thr2 = (long int)d->squelch_level * (long int)d->squelch_level; - if (pwr < thr2) { + /* Advance phase to sample different positions next block */ + if (stride > 0) { + int adv = d->lp_len % stride; + d->squelch_decim_phase = (phase + adv) % stride; + } + if (count > 0) { + int64_t block_mean = p / count; + if (d->squelch_running_power == 0) { + /* Initialize on first measurement to avoid long ramp */ + d->squelch_running_power = block_mean; + } else { + /* EMA: running += (block_mean - running) / window */ + int w = (d->squelch_window > 0) ? d->squelch_window : 2048; + int shift = 0; + /* approximate log2(window), prefer power-of-two windows */ + while ((1 << shift) < w && shift < 30) { shift++; } + int64_t delta = (block_mean - d->squelch_running_power); + d->squelch_running_power += (delta >> shift); + } + } + int64_t thr2 = (int64_t)d->squelch_level * (int64_t)d->squelch_level; + if (d->squelch_running_power < thr2) { d->squelch_hits++; for (i=0; ilp_len; i++) { d->lowpassed[i] = 0; @@ -1041,6 +1061,11 @@ void demod_init_analog(struct demod_state *s) s->now_lpr = 0; s->dc_block = 1; // s->dc_avg = 0; + /* Squelch estimator init */ + s->squelch_running_power = 0; + s->squelch_decim_stride = 16; /* evaluate 1/16th samples for low CPU */ + s->squelch_decim_phase = 0; + s->squelch_window = 2048; /* EMA window ~2048 samples */ /* Double-buffer init */ s->write_buf_index.store(0); s->ready_buf_index.store(1); @@ -1079,6 +1104,11 @@ void demod_init_ro2(struct demod_state *s) s->now_lpr = 0; s->dc_block = 1; //enabling by default, but offset tuning is also enabled, so center spike shouldn't be an issue s->dc_avg = 0; + /* Squelch estimator init */ + s->squelch_running_power = 0; + s->squelch_decim_stride = 16; + s->squelch_decim_phase = 0; + s->squelch_window = 2048; /* Double-buffer init */ s->write_buf_index.store(0); s->ready_buf_index.store(1); @@ -1117,6 +1147,11 @@ void demod_init(struct demod_state *s) s->now_lpr = 0; s->dc_block = 1; //enabling by default, but offset tuning is also enabled, so center spike shouldn't be an issue s->dc_avg = 0; + /* Squelch estimator init */ + s->squelch_running_power = 0; + s->squelch_decim_stride = 16; + s->squelch_decim_phase = 0; + s->squelch_window = 2048; /* Double-buffer init */ s->write_buf_index.store(0); s->ready_buf_index.store(1); From 30766b34bde9af1168f5e0355d64a904b3660fd1 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 14:21:50 -0500 Subject: [PATCH 09/48] rtl_sdr_fm: fix timespec normalization in timed wait --- src/rtl_sdr_fm.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index f78aaa9..065aca3 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -259,7 +259,11 @@ static int ring_read_one(struct output_state *o, int16_t *out) while (ring_is_empty(o)) { struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); - ts.tv_nsec += 10e6; /* 10ms */ + ts.tv_nsec += 10L * 1000000L; /* 10ms */ + if (ts.tv_nsec >= 1000000000L) { + ts.tv_sec += ts.tv_nsec / 1000000000L; + ts.tv_nsec = ts.tv_nsec % 1000000000L; + } pthread_mutex_lock(&o->ready_m); pthread_cond_timedwait(&o->ready, &o->ready_m, &ts); pthread_mutex_unlock(&o->ready_m); From 7dd9cd756678a2274cc6dcc442def8a1b86d394d Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 14:51:45 -0500 Subject: [PATCH 10/48] rtl_sdr_fm: add batched consumer API --- src/rtl_sdr_fm.cpp | 79 ++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 65 insertions(+), 14 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 065aca3..92a8b01 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -279,6 +279,43 @@ static int ring_read_one(struct output_state *o, int16_t *out) return 0; } +/* Read up to max_count samples into out. Blocks until at least one sample is available or exit. Returns + number of samples read (>=1) or -1 on exit. Signals producer space once after the batch. */ +static int ring_read_batch(struct output_state *o, int16_t *out, size_t max_count) +{ + if (max_count == 0) return 0; + while (ring_is_empty(o)) { + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_nsec += 10L * 1000000L; /* 10ms */ + if (ts.tv_nsec >= 1000000000L) { + ts.tv_sec += ts.tv_nsec / 1000000000L; + ts.tv_nsec = ts.tv_nsec % 1000000000L; + } + pthread_mutex_lock(&o->ready_m); + pthread_cond_timedwait(&o->ready, &o->ready_m, &ts); + pthread_mutex_unlock(&o->ready_m); + if (exitflag) return -1; + } + + size_t available = ring_used(o); + size_t read_now = (max_count < available) ? max_count : available; + size_t t = o->tail.load(); + size_t first = o->capacity - t; + if (first > read_now) first = read_now; + memcpy(out, o->buffer + t, first * sizeof(int16_t)); + t += first; + if (t == o->capacity) t = 0; + if (read_now > first) { + memcpy(out + first, o->buffer, (read_now - first) * sizeof(int16_t)); + t = read_now - first; + } + o->tail.store(t); + /* Signal space available once for the whole batch */ + safe_cond_signal(&o->space, &o->ready_m); + return (int)read_now; +} + /* {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 @@ -1409,6 +1446,31 @@ void cleanup_rtlsdr_stream() rtlsdr_close(dongle.dev); } +/* Batched consumer API: read up to count samples with fewer wakeups/locks. + Returns number of samples read (>=1) or -1 on exit. Applies volume scaling. */ +int get_rtlsdr_samples(int16_t *out, size_t count, dsd_opts * opts, dsd_state * state) +{ + UNUSED(state); + if (count == 0) return 0; + + /* If PPM Error is Manually Changed, change it here once per batch */ + if (opts->rtlsdr_ppm_error != dongle.ppm_error) + { + dongle.ppm_error = opts->rtlsdr_ppm_error; + verbose_ppm_set(dongle.dev, dongle.ppm_error); + } + + int got = ring_read_batch(&output, out, count); + if (got <= 0) { + return -1; + } + /* Apply volume scaling */ + for (int i = 0; i < got; i++) { + out[i] = out[i] * volume_multiplier; + } + return got; +} + //original for safe keeping // void get_rtlsdr_sample(int16_t *sample, dsd_opts * opts, dsd_state * state) // { @@ -1425,20 +1487,9 @@ void cleanup_rtlsdr_stream() //find way to modify this function to allow hopping (tuning) while squelched and send 0 sample? int get_rtlsdr_sample(int16_t *sample, dsd_opts * opts, dsd_state * state) { - UNUSED(state); - - //if PPM Error is Manually Changed, Change it here now - if (opts->rtlsdr_ppm_error != dongle.ppm_error) - { - dongle.ppm_error = opts->rtlsdr_ppm_error; - verbose_ppm_set(dongle.dev, dongle.ppm_error); - } - - int16_t tmp; - if (ring_read_one(&output, &tmp) < 0) { - return -1; - } - *sample = tmp * volume_multiplier; + /* Delegate to batched API for a single sample */ + int ret = get_rtlsdr_samples(sample, 1, opts, state); + if (ret < 0) return -1; return 0; } From 0decc5934c9dc49f2ab29124ce7a9017605f3ffe Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 14:58:33 -0500 Subject: [PATCH 11/48] rtl_sdr_fm: add batch operations and thread scheduling --- src/rtl_sdr_fm.cpp | 69 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 69 insertions(+) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 92a8b01..d8cea9b 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include "dsd.h" @@ -170,6 +171,64 @@ struct controller_state controller; #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) +/* ===================== + Thread Scheduling Helpers (optional realtime/affinity) + ===================== */ + +static void maybe_set_thread_realtime_and_affinity(const char *role) +{ + const char *enable = getenv("DSD_FME_RT_SCHED"); + if (!enable || enable[0] != '1') { + return; + } + + /* Optional: role-specific priority (1..99) for SCHED_FIFO */ + int policy = SCHED_FIFO; + struct sched_param sp; + int pmax = sched_get_priority_max(policy); + int pmin = sched_get_priority_min(policy); + int def = (pmax > 10) ? (pmax - 10) : pmax; /* default near top, but safe */ + char envname[64]; + + sp.sched_priority = def; + if (role) { + /* e.g., DSD_FME_RT_PRIO_DEMOD, DSD_FME_RT_PRIO_DONGLE */ + snprintf(envname, sizeof(envname), "DSD_FME_RT_PRIO_%s", role); + const char *prio_str = getenv(envname); + if (prio_str && prio_str[0] != '\0') { + int pr = atoi(prio_str); + if (pr < pmin) pr = pmin; + if (pr > pmax) pr = pmax; + sp.sched_priority = pr; + } + } + + if (pthread_setschedparam(pthread_self(), policy, &sp) != 0) { + fprintf(stderr, "WARNING: Failed to set %s thread to SCHED_FIFO (needs CAP_SYS_NICE).\n", role ? role : "RT"); + } else { + fprintf(stderr, "%s thread SCHED_FIFO priority set to %d.\n", role ? role : "RT", sp.sched_priority); + } + + /* Optional: role-specific CPU affinity: DSD_FME_CPU_DEMOD / DSD_FME_CPU_DONGLE */ + if (role) { + snprintf(envname, sizeof(envname), "DSD_FME_CPU_%s", role); + const char *cpu_str = getenv(envname); + if (cpu_str && cpu_str[0] != '\0') { + int cpu = atoi(cpu_str); + if (cpu >= 0) { + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + CPU_SET((unsigned)cpu, &cpuset); + if (pthread_setaffinity_np(pthread_self(), sizeof(cpuset), &cpuset) != 0) { + fprintf(stderr, "WARNING: Failed to set CPU affinity for %s thread to CPU %d.\n", role, cpu); + } else { + fprintf(stderr, "%s thread pinned to CPU %d.\n", role, cpu); + } + } + } + } +} + /* ===================== SPSC Ring Buffer (output path) ===================== */ @@ -798,6 +857,14 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) int i; struct dongle_state *s = static_cast(ctx); struct demod_state *d = s->demod_target; + /* One-time: ensure the USB callback thread gets RT scheduling/affinity if enabled */ + { + static std::atomic usb_sched_applied{0}; + int expected = 0; + if (usb_sched_applied.compare_exchange_strong(expected, 1)) { + maybe_set_thread_realtime_and_affinity("USB"); + } + } if (exitflag) { return;} @@ -826,6 +893,7 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) static void *dongle_thread_fn(void *arg) { struct dongle_state *s = static_cast(arg); + maybe_set_thread_realtime_and_affinity("DONGLE"); rtlsdr_read_async(s->dev, rtlsdr_callback, s, 16, s->buf_len); return 0; } @@ -834,6 +902,7 @@ static void *demod_thread_fn(void *arg) { struct demod_state *d = static_cast(arg); struct output_state *o = d->output_target; + maybe_set_thread_realtime_and_affinity("DEMOD"); while (!exitflag) { safe_cond_wait(&d->ready, &d->ready_m); /* Consume from last fully-written input buffer */ From 2ec3f9a2630b84063f4adfae17bd7b6051419cd0 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 15:14:26 -0500 Subject: [PATCH 12/48] rtl_sdr_fm: hoist divisions and use integer scalers in hot loops --- src/rtl_sdr_fm.cpp | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index d8cea9b..cb8e561 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -458,6 +458,11 @@ void low_pass_real(struct demod_state *s) int i=0, i2=0; int fast = (int)s->rate_out; int slow = s->rate_out2; + /* Precompute fixed-point reciprocal of decimation factor to avoid per-sample division */ + int decim = (slow != 0) ? (fast / slow) : 1; + if (decim < 1) decim = 1; + const int kShiftLPR = 15; /* Q15 reciprocal */ + int recip_decim_q = (1 << kShiftLPR) / decim; while (i < s->result_len) { s->now_lpr += s->result[i]; i++; @@ -465,7 +470,9 @@ void low_pass_real(struct demod_state *s) if (s->prev_lpr_index < fast) { continue; } - s->result[i2] = (int16_t)(s->now_lpr / (fast/slow)); + /* Multiply by reciprocal and shift instead of dividing by (fast/slow) */ + int64_t scaled = ((int64_t)s->now_lpr * recip_decim_q); + s->result[i2] = (int16_t)(scaled >> kShiftLPR); s->prev_lpr_index -= fast; s->now_lpr = 0; i2 += 1; @@ -720,15 +727,23 @@ void deemph_filter(struct demod_state *fm) { static int avg; // cheating... int i, d; + /* Precompute fixed-point reciprocal of deemphasis constant to avoid per-sample division */ + const int kShiftDeemph = 15; /* Q15 */ + int a = fm->deemph_a; + if (a <= 0) a = 1; + int recip_q = (1 << kShiftDeemph) / a; // de-emph IIR // avg = avg * (1 - alpha) + sample * alpha; for (i = 0; i < fm->result_len; i++) { d = fm->result[i] - avg; + /* Use multiply+shift with sign-aware rounding to mirror original behavior */ + int64_t delta = (int64_t)d * recip_q; if (d > 0) { - avg += (d + fm->deemph_a/2) / fm->deemph_a; - } else { - avg += (d - fm->deemph_a/2) / fm->deemph_a; + delta += (1LL << (kShiftDeemph - 1)); + } else if (d < 0) { + delta -= (1LL << (kShiftDeemph - 1)); } + avg += (int)(delta >> kShiftDeemph); fm->result[i] = (int16_t)avg; } } From 9b399f31da053e9d01c13536b74e26b11224f04f Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 15:25:15 -0500 Subject: [PATCH 13/48] rtl_sdr_fm: enable vectorization and alignment for DSP arrays --- src/rtl_sdr_fm.cpp | 58 ++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 48 insertions(+), 10 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index cb8e561..5adf676 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -45,6 +45,27 @@ static int ACTUAL_BUF_LENGTH; static const double kPi = 3.14159265358979323846; +/* ===================== + Vectorization helpers and alignment + ===================== */ +#if defined(__GNUC__) || defined(__clang__) +#define DSD_FME_PRAGMA(x) _Pragma(#x) +#define DSD_FME_IVDEP DSD_FME_PRAGMA(GCC ivdep) +template +static inline T* assume_aligned_ptr(T* p, size_t /*align_unused*/) { + return (T*)__builtin_assume_aligned(p, 64); +} +#else +#define DSD_FME_IVDEP +template +static inline T* assume_aligned_ptr(T* p, size_t /*align_unused*/) { + return p; +} +#endif +#ifndef DSD_FME_ALIGN +#define DSD_FME_ALIGN 64 +#endif + static int *atan_lut = NULL; static int atan_lut_size = 131072; /* 512 KB */ static int atan_lut_coef = 8; @@ -98,14 +119,14 @@ struct demod_state pthread_t thread; int16_t *lowpassed; /* Double-buffered input for callback→demod handoff */ - int16_t input_buffers[2][MAXIMUM_BUF_LENGTH]; + alignas(DSD_FME_ALIGN) int16_t input_buffers[2][MAXIMUM_BUF_LENGTH]; std::atomic write_buf_index; std::atomic ready_buf_index; std::atomic input_len[2]; int lp_len; int16_t lp_i_hist[10][6]; int16_t lp_q_hist[10][6]; - int16_t result[MAXIMUM_BUF_LENGTH]; + alignas(DSD_FME_ALIGN) int16_t result[MAXIMUM_BUF_LENGTH]; int16_t droop_i_hist[9]; int16_t droop_q_hist[9]; int result_len; @@ -456,6 +477,7 @@ void low_pass_real(struct demod_state *s) // add support for upsampling? { int i=0, i2=0; + int16_t *r = assume_aligned_ptr(s->result, DSD_FME_ALIGN); int fast = (int)s->rate_out; int slow = s->rate_out2; /* Precompute fixed-point reciprocal of decimation factor to avoid per-sample division */ @@ -463,8 +485,9 @@ void low_pass_real(struct demod_state *s) if (decim < 1) decim = 1; const int kShiftLPR = 15; /* Q15 reciprocal */ int recip_decim_q = (1 << kShiftLPR) / decim; + DSD_FME_IVDEP while (i < s->result_len) { - s->now_lpr += s->result[i]; + s->now_lpr += r[i]; i++; s->prev_lpr_index += slow; if (s->prev_lpr_index < fast) { @@ -472,7 +495,7 @@ void low_pass_real(struct demod_state *s) } /* Multiply by reciprocal and shift instead of dividing by (fast/slow) */ int64_t scaled = ((int64_t)s->now_lpr * recip_decim_q); - s->result[i2] = (int16_t)(scaled >> kShiftLPR); + r[i2] = (int16_t)(scaled >> kShiftLPR); s->prev_lpr_index -= fast; s->now_lpr = 0; i2 += 1; @@ -701,13 +724,15 @@ int polar_disc_lut(int ar, int aj, int br, int bj) void fm_demod(struct demod_state *fm) { int i, pcm; - int16_t *lp = fm->lowpassed; + int16_t *lp = assume_aligned_ptr(fm->lowpassed, DSD_FME_ALIGN); + int16_t *res = assume_aligned_ptr(fm->result, DSD_FME_ALIGN); /* Use selected discriminator from the very first sample */ pcm = fm->discriminator(lp[0], lp[1], fm->pre_r, fm->pre_j); - fm->result[0] = (int16_t)pcm; + res[0] = (int16_t)pcm; + DSD_FME_IVDEP for (i = 2; i < (fm->lp_len-1); i += 2) { pcm = fm->discriminator(lp[i], lp[i+1], lp[i-2], lp[i-1]); - fm->result[i/2] = (int16_t)pcm; + res[i/2] = (int16_t)pcm; } fm->pre_r = lp[fm->lp_len - 2]; fm->pre_j = lp[fm->lp_len - 1]; @@ -727,6 +752,7 @@ void deemph_filter(struct demod_state *fm) { static int avg; // cheating... int i, d; + int16_t *res = assume_aligned_ptr(fm->result, DSD_FME_ALIGN); /* Precompute fixed-point reciprocal of deemphasis constant to avoid per-sample division */ const int kShiftDeemph = 15; /* Q15 */ int a = fm->deemph_a; @@ -734,8 +760,9 @@ void deemph_filter(struct demod_state *fm) int recip_q = (1 << kShiftDeemph) / a; // de-emph IIR // avg = avg * (1 - alpha) + sample * alpha; + DSD_FME_IVDEP for (i = 0; i < fm->result_len; i++) { - d = fm->result[i] - avg; + d = res[i] - avg; /* Use multiply+shift with sign-aware rounding to mirror original behavior */ int64_t delta = (int64_t)d * recip_q; if (d > 0) { @@ -744,7 +771,7 @@ void deemph_filter(struct demod_state *fm) delta -= (1LL << (kShiftDeemph - 1)); } avg += (int)(delta >> kShiftDeemph); - fm->result[i] = (int16_t)avg; + res[i] = (int16_t)avg; } } @@ -1307,7 +1334,18 @@ void output_init(struct output_state *s) pthread_mutex_init(&s->ready_m, NULL); /* Allocate SPSC ring buffer */ s->capacity = (size_t)(MAXIMUM_BUF_LENGTH * 8); - s->buffer = static_cast(malloc(s->capacity * sizeof(int16_t))); + /* Try aligned allocation for better vectorized copies; fall back if unavailable */ + { + void *mem_ptr = NULL; +#if defined(_POSIX_C_SOURCE) && (_POSIX_C_SOURCE >= 200112L) + if (posix_memalign(&mem_ptr, DSD_FME_ALIGN, s->capacity * sizeof(int16_t)) != 0) { + mem_ptr = malloc(s->capacity * sizeof(int16_t)); + } +#else + mem_ptr = malloc(s->capacity * sizeof(int16_t)); +#endif + s->buffer = static_cast(mem_ptr); + } s->head.store(0); s->tail.store(0); } From 2eedcc125b0bd875bfbf060d1401b1d02768379a Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 17:01:06 -0500 Subject: [PATCH 14/48] rtl_sdr_fm: fix deemphasis state race --- src/rtl_sdr_fm.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 5adf676..853601e 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -149,6 +149,7 @@ struct demod_state int comp_fir_size; int custom_atan; int deemph, deemph_a; + int deemph_avg; int now_lpr; int prev_lpr_index; int dc_block, dc_avg; @@ -750,7 +751,7 @@ void raw_demod(struct demod_state *fm) void deemph_filter(struct demod_state *fm) { - static int avg; // cheating... + int avg = fm->deemph_avg; /* per-instance state */ int i, d; int16_t *res = assume_aligned_ptr(fm->result, DSD_FME_ALIGN); /* Precompute fixed-point reciprocal of deemphasis constant to avoid per-sample division */ @@ -758,13 +759,12 @@ void deemph_filter(struct demod_state *fm) int a = fm->deemph_a; if (a <= 0) a = 1; int recip_q = (1 << kShiftDeemph) / a; - // de-emph IIR - // avg = avg * (1 - alpha) + sample * alpha; + /* Single-pole IIR: avg += (x - avg) * (1 - a) with fixed-point scaling */ DSD_FME_IVDEP for (i = 0; i < fm->result_len; i++) { d = res[i] - avg; - /* Use multiply+shift with sign-aware rounding to mirror original behavior */ int64_t delta = (int64_t)d * recip_q; + /* symmetric rounding */ if (d > 0) { delta += (1LL << (kShiftDeemph - 1)); } else if (d < 0) { @@ -773,6 +773,7 @@ void deemph_filter(struct demod_state *fm) avg += (int)(delta >> kShiftDeemph); res[i] = (int16_t)avg; } + fm->deemph_avg = avg; /* write back state */ } void dc_block_filter(struct demod_state *fm) @@ -1210,6 +1211,7 @@ void demod_init_analog(struct demod_state *s) s->pre_j = s->pre_r = s->now_r = s->now_j = 0; s->prev_lpr_index = 0; s->deemph_a = 0; // + s->deemph_avg = 0; s->now_lpr = 0; s->dc_block = 1; // s->dc_avg = 0; @@ -1253,6 +1255,7 @@ void demod_init_ro2(struct demod_state *s) s->pre_j = s->pre_r = s->now_r = s->now_j = 0; s->prev_lpr_index = 0; s->deemph_a = 0; + s->deemph_avg = 0; s->now_lpr = 0; s->dc_block = 1; //enabling by default, but offset tuning is also enabled, so center spike shouldn't be an issue s->dc_avg = 0; @@ -1296,6 +1299,7 @@ void demod_init(struct demod_state *s) s->pre_j = s->pre_r = s->now_r = s->now_j = 0; s->prev_lpr_index = 0; s->deemph_a = 0; + s->deemph_avg = 0; s->now_lpr = 0; s->dc_block = 1; //enabling by default, but offset tuning is also enabled, so center spike shouldn't be an issue s->dc_avg = 0; From 412a4dc3b13be6a15698aeffd7e2cac96580c510 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 17:05:28 -0500 Subject: [PATCH 15/48] rtl_sdr_fm: add saturating arithmetic for decimation and volume scaling --- src/rtl_sdr_fm.cpp | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 853601e..e0b4684 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -72,6 +72,16 @@ static int atan_lut_coef = 8; static pthread_once_t atan_lut_once = PTHREAD_ONCE_INIT; static pthread_mutex_t atan_lut_mutex = PTHREAD_MUTEX_INITIALIZER; +/* ===================== + Saturating helpers + ===================== */ +static inline int16_t sat16(int32_t x) +{ + if (x > 32767) return 32767; + if (x < -32768) return -32768; + return (int16_t)x; +} + static void atan_lut_once_init(void) { int i; @@ -447,8 +457,9 @@ void low_pass(struct demod_state *d) 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; + /* Saturate accumulated sums when writing back to int16 */ + d->lowpassed[i2] = sat16(d->now_r); + d->lowpassed[i2+1] = sat16(d->now_j); d->prev_index = 0; d->now_r = 0; d->now_j = 0; @@ -467,7 +478,8 @@ int low_pass_simple(int16_t *signal2, int len, int step) sum += (int)signal2[i + i2]; } //signal2[i/step] = (int16_t)(sum / step); - signal2[i/step] = (int16_t)(sum); + /* Saturate accumulated sum on write */ + signal2[i/step] = sat16(sum); } signal2[i/step + 1] = signal2[i/step]; return len / step; @@ -1590,9 +1602,10 @@ int get_rtlsdr_samples(int16_t *out, size_t count, dsd_opts * opts, dsd_state * if (got <= 0) { return -1; } - /* Apply volume scaling */ + /* Apply volume scaling with saturation */ for (int i = 0; i < got; i++) { - out[i] = out[i] * volume_multiplier; + int32_t y = (int32_t)out[i] * (int32_t)volume_multiplier; + out[i] = sat16(y); } return got; } From f3ec024000c0efbffe29931bb0a58c7438fcbb4d Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 17:18:00 -0500 Subject: [PATCH 16/48] rtl_sdr_fm: replace DC block with leaky integrator high-pass (IIR) --- src/rtl_sdr_fm.cpp | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index e0b4684..5c05c70 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -790,17 +790,19 @@ void deemph_filter(struct demod_state *fm) 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]; + int i; + /* Leaky integrator high-pass: dc += (x - dc) >> k; y = x - dc */ + int dc = fm->dc_avg; + const int k = 11; /* cutoff ~ Fs / 2^k (k in 10..12) */ + int16_t *res = assume_aligned_ptr(fm->result, DSD_FME_ALIGN); + DSD_FME_IVDEP + for (i = 0; i < fm->result_len; i++) { + int x = (int)res[i]; + dc += (x - dc) >> k; + int y = x - dc; + res[i] = sat16(y); } - 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; + fm->dc_avg = dc; } long int rms(int16_t *samples, int len, int step) From 29be104345d5e4cd00e777893ca6b453237e588e Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 17:30:38 -0500 Subject: [PATCH 17/48] rtl_sdr_fm: replace sample duplication with simple resampling (linear interpolation) --- src/rtl_sdr_fm.cpp | 33 ++++++++++++++++++++++++++++----- 1 file changed, 28 insertions(+), 5 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 5c05c70..e03a19c 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include "dsd.h" @@ -975,15 +976,37 @@ static void *demod_thread_fn(void *arg) safe_cond_signal(&controller.hop, &controller.hop_m); continue; } - /* Write demod block to SPSC ring, duplicating per bandwidth_multiplier as before. - Wake consumer once per produced block. */ + /* Write demod block to SPSC ring. If upsampling (bandwidth_multiplier > 1), + linearly interpolate between adjacent samples instead of duplicating. */ if (bandwidth_multiplier <= 1) { ring_write_no_signal(o, d->result, (size_t)d->result_len); } else { - for (int i = 0; i < d->result_len; i++) { - for (int j = 0; j < bandwidth_multiplier; j++) { - ring_write_no_signal(o, &d->result[i], 1); + const int M = bandwidth_multiplier; + const int N = d->result_len; + if (N <= 0) { + /* nothing to write */ + } else if (N == 1) { + /* Degenerate case: only one sample, replicate M times */ + std::vector tmp(M); + for (int m = 0; m < M; m++) tmp[m] = d->result[0]; + ring_write_no_signal(o, tmp.data(), (size_t)M); + } else { + /* N >= 2: perform linear interpolation between successive samples */ + const size_t up_len = (size_t)N * (size_t)M; + std::vector upsampled; + upsampled.resize(up_len); + for (int n = 0; n < N - 1; n++) { + int16_t x0 = d->result[n]; + int16_t x1 = d->result[n + 1]; + int32_t dx = (int32_t)x1 - (int32_t)x0; + for (int m = 0; m < M; m++) { + int32_t interp = (int32_t)x0 + (dx * m) / M; + upsampled[(size_t)n * (size_t)M + (size_t)m] = (int16_t)interp; + } } + /* Last original sample maps to the last position */ + upsampled[(size_t)(N - 1) * (size_t)M] = d->result[N - 1]; + ring_write_no_signal(o, upsampled.data(), up_len); } } safe_cond_signal(&o->ready, &o->ready_m); From 402cef34a94f38d74b9cd47b1056f8533f074f20 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 17:53:44 -0500 Subject: [PATCH 18/48] rtl_sdr_fm: improve decimation filters (half-band FIR cascade) --- src/rtl_sdr_fm.cpp | 175 ++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 164 insertions(+), 11 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index e03a19c..9630609 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -83,6 +83,109 @@ static inline int16_t sat16(int32_t x) return (int16_t)x; } +/* ===================== + Half-band FIR decimator (2:1) - Q15 taps + ===================== */ + +/* Runtime flag (default enabled). Set DSD_FME_HB_DECIM=0 to use legacy decimator */ +static int use_halfband_decimator = 1; + +/* 15-tap half-band low-pass coefficients, Q15 scaled. + Odd-indexed taps are zero; center tap is 0.5 (16384). The remaining even taps + sum to 0.5 to yield unity DC gain. Coefficients are symmetric. */ +#define HB_TAPS 15 +#define HB_HALF ((HB_TAPS - 1) / 2) +static const int16_t hb_q15_taps[HB_TAPS] = { + -108, 0, 1800, 0, -500, 0, 7000, 16384, + 7000, 0, -500, 0, 1800, 0, -108 +}; + +/* Decimate one real channel by 2 using half-band FIR with persistent left history. + - in: pointer to real samples + - in_len: number of real samples + - out: pointer to output buffer (size >= in_len/2) + - hist: persistent history of length HB_TAPS-1 (left wing) + Returns number of output samples written (in_len/2). */ +static inline int hb_decim2_real(const int16_t *in, int in_len, int16_t *out, int16_t *hist) +{ + const int hist_len = HB_TAPS - 1; + /* Pad right side by repeating last sample to avoid needing future context */ + int16_t last = (in_len > 0) ? in[in_len - 1] : 0; + /* For simplicity, operate via a small ringless window into a temp view using hist + in + right pad (virtually). */ + int out_len = in_len >> 1; /* floor */ + for (int n = 0; n < out_len; n++) { + int center_idx = hist_len + (n << 1); /* position in the concatenated [hist | in] domain */ + int64_t acc = 0; + /* Convolution around center: taps indexed 0..HB_TAPS-1 */ + for (int t = 0; t < HB_TAPS; t++) { + int src_idx = center_idx - HB_HALF + t; + int16_t x; + if (src_idx < hist_len) { + x = hist[src_idx]; + } else { + int rel = src_idx - hist_len; + x = (rel < in_len) ? in[rel] : last; + } + acc += (int32_t)hb_q15_taps[t] * (int32_t)x; + } + /* Q15 -> Q0 with rounding */ + acc += (1 << 14); + int32_t y = (int32_t)(acc >> 15); + out[n] = sat16(y); + } + /* Update history with the last (HB_TAPS-1) input samples for next call */ + if (in_len >= hist_len) { + memcpy(hist, in + (in_len - hist_len), (size_t)hist_len * sizeof(int16_t)); + } else { + /* Not enough samples: keep tail of previous hist and append current */ + int need = hist_len - in_len; + if (need > 0) { + /* shift left existing hist */ + memmove(hist, hist + in_len, (size_t)need * sizeof(int16_t)); + } + memcpy(hist + need, in, (size_t)in_len * sizeof(int16_t)); + } + return out_len; +} + +/* One 2:1 decimation stage on interleaved I/Q, using per-stage histories. */ +static inline int hb_decim2_complex_stage(const int16_t *in_iq, int in_iq_len, + int16_t *out_iq, int16_t *hist_i, int16_t *hist_q) +{ + /* in_iq_len is count of interleaved samples (I,Q,I,Q,...) */ + int ch_len = in_iq_len >> 1; /* samples per channel */ + if (ch_len <= 0) { + return 0; + } + /* Deinterleave into contiguous I and Q working buffers */ + std::vector i_buf; + std::vector q_buf; + i_buf.resize((size_t)ch_len); + q_buf.resize((size_t)ch_len); + for (int k = 0, j = 0; j < in_iq_len; j += 2, k++) { + i_buf[(size_t)k] = in_iq[(size_t)j]; + q_buf[(size_t)k] = in_iq[(size_t)j + 1]; + } + /* Output per channel */ + int out_ch_len; + { + /* Reuse i_buf as input; produce into temporary then interleave */ + std::vector i_out; + std::vector q_out; + i_out.resize((size_t)(ch_len >> 1)); + q_out.resize((size_t)(ch_len >> 1)); + int ilen = hb_decim2_real(i_buf.data(), ch_len, i_out.data(), hist_i); + int qlen = hb_decim2_real(q_buf.data(), ch_len, q_out.data(), hist_q); + out_ch_len = (ilen < qlen) ? ilen : qlen; + /* Interleave back */ + for (int n = 0; n < out_ch_len; n++) { + out_iq[(size_t)(2*n)] = i_out[(size_t)n]; + out_iq[(size_t)(2*n + 1)] = q_out[(size_t)n]; + } + } + return out_ch_len << 1; /* interleaved sample count */ +} + static void atan_lut_once_init(void) { int i; @@ -164,6 +267,10 @@ struct demod_state int now_lpr; int prev_lpr_index; int dc_block, dc_avg; + /* Half-band decimator state */ + int16_t hb_workbuf[MAXIMUM_BUF_LENGTH]; + int16_t hb_hist_i[10][HB_TAPS-1]; + int16_t hb_hist_q[10][HB_TAPS-1]; int (*discriminator)(int, int, int, int); void (*mode_demod)(struct demod_state*); pthread_cond_t ready; @@ -835,17 +942,39 @@ void full_demod(struct demod_state *d) int i, ds_p; 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); + /* Choose decimator: half-band cascade (default) or legacy path */ + if (use_halfband_decimator) { + /* Apply ds_p stages of 2:1 half-band decimation on interleaved lowpassed */ + int in_len = d->lp_len; + int16_t *src = d->lowpassed; + int16_t *dst = d->hb_workbuf; + for (i = 0; i < ds_p; i++) { + int out_len = hb_decim2_complex_stage(src, in_len, dst, d->hb_hist_i[i], d->hb_hist_q[i]); + /* Next stage uses previous output as input */ + src = dst; + in_len = out_len; + /* swap buffers for next stage to avoid overwrite if needed */ + dst = (src == d->hb_workbuf) ? d->lowpassed : d->hb_workbuf; + } + /* Final output resides in 'src' with length in_len */ + if (d->lowpassed != src) { + memcpy(d->lowpassed, src, (size_t)in_len * sizeof(int16_t)); + } + d->lp_len = in_len; + /* No droop compensation for half-band cascade */ + } else { + 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); @@ -1257,6 +1386,11 @@ void demod_init_analog(struct demod_state *s) s->squelch_decim_stride = 16; /* evaluate 1/16th samples for low CPU */ s->squelch_decim_phase = 0; s->squelch_window = 2048; /* EMA window ~2048 samples */ + /* HB decimator histories */ + for (int st = 0; st < 10; st++) { + memset(s->hb_hist_i[st], 0, sizeof(s->hb_hist_i[st])); + memset(s->hb_hist_q[st], 0, sizeof(s->hb_hist_q[st])); + } /* Double-buffer init */ s->write_buf_index.store(0); s->ready_buf_index.store(1); @@ -1301,6 +1435,11 @@ void demod_init_ro2(struct demod_state *s) s->squelch_decim_stride = 16; s->squelch_decim_phase = 0; s->squelch_window = 2048; + /* HB decimator histories */ + for (int st = 0; st < 10; st++) { + memset(s->hb_hist_i[st], 0, sizeof(s->hb_hist_i[st])); + memset(s->hb_hist_q[st], 0, sizeof(s->hb_hist_q[st])); + } /* Double-buffer init */ s->write_buf_index.store(0); s->ready_buf_index.store(1); @@ -1345,6 +1484,11 @@ void demod_init(struct demod_state *s) s->squelch_decim_stride = 16; s->squelch_decim_phase = 0; s->squelch_window = 2048; + /* HB decimator histories */ + for (int st = 0; st < 10; st++) { + memset(s->hb_hist_i[st], 0, sizeof(s->hb_hist_i[st])); + memset(s->hb_hist_q[st], 0, sizeof(s->hb_hist_q[st])); + } /* Double-buffer init */ s->write_buf_index.store(0); s->ready_buf_index.store(1); @@ -1518,6 +1662,15 @@ void open_rtlsdr_stream(dsd_opts *opts) output_init(&output); controller_init(&controller); + /* Read optional environment flag for half-band decimator */ + { + const char *hb = getenv("DSD_FME_HB_DECIM"); + if (hb && hb[0] != '\0') { + int v = atoi(hb); + use_halfband_decimator = (v != 0); + } + } + if (opts->rtlsdr_center_freq > 0) { controller.freqs[controller.freq_len] = opts->rtlsdr_center_freq; controller.freq_len++; From e2402d032821845522f6803cbe481e2d39220d77 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 18:08:22 -0500 Subject: [PATCH 19/48] rtl_sdr_fm: correct deemphasis coefficient derivation and make it configurable --- src/rtl_sdr_fm.cpp | 40 +++++++++++++++++++++++++++++++++------- 1 file changed, 33 insertions(+), 7 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 9630609..f28dc4f 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -874,16 +875,16 @@ void deemph_filter(struct demod_state *fm) int avg = fm->deemph_avg; /* per-instance state */ int i, d; int16_t *res = assume_aligned_ptr(fm->result, DSD_FME_ALIGN); - /* Precompute fixed-point reciprocal of deemphasis constant to avoid per-sample division */ + /* Q15 alpha = (1 - a), where a = exp(-1/(Fs*tau)) */ const int kShiftDeemph = 15; /* Q15 */ - int a = fm->deemph_a; - if (a <= 0) a = 1; - int recip_q = (1 << kShiftDeemph) / a; - /* Single-pole IIR: avg += (x - avg) * (1 - a) with fixed-point scaling */ + int alpha_q15 = fm->deemph_a; + if (alpha_q15 < 0) alpha_q15 = 0; + if (alpha_q15 > (1 << kShiftDeemph)) alpha_q15 = (1 << kShiftDeemph); + /* Single-pole IIR: avg += (x - avg) * alpha */ DSD_FME_IVDEP for (i = 0; i < fm->result_len; i++) { d = res[i] - avg; - int64_t delta = (int64_t)d * recip_q; + int64_t delta = (int64_t)d * (int64_t)alpha_q15; /* symmetric rounding */ if (d > 0) { delta += (1LL << (kShiftDeemph - 1)); @@ -1714,7 +1715,32 @@ void open_rtlsdr_stream(dsd_opts *opts) } if (demod.deemph) { - demod.deemph_a = (int)round(1.0/((1.0-exp(-1.0/(demod.rate_out * 75e-6))))); + /* Configure deemphasis time constant via env DSD_FME_DEEMPH: 75 (default), 50, nfm, off */ + double tau_s = 75e-6; /* default 75 microseconds */ + const char *deemph_env = getenv("DSD_FME_DEEMPH"); + if (deemph_env && deemph_env[0] != '\0') { + if (strcasecmp(deemph_env, "off") == 0) { + demod.deemph = 0; + } else if (strcmp(deemph_env, "50") == 0) { + tau_s = 50e-6; + } else if (strcasecmp(deemph_env, "nfm") == 0) { + /* Common NFM value */ + tau_s = 750e-6; + } else if (strcmp(deemph_env, "75") == 0) { + tau_s = 75e-6; + } + } + if (demod.deemph) { + /* a = exp(-1/(Fs*tau)); store alpha=(1-a) in Q15 */ + double Fs = (double)demod.rate_out; + if (Fs < 1.0) Fs = 1.0; + double a = exp(-1.0 / (Fs * tau_s)); + double alpha = 1.0 - a; + int coef_q15 = (int)lrint(alpha * (double)(1 << 15)); + if (coef_q15 < 1) coef_q15 = 1; /* ensure non-zero to move toward steady-state */ + if (coef_q15 > (1 << 15)) coef_q15 = (1 << 15); + demod.deemph_a = coef_q15; + } } /* Set the tuner gain */ From 00e7c51a2137621a51b80b131133f9c10a1eb446 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 18:19:25 -0500 Subject: [PATCH 20/48] rtl_sdr_fm: add a light post-demod audio low-pass filter --- src/rtl_sdr_fm.cpp | 77 +++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 76 insertions(+), 1 deletion(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index f28dc4f..2c52443 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -265,6 +265,10 @@ struct demod_state int custom_atan; int deemph, deemph_a; int deemph_avg; + /* Optional post-demod audio low-pass filter (one-pole) */ + int audio_lpf_enable; + int audio_lpf_alpha; /* Q15 alpha for one-pole LPF */ + int audio_lpf_state; /* state/output y[n-1] in Q0 */ int now_lpr; int prev_lpr_index; int dc_block, dc_avg; @@ -914,6 +918,31 @@ void dc_block_filter(struct demod_state *fm) fm->dc_avg = dc; } +/* Optional light post-demod audio low-pass filter (one-pole IIR) + y[n] = y[n-1] + alpha * (x[n] - y[n-1]) + alpha is Q15 in fm->audio_lpf_alpha. */ +static inline void audio_lpf_filter(struct demod_state *fm) +{ + if (!fm->audio_lpf_enable) return; + int i; + int16_t *res = assume_aligned_ptr(fm->result, DSD_FME_ALIGN); + int y = fm->audio_lpf_state; /* Q0 */ + const int alpha_q15 = fm->audio_lpf_alpha; /* Q15 */ + const int kShift = 15; + DSD_FME_IVDEP + for (i = 0; i < fm->result_len; i++) { + int x = (int)res[i]; + int d = x - y; + int64_t delta = (int64_t)d * (int64_t)alpha_q15; + /* symmetric rounding */ + if (d >= 0) delta += (1LL << (kShift - 1)); + else delta -= (1LL << (kShift - 1)); + y += (int)(delta >> kShift); + res[i] = (int16_t)y; + } + fm->audio_lpf_state = y; +} + long int rms(int16_t *samples, int len, int step) /* largely lifted from rtl_power */ { @@ -1031,7 +1060,9 @@ void full_demod(struct demod_state *d) 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);} + deemph_filter(d);} + /* Optional post-demod audio LPF */ + audio_lpf_filter(d); if (d->dc_block) { dc_block_filter(d);} if (d->rate_out2 > 0) { @@ -1379,6 +1410,10 @@ void demod_init_analog(struct demod_state *s) s->prev_lpr_index = 0; s->deemph_a = 0; // s->deemph_avg = 0; + /* Audio LPF defaults */ + s->audio_lpf_enable = 0; + s->audio_lpf_alpha = 0; + s->audio_lpf_state = 0; s->now_lpr = 0; s->dc_block = 1; // s->dc_avg = 0; @@ -1428,6 +1463,10 @@ void demod_init_ro2(struct demod_state *s) s->prev_lpr_index = 0; s->deemph_a = 0; s->deemph_avg = 0; + /* Audio LPF defaults */ + s->audio_lpf_enable = 0; + s->audio_lpf_alpha = 0; + s->audio_lpf_state = 0; s->now_lpr = 0; s->dc_block = 1; //enabling by default, but offset tuning is also enabled, so center spike shouldn't be an issue s->dc_avg = 0; @@ -1477,6 +1516,10 @@ void demod_init(struct demod_state *s) s->prev_lpr_index = 0; s->deemph_a = 0; s->deemph_avg = 0; + /* Audio LPF defaults */ + s->audio_lpf_enable = 0; + s->audio_lpf_alpha = 0; + s->audio_lpf_state = 0; s->now_lpr = 0; s->dc_block = 1; //enabling by default, but offset tuning is also enabled, so center spike shouldn't be an issue s->dc_avg = 0; @@ -1743,6 +1786,38 @@ void open_rtlsdr_stream(dsd_opts *opts) } } + /* Configure optional post-demod audio LPF via env DSD_FME_AUDIO_LPF. + Values: + - off or 0: disabled (default) + - NNNN: cutoff in Hz (approximate), e.g., 3000 or 5000. Uses one-pole IIR. + */ + { + const char *alpf = getenv("DSD_FME_AUDIO_LPF"); + demod.audio_lpf_enable = 0; + demod.audio_lpf_alpha = 0; + demod.audio_lpf_state = 0; + if (alpf && alpf[0] != '\0') { + if (strcasecmp(alpf, "off") == 0 || strcmp(alpf, "0") == 0) { + /* disabled */ + } else { + int cutoff_hz = atoi(alpf); + if (cutoff_hz < 100) cutoff_hz = 100; /* guard */ + /* One-pole mapping: choose alpha from cutoff and Fs using approx alpha = 1 - exp(-2*pi*fc/Fs) */ + double Fs = (double)demod.rate_out; + if (Fs < 1.0) Fs = 1.0; + double a = 1.0 - exp(-2.0 * kPi * (double)cutoff_hz / Fs); + if (a < 0.0) a = 0.0; + if (a > 1.0) a = 1.0; + int alpha_q15 = (int)lrint(a * (double)(1 << 15)); + if (alpha_q15 < 1) alpha_q15 = 1; + if (alpha_q15 > (1 << 15)) alpha_q15 = (1 << 15); + demod.audio_lpf_alpha = alpha_q15; + demod.audio_lpf_enable = 1; + fprintf(stderr, "Audio LPF enabled: fc≈%d Hz, alpha_q15=%d\n", cutoff_hz, demod.audio_lpf_alpha); + } + } + } + /* Set the tuner gain */ if (dongle.gain == AUTO_GAIN) { verbose_auto_gain(dongle.dev); From 6fc327c3b6ecfa2860bc74f6080730bae0afe736 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 19:15:27 -0500 Subject: [PATCH 21/48] rtl_sdr_fm: avoid sqrt in RMS utility (use power-domain) --- src/rtl_sdr_fm.cpp | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 2c52443..1906b1e 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -943,28 +943,27 @@ static inline void audio_lpf_filter(struct demod_state *fm) fm->audio_lpf_state = y; } -long int rms(int16_t *samples, int len, int step) -/* largely lifted from rtl_power */ +long int mean_power(int16_t *samples, int len, int step) +/* DC-corrected mean power (sqrt-free). Returns squared RMS units. */ { int i; - long int rms; - long p, t, s; + int64_t p = 0; + int64_t t = 0; + int64_t s; double dc, err; - p = t = 0L; - for (i=0; i Date: Wed, 3 Sep 2025 19:20:43 -0500 Subject: [PATCH 22/48] =?UTF-8?q?rtl=5Fsdr=5Ffm:=20SIMD=20optimize=20USB?= =?UTF-8?q?=20byte=E2=86=92int16=20widening=20in=20callback?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/rtl_sdr_fm.cpp | 52 +++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 49 insertions(+), 3 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 1906b1e..aae091d 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -33,6 +33,14 @@ #include #include "dsd.h" +/* Optional SIMD intrinsics for USB byte->int16 widening */ +#if defined(__SSE2__) +#include +#endif +#if defined(__ARM_NEON) || defined(__ARM_NEON__) +#include +#endif + #define DEFAULT_SAMPLE_RATE 48000 #define DEFAULT_BUF_LENGTH (1 * 16384) #define MAXIMUM_OVERSAMPLE 16 @@ -74,6 +82,45 @@ static int atan_lut_coef = 8; static pthread_once_t atan_lut_once = PTHREAD_ONCE_INIT; static pthread_mutex_t atan_lut_mutex = PTHREAD_MUTEX_INITIALIZER; +/* ===================== + USB widening helper (u8 -> s16 centered at 127) with SIMD and scalar fallback + ===================== */ +static inline void widen_u8_to_s16_bias127(const unsigned char *src, int16_t *dst, uint32_t len) +{ + uint32_t i = 0; +#if defined(__SSE2__) + /* Process 16 bytes per iteration */ + const __m128i bias = _mm_set1_epi16(127); + const __m128i zero = _mm_setzero_si128(); + for (; i + 16 <= len; i += 16) { + __m128i b = _mm_loadu_si128((const __m128i*)(src + i)); + __m128i lo = _mm_unpacklo_epi8(b, zero); /* widen to 16-bit unsigned */ + __m128i hi = _mm_unpackhi_epi8(b, zero); + lo = _mm_sub_epi16(lo, bias); /* center at 127 */ + hi = _mm_sub_epi16(hi, bias); + _mm_storeu_si128((__m128i*)(dst + i), lo); + _mm_storeu_si128((__m128i*)(dst + i + 8), hi); + } +#elif defined(__ARM_NEON) || defined(__ARM_NEON__) + /* Process 16 bytes per iteration */ + const uint8x8_t bias8 = vdup_n_u8(127); + for (; i + 16 <= len; i += 16) { + uint8x16_t v = vld1q_u8(src + i); + uint8x8_t v_lo = vget_low_u8(v); + uint8x8_t v_hi = vget_high_u8(v); + /* signed widen subtract: (uint8 - 127) -> int16 */ + int16x8_t lo = vsubl_u8(v_lo, bias8); + int16x8_t hi = vsubl_u8(v_hi, bias8); + vst1q_s16(dst + i, lo); + vst1q_s16(dst + i + 8, hi); + } +#endif + /* Scalar tail or whole buffer on non-SIMD builds */ + for (; i < len; i++) { + dst[i] = (int16_t)src[i] - 127; + } +} + /* ===================== Saturating helpers ===================== */ @@ -1098,9 +1145,8 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) /* Write directly into the current write buffer */ int wb = d->write_buf_index.load(); int16_t *dst = d->input_buffers[wb]; - for (i=0; i<(int)len; i++) { - dst[i] = (int16_t)buf[i] - 127; - } + /* SIMD-accelerated widening (u8 -> s16 centered at 127) with scalar fallback */ + widen_u8_to_s16_bias127(buf, dst, len); d->input_len[wb].store(len); /* Flip buffers atomically: the buffer we just wrote becomes ready */ d->ready_buf_index.store(wb); From ac8f17bf20c0a7abca6519557723544d63955ba4 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 19:23:52 -0500 Subject: [PATCH 23/48] rtl_sdr_fm: guard and document `bandwidth_multiplier` behavior --- src/rtl_sdr_fm.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index aae091d..b3365e6 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -50,6 +50,9 @@ #define FREQUENCIES_LIMIT 1000 +/* Clamp for bandwidth upsampling multiplier to avoid extreme expansion */ +#define MAX_BANDWIDTH_MULTIPLIER 8 + 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; @@ -1738,6 +1741,21 @@ void open_rtlsdr_stream(dsd_opts *opts) int r; rtl_bandwidth = opts->rtl_bandwidth * 1000; //reverted back to straight value bandwidth_multiplier = (bandwidth_divisor / rtl_bandwidth); + /* Guard multiplier to a safe range [1, MAX_BANDWIDTH_MULTIPLIER] */ + { + int orig_mult = bandwidth_multiplier; + if (bandwidth_multiplier < 1) { + fprintf(stderr, + "WARNING: bandwidth_multiplier computed as %d (divisor=%d, bandwidth=%d Hz). Clamping to 1.\n", + orig_mult, bandwidth_divisor, rtl_bandwidth); + bandwidth_multiplier = 1; + } else if (bandwidth_multiplier > MAX_BANDWIDTH_MULTIPLIER) { + fprintf(stderr, + "WARNING: bandwidth_multiplier computed as %d exceeds max %d (divisor=%d, bandwidth=%d Hz). Clamping to %d.\n", + orig_mult, MAX_BANDWIDTH_MULTIPLIER, bandwidth_divisor, rtl_bandwidth, MAX_BANDWIDTH_MULTIPLIER); + bandwidth_multiplier = MAX_BANDWIDTH_MULTIPLIER; + } + } volume_multiplier = 1; //moved to external handling to be more dynamic //this needs to be initted first, then we set the parameters From 2655fccdeacf8544eb3b05f012497ce2ae707217 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 19:30:32 -0500 Subject: [PATCH 24/48] rtl_sdr_fm: ring signaling and atomic annotations --- src/rtl_sdr_fm.cpp | 28 ++++++++++++++++++++++++---- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index b3365e6..6a18218 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -430,6 +430,9 @@ static void maybe_set_thread_realtime_and_affinity(const char *role) static inline size_t ring_used(const struct output_state *o) { + /* Atomics policy: head/tail are atomics. We use default sequential + consistency for simplicity. In an SPSC ring, this could be relaxed + to acquire/release without changing behavior. */ size_t h = o->head.load(); size_t t = o->tail.load(); if (h >= t) return h - t; @@ -443,11 +446,13 @@ static inline size_t ring_free(const struct output_state *o) static inline int ring_is_empty(const struct output_state *o) { + /* See atomics note in ring_used() for ordering considerations. */ return o->head.load() == o->tail.load(); } static inline void ring_clear(struct output_state *o) { + /* Clearing indices; with relaxed ordering this would be a release store. */ o->tail.store(0); o->head.store(0); } @@ -455,6 +460,7 @@ static inline void ring_clear(struct output_state *o) /* Write up to count samples, blocking until space is available. Signals data availability after writes. */ static void ring_write(struct output_state *o, const int16_t *data, size_t count) { + int need_signal = ring_is_empty(o); while (count > 0 && !exitflag) { size_t free_sp = ring_free(o); if (free_sp == 0) { @@ -478,6 +484,9 @@ static void ring_write(struct output_state *o, const int16_t *data, size_t count data += write_now; count -= write_now; } + if (need_signal) { + safe_cond_signal(&o->ready, &o->ready_m); + } } /* Same as ring_write but does not signal; caller decides when to signal */ @@ -507,6 +516,17 @@ static void ring_write_no_signal(struct output_state *o, const int16_t *data, si } } +/* Write and signal consumer only if the ring transitions from empty to + non-empty. This reduces unnecessary wakeups while ensuring timely reads. */ +static void ring_write_signal_on_empty_transition(struct output_state *o, const int16_t *data, size_t count) +{ + int need_signal = ring_is_empty(o); + ring_write_no_signal(o, data, count); + if (need_signal) { + safe_cond_signal(&o->ready, &o->ready_m); + } +} + /* Read one sample, returns 0 on success, -1 on exit */ static int ring_read_one(struct output_state *o, int16_t *out) { @@ -1188,7 +1208,7 @@ static void *demod_thread_fn(void *arg) /* Write demod block to SPSC ring. If upsampling (bandwidth_multiplier > 1), linearly interpolate between adjacent samples instead of duplicating. */ if (bandwidth_multiplier <= 1) { - ring_write_no_signal(o, d->result, (size_t)d->result_len); + ring_write_signal_on_empty_transition(o, d->result, (size_t)d->result_len); } else { const int M = bandwidth_multiplier; const int N = d->result_len; @@ -1198,7 +1218,7 @@ static void *demod_thread_fn(void *arg) /* Degenerate case: only one sample, replicate M times */ std::vector tmp(M); for (int m = 0; m < M; m++) tmp[m] = d->result[0]; - ring_write_no_signal(o, tmp.data(), (size_t)M); + ring_write_signal_on_empty_transition(o, tmp.data(), (size_t)M); } else { /* N >= 2: perform linear interpolation between successive samples */ const size_t up_len = (size_t)N * (size_t)M; @@ -1215,10 +1235,10 @@ static void *demod_thread_fn(void *arg) } /* Last original sample maps to the last position */ upsampled[(size_t)(N - 1) * (size_t)M] = d->result[N - 1]; - ring_write_no_signal(o, upsampled.data(), up_len); + ring_write_signal_on_empty_transition(o, upsampled.data(), up_len); } } - safe_cond_signal(&o->ready, &o->ready_m); + /* Signaling occurs only when the ring transitions from empty to non-empty. */ } return 0; } From 0b00832b1fb570574a223174741c65cd3655fc2b Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 19:52:06 -0500 Subject: [PATCH 25/48] rtl_sdr_fm: enable env-gated intra-block multithreading with a tiny 2-thread pool and preallocated buffers. Parallelize HB I/Q decimation and linear upsampler; add debug log when enabled. --- src/rtl_sdr_fm.cpp | 247 +++++++++++++++++++++++++++++++++++---------- 1 file changed, 192 insertions(+), 55 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 6a18218..a6fa63a 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -29,7 +29,6 @@ #include #include #include -#include #include #include "dsd.h" @@ -200,42 +199,8 @@ static inline int hb_decim2_real(const int16_t *in, int in_len, int16_t *out, in } /* One 2:1 decimation stage on interleaved I/Q, using per-stage histories. */ -static inline int hb_decim2_complex_stage(const int16_t *in_iq, int in_iq_len, - int16_t *out_iq, int16_t *hist_i, int16_t *hist_q) -{ - /* in_iq_len is count of interleaved samples (I,Q,I,Q,...) */ - int ch_len = in_iq_len >> 1; /* samples per channel */ - if (ch_len <= 0) { - return 0; - } - /* Deinterleave into contiguous I and Q working buffers */ - std::vector i_buf; - std::vector q_buf; - i_buf.resize((size_t)ch_len); - q_buf.resize((size_t)ch_len); - for (int k = 0, j = 0; j < in_iq_len; j += 2, k++) { - i_buf[(size_t)k] = in_iq[(size_t)j]; - q_buf[(size_t)k] = in_iq[(size_t)j + 1]; - } - /* Output per channel */ - int out_ch_len; - { - /* Reuse i_buf as input; produce into temporary then interleave */ - std::vector i_out; - std::vector q_out; - i_out.resize((size_t)(ch_len >> 1)); - q_out.resize((size_t)(ch_len >> 1)); - int ilen = hb_decim2_real(i_buf.data(), ch_len, i_out.data(), hist_i); - int qlen = hb_decim2_real(q_buf.data(), ch_len, q_out.data(), hist_q); - out_ch_len = (ilen < qlen) ? ilen : qlen; - /* Interleave back */ - for (int n = 0; n < out_ch_len; n++) { - out_iq[(size_t)(2*n)] = i_out[(size_t)n]; - out_iq[(size_t)(2*n + 1)] = q_out[(size_t)n]; - } - } - return out_ch_len << 1; /* interleaved sample count */ -} +/* Removed hb_decim2_complex_stage in favor of inlined staged processing in full_demod() */ + static void atan_lut_once_init(void) { @@ -326,6 +291,27 @@ struct demod_state int16_t hb_workbuf[MAXIMUM_BUF_LENGTH]; int16_t hb_hist_i[10][HB_TAPS-1]; int16_t hb_hist_q[10][HB_TAPS-1]; + /* Preallocated deinterleave/output buffers for HB decimator */ + alignas(DSD_FME_ALIGN) int16_t hb_i_buf[MAXIMUM_BUF_LENGTH/2]; + alignas(DSD_FME_ALIGN) int16_t hb_q_buf[MAXIMUM_BUF_LENGTH/2]; + alignas(DSD_FME_ALIGN) int16_t hb_i_out[MAXIMUM_BUF_LENGTH/2]; + alignas(DSD_FME_ALIGN) int16_t hb_q_out[MAXIMUM_BUF_LENGTH/2]; + /* Preallocated buffer for linear upsampler (bandwidth_multiplier) */ + alignas(DSD_FME_ALIGN) int16_t upsample_buf[MAXIMUM_BUF_LENGTH * MAX_BANDWIDTH_MULTIPLIER]; + /* Minimal 2-thread worker pool for intra-block parallelism */ + int mt_enabled; + int mt_ready; + pthread_t mt_threads[2]; + pthread_mutex_t mt_lock; + pthread_cond_t mt_cv; + pthread_cond_t mt_done_cv; + int mt_should_exit; + int mt_epoch; + int mt_completed_in_epoch; + int mt_posted_count; + struct { void (*run)(void*); void *arg; } mt_tasks[2]; + int mt_worker_id[2]; + struct { struct demod_state *s; int id; } mt_args[2]; int (*discriminator)(int, int, int, int); void (*mode_demod)(struct demod_state*); pthread_cond_t ready; @@ -333,6 +319,111 @@ struct demod_state struct output_state *output_target; }; +/* Forward declarations for minimal worker pool helpers */ +static void demod_mt_init(struct demod_state *s); +static void demod_mt_destroy(struct demod_state *s); +static void demod_mt_run_two(struct demod_state *s, void (*f0)(void*), void *a0, void (*f1)(void*), void *a1); + +/* ===================== + Minimal 2-thread worker pool for DEMOD intra-block tasks + ===================== */ + +struct demod_mt_worker_arg { struct demod_state *s; int id; }; + +static void *demod_mt_worker(void *arg) +{ + struct demod_mt_worker_arg *wa = (struct demod_mt_worker_arg*)arg; + struct demod_state *s = wa->s; + const int id = wa->id; + int local_epoch = 0; + for (;;) { + pthread_mutex_lock(&s->mt_lock); + while (!s->mt_should_exit && s->mt_epoch == local_epoch) { + pthread_cond_wait(&s->mt_cv, &s->mt_lock); + } + if (s->mt_should_exit) { + pthread_mutex_unlock(&s->mt_lock); + break; + } + local_epoch = s->mt_epoch; + void (*fn)(void*) = NULL; + void *fn_arg = NULL; + if (id < s->mt_posted_count) { + fn = s->mt_tasks[id].run; + fn_arg = s->mt_tasks[id].arg; + } + pthread_mutex_unlock(&s->mt_lock); + if (fn) { + fn(fn_arg); + } + pthread_mutex_lock(&s->mt_lock); + s->mt_completed_in_epoch++; + if (s->mt_completed_in_epoch >= s->mt_posted_count) { + pthread_cond_signal(&s->mt_done_cv); + } + pthread_mutex_unlock(&s->mt_lock); + } + return NULL; +} + +static void demod_mt_init(struct demod_state *s) +{ + const char *mt = getenv("DSD_FME_MT"); + s->mt_enabled = (mt && mt[0] == '1') ? 1 : 0; + s->mt_should_exit = 0; + s->mt_epoch = 0; + s->mt_completed_in_epoch = 0; + s->mt_posted_count = 0; + if (!s->mt_enabled) { + return; + } + pthread_mutex_init(&s->mt_lock, NULL); + pthread_cond_init(&s->mt_cv, NULL); + pthread_cond_init(&s->mt_done_cv, NULL); + /* Start two workers */ + for (int i = 0; i < 2; i++) { + s->mt_args[i].s = s; + s->mt_args[i].id = i; + pthread_create(&s->mt_threads[i], NULL, demod_mt_worker, (void*)&s->mt_args[i]); + } + fprintf(stderr, "Intra-block multithreading enabled (DSD_FME_MT=1), workers: 2.\n"); +} + +static void demod_mt_destroy(struct demod_state *s) +{ + if (!s->mt_enabled) return; + pthread_mutex_lock(&s->mt_lock); + s->mt_should_exit = 1; + pthread_cond_broadcast(&s->mt_cv); + pthread_mutex_unlock(&s->mt_lock); + for (int i = 0; i < 2; i++) { + pthread_join(s->mt_threads[i], NULL); + } + pthread_cond_destroy(&s->mt_done_cv); + pthread_cond_destroy(&s->mt_cv); + pthread_mutex_destroy(&s->mt_lock); +} + +static void demod_mt_run_two(struct demod_state *s, void (*f0)(void*), void *a0, void (*f1)(void*), void *a1) +{ + if (!s->mt_enabled) { + if (f0) f0(a0); + if (f1) f1(a1); + return; + } + pthread_mutex_lock(&s->mt_lock); + s->mt_tasks[0].run = f0; s->mt_tasks[0].arg = a0; + s->mt_tasks[1].run = f1; s->mt_tasks[1].arg = a1; + s->mt_posted_count = (f1 != NULL) ? 2 : 1; + s->mt_completed_in_epoch = 0; + s->mt_epoch++; + pthread_cond_broadcast(&s->mt_cv); + while (s->mt_completed_in_epoch < s->mt_posted_count) { + pthread_cond_wait(&s->mt_done_cv, &s->mt_lock); + } + pthread_mutex_unlock(&s->mt_lock); +} + struct output_state { int rate; @@ -1048,11 +1139,35 @@ void full_demod(struct demod_state *d) int16_t *src = d->lowpassed; int16_t *dst = d->hb_workbuf; for (i = 0; i < ds_p; i++) { - int out_len = hb_decim2_complex_stage(src, in_len, dst, d->hb_hist_i[i], d->hb_hist_q[i]); - /* Next stage uses previous output as input */ + /* Deinterleave src -> hb_i_buf/hb_q_buf */ + int ch_len = in_len >> 1; + for (int k = 0, j = 0; j < in_len; j += 2, k++) { + d->hb_i_buf[(size_t)k] = src[(size_t)j]; + d->hb_q_buf[(size_t)k] = src[(size_t)j + 1]; + } + /* Define per-channel decimation task */ + struct HBArg { const int16_t *in; int in_len; int16_t *out; int16_t *hist; int out_len; } aI, aQ; + aI.in = d->hb_i_buf; aI.in_len = ch_len; aI.out = d->hb_i_out; aI.hist = d->hb_hist_i[i]; aI.out_len = 0; + aQ.in = d->hb_q_buf; aQ.in_len = ch_len; aQ.out = d->hb_q_out; aQ.hist = d->hb_hist_q[i]; aQ.out_len = 0; + auto hb_task = [](void *arg){ + struct HBArg *a = (struct HBArg*)arg; + a->out_len = hb_decim2_real(a->in, a->in_len, a->out, a->hist); + }; + if (d->mt_enabled) { + demod_mt_run_two(d, hb_task, (void*)&aI, hb_task, (void*)&aQ); + } else { + hb_task((void*)&aI); + hb_task((void*)&aQ); + } + int out_ch_len = (aI.out_len < aQ.out_len) ? aI.out_len : aQ.out_len; + /* Interleave back to dst */ + for (int n = 0; n < out_ch_len; n++) { + dst[(size_t)(2*n)] = d->hb_i_out[(size_t)n]; + dst[(size_t)(2*n + 1)] = d->hb_q_out[(size_t)n]; + } + /* Next stage */ src = dst; - in_len = out_len; - /* swap buffers for next stage to avoid overwrite if needed */ + in_len = out_ch_len << 1; dst = (src == d->hb_workbuf) ? d->lowpassed : d->hb_workbuf; } /* Final output resides in 'src' with length in_len */ @@ -1216,26 +1331,40 @@ static void *demod_thread_fn(void *arg) /* nothing to write */ } else if (N == 1) { /* Degenerate case: only one sample, replicate M times */ - std::vector tmp(M); - for (int m = 0; m < M; m++) tmp[m] = d->result[0]; - ring_write_signal_on_empty_transition(o, tmp.data(), (size_t)M); + for (int m = 0; m < M; m++) d->upsample_buf[m] = d->result[0]; + ring_write_signal_on_empty_transition(o, d->upsample_buf, (size_t)M); } else { /* N >= 2: perform linear interpolation between successive samples */ const size_t up_len = (size_t)N * (size_t)M; - std::vector upsampled; - upsampled.resize(up_len); - for (int n = 0; n < N - 1; n++) { - int16_t x0 = d->result[n]; - int16_t x1 = d->result[n + 1]; - int32_t dx = (int32_t)x1 - (int32_t)x0; - for (int m = 0; m < M; m++) { - int32_t interp = (int32_t)x0 + (dx * m) / M; - upsampled[(size_t)n * (size_t)M + (size_t)m] = (int16_t)interp; + /* Define range task */ + struct UpArg { int start; int end; int M; const int16_t *src; int16_t *dst; }; + auto up_task = [](void *arg){ + UpArg *a = (UpArg*)arg; + for (int n = a->start; n < a->end; n++) { + int16_t x0 = a->src[n]; + int16_t x1 = a->src[n + 1]; + int32_t dx = (int32_t)x1 - (int32_t)x0; + int16_t *row = a->dst + (size_t)n * (size_t)a->M; + for (int m = 0; m < a->M; m++) { + int32_t interp = (int32_t)x0 + (dx * m) / a->M; + row[m] = (int16_t)interp; + } } + }; + /* Split [0, N-1) into two ranges */ + int mid = (N - 1) / 2; + UpArg a0 = {0, mid, M, d->result, d->upsample_buf}; + UpArg a1 = {mid, N - 1, M, d->result, d->upsample_buf}; + if (d->mt_enabled) { + /* Post two tasks to the worker pool and wait */ + demod_mt_run_two(d, up_task, (void*)&a0, up_task, (void*)&a1); + } else { + up_task((void*)&a0); + up_task((void*)&a1); } /* Last original sample maps to the last position */ - upsampled[(size_t)(N - 1) * (size_t)M] = d->result[N - 1]; - ring_write_signal_on_empty_transition(o, upsampled.data(), up_len); + d->upsample_buf[(size_t)(N - 1) * (size_t)M] = d->result[N - 1]; + ring_write_signal_on_empty_transition(o, d->upsample_buf, up_len); } } /* Signaling occurs only when the ring transitions from empty to non-empty. */ @@ -1509,6 +1638,8 @@ void demod_init_analog(struct demod_state *s) /* set discriminator function pointer */ s->discriminator = (s->custom_atan == 0) ? &polar_discriminant : (s->custom_atan == 1) ? &polar_disc_fast : &polar_disc_lut; + /* Init minimal worker pool (env-gated) */ + demod_mt_init(s); } void demod_init_ro2(struct demod_state *s) @@ -1562,6 +1693,8 @@ void demod_init_ro2(struct demod_state *s) /* set discriminator function pointer */ s->discriminator = (s->custom_atan == 0) ? &polar_discriminant : (s->custom_atan == 1) ? &polar_disc_fast : &polar_disc_lut; + /* Init minimal worker pool (env-gated) */ + demod_mt_init(s); } void demod_init(struct demod_state *s) @@ -1615,12 +1748,16 @@ void demod_init(struct demod_state *s) /* set discriminator function pointer */ s->discriminator = (s->custom_atan == 0) ? &polar_discriminant : (s->custom_atan == 1) ? &polar_disc_fast : &polar_disc_lut; + /* Init minimal worker pool (env-gated) */ + demod_mt_init(s); } void demod_cleanup(struct demod_state *s) { pthread_cond_destroy(&s->ready); pthread_mutex_destroy(&s->ready_m); + /* Destroy worker pool if enabled */ + demod_mt_destroy(s); } void output_init(struct output_state *s) From 86d91512bb2feab850a24019555d11611d4f9314 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 20:20:41 -0500 Subject: [PATCH 26/48] =?UTF-8?q?rtl=5Fsdr=5Ffm:=20add=20AVX2=20widening,?= =?UTF-8?q?=20optional=20combined=20rotate+widen,=20and=20fixed=E2=80=91po?= =?UTF-8?q?int=20linear=20upsampler=20with=20env=20flags;=20preserve=20leg?= =?UTF-8?q?acy=20behavior=20by=20default.=20Fix=20decoding=20by=20matching?= =?UTF-8?q?=20legacy=20upsample=20length=20(NM)=20with=20tail=20fill=20and?= =?UTF-8?q?=20reproducing=20unsigned=20rotate=20semantics.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/rtl_sdr_fm.cpp | 133 ++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 119 insertions(+), 14 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index a6fa63a..bd245c6 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -36,6 +36,9 @@ #if defined(__SSE2__) #include #endif +#if defined(__AVX2__) +#include +#endif #if defined(__ARM_NEON) || defined(__ARM_NEON__) #include #endif @@ -78,18 +81,43 @@ static inline T* assume_aligned_ptr(T* p, size_t /*align_unused*/) { #define DSD_FME_ALIGN 64 #endif +/* Compiler-friendly restrict qualifier */ +#if defined(__GNUC__) || defined(__clang__) +#define DSD_FME_RESTRICT __restrict__ +#else +#define DSD_FME_RESTRICT +#endif + static int *atan_lut = NULL; static int atan_lut_size = 131072; /* 512 KB */ static int atan_lut_coef = 8; static pthread_once_t atan_lut_once = PTHREAD_ONCE_INIT; static pthread_mutex_t atan_lut_mutex = PTHREAD_MUTEX_INITIALIZER; +/* Debug/compat toggles via env */ +static int combine_rotate_enabled = 1; /* DSD_FME_COMBINE_ROT (1 default) */ +static int upsample_fixedpoint_enabled = 1;/* DSD_FME_UPSAMPLE_FP (1 default) */ /* ===================== USB widening helper (u8 -> s16 centered at 127) with SIMD and scalar fallback ===================== */ -static inline void widen_u8_to_s16_bias127(const unsigned char *src, int16_t *dst, uint32_t len) +static inline void widen_u8_to_s16_bias127(const unsigned char * DSD_FME_RESTRICT src, + int16_t * DSD_FME_RESTRICT dst, uint32_t len) { uint32_t i = 0; +#if defined(__AVX2__) + /* Process 32 bytes per iteration */ + const __m256i bias256 = _mm256_set1_epi16(127); + for (; i + 32 <= len; i += 32) { + __m128i b0 = _mm_loadu_si128((const __m128i*)(src + i)); + __m128i b1 = _mm_loadu_si128((const __m128i*)(src + i + 16)); + __m256i lo = _mm256_cvtepu8_epi16(b0); + __m256i hi = _mm256_cvtepu8_epi16(b1); + lo = _mm256_sub_epi16(lo, bias256); + hi = _mm256_sub_epi16(hi, bias256); + _mm256_storeu_si256((__m256i*)(dst + i), lo); + _mm256_storeu_si256((__m256i*)(dst + i + 16), hi); + } +#endif #if defined(__SSE2__) /* Process 16 bytes per iteration */ const __m128i bias = _mm_set1_epi16(127); @@ -123,6 +151,47 @@ static inline void widen_u8_to_s16_bias127(const unsigned char *src, int16_t *ds } } +/* ===================== + Combined rotate_90 (1, j, -1, -j) + widen (u8->s16 centered at 127) + Processes 4 IQ samples per iteration to avoid branches. + ===================== */ +static inline void widen_rotate90_u8_to_s16_bias127(const unsigned char * DSD_FME_RESTRICT src, + int16_t * DSD_FME_RESTRICT dst, uint32_t len) +{ + /* len is expected to be even (I/Q interleaved). Process in 8-byte blocks (4 IQ pairs). */ + uint32_t i = 0; + for (; i + 8 <= len; i += 8) { + /* sample 0: multiply by 1 */ + int16_t i0 = (int16_t)src[i + 0] - 127; + int16_t q0 = (int16_t)src[i + 1] - 127; + dst[i + 0] = i0; + dst[i + 1] = q0; + + /* sample 1: multiply by j -> (1 - Q, I) to match unsigned 255-x behavior */ + int16_t i1 = (int16_t)src[i + 2] - 127; + int16_t q1 = (int16_t)src[i + 3] - 127; + dst[i + 2] = (int16_t)(1 - q1); + dst[i + 3] = i1; + + /* sample 2: multiply by -1 -> (1 - I, 1 - Q) */ + int16_t i2 = (int16_t)src[i + 4] - 127; + int16_t q2 = (int16_t)src[i + 5] - 127; + dst[i + 4] = (int16_t)(1 - i2); + dst[i + 5] = (int16_t)(1 - q2); + + /* sample 3: multiply by -j -> (Q, 1 - I) */ + int16_t i3 = (int16_t)src[i + 6] - 127; + int16_t q3 = (int16_t)src[i + 7] - 127; + dst[i + 6] = q3; + dst[i + 7] = (int16_t)(1 - i3); + } + /* Tail (up to 3 IQ pairs). Match original semantics: no rotation applied, widen only. */ + for (; i + 1 < len; i += 2) { + dst[i + 0] = (int16_t)src[i + 0] - 127; + dst[i + 1] = (int16_t)src[i + 1] - 127; + } +} + /* ===================== Saturating helpers ===================== */ @@ -1278,13 +1347,21 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) buf[i] = 127;} s->mute = 0; } - if (!s->offset_tuning) { - rotate_90(buf, len);} /* Write directly into the current write buffer */ int wb = d->write_buf_index.load(); int16_t *dst = d->input_buffers[wb]; - /* SIMD-accelerated widening (u8 -> s16 centered at 127) with scalar fallback */ - widen_u8_to_s16_bias127(buf, dst, len); + /* If offset tuning disabled, combine 90° rotation with widening in one pass + unless DSD_FME_COMBINE_ROT=0. */ + if (!s->offset_tuning && combine_rotate_enabled) { + widen_rotate90_u8_to_s16_bias127(buf, dst, len); + } else if (!s->offset_tuning && !combine_rotate_enabled) { + /* Legacy order: rotate u8 in-place, then widen */ + rotate_90(buf, len); + widen_u8_to_s16_bias127(buf, dst, len); + } else { + /* Offset tuning active: no rotation, just widen */ + widen_u8_to_s16_bias127(buf, dst, len); + } d->input_len[wb].store(len); /* Flip buffers atomically: the buffer we just wrote becomes ready */ d->ready_buf_index.store(wb); @@ -1340,14 +1417,29 @@ static void *demod_thread_fn(void *arg) struct UpArg { int start; int end; int M; const int16_t *src; int16_t *dst; }; auto up_task = [](void *arg){ UpArg *a = (UpArg*)arg; + const int Mloc = a->M; for (int n = a->start; n < a->end; n++) { - int16_t x0 = a->src[n]; - int16_t x1 = a->src[n + 1]; - int32_t dx = (int32_t)x1 - (int32_t)x0; - int16_t *row = a->dst + (size_t)n * (size_t)a->M; - for (int m = 0; m < a->M; m++) { - int32_t interp = (int32_t)x0 + (dx * m) / a->M; - row[m] = (int16_t)interp; + int32_t x0 = a->src[n]; + int32_t x1 = a->src[n + 1]; + int16_t *row = a->dst + (size_t)n * (size_t)Mloc; + if (upsample_fixedpoint_enabled) { + int32_t dx = x1 - x0; + /* Q15 step to avoid per-sample division */ + int64_t step_q15_64 = ((int64_t)dx << 15) / (int64_t)Mloc; + int32_t step_q15 = (int32_t)step_q15_64; + int32_t acc_q15 = 0; + for (int m = 0; m < Mloc; m++) { + int32_t frac = (acc_q15 >= 0) ? ((acc_q15 + (1 << 14)) >> 15) : -(((-acc_q15) + (1 << 14)) >> 15); + int32_t interp = x0 + frac; + row[m] = (int16_t)interp; + acc_q15 += step_q15; + } + } else { + int32_t dx = x1 - x0; + for (int m = 0; m < Mloc; m++) { + int32_t interp = x0 + (dx * m) / Mloc; + row[m] = (int16_t)interp; + } } } }; @@ -1362,8 +1454,13 @@ static void *demod_thread_fn(void *arg) up_task((void*)&a0); up_task((void*)&a1); } - /* Last original sample maps to the last position */ + /* Last original sample maps to the last position; fill trailing with last value */ d->upsample_buf[(size_t)(N - 1) * (size_t)M] = d->result[N - 1]; + if (upsample_fixedpoint_enabled) { + for (int t = 1; t < M; t++) { + d->upsample_buf[(size_t)(N - 1) * (size_t)M + (size_t)t] = d->result[N - 1]; + } + } ring_write_signal_on_empty_transition(o, d->upsample_buf, up_len); } } @@ -1926,13 +2023,21 @@ void open_rtlsdr_stream(dsd_opts *opts) output_init(&output); controller_init(&controller); - /* Read optional environment flag for half-band decimator */ + /* Read optional environment flags */ { const char *hb = getenv("DSD_FME_HB_DECIM"); if (hb && hb[0] != '\0') { int v = atoi(hb); use_halfband_decimator = (v != 0); } + const char *cr = getenv("DSD_FME_COMBINE_ROT"); + if (cr && cr[0] != '\0') { + combine_rotate_enabled = (atoi(cr) != 0); + } + const char *ufp = getenv("DSD_FME_UPSAMPLE_FP"); + if (ufp && ufp[0] != '\0') { + upsample_fixedpoint_enabled = (atoi(ufp) != 0); + } } if (opts->rtlsdr_center_freq > 0) { From 9e9ae9521902c65ad563642fc021ecc86ee06212 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 20:33:04 -0500 Subject: [PATCH 27/48] rtl_sdr_fm: use fast atan2 discriminator for analog mode to reduce quantization noise --- src/rtl_sdr_fm.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index bd245c6..a0c107e 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -1693,10 +1693,10 @@ void demod_init_analog(struct demod_state *s) s->terminate_on_squelch = 0; s->squelch_hits = 11; s->downsample_passes = 1; // - s->comp_fir_size = 0; + s->comp_fir_size = 9; s->prev_index = 0; s->post_downsample = 1; //1 -- once this works, default = 4 -- doesn't work on the official rtl-sdr source code either - s->custom_atan = 2; + s->custom_atan = 1; s->deemph = 1; // s->rate_out2 = rtl_bandwidth; // -1 flag for disabled -- this enables low_pass_real, seems to work okay s->mode_demod = &fm_demod; From e9b12b8bc6f2991e309172ebec2d91758132a6b3 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 21:02:19 -0500 Subject: [PATCH 28/48] rtl_sdr_fm: add polyphase rational resampler (L/M) enabled by default at 48 kHz (override via DSD_FME_RESAMP), integrate post-demod and update output.rate while keeping legacy upsampler as fallback. Implement tap design with L-phase gain compensation. --- src/rtl_sdr_fm.cpp | 330 +++++++++++++++++++++++++++++++++++++-------- 1 file changed, 272 insertions(+), 58 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index a0c107e..7f5975c 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -367,6 +367,18 @@ struct demod_state alignas(DSD_FME_ALIGN) int16_t hb_q_out[MAXIMUM_BUF_LENGTH/2]; /* Preallocated buffer for linear upsampler (bandwidth_multiplier) */ alignas(DSD_FME_ALIGN) int16_t upsample_buf[MAXIMUM_BUF_LENGTH * MAX_BANDWIDTH_MULTIPLIER]; + /* Polyphase rational resampler (L/M) state and output buffer */ + int resamp_enabled; + int resamp_target_hz; /* desired output sample rate */ + int resamp_L; /* upsample factor */ + int resamp_M; /* downsample factor */ + int resamp_phase; /* 0..L-1 accumulator */ + int resamp_taps_len; /* prototype taps length (padded to K*L) */ + int resamp_taps_per_phase;/* K = ceil(taps_len/L) */ + int16_t *resamp_taps; /* Q15 taps, length = K*L */ + int16_t *resamp_hist; /* most-recent-first history, length = K */ + /* Output buffer for resampler (worst-case 4x expansion) */ + alignas(DSD_FME_ALIGN) int16_t resamp_outbuf[MAXIMUM_BUF_LENGTH * 4]; /* Minimal 2-thread worker pool for intra-block parallelism */ int mt_enabled; int mt_ready; @@ -1173,6 +1185,133 @@ static inline void audio_lpf_filter(struct demod_state *fm) fm->audio_lpf_state = y; } +/* ===================== + Polyphase Rational Resampler (L/M) + ===================== */ + +static inline int gcd_int(int a, int b) +{ + if (a < 0) a = -a; + if (b < 0) b = -b; + while (b != 0) { + int t = a % b; + a = b; + b = t; + } + return (a == 0) ? 1 : a; +} + +static inline double dsd_fme_sinc(double x) +{ + if (x == 0.0) return 1.0; + return sin(kPi * x) / (kPi * x); +} + +/* Design windowed-sinc low-pass prototype for upfirdn (runs at L*Fs_in). + - cutoff normalized to 0..0.5 (relative to L*Fs_in), use conservative margin. */ +static void resamp_design(struct demod_state *s, int L, int M) +{ + /* Per-phase taps K; total taps = K*L. Keep small for CPU, but adequate stopband. */ + int taps_per_phase = 16; /* K */ + int total_taps = taps_per_phase * L; + if (total_taps < L) total_taps = L; + if (taps_per_phase < 8) taps_per_phase = 8; + + /* Normalized cutoff: conservative 0.45 * min(1/L, 1/M) of Nyquist at L*Fs. */ + double fc = 0.45 / (double)((L > M) ? L : M); /* 0..0.5 at L*Fs */ + int N = total_taps; + int mid = (N - 1) / 2; + + /* Allocate taps if needed */ + if (s->resamp_taps) { free(s->resamp_taps); s->resamp_taps = NULL; } + if (s->resamp_hist) { free(s->resamp_hist); s->resamp_hist = NULL; } + s->resamp_taps = static_cast(malloc((size_t)N * sizeof(int16_t))); + s->resamp_hist = static_cast(malloc((size_t)taps_per_phase * sizeof(int16_t))); + if (!s->resamp_taps || !s->resamp_hist) { + if (s->resamp_taps) { free(s->resamp_taps); s->resamp_taps = NULL; } + if (s->resamp_hist) { free(s->resamp_hist); s->resamp_hist = NULL; } + s->resamp_enabled = 0; + fprintf(stderr, "Rational resampler: allocation failed, disabling.\n"); + return; + } + memset(s->resamp_hist, 0, (size_t)taps_per_phase * sizeof(int16_t)); + + /* Windowed-sinc (Hamming) */ + double gain = 0.0; + for (int n = 0; n < N; n++) { + int m = n - mid; + double w = 0.54 - 0.46 * cos(2.0 * kPi * (double)n / (double)(N - 1)); + double h = 2.0 * fc * dsd_fme_sinc(2.0 * fc * (double)m); + double t = h * w; + gain += t; + } + /* Normalize to unity DC gain */ + if (gain == 0.0) gain = 1.0; + /* Compensate for polyphase upsampling: scale taps by L so that each phase + has approximately unity DC gain (preserves amplitude through upfirdn). */ + const double phase_gain_comp = (double)L; + for (int n = 0; n < N; n++) { + int m = n - mid; + double w = 0.54 - 0.46 * cos(2.0 * kPi * (double)n / (double)(N - 1)); + double h = 2.0 * fc * dsd_fme_sinc(2.0 * fc * (double)m); + double t = (h * w / gain) * phase_gain_comp; + int v = (int)lrint(t * (double)(1 << 15)); + if (v > 32767) v = 32767; + if (v < -32768) v = -32768; + s->resamp_taps[n] = (int16_t)v; + } + + s->resamp_L = L; + s->resamp_M = M; + s->resamp_phase = 0; + s->resamp_taps_len = N; + s->resamp_taps_per_phase = taps_per_phase; +} + +/* Process one block using polyphase upfirdn with history. + Returns number of output samples written. */ +static int resamp_process_block(struct demod_state *s, const int16_t *in, int in_len, int16_t *out) +{ + if (!s->resamp_enabled || !s->resamp_taps || !s->resamp_hist) { + /* passthrough */ + memcpy(out, in, (size_t)in_len * sizeof(int16_t)); + return in_len; + } + const int L = s->resamp_L; + const int M = s->resamp_M; + const int K = s->resamp_taps_per_phase; /* taps per phase */ + const int16_t *taps = s->resamp_taps; /* length K*L, phase-major stride L */ + int phase = s->resamp_phase; /* 0..L-1 */ + int out_len = 0; + + for (int n = 0; n < in_len; n++) { + /* Push new sample into history (most-recent-first) */ + memmove(s->resamp_hist + 1, s->resamp_hist, (size_t)(K - 1) * sizeof(int16_t)); + s->resamp_hist[0] = in[n]; + + /* While we owe outputs with current input available */ + int local_phase = phase; + while (local_phase < L) { + /* Dot: y = sum_{k=0..K-1} hist[k] * taps[k*L + local_phase] */ + int64_t acc = 0; + const int16_t *tk = taps + local_phase; + for (int k = 0; k < K; k++) { + acc += (int32_t)s->resamp_hist[k] * (int32_t)tk[0]; + tk += L; + } + /* Q15 -> Q0 with rounding and saturation */ + acc += (1 << 14); + int32_t y = (int32_t)(acc >> 15); + out[out_len++] = sat16(y); + local_phase += M; + } + phase = local_phase - L; + } + + s->resamp_phase = phase; + return out_len; +} + long int mean_power(int16_t *samples, int len, int step) /* DC-corrected mean power (sqrt-free). Returns squared RMS units. */ { @@ -1397,71 +1536,69 @@ static void *demod_thread_fn(void *arg) safe_cond_signal(&controller.hop, &controller.hop_m); continue; } - /* Write demod block to SPSC ring. If upsampling (bandwidth_multiplier > 1), - linearly interpolate between adjacent samples instead of duplicating. */ - if (bandwidth_multiplier <= 1) { - ring_write_signal_on_empty_transition(o, d->result, (size_t)d->result_len); + /* Preferred path: rational resampler when enabled; otherwise legacy upsampler */ + if (d->resamp_enabled) { + int out_n = resamp_process_block(d, d->result, d->result_len, d->resamp_outbuf); + if (out_n > 0) ring_write_signal_on_empty_transition(o, d->resamp_outbuf, (size_t)out_n); } else { - const int M = bandwidth_multiplier; - const int N = d->result_len; - if (N <= 0) { - /* nothing to write */ - } else if (N == 1) { - /* Degenerate case: only one sample, replicate M times */ - for (int m = 0; m < M; m++) d->upsample_buf[m] = d->result[0]; - ring_write_signal_on_empty_transition(o, d->upsample_buf, (size_t)M); + /* Legacy path: optional simple upsampler */ + if (bandwidth_multiplier <= 1) { + ring_write_signal_on_empty_transition(o, d->result, (size_t)d->result_len); } else { - /* N >= 2: perform linear interpolation between successive samples */ - const size_t up_len = (size_t)N * (size_t)M; - /* Define range task */ - struct UpArg { int start; int end; int M; const int16_t *src; int16_t *dst; }; - auto up_task = [](void *arg){ - UpArg *a = (UpArg*)arg; - const int Mloc = a->M; - for (int n = a->start; n < a->end; n++) { - int32_t x0 = a->src[n]; - int32_t x1 = a->src[n + 1]; - int16_t *row = a->dst + (size_t)n * (size_t)Mloc; - if (upsample_fixedpoint_enabled) { - int32_t dx = x1 - x0; - /* Q15 step to avoid per-sample division */ - int64_t step_q15_64 = ((int64_t)dx << 15) / (int64_t)Mloc; - int32_t step_q15 = (int32_t)step_q15_64; - int32_t acc_q15 = 0; - for (int m = 0; m < Mloc; m++) { - int32_t frac = (acc_q15 >= 0) ? ((acc_q15 + (1 << 14)) >> 15) : -(((-acc_q15) + (1 << 14)) >> 15); - int32_t interp = x0 + frac; - row[m] = (int16_t)interp; - acc_q15 += step_q15; - } - } else { - int32_t dx = x1 - x0; - for (int m = 0; m < Mloc; m++) { - int32_t interp = x0 + (dx * m) / Mloc; - row[m] = (int16_t)interp; + const int M = bandwidth_multiplier; + const int N = d->result_len; + if (N <= 0) { + /* nothing to write */ + } else if (N == 1) { + for (int m = 0; m < M; m++) d->upsample_buf[m] = d->result[0]; + ring_write_signal_on_empty_transition(o, d->upsample_buf, (size_t)M); + } else { + const size_t up_len = (size_t)N * (size_t)M; + struct UpArg { int start; int end; int M; const int16_t *src; int16_t *dst; }; + auto up_task = [](void *arg){ + UpArg *a = (UpArg*)arg; + const int Mloc = a->M; + for (int n = a->start; n < a->end; n++) { + int32_t x0 = a->src[n]; + int32_t x1 = a->src[n + 1]; + int16_t *row = a->dst + (size_t)n * (size_t)Mloc; + if (upsample_fixedpoint_enabled) { + int32_t dx = x1 - x0; + int64_t step_q15_64 = ((int64_t)dx << 15) / (int64_t)Mloc; + int32_t step_q15 = (int32_t)step_q15_64; + int32_t acc_q15 = 0; + for (int m = 0; m < Mloc; m++) { + int32_t frac = (acc_q15 >= 0) ? ((acc_q15 + (1 << 14)) >> 15) : -(((-acc_q15) + (1 << 14)) >> 15); + int32_t interp = x0 + frac; + row[m] = (int16_t)interp; + acc_q15 += step_q15; + } + } else { + int32_t dx = x1 - x0; + for (int m = 0; m < Mloc; m++) { + int32_t interp = x0 + (dx * m) / Mloc; + row[m] = (int16_t)interp; + } } } + }; + int mid = (N - 1) / 2; + UpArg a0 = {0, mid, M, d->result, d->upsample_buf}; + UpArg a1 = {mid, N - 1, M, d->result, d->upsample_buf}; + if (d->mt_enabled) { + demod_mt_run_two(d, up_task, (void*)&a0, up_task, (void*)&a1); + } else { + up_task((void*)&a0); + up_task((void*)&a1); } - }; - /* Split [0, N-1) into two ranges */ - int mid = (N - 1) / 2; - UpArg a0 = {0, mid, M, d->result, d->upsample_buf}; - UpArg a1 = {mid, N - 1, M, d->result, d->upsample_buf}; - if (d->mt_enabled) { - /* Post two tasks to the worker pool and wait */ - demod_mt_run_two(d, up_task, (void*)&a0, up_task, (void*)&a1); - } else { - up_task((void*)&a0); - up_task((void*)&a1); - } - /* Last original sample maps to the last position; fill trailing with last value */ - d->upsample_buf[(size_t)(N - 1) * (size_t)M] = d->result[N - 1]; - if (upsample_fixedpoint_enabled) { - for (int t = 1; t < M; t++) { - d->upsample_buf[(size_t)(N - 1) * (size_t)M + (size_t)t] = d->result[N - 1]; + d->upsample_buf[(size_t)(N - 1) * (size_t)M] = d->result[N - 1]; + if (upsample_fixedpoint_enabled) { + for (int t = 1; t < M; t++) { + d->upsample_buf[(size_t)(N - 1) * (size_t)M + (size_t)t] = d->result[N - 1]; + } } + ring_write_signal_on_empty_transition(o, d->upsample_buf, up_len); } - ring_write_signal_on_empty_transition(o, d->upsample_buf, up_len); } } /* Signaling occurs only when the ring transitions from empty to non-empty. */ @@ -1711,6 +1848,16 @@ void demod_init_analog(struct demod_state *s) s->now_lpr = 0; s->dc_block = 1; // s->dc_avg = 0; + /* Resampler defaults */ + s->resamp_enabled = 0; + s->resamp_target_hz = 0; + s->resamp_L = 1; + s->resamp_M = 1; + s->resamp_phase = 0; + s->resamp_taps_len = 0; + s->resamp_taps_per_phase = 0; + s->resamp_taps = NULL; + s->resamp_hist = NULL; /* Squelch estimator init */ s->squelch_running_power = 0; s->squelch_decim_stride = 16; /* evaluate 1/16th samples for low CPU */ @@ -1766,6 +1913,16 @@ void demod_init_ro2(struct demod_state *s) s->now_lpr = 0; s->dc_block = 1; //enabling by default, but offset tuning is also enabled, so center spike shouldn't be an issue s->dc_avg = 0; + /* Resampler defaults */ + s->resamp_enabled = 0; + s->resamp_target_hz = 0; + s->resamp_L = 1; + s->resamp_M = 1; + s->resamp_phase = 0; + s->resamp_taps_len = 0; + s->resamp_taps_per_phase = 0; + s->resamp_taps = NULL; + s->resamp_hist = NULL; /* Squelch estimator init */ s->squelch_running_power = 0; s->squelch_decim_stride = 16; @@ -1821,6 +1978,16 @@ void demod_init(struct demod_state *s) s->now_lpr = 0; s->dc_block = 1; //enabling by default, but offset tuning is also enabled, so center spike shouldn't be an issue s->dc_avg = 0; + /* Resampler defaults */ + s->resamp_enabled = 0; + s->resamp_target_hz = 0; + s->resamp_L = 1; + s->resamp_M = 1; + s->resamp_phase = 0; + s->resamp_taps_len = 0; + s->resamp_taps_per_phase = 0; + s->resamp_taps = NULL; + s->resamp_hist = NULL; /* Squelch estimator init */ s->squelch_running_power = 0; s->squelch_decim_stride = 16; @@ -1855,6 +2022,9 @@ void demod_cleanup(struct demod_state *s) pthread_mutex_destroy(&s->ready_m); /* Destroy worker pool if enabled */ demod_mt_destroy(s); + /* Free resampler resources */ + if (s->resamp_taps) { free(s->resamp_taps); s->resamp_taps = NULL; } + if (s->resamp_hist) { free(s->resamp_hist); s->resamp_hist = NULL; } } void output_init(struct output_state *s) @@ -2038,6 +2208,42 @@ void open_rtlsdr_stream(dsd_opts *opts) if (ufp && ufp[0] != '\0') { upsample_fixedpoint_enabled = (atoi(ufp) != 0); } + /* Configure rational resampler target rate via DSD_FME_RESAMP (Hz). + Defaults: enabled at 48000 Hz unless set to "off" or "0". */ + const char *rs = getenv("DSD_FME_RESAMP"); + int enable_resamp = 1; + int target = 48000; + if (rs && rs[0] != '\0') { + if (strcasecmp(rs, "off") == 0 || strcmp(rs, "0") == 0) { + enable_resamp = 0; + } else { + int v = atoi(rs); + if (v > 0) target = v; else target = 48000; + } + } + if (enable_resamp) { + demod.resamp_target_hz = target; + int inRate = (demod.rate_out > 0) ? demod.rate_out : rtl_bandwidth; + int g = gcd_int(inRate, target); + int L = target / g; + int M = inRate / g; + if (L < 1) L = 1; + if (M < 1) M = 1; + /* Guard output buffer growth (limited to ~4x expansion) */ + int scale_num = L; + int scale_den = M; + int scale = (scale_den > 0) ? ( (scale_num + scale_den - 1) / scale_den ) : 1; + if (scale > 4) { + fprintf(stderr, "Resampler ratio too large (L=%d,M=%d). Clamping not supported; disabling resampler.\n", L, M); + demod.resamp_enabled = 0; + } else { + demod.resamp_enabled = 1; + resamp_design(&demod, L, M); + fprintf(stderr, "Rational resampler enabled: %d -> %d Hz (L=%d,M=%d).\n", inRate, target, L, M); + } + } else { + demod.resamp_enabled = 0; + } } if (opts->rtlsdr_center_freq > 0) { @@ -2164,6 +2370,14 @@ void open_rtlsdr_stream(dsd_opts *opts) pthread_create(&dongle.thread, NULL, dongle_thread_fn, (void*)(&dongle)); //only create socket thread IF user specified (for legacy uses), else don't use it if (port != 0) pthread_create(&socket_freq, NULL, socket_thread_fn, (void *)(&controller)); + + /* If resampler is enabled, update output.rate for downstream consumers */ + if (demod.resamp_enabled && demod.resamp_target_hz > 0) { + output.rate = demod.resamp_target_hz; + fprintf(stderr, "Output rate set to %d Hz via resampler.\n", output.rate); + } else { + output.rate = demod.rate_out; + } } void cleanup_rtlsdr_stream() From 1b9c3aa65993e6e66d82cd25853c8558d3913aa3 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 21:18:20 -0500 Subject: [PATCH 29/48] rtl_sdr_fm: add residual-CFO FLL and optional Gardner timing correction before discriminator (TED disabled by default and applied only to non-FM unless forced), with linear interpolation, nominal 1/sps advance, and env-tunable gains. --- src/rtl_sdr_fm.cpp | 228 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 228 insertions(+) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 7f5975c..3169486 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -379,6 +379,22 @@ struct demod_state int16_t *resamp_hist; /* most-recent-first history, length = K */ /* Output buffer for resampler (worst-case 4x expansion) */ alignas(DSD_FME_ALIGN) int16_t resamp_outbuf[MAXIMUM_BUF_LENGTH * 4]; + /* Residual CFO loop (FLL) state */ + int fll_enabled; + int fll_alpha_q15; /* proportional gain (Q15) */ + int fll_beta_q15; /* integral gain (Q15) */ + int fll_freq_q15; /* NCO frequency increment (Q15 radians/sample scaled) */ + int fll_phase_q15; /* NCO phase accumulator (wrap at 2*pi -> 1<<15 scale) */ + int fll_prev_r; + int fll_prev_j; + /* Timing error detector (Gardner) fractional-delay state */ + int ted_enabled; + int ted_force; /* allow forcing TED even for FM/C4FM paths */ + int ted_gain_q20; /* small gain (Q20) for stability */ + int ted_sps; /* nominal samples per symbol (e.g., 10 for 4800 sym/s at 48k) */ + int ted_mu_q20; /* fractional phase [0,1) in Q20 */ + /* Work buffer for timing-adjusted I/Q */ + alignas(DSD_FME_ALIGN) int16_t timing_buf[MAXIMUM_BUF_LENGTH]; /* Minimal 2-thread worker pool for intra-block parallelism */ int mt_enabled; int mt_ready; @@ -1185,6 +1201,125 @@ static inline void audio_lpf_filter(struct demod_state *fm) fm->audio_lpf_state = y; } +/* ===================== + Residual CFO loop (FLL) and Gardner timing correction + ===================== */ + +/* Mix lowpassed I/Q by NCO e^{j*phi}, update phi by fll_freq_q15 (Q15 0..2pi maps to 0..1<<15). */ +static inline void fll_mix_and_update(struct demod_state *d) +{ + if (!d->fll_enabled) return; + int16_t *x = d->lowpassed; + const int N = d->lp_len; + int phase = d->fll_phase_q15; /* Q15 wraps at 1<<15 ~ 2*pi */ + const int freq = d->fll_freq_q15; /* Q15 increment per sample */ + /* Use small LUT-free rotator: sin/cos approximation via angle wrapping into quadrants. */ + for (int i = 0; i + 1 < N; i += 2) { + int p = phase & 0x7FFF; /* 0..32767 */ + int q = p >> 13; /* quadrant 0..3 */ + int16_t c = 32767, s = 0; + int16_t r = (int16_t)(p & 0x1FFF); /* 0..8191 */ + switch (q) { + case 0: c = 32767; s = (int16_t)((r * 4)); break; + case 1: c = (int16_t)(32767 - (r * 4)); s = 32767; break; + case 2: c = -32767; s = (int16_t)(32767 - (r * 4)); break; + default: c = (int16_t)(-32767 + (r * 4)); s = -32767; break; + } + int xr = x[i]; + int xj = x[i+1]; + int32_t yr = ((int32_t)xr * c + (int32_t)xj * s) >> 15; + int32_t yj = ((int32_t)xj * c - (int32_t)xr * s) >> 15; + x[i] = (int16_t)yr; + x[i+1] = (int16_t)yj; + phase += freq; + } + d->fll_phase_q15 = phase & 0x7FFF; +} + +/* Estimate frequency error using a simple phase-difference discriminator and update FLL integrator. */ +static inline void fll_update_error(struct demod_state *d) +{ + if (!d->fll_enabled) return; + int16_t *x = d->lowpassed; + const int N = d->lp_len; + int alpha = d->fll_alpha_q15; /* Q15 */ + int beta = d->fll_beta_q15; /* Q15 */ + int prev_r = d->fll_prev_r; + int prev_j = d->fll_prev_j; + int32_t err_acc = 0; + int count = 0; + for (int i = 0; i + 1 < N; i += 2) { + int r = x[i]; + int j = x[i+1]; + if (i > 0 || (prev_r != 0 || prev_j != 0)) { + int e = polar_disc_fast(r, j, prev_r, prev_j); /* Q14 */ + err_acc += e; + count++; + } + prev_r = r; prev_j = j; + } + d->fll_prev_r = prev_r; + d->fll_prev_j = prev_j; + if (count == 0) return; + int32_t err = err_acc / count; /* Q14 */ + int32_t p = ( (int64_t)alpha * err ) >> 14; /* Q14 */ + int32_t iacc = ( (int64_t)beta * err ) >> 14; + int32_t df = p + iacc; + d->fll_freq_q15 += (int)df; /* Q15 */ +} + +/* Lightweight Gardner timing correction: nearest-neighbor fractional selection around nominal sps. */ +static inline void gardner_timing_adjust(struct demod_state *d) +{ + if (!d->ted_enabled || d->ted_sps <= 1) return; + int sps = d->ted_sps; + int mu = d->ted_mu_q20; /* Q20 */ + int gain = d->ted_gain_q20; /* Q20 */ + int16_t *x = d->lowpassed; + int16_t *y = d->timing_buf; + const int N = d->lp_len; + int out_n = 0; + const int one = (1<<20); + int mu_nom = one / (sps > 0 ? sps : 1); /* Q20 increment per complex sample */ + for (int n = 0; n + 3 < N; n += 2) { + int a = n; /* base complex sample index */ + int b = n + 2; /* next complex sample index */ + if (b + 1 >= N) break; + int frac = mu & (one - 1); /* 0..one-1 */ + int inv = one - frac; + /* Linear interpolation between x[a] and x[b] (complex) */ + int32_t ar = x[a]; int32_t aj = x[a+1]; + int32_t br = x[b]; int32_t bj = x[b+1]; + int32_t ir = (int32_t)(( (int64_t)inv * ar + (int64_t)frac * br ) >> 20); + int32_t ij = (int32_t)(( (int64_t)inv * aj + (int64_t)frac * bj ) >> 20); + y[out_n++] = (int16_t)ir; + y[out_n++] = (int16_t)ij; + + /* Gardner error using previous and next symbol-spaced samples */ + int km1 = a - 2; if (km1 < 0) km1 = 0; + int kp1 = b + 2; if (kp1 + 1 >= N) { kp1 = b; } + int16_t xr1 = x[kp1]; + int16_t xj1 = x[kp1+1]; + int16_t xrm = x[km1]; + int16_t xjm = x[km1+1]; + int16_t dr = xr1 - xrm; + int16_t dj = xj1 - xjm; + int32_t e = (int32_t)dr * (int32_t)ir + (int32_t)dj * (int32_t)ij; /* Q0 */ + + /* Update fractional phase: nominal advance + small correction */ + int64_t corr = ((int64_t)gain * (int64_t)e) >> 15; /* scale */ + mu += mu_nom + (int)corr; + /* Wrap mu to [0, one) */ + if (mu >= one) mu -= one; + if (mu < 0) mu += one; + } + if (out_n >= 2) { + memcpy(d->lowpassed, y, (size_t)out_n * sizeof(int16_t)); + d->lp_len = out_n; + } + d->ted_mu_q20 = mu; +} + /* ===================== Polyphase Rational Resampler (L/M) ===================== */ @@ -1401,6 +1536,12 @@ void full_demod(struct demod_state *d) } else { low_pass(d); } + /* Residual CFO correction before discriminator */ + fll_mix_and_update(d); + /* Lightweight timing error correction (optional, avoid for analog FM demod) */ + if (d->ted_enabled && (d->mode_demod != &fm_demod || d->ted_force)) { + gardner_timing_adjust(d); + } /* power squelch (sqrt-free): compare mean power to squared threshold */ if (d->squelch_level) { /* Decimated block power estimate (no DC correction; EMA smooths) */ @@ -1858,6 +1999,18 @@ void demod_init_analog(struct demod_state *s) s->resamp_taps_per_phase = 0; s->resamp_taps = NULL; s->resamp_hist = NULL; + /* FLL/TED defaults */ + s->fll_enabled = 0; + s->fll_alpha_q15 = 0; + s->fll_beta_q15 = 0; + s->fll_freq_q15 = 0; + s->fll_phase_q15 = 0; + s->fll_prev_r = 0; + s->fll_prev_j = 0; + s->ted_enabled = 0; + s->ted_gain_q20 = 0; + s->ted_sps = 0; + s->ted_mu_q20 = 0; /* Squelch estimator init */ s->squelch_running_power = 0; s->squelch_decim_stride = 16; /* evaluate 1/16th samples for low CPU */ @@ -1923,6 +2076,18 @@ void demod_init_ro2(struct demod_state *s) s->resamp_taps_per_phase = 0; s->resamp_taps = NULL; s->resamp_hist = NULL; + /* FLL/TED defaults */ + s->fll_enabled = 0; + s->fll_alpha_q15 = 0; + s->fll_beta_q15 = 0; + s->fll_freq_q15 = 0; + s->fll_phase_q15 = 0; + s->fll_prev_r = 0; + s->fll_prev_j = 0; + s->ted_enabled = 0; + s->ted_gain_q20 = 0; + s->ted_sps = 0; + s->ted_mu_q20 = 0; /* Squelch estimator init */ s->squelch_running_power = 0; s->squelch_decim_stride = 16; @@ -1988,6 +2153,18 @@ void demod_init(struct demod_state *s) s->resamp_taps_per_phase = 0; s->resamp_taps = NULL; s->resamp_hist = NULL; + /* FLL/TED defaults */ + s->fll_enabled = 0; + s->fll_alpha_q15 = 0; + s->fll_beta_q15 = 0; + s->fll_freq_q15 = 0; + s->fll_phase_q15 = 0; + s->fll_prev_r = 0; + s->fll_prev_j = 0; + s->ted_enabled = 0; + s->ted_gain_q20 = 0; + s->ted_sps = 0; + s->ted_mu_q20 = 0; /* Squelch estimator init */ s->squelch_running_power = 0; s->squelch_decim_stride = 16; @@ -2244,6 +2421,57 @@ void open_rtlsdr_stream(dsd_opts *opts) } else { demod.resamp_enabled = 0; } + + /* Configure FLL/TED via envs. Defaults: FLL on with small gains; TED off by default. */ + const char *fll = getenv("DSD_FME_FLL"); + demod.fll_enabled = (!fll || fll[0] == '\0' || fll[0] == '1') ? 1 : 0; + /* Gains in Q15; very conservative defaults */ + const char *fa = getenv("DSD_FME_FLL_ALPHA"); + const char *fb = getenv("DSD_FME_FLL_BETA"); + demod.fll_alpha_q15 = fa ? atoi(fa) : 100; /* ~0.003 */ + demod.fll_beta_q15 = fb ? atoi(fb) : 10; /* ~0.0003 */ + demod.fll_freq_q15 = 0; + demod.fll_phase_q15 = 0; + demod.fll_prev_r = demod.fll_prev_j = 0; + + const char *ted = getenv("DSD_FME_TED"); + demod.ted_enabled = (ted && ted[0] == '1') ? 1 : 0; + const char *tg = getenv("DSD_FME_TED_GAIN"); + const char *ts = getenv("DSD_FME_TED_SPS"); + const char *tf = getenv("DSD_FME_TED_FORCE"); + demod.ted_gain_q20 = tg ? atoi(tg) : 64; /* tiny default */ + demod.ted_sps = ts ? atoi(ts) : 10; /* e.g., 4800 sym/s @ 48k */ + demod.ted_mu_q20 = 0; + demod.ted_force = (tf && tf[0] == '1') ? 1 : 0; + + /* Mode-aware defaults (only if envs not provided) */ + int env_ted_set = (ted && ted[0] != '\0'); + int env_fll_alpha_set = (fa && fa[0] != '\0'); + int env_fll_beta_set = (fb && fb[0] != '\0'); + int env_ted_sps_set = (ts && ts[0] != '\0'); + int env_ted_gain_set = (tg && tg[0] != '\0'); + int digital_mode = (opts->frame_p25p1 == 1 || opts->frame_p25p2 == 1 || opts->frame_provoice == 1); + /* Default: for common digital modes compute reasonable defaults, but keep TED disabled unless user opts in. */ + if (digital_mode) { + if (!env_ted_set) demod.ted_enabled = 0; + if (!env_ted_sps_set) { + int Fs = (int)output.rate; + int sps = (Fs + 2400) / 4800; /* round(Fs/4800) */ + if (sps < 2) sps = 2; + demod.ted_sps = sps; + } + if (!env_ted_gain_set) { + /* Slightly higher but safe default gain for digital */ + demod.ted_gain_q20 = 96; + } + if (!env_fll_alpha_set) demod.fll_alpha_q15 = 150; /* ~0.0046 */ + if (!env_fll_beta_set) demod.fll_beta_q15 = 15; /* ~0.00046 */ + } else { + /* Analog defaults: keep TED off; gentle FLL */ + if (!env_ted_set) demod.ted_enabled = 0; + if (!env_fll_alpha_set) demod.fll_alpha_q15 = 50; /* ~0.0015 */ + if (!env_fll_beta_set) demod.fll_beta_q15 = 5; /* ~0.00015 */ + } } if (opts->rtlsdr_center_freq > 0) { From 391048a45adfd7edd74239196a014aa2002f6e51 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 21:23:05 -0500 Subject: [PATCH 30/48] =?UTF-8?q?rtl=5Fsdr=5Ffm:=20vectorize=20rotate+wide?= =?UTF-8?q?n=20path:=20add=20AVX2/NEON=20implementations=20for=20widen=5Fr?= =?UTF-8?q?otate90=5Fu8=5Fto=5Fs16=5Fbias127=20with=20scalar=20tail=20fall?= =?UTF-8?q?back.=20This=20applies=20the=2090=C2=B0=20rotation=20and=20bias?= =?UTF-8?q?-127=20widening=20in=20vectors=20to=20reduce=20callback=20CPU?= =?UTF-8?q?=20without=20changing=20behavior.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/rtl_sdr_fm.cpp | 65 +++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 59 insertions(+), 6 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 3169486..29a7fdb 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -158,34 +158,87 @@ static inline void widen_u8_to_s16_bias127(const unsigned char * DSD_FME_RESTRIC static inline void widen_rotate90_u8_to_s16_bias127(const unsigned char * DSD_FME_RESTRICT src, int16_t * DSD_FME_RESTRICT dst, uint32_t len) { - /* len is expected to be even (I/Q interleaved). Process in 8-byte blocks (4 IQ pairs). */ +#if defined(__AVX2__) + /* AVX2 path: process 32 bytes per loop, applying rotation+wide. + Pattern per 8-byte group: [0,1,3,2,4,5,7,6] with select mask for 128-x on lanes {2,4,5,7}. */ + const __m256i shuffle = _mm256_setr_epi8( + 0, 1, 3, 2, 4, 5, 7, 6, 8, 9,11,10,12,13,15,14, + 0, 1, 3, 2, 4, 5, 7, 6, 8, 9,11,10,12,13,15,14); + const __m256i mask_sel = _mm256_setr_epi16( + 0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF, + 0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF); + const __m256i c127 = _mm256_set1_epi16(127); + const __m256i c128 = _mm256_set1_epi16(128); uint32_t i = 0; + for (; i + 32 <= len; i += 32) { + __m256i v8 = _mm256_loadu_si256((const __m256i*)(src + i)); + __m256i sh = _mm256_shuffle_epi8(v8, shuffle); + __m128i sh_lo = _mm256_castsi256_si128(sh); + __m128i sh_hi = _mm256_extracti128_si256(sh, 1); + __m256i v16_lo = _mm256_cvtepu8_epi16(sh_lo); + __m256i v16_hi = _mm256_cvtepu8_epi16(sh_hi); + __m256i bs_lo = _mm256_sub_epi16(v16_lo, c127); + __m256i bm_lo = _mm256_sub_epi16(c128, v16_lo); + __m256i bs_hi = _mm256_sub_epi16(v16_hi, c127); + __m256i bm_hi = _mm256_sub_epi16(c128, v16_hi); + __m256i out_lo = _mm256_blendv_epi8(bs_lo, bm_lo, mask_sel); + __m256i out_hi = _mm256_blendv_epi8(bs_hi, bm_hi, mask_sel); + _mm256_storeu_si256((__m256i*)(dst + i), out_lo); + _mm256_storeu_si256((__m256i*)(dst + i + 16), out_hi); + } +#elif defined(__ARM_NEON) || defined(__ARM_NEON__) + /* NEON path: process 16 bytes per loop */ + const uint8x16_t tbl_idx = {0,1,3,2,4,5,7,6, 8,9,11,10,12,13,15,14}; + const int16x8_t c127 = vdupq_n_s16(127); + const int16x8_t c128 = vdupq_n_s16(128); + /* mask lanes to select 128-x for lanes {2,4,5,7} in each 8-lane group */ + const uint16_t mpat[8] = {0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF}; + const uint16x8_t msel = vld1q_u16(mpat); + uint32_t i = 0; + for (; i + 16 <= len; i += 16) { + uint8x16_t v = vld1q_u8(src + i); + /* lane-wise shuffle */ + uint8x16_t sh = vqtbl1q_u8(v, tbl_idx); + /* widen low */ + uint8x8_t sh_lo8 = vget_low_u8(sh); + uint8x8_t sh_hi8 = vget_high_u8(sh); + int16x8_t v16_lo = vreinterpretq_s16_u16(vmovl_u8(sh_lo8)); + int16x8_t v16_hi = vreinterpretq_s16_u16(vmovl_u8(sh_hi8)); + int16x8_t bs_lo = vsubq_s16(v16_lo, c127); + int16x8_t bm_lo = vsubq_s16(c128, v16_lo); + int16x8_t bs_hi = vsubq_s16(v16_hi, c127); + int16x8_t bm_hi = vsubq_s16(c128, v16_hi); + /* blend: out = (msel ? bm : bs), msel repeated for both halves */ + int16x8_t out_lo = vbslq_s16(msel, bm_lo, bs_lo); + int16x8_t out_hi = vbslq_s16(msel, bm_hi, bs_hi); + vst1q_s16(dst + i, out_lo); + vst1q_s16(dst + i + 8, out_hi); + } +#else + uint32_t i = 0; +#endif + /* Scalar tail or generic path */ for (; i + 8 <= len; i += 8) { - /* sample 0: multiply by 1 */ int16_t i0 = (int16_t)src[i + 0] - 127; int16_t q0 = (int16_t)src[i + 1] - 127; dst[i + 0] = i0; dst[i + 1] = q0; - /* sample 1: multiply by j -> (1 - Q, I) to match unsigned 255-x behavior */ int16_t i1 = (int16_t)src[i + 2] - 127; int16_t q1 = (int16_t)src[i + 3] - 127; dst[i + 2] = (int16_t)(1 - q1); dst[i + 3] = i1; - /* sample 2: multiply by -1 -> (1 - I, 1 - Q) */ int16_t i2 = (int16_t)src[i + 4] - 127; int16_t q2 = (int16_t)src[i + 5] - 127; dst[i + 4] = (int16_t)(1 - i2); dst[i + 5] = (int16_t)(1 - q2); - /* sample 3: multiply by -j -> (Q, 1 - I) */ int16_t i3 = (int16_t)src[i + 6] - 127; int16_t q3 = (int16_t)src[i + 7] - 127; dst[i + 6] = q3; dst[i + 7] = (int16_t)(1 - i3); } - /* Tail (up to 3 IQ pairs). Match original semantics: no rotation applied, widen only. */ for (; i + 1 < len; i += 2) { dst[i + 0] = (int16_t)src[i + 0] - 127; dst[i + 1] = (int16_t)src[i + 1] - 127; From 106aae18149fcf48bb279c8bf44e8e311e99d2fc Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 21:35:34 -0500 Subject: [PATCH 31/48] =?UTF-8?q?rtl=5Fsdr=5Ffm:=20fuse=20interleaved=20co?= =?UTF-8?q?mplex=20half-band=20decimation=20to=20avoid=20deinterleave/rein?= =?UTF-8?q?terleave=20in=20full=5Fdemod,=20reducing=20memory=20traffic=20a?= =?UTF-8?q?nd=20copies.=20Maintain=20per=E2=80=91stage=20histories=20and?= =?UTF-8?q?=20equivalent=20output=20while=20improving=20throughput.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/rtl_sdr_fm.cpp | 96 +++++++++++++++++++++++++++++++++------------- 1 file changed, 69 insertions(+), 27 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 29a7fdb..88764f8 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -323,6 +323,72 @@ static inline int hb_decim2_real(const int16_t *in, int in_len, int16_t *out, in /* One 2:1 decimation stage on interleaved I/Q, using per-stage histories. */ /* Removed hb_decim2_complex_stage in favor of inlined staged processing in full_demod() */ +/* Fused interleaved complex half-band decimator: decimate I and Q in one pass without deinterleave/reinterleave. + in_len is number of interleaved int16_t (I,Q,I,Q,...). Returns interleaved output length (in_len/2). */ +static inline int hb_decim2_complex_interleaved(const int16_t *in, int in_len, int16_t *out, + int16_t *hist_i, int16_t *hist_q) +{ + const int hist_len = HB_TAPS - 1; + int ch_len = in_len >> 1; /* per-channel samples */ + int out_ch_len = ch_len >> 1; /* decimated per-channel */ + if (out_ch_len <= 0) { + return 0; + } + int16_t lastI = (ch_len > 0) ? in[in_len - 2] : 0; + int16_t lastQ = (ch_len > 0) ? in[in_len - 1] : 0; + for (int n = 0; n < out_ch_len; n++) { + int center_idx = hist_len + (n << 1); /* per-channel index */ + int64_t accI = 0; + int64_t accQ = 0; + for (int t = 0; t < HB_TAPS; t++) { + int src_idx = center_idx - HB_HALF + t; + int16_t xi, xq; + if (src_idx < hist_len) { + xi = hist_i[src_idx]; + xq = hist_q[src_idx]; + } else { + int rel = src_idx - hist_len; + if (rel < ch_len) { + xi = in[(size_t)(rel << 1)]; + xq = in[(size_t)(rel << 1) + 1]; + } else { + xi = lastI; + xq = lastQ; + } + } + int16_t c = hb_q15_taps[t]; + accI += (int32_t)c * (int32_t)xi; + accQ += (int32_t)c * (int32_t)xq; + } + accI += (1 << 14); + accQ += (1 << 14); + int32_t yI = (int32_t)(accI >> 15); + int32_t yQ = (int32_t)(accQ >> 15); + out[(size_t)(n << 1)] = sat16(yI); + out[(size_t)(n << 1) + 1] = sat16(yQ); + } + /* Update histories with last HB_TAPS-1 per-channel input samples */ + if (ch_len >= hist_len) { + int start = ch_len - hist_len; + for (int k = 0; k < hist_len; k++) { + int rel = start + k; + hist_i[k] = in[(size_t)(rel << 1)]; + hist_q[k] = in[(size_t)(rel << 1) + 1]; + } + } else { + int need = hist_len - ch_len; + if (need > 0) { + memmove(hist_i, hist_i + ch_len, (size_t)need * sizeof(int16_t)); + memmove(hist_q, hist_q + ch_len, (size_t)need * sizeof(int16_t)); + } + for (int k = 0; k < ch_len; k++) { + hist_i[need + k] = in[(size_t)(k << 1)]; + hist_q[need + k] = in[(size_t)(k << 1) + 1]; + } + } + return out_ch_len << 1; /* interleaved length */ +} + static void atan_lut_once_init(void) { @@ -1535,35 +1601,11 @@ void full_demod(struct demod_state *d) int16_t *src = d->lowpassed; int16_t *dst = d->hb_workbuf; for (i = 0; i < ds_p; i++) { - /* Deinterleave src -> hb_i_buf/hb_q_buf */ - int ch_len = in_len >> 1; - for (int k = 0, j = 0; j < in_len; j += 2, k++) { - d->hb_i_buf[(size_t)k] = src[(size_t)j]; - d->hb_q_buf[(size_t)k] = src[(size_t)j + 1]; - } - /* Define per-channel decimation task */ - struct HBArg { const int16_t *in; int in_len; int16_t *out; int16_t *hist; int out_len; } aI, aQ; - aI.in = d->hb_i_buf; aI.in_len = ch_len; aI.out = d->hb_i_out; aI.hist = d->hb_hist_i[i]; aI.out_len = 0; - aQ.in = d->hb_q_buf; aQ.in_len = ch_len; aQ.out = d->hb_q_out; aQ.hist = d->hb_hist_q[i]; aQ.out_len = 0; - auto hb_task = [](void *arg){ - struct HBArg *a = (struct HBArg*)arg; - a->out_len = hb_decim2_real(a->in, a->in_len, a->out, a->hist); - }; - if (d->mt_enabled) { - demod_mt_run_two(d, hb_task, (void*)&aI, hb_task, (void*)&aQ); - } else { - hb_task((void*)&aI); - hb_task((void*)&aQ); - } - int out_ch_len = (aI.out_len < aQ.out_len) ? aI.out_len : aQ.out_len; - /* Interleave back to dst */ - for (int n = 0; n < out_ch_len; n++) { - dst[(size_t)(2*n)] = d->hb_i_out[(size_t)n]; - dst[(size_t)(2*n + 1)] = d->hb_q_out[(size_t)n]; - } + /* Fused complex HB decimation on interleaved I/Q */ + int out_len_interleaved = hb_decim2_complex_interleaved(src, in_len, dst, d->hb_hist_i[i], d->hb_hist_q[i]); /* Next stage */ src = dst; - in_len = out_ch_len << 1; + in_len = out_len_interleaved; dst = (src == d->hb_workbuf) ? d->lowpassed : d->hb_workbuf; } /* Final output resides in 'src' with length in_len */ From 8b4fa817117e28f3962568dc865454849a02abb7 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 21:38:50 -0500 Subject: [PATCH 32/48] =?UTF-8?q?rtl=5Fsdr=5Ffm:=20consume=20HB=20cascade?= =?UTF-8?q?=20output=20in-place=20to=20avoid=20the=20final=20memcpy.=20Tra?= =?UTF-8?q?ck=20the=20current=20buffer=20and=20assign=20lowpassed=20to=20t?= =?UTF-8?q?he=20last=20stage=E2=80=99s=20output,=20preserving=20correctnes?= =?UTF-8?q?s=20while=20reducing=20memory=20traffic.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/rtl_sdr_fm.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 88764f8..186ac74 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -1608,10 +1608,8 @@ void full_demod(struct demod_state *d) in_len = out_len_interleaved; dst = (src == d->hb_workbuf) ? d->lowpassed : d->hb_workbuf; } - /* Final output resides in 'src' with length in_len */ - if (d->lowpassed != src) { - memcpy(d->lowpassed, src, (size_t)in_len * sizeof(int16_t)); - } + /* Final output resides in 'src' with length in_len; consume in-place (no copy) */ + d->lowpassed = src; d->lp_len = in_len; /* No droop compensation for half-band cascade */ } else { From c764cf0eda96ab236370f3522a4ebfe0d891b1ee Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 21:45:18 -0500 Subject: [PATCH 33/48] =?UTF-8?q?rtl=5Fsdr=5Ffm:=20replace=20USB=E2=86=92d?= =?UTF-8?q?emod=20double-buffer/condvar=20handoff=20with=20a=20lock-free?= =?UTF-8?q?=20SPSC=20ring;=20enqueue=20widened=20blocks=20and=20drop=20old?= =?UTF-8?q?est=20on=20overflow=20to=20keep=20the=20callback=20non-blocking?= =?UTF-8?q?.=20Demod=20now=20reads=20ring=20blocks;=20ring=20is=20initiali?= =?UTF-8?q?zed/cleaned=20up,=20reducing=20wakeups=20and=20latency.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/rtl_sdr_fm.cpp | 182 +++++++++++++++++++++++++++++++++++---------- 1 file changed, 143 insertions(+), 39 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 186ac74..fd7045a 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -436,11 +436,9 @@ struct demod_state int exit_flag; pthread_t thread; int16_t *lowpassed; - /* Double-buffered input for callback→demod handoff */ - alignas(DSD_FME_ALIGN) int16_t input_buffers[2][MAXIMUM_BUF_LENGTH]; - std::atomic write_buf_index; - std::atomic ready_buf_index; - std::atomic input_len[2]; + /* SPSC input ring buffer for callback→demod handoff */ + /* Scratch buffer used by callback to widen+rotate before enqueue */ + alignas(DSD_FME_ALIGN) int16_t input_cb_buf[MAXIMUM_BUF_LENGTH]; int lp_len; int16_t lp_i_hist[10][6]; int16_t lp_q_hist[10][6]; @@ -530,7 +528,8 @@ struct demod_state struct { struct demod_state *s; int id; } mt_args[2]; int (*discriminator)(int, int, int, int); void (*mode_demod)(struct demod_state*); - pthread_cond_t ready; + /* Input SPSC ring (embedded below) replaces ready/condvar handoff */ + pthread_cond_t ready; /* kept for cleanup compatibility; unused now */ pthread_mutex_t ready_m; struct output_state *output_target; }; @@ -652,6 +651,108 @@ struct output_state pthread_mutex_t ready_m; }; +/* Simple SPSC ring for interleaved I/Q int16_t samples (input path) */ +struct input_ring_state +{ + int16_t *buffer; + size_t capacity; /* in int16_t elements */ + std::atomic head; + std::atomic tail; + pthread_cond_t ready; + pthread_mutex_t ready_m; +}; + +static inline size_t input_ring_used(const struct input_ring_state *r) +{ + size_t h = r->head.load(); + size_t t = r->tail.load(); + if (h >= t) return h - t; + return r->capacity - (t - h); +} + +static inline size_t input_ring_free(const struct input_ring_state *r) +{ + return (r->capacity - 1) - input_ring_used(r); +} + +static inline int input_ring_is_empty(const struct input_ring_state *r) +{ + return r->head.load() == r->tail.load(); +} + +static inline void input_ring_clear(struct input_ring_state *r) +{ + r->tail.store(0); + r->head.store(0); +} + +static void input_ring_write(struct input_ring_state *r, const int16_t *data, size_t count) +{ + int need_signal = input_ring_is_empty(r); + while (count > 0 && !exitflag) { + size_t free_sp = input_ring_free(r); + if (free_sp == 0) { + /* drop oldest half to avoid blocking USB callback */ + size_t t = r->tail.load(); + size_t drop = r->capacity / 2; + t = (t + drop) % r->capacity; + r->tail.store(t); + free_sp = input_ring_free(r); + } + size_t write_now = (count < free_sp) ? count : free_sp; + size_t h = r->head.load(); + size_t first = r->capacity - h; + if (first > write_now) first = write_now; + memcpy(r->buffer + h, data, first * sizeof(int16_t)); + if (write_now > first) { + memcpy(r->buffer, data + first, (write_now - first) * sizeof(int16_t)); + h = write_now - first; + } else { + h += first; + if (h == r->capacity) h = 0; + } + r->head.store(h); + data += write_now; + count -= write_now; + } + if (need_signal) { + pthread_mutex_lock(&r->ready_m); + pthread_cond_signal(&r->ready); + pthread_mutex_unlock(&r->ready_m); + } +} + +static int input_ring_read_block(struct input_ring_state *r, int16_t *out, size_t max_count) +{ + if (max_count == 0) return 0; + while (input_ring_is_empty(r)) { + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_nsec += 10L * 1000000L; /* 10ms */ + if (ts.tv_nsec >= 1000000000L) { + ts.tv_sec += ts.tv_nsec / 1000000000L; + ts.tv_nsec = ts.tv_nsec % 1000000000L; + } + pthread_mutex_lock(&r->ready_m); + pthread_cond_timedwait(&r->ready, &r->ready_m, &ts); + pthread_mutex_unlock(&r->ready_m); + if (exitflag) return -1; + } + size_t available = input_ring_used(r); + size_t read_now = (max_count < available) ? max_count : available; + size_t t = r->tail.load(); + size_t first = r->capacity - t; + if (first > read_now) first = read_now; + memcpy(out, r->buffer + t, first * sizeof(int16_t)); + t += first; + if (t == r->capacity) t = 0; + if (read_now > first) { + memcpy(out + first, r->buffer, (read_now - first) * sizeof(int16_t)); + t = read_now - first; + } + r->tail.store(t); + return (int)read_now; +} struct controller_state { int exit_flag; @@ -669,6 +770,7 @@ struct dongle_state dongle; struct demod_state demod; struct output_state output; struct controller_state controller; +static struct input_ring_state input_ring; #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) @@ -1720,26 +1822,20 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) buf[i] = 127;} s->mute = 0; } - /* Write directly into the current write buffer */ - int wb = d->write_buf_index.load(); - int16_t *dst = d->input_buffers[wb]; + /* Convert incoming u8 I/Q into temp int16 buffer and enqueue to SPSC input ring */ + int16_t *dst = d->input_cb_buf; /* If offset tuning disabled, combine 90° rotation with widening in one pass unless DSD_FME_COMBINE_ROT=0. */ if (!s->offset_tuning && combine_rotate_enabled) { widen_rotate90_u8_to_s16_bias127(buf, dst, len); } else if (!s->offset_tuning && !combine_rotate_enabled) { - /* Legacy order: rotate u8 in-place, then widen */ rotate_90(buf, len); widen_u8_to_s16_bias127(buf, dst, len); } else { - /* Offset tuning active: no rotation, just widen */ widen_u8_to_s16_bias127(buf, dst, len); } - d->input_len[wb].store(len); - /* Flip buffers atomically: the buffer we just wrote becomes ready */ - d->ready_buf_index.store(wb); - d->write_buf_index.store(wb ^ 1); - safe_cond_signal(&d->ready, &d->ready_m); + /* Enqueue widened samples */ + input_ring_write(&input_ring, dst, (size_t)len); } static void *dongle_thread_fn(void *arg) @@ -1756,11 +1852,11 @@ static void *demod_thread_fn(void *arg) struct output_state *o = d->output_target; maybe_set_thread_realtime_and_affinity("DEMOD"); while (!exitflag) { - safe_cond_wait(&d->ready, &d->ready_m); - /* Consume from last fully-written input buffer */ - int rb = d->ready_buf_index.load(); - d->lowpassed = d->input_buffers[rb]; - d->lp_len = (int)d->input_len[rb].load(); + /* Read a block from input ring */ + int got = input_ring_read_block(&input_ring, d->input_cb_buf, MAXIMUM_BUF_LENGTH); + if (got <= 0) continue; + d->lowpassed = d->input_cb_buf; + d->lp_len = got; full_demod(d); if (d->exit_flag) { exitflag = 1; @@ -2114,12 +2210,8 @@ void demod_init_analog(struct demod_state *s) memset(s->hb_hist_i[st], 0, sizeof(s->hb_hist_i[st])); memset(s->hb_hist_q[st], 0, sizeof(s->hb_hist_q[st])); } - /* Double-buffer init */ - s->write_buf_index.store(0); - s->ready_buf_index.store(1); - s->input_len[0].store(0); - s->input_len[1].store(0); - s->lowpassed = s->input_buffers[s->ready_buf_index.load()]; + /* Input ring does not require double-buffer init */ + s->lowpassed = s->input_cb_buf; s->lp_len = 0; pthread_cond_init(&s->ready, NULL); pthread_mutex_init(&s->ready_m, NULL); @@ -2191,12 +2283,8 @@ void demod_init_ro2(struct demod_state *s) memset(s->hb_hist_i[st], 0, sizeof(s->hb_hist_i[st])); memset(s->hb_hist_q[st], 0, sizeof(s->hb_hist_q[st])); } - /* Double-buffer init */ - s->write_buf_index.store(0); - s->ready_buf_index.store(1); - s->input_len[0].store(0); - s->input_len[1].store(0); - s->lowpassed = s->input_buffers[s->ready_buf_index.load()]; + /* Input ring does not require double-buffer init */ + s->lowpassed = s->input_cb_buf; s->lp_len = 0; pthread_cond_init(&s->ready, NULL); pthread_mutex_init(&s->ready_m, NULL); @@ -2268,12 +2356,8 @@ void demod_init(struct demod_state *s) memset(s->hb_hist_i[st], 0, sizeof(s->hb_hist_i[st])); memset(s->hb_hist_q[st], 0, sizeof(s->hb_hist_q[st])); } - /* Double-buffer init */ - s->write_buf_index.store(0); - s->ready_buf_index.store(1); - s->input_len[0].store(0); - s->input_len[1].store(0); - s->lowpassed = s->input_buffers[s->ready_buf_index.load()]; + /* Input ring does not require double-buffer init */ + s->lowpassed = s->input_cb_buf; s->lp_len = 0; pthread_cond_init(&s->ready, NULL); pthread_mutex_init(&s->ready_m, NULL); @@ -2461,6 +2545,23 @@ void open_rtlsdr_stream(dsd_opts *opts) demod_init_analog(&demod); else demod_init(&demod); output_init(&output); + /* Init input ring */ + { + void *mem_ptr = NULL; +#if defined(_POSIX_C_SOURCE) && (_POSIX_C_SOURCE >= 200112L) + if (posix_memalign(&mem_ptr, DSD_FME_ALIGN, (size_t)(MAXIMUM_BUF_LENGTH * 8) * sizeof(int16_t)) != 0) { + mem_ptr = malloc((size_t)(MAXIMUM_BUF_LENGTH * 8) * sizeof(int16_t)); + } +#else + mem_ptr = malloc((size_t)(MAXIMUM_BUF_LENGTH * 8) * sizeof(int16_t)); +#endif + input_ring.buffer = static_cast(mem_ptr); + input_ring.capacity = (size_t)(MAXIMUM_BUF_LENGTH * 8); + input_ring.head.store(0); + input_ring.tail.store(0); + pthread_cond_init(&input_ring.ready, NULL); + pthread_mutex_init(&input_ring.ready_m, NULL); + } controller_init(&controller); /* Read optional environment flags */ @@ -2717,6 +2818,9 @@ void cleanup_rtlsdr_stream() output_cleanup(&output); controller_cleanup(&controller); + /* free input ring */ + if (input_ring.buffer) { free(input_ring.buffer); input_ring.buffer = NULL; } + /* free LUT memory if allocated */ atan_lut_free(); From 4e64ef1c11d032e4b7e348b8eb0ae23981ea2c5c Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 22:06:55 -0500 Subject: [PATCH 34/48] rtl_sdr_fm: add runtime SIMD dispatch for widen/rotate paths, selecting AVX2/SSE2/NEON via CPUID/XGETBV (x86) or getauxval (ARM) with scalar fallbacks. Replace compile-time guards with lazy-initialized function-pointer wrappers so binaries run optimally across CPUs without changing call sites. --- src/rtl_sdr_fm.cpp | 370 ++++++++++++++++++++++++++++++++------------- 1 file changed, 262 insertions(+), 108 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index fd7045a..b03f064 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -33,16 +33,25 @@ #include "dsd.h" /* Optional SIMD intrinsics for USB byte->int16 widening */ -#if defined(__SSE2__) +#if defined(__x86_64__) || defined(__i386__) +#include #include #endif -#if defined(__AVX2__) -#include -#endif -#if defined(__ARM_NEON) || defined(__ARM_NEON__) +#if defined(__ARM_NEON) || defined(__ARM_NEON__) || defined(__aarch64__) #include #endif +/* Runtime CPU feature detection headers */ +#if defined(__x86_64__) || defined(__i386__) +#include +#endif +#if defined(__linux__) +#include +#ifndef AT_HWCAP +#define AT_HWCAP 16 +#endif +#endif + #define DEFAULT_SAMPLE_RATE 48000 #define DEFAULT_BUF_LENGTH (1 * 16384) #define MAXIMUM_OVERSAMPLE 16 @@ -97,55 +106,38 @@ static pthread_mutex_t atan_lut_mutex = PTHREAD_MUTEX_INITIALIZER; static int combine_rotate_enabled = 1; /* DSD_FME_COMBINE_ROT (1 default) */ static int upsample_fixedpoint_enabled = 1;/* DSD_FME_UPSAMPLE_FP (1 default) */ +/* Forward declaration for runtime SIMD dispatch initializer */ +static void dsd_fme_init_runtime_dispatch(void); + /* ===================== - USB widening helper (u8 -> s16 centered at 127) with SIMD and scalar fallback + USB widening helper dispatch setup ===================== */ +typedef void (*dsd_fme_widen_fn)(const unsigned char*, int16_t*, uint32_t); +typedef void (*dsd_fme_widen_rot_fn)(const unsigned char*, int16_t*, uint32_t); +static dsd_fme_widen_fn g_widen_impl = NULL; +static dsd_fme_widen_rot_fn g_widen_rot_impl = NULL; + +/* Public wrappers that lazy-init the runtime dispatch and call the chosen impl */ static inline void widen_u8_to_s16_bias127(const unsigned char * DSD_FME_RESTRICT src, int16_t * DSD_FME_RESTRICT dst, uint32_t len) +{ + if (!g_widen_impl) dsd_fme_init_runtime_dispatch(); + g_widen_impl(src, dst, len); +} + +static inline void widen_rotate90_u8_to_s16_bias127(const unsigned char * DSD_FME_RESTRICT src, + int16_t * DSD_FME_RESTRICT dst, uint32_t len) +{ + if (!g_widen_rot_impl) dsd_fme_init_runtime_dispatch(); + g_widen_rot_impl(src, dst, len); +} + +/* Scalar fallbacks (always available) */ +static inline void widen_u8_to_s16_bias127_scalar(const unsigned char * DSD_FME_RESTRICT src, + int16_t * DSD_FME_RESTRICT dst, uint32_t len) { uint32_t i = 0; -#if defined(__AVX2__) - /* Process 32 bytes per iteration */ - const __m256i bias256 = _mm256_set1_epi16(127); - for (; i + 32 <= len; i += 32) { - __m128i b0 = _mm_loadu_si128((const __m128i*)(src + i)); - __m128i b1 = _mm_loadu_si128((const __m128i*)(src + i + 16)); - __m256i lo = _mm256_cvtepu8_epi16(b0); - __m256i hi = _mm256_cvtepu8_epi16(b1); - lo = _mm256_sub_epi16(lo, bias256); - hi = _mm256_sub_epi16(hi, bias256); - _mm256_storeu_si256((__m256i*)(dst + i), lo); - _mm256_storeu_si256((__m256i*)(dst + i + 16), hi); - } -#endif -#if defined(__SSE2__) - /* Process 16 bytes per iteration */ - const __m128i bias = _mm_set1_epi16(127); - const __m128i zero = _mm_setzero_si128(); - for (; i + 16 <= len; i += 16) { - __m128i b = _mm_loadu_si128((const __m128i*)(src + i)); - __m128i lo = _mm_unpacklo_epi8(b, zero); /* widen to 16-bit unsigned */ - __m128i hi = _mm_unpackhi_epi8(b, zero); - lo = _mm_sub_epi16(lo, bias); /* center at 127 */ - hi = _mm_sub_epi16(hi, bias); - _mm_storeu_si128((__m128i*)(dst + i), lo); - _mm_storeu_si128((__m128i*)(dst + i + 8), hi); - } -#elif defined(__ARM_NEON) || defined(__ARM_NEON__) - /* Process 16 bytes per iteration */ - const uint8x8_t bias8 = vdup_n_u8(127); - for (; i + 16 <= len; i += 16) { - uint8x16_t v = vld1q_u8(src + i); - uint8x8_t v_lo = vget_low_u8(v); - uint8x8_t v_hi = vget_high_u8(v); - /* signed widen subtract: (uint8 - 127) -> int16 */ - int16x8_t lo = vsubl_u8(v_lo, bias8); - int16x8_t hi = vsubl_u8(v_hi, bias8); - vst1q_s16(dst + i, lo); - vst1q_s16(dst + i + 8, hi); - } -#endif - /* Scalar tail or whole buffer on non-SIMD builds */ + /* Scalar conversion: (u8 - 127) -> s16 */ for (; i < len; i++) { dst[i] = (int16_t)src[i] - 127; } @@ -155,69 +147,10 @@ static inline void widen_u8_to_s16_bias127(const unsigned char * DSD_FME_RESTRIC Combined rotate_90 (1, j, -1, -j) + widen (u8->s16 centered at 127) Processes 4 IQ samples per iteration to avoid branches. ===================== */ -static inline void widen_rotate90_u8_to_s16_bias127(const unsigned char * DSD_FME_RESTRICT src, +static inline void widen_rotate90_u8_to_s16_bias127_scalar(const unsigned char * DSD_FME_RESTRICT src, int16_t * DSD_FME_RESTRICT dst, uint32_t len) { -#if defined(__AVX2__) - /* AVX2 path: process 32 bytes per loop, applying rotation+wide. - Pattern per 8-byte group: [0,1,3,2,4,5,7,6] with select mask for 128-x on lanes {2,4,5,7}. */ - const __m256i shuffle = _mm256_setr_epi8( - 0, 1, 3, 2, 4, 5, 7, 6, 8, 9,11,10,12,13,15,14, - 0, 1, 3, 2, 4, 5, 7, 6, 8, 9,11,10,12,13,15,14); - const __m256i mask_sel = _mm256_setr_epi16( - 0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF, - 0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF); - const __m256i c127 = _mm256_set1_epi16(127); - const __m256i c128 = _mm256_set1_epi16(128); uint32_t i = 0; - for (; i + 32 <= len; i += 32) { - __m256i v8 = _mm256_loadu_si256((const __m256i*)(src + i)); - __m256i sh = _mm256_shuffle_epi8(v8, shuffle); - __m128i sh_lo = _mm256_castsi256_si128(sh); - __m128i sh_hi = _mm256_extracti128_si256(sh, 1); - __m256i v16_lo = _mm256_cvtepu8_epi16(sh_lo); - __m256i v16_hi = _mm256_cvtepu8_epi16(sh_hi); - __m256i bs_lo = _mm256_sub_epi16(v16_lo, c127); - __m256i bm_lo = _mm256_sub_epi16(c128, v16_lo); - __m256i bs_hi = _mm256_sub_epi16(v16_hi, c127); - __m256i bm_hi = _mm256_sub_epi16(c128, v16_hi); - __m256i out_lo = _mm256_blendv_epi8(bs_lo, bm_lo, mask_sel); - __m256i out_hi = _mm256_blendv_epi8(bs_hi, bm_hi, mask_sel); - _mm256_storeu_si256((__m256i*)(dst + i), out_lo); - _mm256_storeu_si256((__m256i*)(dst + i + 16), out_hi); - } -#elif defined(__ARM_NEON) || defined(__ARM_NEON__) - /* NEON path: process 16 bytes per loop */ - const uint8x16_t tbl_idx = {0,1,3,2,4,5,7,6, 8,9,11,10,12,13,15,14}; - const int16x8_t c127 = vdupq_n_s16(127); - const int16x8_t c128 = vdupq_n_s16(128); - /* mask lanes to select 128-x for lanes {2,4,5,7} in each 8-lane group */ - const uint16_t mpat[8] = {0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF}; - const uint16x8_t msel = vld1q_u16(mpat); - uint32_t i = 0; - for (; i + 16 <= len; i += 16) { - uint8x16_t v = vld1q_u8(src + i); - /* lane-wise shuffle */ - uint8x16_t sh = vqtbl1q_u8(v, tbl_idx); - /* widen low */ - uint8x8_t sh_lo8 = vget_low_u8(sh); - uint8x8_t sh_hi8 = vget_high_u8(sh); - int16x8_t v16_lo = vreinterpretq_s16_u16(vmovl_u8(sh_lo8)); - int16x8_t v16_hi = vreinterpretq_s16_u16(vmovl_u8(sh_hi8)); - int16x8_t bs_lo = vsubq_s16(v16_lo, c127); - int16x8_t bm_lo = vsubq_s16(c128, v16_lo); - int16x8_t bs_hi = vsubq_s16(v16_hi, c127); - int16x8_t bm_hi = vsubq_s16(c128, v16_hi); - /* blend: out = (msel ? bm : bs), msel repeated for both halves */ - int16x8_t out_lo = vbslq_s16(msel, bm_lo, bs_lo); - int16x8_t out_hi = vbslq_s16(msel, bm_hi, bs_hi); - vst1q_s16(dst + i, out_lo); - vst1q_s16(dst + i + 8, out_hi); - } -#else - uint32_t i = 0; -#endif - /* Scalar tail or generic path */ for (; i + 8 <= len; i += 8) { int16_t i0 = (int16_t)src[i + 0] - 127; int16_t q0 = (int16_t)src[i + 1] - 127; @@ -245,6 +178,227 @@ static inline void widen_rotate90_u8_to_s16_bias127(const unsigned char * DSD_FM } } +/* ===================== + Runtime CPU feature dispatch (AVX2/SSE2/NEON) + ===================== */ +#if defined(__GNUC__) || defined(__clang__) +#define DSD_FME_TARGET_ATTR(x) __attribute__((target(x))) +#else +#define DSD_FME_TARGET_ATTR(x) +#endif + +#if defined(__x86_64__) || defined(__i386__) +/* AVX2 specializations */ +static void DSD_FME_TARGET_ATTR("avx2") widen_u8_to_s16_bias127_avx2(const unsigned char *src, int16_t *dst, uint32_t len) +{ +#if defined(__AVX2__) + uint32_t i = 0; + const __m256i bias256 = _mm256_set1_epi16(127); + for (; i + 32 <= len; i += 32) { + __m128i b0 = _mm_loadu_si128((const __m128i*)(src + i)); + __m128i b1 = _mm_loadu_si128((const __m128i*)(src + i + 16)); + __m256i lo = _mm256_cvtepu8_epi16(b0); + __m256i hi = _mm256_cvtepu8_epi16(b1); + lo = _mm256_sub_epi16(lo, bias256); + hi = _mm256_sub_epi16(hi, bias256); + _mm256_storeu_si256((__m256i*)(dst + i), lo); + _mm256_storeu_si256((__m256i*)(dst + i + 16), hi); + } + for (; i < len; i++) dst[i] = (int16_t)src[i] - 127; +#else + widen_u8_to_s16_bias127_scalar(src, dst, len); +#endif +} + +static void DSD_FME_TARGET_ATTR("avx2") widen_rotate90_u8_to_s16_bias127_avx2(const unsigned char *src, int16_t *dst, uint32_t len) +{ +#if defined(__AVX2__) + const __m256i shuffle = _mm256_setr_epi8( + 0, 1, 3, 2, 4, 5, 7, 6, 8, 9,11,10,12,13,15,14, + 0, 1, 3, 2, 4, 5, 7, 6, 8, 9,11,10,12,13,15,14); + const __m256i mask_sel = _mm256_setr_epi16( + 0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF, + 0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF); + const __m256i c127 = _mm256_set1_epi16(127); + const __m256i c128 = _mm256_set1_epi16(128); + uint32_t i = 0; + for (; i + 32 <= len; i += 32) { + __m256i v8 = _mm256_loadu_si256((const __m256i*)(src + i)); + __m256i sh = _mm256_shuffle_epi8(v8, shuffle); + __m128i sh_lo = _mm256_castsi256_si128(sh); + __m128i sh_hi = _mm256_extracti128_si256(sh, 1); + __m256i v16_lo = _mm256_cvtepu8_epi16(sh_lo); + __m256i v16_hi = _mm256_cvtepu8_epi16(sh_hi); + __m256i bs_lo = _mm256_sub_epi16(v16_lo, c127); + __m256i bm_lo = _mm256_sub_epi16(c128, v16_lo); + __m256i bs_hi = _mm256_sub_epi16(v16_hi, c127); + __m256i bm_hi = _mm256_sub_epi16(c128, v16_hi); + __m256i out_lo = _mm256_blendv_epi8(bs_lo, bm_lo, mask_sel); + __m256i out_hi = _mm256_blendv_epi8(bs_hi, bm_hi, mask_sel); + _mm256_storeu_si256((__m256i*)(dst + i), out_lo); + _mm256_storeu_si256((__m256i*)(dst + i + 16), out_hi); + } + for (; i + 1 < len; i += 2) { + dst[i + 0] = (int16_t)src[i + 0] - 127; + dst[i + 1] = (int16_t)src[i + 1] - 127; + } +#else + widen_rotate90_u8_to_s16_bias127_scalar(src, dst, len); +#endif +} + +/* SSE2 specializations (safe on x86_64; guarded by runtime dispatch) */ +static void DSD_FME_TARGET_ATTR("sse2") widen_u8_to_s16_bias127_sse2(const unsigned char *src, int16_t *dst, uint32_t len) +{ +#if defined(__SSE2__) + uint32_t i = 0; + const __m128i bias = _mm_set1_epi16(127); + const __m128i zero = _mm_setzero_si128(); + for (; i + 16 <= len; i += 16) { + __m128i b = _mm_loadu_si128((const __m128i*)(src + i)); + __m128i lo = _mm_unpacklo_epi8(b, zero); + __m128i hi = _mm_unpackhi_epi8(b, zero); + lo = _mm_sub_epi16(lo, bias); + hi = _mm_sub_epi16(hi, bias); + _mm_storeu_si128((__m128i*)(dst + i), lo); + _mm_storeu_si128((__m128i*)(dst + i + 8), hi); + } + for (; i < len; i++) dst[i] = (int16_t)src[i] - 127; +#else + widen_u8_to_s16_bias127_scalar(src, dst, len); +#endif +} + +static void DSD_FME_TARGET_ATTR("sse2") widen_rotate90_u8_to_s16_bias127_sse2(const unsigned char *src, int16_t *dst, uint32_t len) +{ + /* Keep scalar logic for correctness without SSSE3 pshufb */ + widen_rotate90_u8_to_s16_bias127_scalar(src, dst, len); +} +#endif /* x86 */ + +/* NEON specializations */ +#if defined(__ARM_NEON) || defined(__ARM_NEON__) || defined(__aarch64__) +static void widen_u8_to_s16_bias127_neon(const unsigned char *src, int16_t *dst, uint32_t len) +{ + uint32_t i = 0; + const uint8x8_t bias8 = vdup_n_u8(127); + for (; i + 16 <= len; i += 16) { + uint8x16_t v = vld1q_u8(src + i); + uint8x8_t v_lo = vget_low_u8(v); + uint8x8_t v_hi = vget_high_u8(v); + int16x8_t lo = vsubl_u8(v_lo, bias8); + int16x8_t hi = vsubl_u8(v_hi, bias8); + vst1q_s16(dst + i, lo); + vst1q_s16(dst + i + 8, hi); + } + for (; i < len; i++) dst[i] = (int16_t)src[i] - 127; +} + +static void widen_rotate90_u8_to_s16_bias127_neon(const unsigned char *src, int16_t *dst, uint32_t len) +{ + const uint8x16_t tbl_idx = {0,1,3,2,4,5,7,6, 8,9,11,10,12,13,15,14}; + const int16x8_t c127 = vdupq_n_s16(127); + const int16x8_t c128 = vdupq_n_s16(128); + const uint16_t mpat[8] = {0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF}; + const uint16x8_t msel = vld1q_u16(mpat); + uint32_t i = 0; + for (; i + 16 <= len; i += 16) { + uint8x16_t v = vld1q_u8(src + i); + uint8x16_t sh = vqtbl1q_u8(v, tbl_idx); + uint8x8_t sh_lo8 = vget_low_u8(sh); + uint8x8_t sh_hi8 = vget_high_u8(sh); + int16x8_t v16_lo = vreinterpretq_s16_u16(vmovl_u8(sh_lo8)); + int16x8_t v16_hi = vreinterpretq_s16_u16(vmovl_u8(sh_hi8)); + int16x8_t bs_lo = vsubq_s16(v16_lo, c127); + int16x8_t bm_lo = vsubq_s16(c128, v16_lo); + int16x8_t bs_hi = vsubq_s16(v16_hi, c127); + int16x8_t bm_hi = vsubq_s16(c128, v16_hi); + int16x8_t out_lo = vbslq_s16(msel, bm_lo, bs_lo); + int16x8_t out_hi = vbslq_s16(msel, bm_hi, bs_hi); + vst1q_s16(dst + i, out_lo); + vst1q_s16(dst + i + 8, out_hi); + } + for (; i + 1 < len; i += 2) { + dst[i + 0] = (int16_t)src[i + 0] - 127; + dst[i + 1] = (int16_t)src[i + 1] - 127; + } +} +#endif + +/* Non-privileged CPU feature checks (CPUID/XGETBV on x86, getauxval on Linux/ARM) */ +static void dsd_fme_init_runtime_dispatch(void) +{ + static int done = 0; + if (done) return; + done = 1; + + int use_avx2 = 0; + int use_sse2 = 0; + +#if defined(__x86_64__) || defined(__i386__) + unsigned int eax=0, ebx=0, ecx=0, edx=0; + if (__get_cpuid(1, &eax, &ebx, &ecx, &edx)) { + use_sse2 = (edx & bit_SSE2) ? 1 : 0; + int osxsave = (ecx & bit_OSXSAVE) ? 1 : 0; + int avx = (ecx & bit_AVX) ? 1 : 0; + if (osxsave && avx) { + uint32_t xcr0_lo=0, xcr0_hi=0; + /* xgetbv ecx=0 is non-privileged on userland */ + __asm__ volatile ("xgetbv" : "=a"(xcr0_lo), "=d"(xcr0_hi) : "c"(0)); + uint64_t xcr0 = ((uint64_t)xcr0_hi << 32) | xcr0_lo; + if ((xcr0 & 0x6) == 0x6) { + unsigned int eax7=0, ebx7=0, ecx7=0, edx7=0; + if (__get_cpuid_count(7, 0, &eax7, &ebx7, &ecx7, &edx7)) { + use_avx2 = (ebx7 & bit_AVX2) ? 1 : 0; + } + } + } + } +#if defined(__x86_64__) + if (!use_sse2) use_sse2 = 1; /* mandatory on x86_64 */ +#endif +#endif + +#if defined(__aarch64__) + int use_neon = 1; /* ASIMD mandatory */ +#elif defined(__arm__) + int use_neon = 0; +#if defined(__linux__) + unsigned long hw = getauxval(AT_HWCAP); + /* HWCAP_NEON may be undefined on some headers; fallback to known value 4096 */ +#ifndef HWCAP_NEON +#define HWCAP_NEON 4096 +#endif + use_neon = (hw & HWCAP_NEON) ? 1 : 0; +#endif +#endif + + /* Fallbacks */ + g_widen_impl = &widen_u8_to_s16_bias127_scalar; + g_widen_rot_impl = &widen_rotate90_u8_to_s16_bias127_scalar; + +#if defined(__ARM_NEON) || defined(__ARM_NEON__) || defined(__aarch64__) + if (use_neon) { + g_widen_impl = &widen_u8_to_s16_bias127_neon; + g_widen_rot_impl = &widen_rotate90_u8_to_s16_bias127_neon; + return; + } +#endif + +#if defined(__x86_64__) || defined(__i386__) + if (use_avx2) { + g_widen_impl = &widen_u8_to_s16_bias127_avx2; + g_widen_rot_impl = &widen_rotate90_u8_to_s16_bias127_avx2; + return; + } + if (use_sse2) { + g_widen_impl = &widen_u8_to_s16_bias127_sse2; + g_widen_rot_impl = &widen_rotate90_u8_to_s16_bias127_sse2; + return; + } +#endif +} + /* ===================== Saturating helpers ===================== */ From 2ce089ccf8f6d049a002a0863405f28656e5177b Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 22:21:37 -0500 Subject: [PATCH 35/48] rtl_sdr_fm: make SIMD runtime dispatch thread-safe with pthread_once and ensure AVX2/SSE2 paths compile by relying on function-level target attributes (remove AVX2/SSE2 guards). Gate NEON rotate to AArch64 (ARMv7 falls back to scalar) while retaining NEON widen for portability and performance. --- src/rtl_sdr_fm.cpp | 30 ++++++++++++++---------------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index b03f064..c899299 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -108,6 +108,8 @@ static int upsample_fixedpoint_enabled = 1;/* DSD_FME_UPSAMPLE_FP (1 default) */ /* Forward declaration for runtime SIMD dispatch initializer */ static void dsd_fme_init_runtime_dispatch(void); +static void dsd_fme_init_runtime_dispatch_once(void); +static pthread_once_t dsd_fme_dispatch_once_control = PTHREAD_ONCE_INIT; /* ===================== USB widening helper dispatch setup @@ -191,7 +193,6 @@ static inline void widen_rotate90_u8_to_s16_bias127_scalar(const unsigned char * /* AVX2 specializations */ static void DSD_FME_TARGET_ATTR("avx2") widen_u8_to_s16_bias127_avx2(const unsigned char *src, int16_t *dst, uint32_t len) { -#if defined(__AVX2__) uint32_t i = 0; const __m256i bias256 = _mm256_set1_epi16(127); for (; i + 32 <= len; i += 32) { @@ -205,14 +206,10 @@ static void DSD_FME_TARGET_ATTR("avx2") widen_u8_to_s16_bias127_avx2(const unsig _mm256_storeu_si256((__m256i*)(dst + i + 16), hi); } for (; i < len; i++) dst[i] = (int16_t)src[i] - 127; -#else - widen_u8_to_s16_bias127_scalar(src, dst, len); -#endif } static void DSD_FME_TARGET_ATTR("avx2") widen_rotate90_u8_to_s16_bias127_avx2(const unsigned char *src, int16_t *dst, uint32_t len) { -#if defined(__AVX2__) const __m256i shuffle = _mm256_setr_epi8( 0, 1, 3, 2, 4, 5, 7, 6, 8, 9,11,10,12,13,15,14, 0, 1, 3, 2, 4, 5, 7, 6, 8, 9,11,10,12,13,15,14); @@ -242,15 +239,11 @@ static void DSD_FME_TARGET_ATTR("avx2") widen_rotate90_u8_to_s16_bias127_avx2(co dst[i + 0] = (int16_t)src[i + 0] - 127; dst[i + 1] = (int16_t)src[i + 1] - 127; } -#else - widen_rotate90_u8_to_s16_bias127_scalar(src, dst, len); -#endif } /* SSE2 specializations (safe on x86_64; guarded by runtime dispatch) */ static void DSD_FME_TARGET_ATTR("sse2") widen_u8_to_s16_bias127_sse2(const unsigned char *src, int16_t *dst, uint32_t len) { -#if defined(__SSE2__) uint32_t i = 0; const __m128i bias = _mm_set1_epi16(127); const __m128i zero = _mm_setzero_si128(); @@ -264,9 +257,6 @@ static void DSD_FME_TARGET_ATTR("sse2") widen_u8_to_s16_bias127_sse2(const unsig _mm_storeu_si128((__m128i*)(dst + i + 8), hi); } for (; i < len; i++) dst[i] = (int16_t)src[i] - 127; -#else - widen_u8_to_s16_bias127_scalar(src, dst, len); -#endif } static void DSD_FME_TARGET_ATTR("sse2") widen_rotate90_u8_to_s16_bias127_sse2(const unsigned char *src, int16_t *dst, uint32_t len) @@ -296,6 +286,7 @@ static void widen_u8_to_s16_bias127_neon(const unsigned char *src, int16_t *dst, static void widen_rotate90_u8_to_s16_bias127_neon(const unsigned char *src, int16_t *dst, uint32_t len) { +#if defined(__aarch64__) const uint8x16_t tbl_idx = {0,1,3,2,4,5,7,6, 8,9,11,10,12,13,15,14}; const int16x8_t c127 = vdupq_n_s16(127); const int16x8_t c128 = vdupq_n_s16(128); @@ -322,15 +313,16 @@ static void widen_rotate90_u8_to_s16_bias127_neon(const unsigned char *src, int1 dst[i + 0] = (int16_t)src[i + 0] - 127; dst[i + 1] = (int16_t)src[i + 1] - 127; } +#else + /* ARMv7 NEON lacks vqtbl1q_u8; use scalar fallback for rotate+widen */ + widen_rotate90_u8_to_s16_bias127_scalar(src, dst, len); +#endif } #endif /* Non-privileged CPU feature checks (CPUID/XGETBV on x86, getauxval on Linux/ARM) */ -static void dsd_fme_init_runtime_dispatch(void) +static void dsd_fme_init_runtime_dispatch_once(void) { - static int done = 0; - if (done) return; - done = 1; int use_avx2 = 0; int use_sse2 = 0; @@ -399,6 +391,12 @@ static void dsd_fme_init_runtime_dispatch(void) #endif } +static void dsd_fme_init_runtime_dispatch(void) +{ + /* Thread-safe, idempotent initialization */ + pthread_once(&dsd_fme_dispatch_once_control, dsd_fme_init_runtime_dispatch_once); +} + /* ===================== Saturating helpers ===================== */ From 5834127ff7704db92c1c4eb6961afafa6854a8ca Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Wed, 3 Sep 2025 22:33:32 -0500 Subject: [PATCH 36/48] =?UTF-8?q?rtl=5Fsdr=5Ffm:=20fix=20rotate+widen=20ta?= =?UTF-8?q?il=20handling=20(scalar/AVX2/NEON)=20to=20preserve=2090=C2=B0?= =?UTF-8?q?=20rotation=20and=20add=20SSSE3=20rotate+widen=20with=20runtime?= =?UTF-8?q?=20dispatch=20on=20x86.=20Also=20include=20tmmintrin=20and=20cl?= =?UTF-8?q?arify=20SSE2=20fallback=20comment.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/rtl_sdr_fm.cpp | 81 ++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 68 insertions(+), 13 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index c899299..6c1ac19 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -36,6 +36,7 @@ #if defined(__x86_64__) || defined(__i386__) #include #include +#include #endif #if defined(__ARM_NEON) || defined(__ARM_NEON__) || defined(__aarch64__) #include @@ -174,9 +175,28 @@ static inline void widen_rotate90_u8_to_s16_bias127_scalar(const unsigned char * dst[i + 6] = q3; dst[i + 7] = (int16_t)(1 - i3); } - for (; i + 1 < len; i += 2) { - dst[i + 0] = (int16_t)src[i + 0] - 127; - dst[i + 1] = (int16_t)src[i + 1] - 127; + /* Tail: apply rotation pattern for remaining up to 6 samples */ + if (i < len) { + uint32_t base = i; + uint32_t rem = len - base; + if (rem >= 2) { + int16_t i0 = (int16_t)src[base + 0] - 127; + int16_t q0 = (int16_t)src[base + 1] - 127; + dst[base + 0] = i0; + dst[base + 1] = q0; + } + if (rem >= 4) { + int16_t i1 = (int16_t)src[base + 2] - 127; + int16_t q1 = (int16_t)src[base + 3] - 127; + dst[base + 2] = (int16_t)(1 - q1); + dst[base + 3] = i1; + } + if (rem >= 6) { + int16_t i2 = (int16_t)src[base + 4] - 127; + int16_t q2 = (int16_t)src[base + 5] - 127; + dst[base + 4] = (int16_t)(1 - i2); + dst[base + 5] = (int16_t)(1 - q2); + } } } @@ -235,9 +255,9 @@ static void DSD_FME_TARGET_ATTR("avx2") widen_rotate90_u8_to_s16_bias127_avx2(co _mm256_storeu_si256((__m256i*)(dst + i), out_lo); _mm256_storeu_si256((__m256i*)(dst + i + 16), out_hi); } - for (; i + 1 < len; i += 2) { - dst[i + 0] = (int16_t)src[i + 0] - 127; - dst[i + 1] = (int16_t)src[i + 1] - 127; + /* Tail: preserve rotation via scalar helper */ + if (i < len) { + widen_rotate90_u8_to_s16_bias127_scalar(src + i, dst + i, len - i); } } @@ -261,11 +281,43 @@ static void DSD_FME_TARGET_ATTR("sse2") widen_u8_to_s16_bias127_sse2(const unsig static void DSD_FME_TARGET_ATTR("sse2") widen_rotate90_u8_to_s16_bias127_sse2(const unsigned char *src, int16_t *dst, uint32_t len) { - /* Keep scalar logic for correctness without SSSE3 pshufb */ + /* Keep scalar logic for correctness without SSSE3 pshufb (SSE2 lacks byte shuffle). */ widen_rotate90_u8_to_s16_bias127_scalar(src, dst, len); } #endif /* x86 */ +#if defined(__x86_64__) || defined(__i386__) +/* SSSE3 specialization (rotate+widen) */ +static void DSD_FME_TARGET_ATTR("ssse3") widen_rotate90_u8_to_s16_bias127_ssse3(const unsigned char *src, int16_t *dst, uint32_t len) +{ + uint32_t i = 0; + const __m128i shuffle = _mm_setr_epi8( + 0, 1, 3, 2, 4, 5, 7, 6, 8, 9,11,10,12,13,15,14); + const __m128i mask_sel = _mm_setr_epi16( + 0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF); + const __m128i c127 = _mm_set1_epi16(127); + const __m128i c128 = _mm_set1_epi16(128); + const __m128i zero = _mm_setzero_si128(); + for (; i + 16 <= len; i += 16) { + __m128i v8 = _mm_loadu_si128((const __m128i*)(src + i)); + __m128i sh = _mm_shuffle_epi8(v8, shuffle); + __m128i v16_lo = _mm_unpacklo_epi8(sh, zero); + __m128i v16_hi = _mm_unpackhi_epi8(sh, zero); + __m128i bs_lo = _mm_sub_epi16(v16_lo, c127); + __m128i bm_lo = _mm_sub_epi16(c128, v16_lo); + __m128i bs_hi = _mm_sub_epi16(v16_hi, c127); + __m128i bm_hi = _mm_sub_epi16(c128, v16_hi); + __m128i out_lo = _mm_or_si128(_mm_and_si128(bm_lo, mask_sel), _mm_andnot_si128(mask_sel, bs_lo)); + __m128i out_hi = _mm_or_si128(_mm_and_si128(bm_hi, mask_sel), _mm_andnot_si128(mask_sel, bs_hi)); + _mm_storeu_si128((__m128i*)(dst + i), out_lo); + _mm_storeu_si128((__m128i*)(dst + i + 8), out_hi); + } + if (i < len) { + widen_rotate90_u8_to_s16_bias127_scalar(src + i, dst + i, len - i); + } +} +#endif /* x86 */ + /* NEON specializations */ #if defined(__ARM_NEON) || defined(__ARM_NEON__) || defined(__aarch64__) static void widen_u8_to_s16_bias127_neon(const unsigned char *src, int16_t *dst, uint32_t len) @@ -309,9 +361,9 @@ static void widen_rotate90_u8_to_s16_bias127_neon(const unsigned char *src, int1 vst1q_s16(dst + i, out_lo); vst1q_s16(dst + i + 8, out_hi); } - for (; i + 1 < len; i += 2) { - dst[i + 0] = (int16_t)src[i + 0] - 127; - dst[i + 1] = (int16_t)src[i + 1] - 127; + /* Tail: preserve rotation via scalar helper */ + if (i < len) { + widen_rotate90_u8_to_s16_bias127_scalar(src + i, dst + i, len - i); } #else /* ARMv7 NEON lacks vqtbl1q_u8; use scalar fallback for rotate+widen */ @@ -326,11 +378,13 @@ static void dsd_fme_init_runtime_dispatch_once(void) int use_avx2 = 0; int use_sse2 = 0; + int use_ssse3 = 0; #if defined(__x86_64__) || defined(__i386__) unsigned int eax=0, ebx=0, ecx=0, edx=0; if (__get_cpuid(1, &eax, &ebx, &ecx, &edx)) { use_sse2 = (edx & bit_SSE2) ? 1 : 0; + use_ssse3 = (ecx & bit_SSSE3) ? 1 : 0; int osxsave = (ecx & bit_OSXSAVE) ? 1 : 0; int avx = (ecx & bit_AVX) ? 1 : 0; if (osxsave && avx) { @@ -383,9 +437,10 @@ static void dsd_fme_init_runtime_dispatch_once(void) g_widen_rot_impl = &widen_rotate90_u8_to_s16_bias127_avx2; return; } - if (use_sse2) { - g_widen_impl = &widen_u8_to_s16_bias127_sse2; - g_widen_rot_impl = &widen_rotate90_u8_to_s16_bias127_sse2; + if (use_ssse3 || use_sse2) { + if (use_sse2) g_widen_impl = &widen_u8_to_s16_bias127_sse2; + if (use_ssse3) g_widen_rot_impl = &widen_rotate90_u8_to_s16_bias127_ssse3; + else g_widen_rot_impl = &widen_rotate90_u8_to_s16_bias127_sse2; return; } #endif From 283d000b59b761dba5211cfe757c4b58843a5075 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Thu, 4 Sep 2025 08:27:59 -0500 Subject: [PATCH 37/48] rtl_sdr_fm: fix critical correctness bug: +1 DC bias in rotate+widen paths --- src/rtl_sdr_fm.cpp | 44 +++++++++++++++++++++++++++----------------- 1 file changed, 27 insertions(+), 17 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 6c1ac19..5b8e883 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -146,6 +146,18 @@ static inline void widen_u8_to_s16_bias127_scalar(const unsigned char * DSD_FME_ } } +/* Scalar widening that subtracts 128 instead of 127. + Used to pair with legacy byte-wise rotate_90(u8) which performs 255 - x negation, + so that the overall effect equals correct centered negation (127 - x). */ +static inline void widen_u8_to_s16_bias128_scalar(const unsigned char * DSD_FME_RESTRICT src, + int16_t * DSD_FME_RESTRICT dst, uint32_t len) +{ + uint32_t i = 0; + for (; i < len; i++) { + dst[i] = (int16_t)src[i] - 128; + } +} + /* ===================== Combined rotate_90 (1, j, -1, -j) + widen (u8->s16 centered at 127) Processes 4 IQ samples per iteration to avoid branches. @@ -162,18 +174,18 @@ static inline void widen_rotate90_u8_to_s16_bias127_scalar(const unsigned char * int16_t i1 = (int16_t)src[i + 2] - 127; int16_t q1 = (int16_t)src[i + 3] - 127; - dst[i + 2] = (int16_t)(1 - q1); + dst[i + 2] = (int16_t)(-q1); dst[i + 3] = i1; int16_t i2 = (int16_t)src[i + 4] - 127; int16_t q2 = (int16_t)src[i + 5] - 127; - dst[i + 4] = (int16_t)(1 - i2); - dst[i + 5] = (int16_t)(1 - q2); + dst[i + 4] = (int16_t)(-i2); + dst[i + 5] = (int16_t)(-q2); int16_t i3 = (int16_t)src[i + 6] - 127; int16_t q3 = (int16_t)src[i + 7] - 127; dst[i + 6] = q3; - dst[i + 7] = (int16_t)(1 - i3); + dst[i + 7] = (int16_t)(-i3); } /* Tail: apply rotation pattern for remaining up to 6 samples */ if (i < len) { @@ -188,14 +200,14 @@ static inline void widen_rotate90_u8_to_s16_bias127_scalar(const unsigned char * if (rem >= 4) { int16_t i1 = (int16_t)src[base + 2] - 127; int16_t q1 = (int16_t)src[base + 3] - 127; - dst[base + 2] = (int16_t)(1 - q1); + dst[base + 2] = (int16_t)(-q1); dst[base + 3] = i1; } if (rem >= 6) { int16_t i2 = (int16_t)src[base + 4] - 127; int16_t q2 = (int16_t)src[base + 5] - 127; - dst[base + 4] = (int16_t)(1 - i2); - dst[base + 5] = (int16_t)(1 - q2); + dst[base + 4] = (int16_t)(-i2); + dst[base + 5] = (int16_t)(-q2); } } } @@ -237,7 +249,6 @@ static void DSD_FME_TARGET_ATTR("avx2") widen_rotate90_u8_to_s16_bias127_avx2(co 0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF, 0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF); const __m256i c127 = _mm256_set1_epi16(127); - const __m256i c128 = _mm256_set1_epi16(128); uint32_t i = 0; for (; i + 32 <= len; i += 32) { __m256i v8 = _mm256_loadu_si256((const __m256i*)(src + i)); @@ -247,9 +258,9 @@ static void DSD_FME_TARGET_ATTR("avx2") widen_rotate90_u8_to_s16_bias127_avx2(co __m256i v16_lo = _mm256_cvtepu8_epi16(sh_lo); __m256i v16_hi = _mm256_cvtepu8_epi16(sh_hi); __m256i bs_lo = _mm256_sub_epi16(v16_lo, c127); - __m256i bm_lo = _mm256_sub_epi16(c128, v16_lo); + __m256i bm_lo = _mm256_sub_epi16(c127, v16_lo); __m256i bs_hi = _mm256_sub_epi16(v16_hi, c127); - __m256i bm_hi = _mm256_sub_epi16(c128, v16_hi); + __m256i bm_hi = _mm256_sub_epi16(c127, v16_hi); __m256i out_lo = _mm256_blendv_epi8(bs_lo, bm_lo, mask_sel); __m256i out_hi = _mm256_blendv_epi8(bs_hi, bm_hi, mask_sel); _mm256_storeu_si256((__m256i*)(dst + i), out_lo); @@ -296,7 +307,6 @@ static void DSD_FME_TARGET_ATTR("ssse3") widen_rotate90_u8_to_s16_bias127_ssse3( const __m128i mask_sel = _mm_setr_epi16( 0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF); const __m128i c127 = _mm_set1_epi16(127); - const __m128i c128 = _mm_set1_epi16(128); const __m128i zero = _mm_setzero_si128(); for (; i + 16 <= len; i += 16) { __m128i v8 = _mm_loadu_si128((const __m128i*)(src + i)); @@ -304,9 +314,9 @@ static void DSD_FME_TARGET_ATTR("ssse3") widen_rotate90_u8_to_s16_bias127_ssse3( __m128i v16_lo = _mm_unpacklo_epi8(sh, zero); __m128i v16_hi = _mm_unpackhi_epi8(sh, zero); __m128i bs_lo = _mm_sub_epi16(v16_lo, c127); - __m128i bm_lo = _mm_sub_epi16(c128, v16_lo); + __m128i bm_lo = _mm_sub_epi16(c127, v16_lo); __m128i bs_hi = _mm_sub_epi16(v16_hi, c127); - __m128i bm_hi = _mm_sub_epi16(c128, v16_hi); + __m128i bm_hi = _mm_sub_epi16(c127, v16_hi); __m128i out_lo = _mm_or_si128(_mm_and_si128(bm_lo, mask_sel), _mm_andnot_si128(mask_sel, bs_lo)); __m128i out_hi = _mm_or_si128(_mm_and_si128(bm_hi, mask_sel), _mm_andnot_si128(mask_sel, bs_hi)); _mm_storeu_si128((__m128i*)(dst + i), out_lo); @@ -341,7 +351,6 @@ static void widen_rotate90_u8_to_s16_bias127_neon(const unsigned char *src, int1 #if defined(__aarch64__) const uint8x16_t tbl_idx = {0,1,3,2,4,5,7,6, 8,9,11,10,12,13,15,14}; const int16x8_t c127 = vdupq_n_s16(127); - const int16x8_t c128 = vdupq_n_s16(128); const uint16_t mpat[8] = {0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF}; const uint16x8_t msel = vld1q_u16(mpat); uint32_t i = 0; @@ -353,9 +362,9 @@ static void widen_rotate90_u8_to_s16_bias127_neon(const unsigned char *src, int1 int16x8_t v16_lo = vreinterpretq_s16_u16(vmovl_u8(sh_lo8)); int16x8_t v16_hi = vreinterpretq_s16_u16(vmovl_u8(sh_hi8)); int16x8_t bs_lo = vsubq_s16(v16_lo, c127); - int16x8_t bm_lo = vsubq_s16(c128, v16_lo); + int16x8_t bm_lo = vsubq_s16(c127, v16_lo); int16x8_t bs_hi = vsubq_s16(v16_hi, c127); - int16x8_t bm_hi = vsubq_s16(c128, v16_hi); + int16x8_t bm_hi = vsubq_s16(c127, v16_hi); int16x8_t out_lo = vbslq_s16(msel, bm_lo, bs_lo); int16x8_t out_hi = vbslq_s16(msel, bm_hi, bs_hi); vst1q_s16(dst + i, out_lo); @@ -2037,7 +2046,8 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) widen_rotate90_u8_to_s16_bias127(buf, dst, len); } else if (!s->offset_tuning && !combine_rotate_enabled) { rotate_90(buf, len); - widen_u8_to_s16_bias127(buf, dst, len); + /* Use 128 subtraction to avoid +1 bias after byte-wise negation */ + widen_u8_to_s16_bias128_scalar(buf, dst, len); } else { widen_u8_to_s16_bias127(buf, dst, len); } From dd8620bc392c797c4f842577a3a4ca84ba28caba Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Thu, 4 Sep 2025 08:34:09 -0500 Subject: [PATCH 38/48] rtl_sdr_fm: remove per-sample `memmove` from resampler, use ring buffer. --- src/rtl_sdr_fm.cpp | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 5b8e883..d4d372a 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -709,7 +709,8 @@ struct demod_state int resamp_taps_len; /* prototype taps length (padded to K*L) */ int resamp_taps_per_phase;/* K = ceil(taps_len/L) */ int16_t *resamp_taps; /* Q15 taps, length = K*L */ - int16_t *resamp_hist; /* most-recent-first history, length = K */ + int16_t *resamp_hist; /* circular history, length = K */ + int resamp_hist_head; /* head index into circular history [0..K-1] */ /* Output buffer for resampler (worst-case 4x expansion) */ alignas(DSD_FME_ALIGN) int16_t resamp_outbuf[MAXIMUM_BUF_LENGTH * 4]; /* Residual CFO loop (FLL) state */ @@ -1807,6 +1808,7 @@ static void resamp_design(struct demod_state *s, int L, int M) return; } memset(s->resamp_hist, 0, (size_t)taps_per_phase * sizeof(int16_t)); + s->resamp_hist_head = 0; /* Windowed-sinc (Hamming) */ double gain = 0.0; @@ -1854,22 +1856,27 @@ static int resamp_process_block(struct demod_state *s, const int16_t *in, int in const int K = s->resamp_taps_per_phase; /* taps per phase */ const int16_t *taps = s->resamp_taps; /* length K*L, phase-major stride L */ int phase = s->resamp_phase; /* 0..L-1 */ + int head = s->resamp_hist_head; /* circular head index */ int out_len = 0; for (int n = 0; n < in_len; n++) { - /* Push new sample into history (most-recent-first) */ - memmove(s->resamp_hist + 1, s->resamp_hist, (size_t)(K - 1) * sizeof(int16_t)); - s->resamp_hist[0] = in[n]; + /* Push new sample into circular history at head */ + s->resamp_hist[head] = in[n]; + head++; + if (head == K) head = 0; /* While we owe outputs with current input available */ int local_phase = phase; while (local_phase < L) { - /* Dot: y = sum_{k=0..K-1} hist[k] * taps[k*L + local_phase] */ + /* Dot: y = sum_{k=0..K-1} hist[idx] * taps[k*L + local_phase] + Access history most-recent-first starting from head-1. */ int64_t acc = 0; const int16_t *tk = taps + local_phase; + int idx = head - 1; if (idx < 0) idx += K; for (int k = 0; k < K; k++) { - acc += (int32_t)s->resamp_hist[k] * (int32_t)tk[0]; + acc += (int32_t)s->resamp_hist[idx] * (int32_t)tk[0]; tk += L; + idx--; if (idx < 0) idx += K; } /* Q15 -> Q0 with rounding and saturation */ acc += (1 << 14); @@ -1881,6 +1888,7 @@ static int resamp_process_block(struct demod_state *s, const int16_t *in, int in } s->resamp_phase = phase; + s->resamp_hist_head = head; return out_len; } From 836da1f5561c2825bf14266ebdd178e1faacb49f Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Thu, 4 Sep 2025 08:48:17 -0500 Subject: [PATCH 39/48] rtl_sdr_fm: fix USB callback - avoid extra copy when enqueuing into input ring. --- src/rtl_sdr_fm.cpp | 95 +++++++++++++++++++++++++++++++++++++++------- 1 file changed, 81 insertions(+), 14 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index d4d372a..53038b3 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -903,6 +903,52 @@ static inline void input_ring_clear(struct input_ring_state *r) r->head.store(0); } +/* Reserve up to two contiguous writable regions totaling at least min_needed (or less if near full). + Returns available regions via p1/n1 and p2/n2. May drop oldest half when full to avoid blocking. */ +static int input_ring_reserve(struct input_ring_state *r, size_t min_needed, + int16_t **p1, size_t *n1, int16_t **p2, size_t *n2) +{ + size_t free_sp = input_ring_free(r); + if (free_sp == 0) { + /* match write() behavior: drop oldest half to free space */ + size_t t = r->tail.load(); + size_t drop = r->capacity / 2; + t = (t + drop) % r->capacity; + r->tail.store(t); + free_sp = input_ring_free(r); + } + /* Provide up to min(free_sp, min_needed) across at most two regions */ + size_t grant = (min_needed < free_sp) ? min_needed : free_sp; + size_t h = r->head.load(); + size_t first = r->capacity - h; + if (first > grant) first = grant; + *p1 = (first > 0) ? (r->buffer + h) : NULL; + *n1 = first; + *p2 = NULL; + *n2 = 0; + if (grant > first) { + *p2 = r->buffer; + *n2 = grant - first; + } + return (int)(*n1 + *n2); +} + +/* Commit produced samples and signal consumer on empty->non-empty transition */ +static void input_ring_commit(struct input_ring_state *r, size_t produced) +{ + if (produced == 0) return; + int need_signal = input_ring_is_empty(r); + size_t h = r->head.load(); + h += produced; + if (h >= r->capacity) h %= r->capacity; + r->head.store(h); + if (need_signal) { + pthread_mutex_lock(&r->ready_m); + pthread_cond_signal(&r->ready); + pthread_mutex_unlock(&r->ready_m); + } +} + static void input_ring_write(struct input_ring_state *r, const int16_t *data, size_t count) { int need_signal = input_ring_is_empty(r); @@ -2027,7 +2073,6 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) { int i; struct dongle_state *s = static_cast(ctx); - struct demod_state *d = s->demod_target; /* One-time: ensure the USB callback thread gets RT scheduling/affinity if enabled */ { static std::atomic usb_sched_applied{0}; @@ -2046,21 +2091,43 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) buf[i] = 127;} s->mute = 0; } - /* Convert incoming u8 I/Q into temp int16 buffer and enqueue to SPSC input ring */ - int16_t *dst = d->input_cb_buf; - /* If offset tuning disabled, combine 90° rotation with widening in one pass - unless DSD_FME_COMBINE_ROT=0. */ - if (!s->offset_tuning && combine_rotate_enabled) { - widen_rotate90_u8_to_s16_bias127(buf, dst, len); - } else if (!s->offset_tuning && !combine_rotate_enabled) { + /* Convert incoming u8 I/Q and write directly into input ring without extra copy */ + size_t need = len; + size_t done = 0; + /* For legacy two-pass path, rotate the incoming byte buffer once up front */ + int use_two_pass = (!s->offset_tuning && !combine_rotate_enabled); + if (use_two_pass) { rotate_90(buf, len); - /* Use 128 subtraction to avoid +1 bias after byte-wise negation */ - widen_u8_to_s16_bias128_scalar(buf, dst, len); - } else { - widen_u8_to_s16_bias127(buf, dst, len); } - /* Enqueue widened samples */ - input_ring_write(&input_ring, dst, (size_t)len); + while (need > 0) { + int16_t *p1 = NULL, *p2 = NULL; size_t n1 = 0, n2 = 0; + input_ring_reserve(&input_ring, need, &p1, &n1, &p2, &n2); + if (n1 == 0 && n2 == 0) { + /* Ring still full after drop attempt; give up this callback to avoid stall */ + break; + } + /* Ensure even counts to keep I/Q pairs aligned */ + if (n1 & 1) n1--; + size_t w1 = (n1 < need) ? n1 : need; + size_t rem_after_w1 = need - w1; + if (n2 & 1) n2--; + size_t w2 = (n2 < rem_after_w1) ? n2 : rem_after_w1; + + if (!s->offset_tuning && combine_rotate_enabled) { + if (w1) widen_rotate90_u8_to_s16_bias127(buf + done, p1, (uint32_t)w1); + if (w2) widen_rotate90_u8_to_s16_bias127(buf + done + w1, p2, (uint32_t)w2); + } else if (use_two_pass) { + /* bytes already rotated in-place; widen with 128 subtraction to avoid bias */ + if (w1) widen_u8_to_s16_bias128_scalar(buf + done, p1, (uint32_t)w1); + if (w2) widen_u8_to_s16_bias128_scalar(buf + done + w1, p2, (uint32_t)w2); + } else { + if (w1) widen_u8_to_s16_bias127(buf + done, p1, (uint32_t)w1); + if (w2) widen_u8_to_s16_bias127(buf + done + w1, p2, (uint32_t)w2); + } + input_ring_commit(&input_ring, w1 + w2); + done += w1 + w2; + need -= w1 + w2; + } } static void *dongle_thread_fn(void *arg) From 046846337a4e7ff51c51370ae6cb2b6e1cbc4741 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Thu, 4 Sep 2025 08:57:50 -0500 Subject: [PATCH 40/48] rtl_sdr_fm: fix half-band decimator - exploit symmetry/zeros and consider SIMD --- src/rtl_sdr_fm.cpp | 77 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 59 insertions(+), 18 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 53038b3..2d1c6d7 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -503,19 +503,35 @@ static inline int hb_decim2_real(const int16_t *in, int in_len, int16_t *out, in int out_len = in_len >> 1; /* floor */ for (int n = 0; n < out_len; n++) { int center_idx = hist_len + (n << 1); /* position in the concatenated [hist | in] domain */ - int64_t acc = 0; - /* Convolution around center: taps indexed 0..HB_TAPS-1 */ - for (int t = 0; t < HB_TAPS; t++) { - int src_idx = center_idx - HB_HALF + t; - int16_t x; + /* Half-band optimization: only even taps and the center tap contribute (symmetric). */ + const int16_t c0 = hb_q15_taps[0]; + const int16_t c2 = hb_q15_taps[2]; + const int16_t c4 = hb_q15_taps[4]; + const int16_t c6 = hb_q15_taps[6]; + const int16_t c7 = hb_q15_taps[7]; /* center */ + auto get_sample = [&](int src_idx) -> int16_t { if (src_idx < hist_len) { - x = hist[src_idx]; + return hist[src_idx]; } else { int rel = src_idx - hist_len; - x = (rel < in_len) ? in[rel] : last; + return (rel < in_len) ? in[rel] : last; } - acc += (int32_t)hb_q15_taps[t] * (int32_t)x; - } + }; + int16_t xc = get_sample(center_idx); + int16_t xm1 = get_sample(center_idx - 1); + int16_t xp1 = get_sample(center_idx + 1); + int16_t xm3 = get_sample(center_idx - 3); + int16_t xp3 = get_sample(center_idx + 3); + int16_t xm5 = get_sample(center_idx - 5); + int16_t xp5 = get_sample(center_idx + 5); + int16_t xm7 = get_sample(center_idx - 7); + int16_t xp7 = get_sample(center_idx + 7); + int64_t acc = 0; + acc += (int32_t)c7 * (int32_t)xc; + acc += (int32_t)c6 * (int32_t)(xm1 + xp1); + acc += (int32_t)c4 * (int32_t)(xm3 + xp3); + acc += (int32_t)c2 * (int32_t)(xm5 + xp5); + acc += (int32_t)c0 * (int32_t)(xm7 + xp7); /* Q15 -> Q0 with rounding */ acc += (1 << 14); int32_t y = (int32_t)(acc >> 15); @@ -554,11 +570,13 @@ static inline int hb_decim2_complex_interleaved(const int16_t *in, int in_len, i int16_t lastQ = (ch_len > 0) ? in[in_len - 1] : 0; for (int n = 0; n < out_ch_len; n++) { int center_idx = hist_len + (n << 1); /* per-channel index */ - int64_t accI = 0; - int64_t accQ = 0; - for (int t = 0; t < HB_TAPS; t++) { - int src_idx = center_idx - HB_HALF + t; - int16_t xi, xq; + /* Half-band optimization: only even taps and the center tap contribute (symmetric). */ + const int16_t c0 = hb_q15_taps[0]; + const int16_t c2 = hb_q15_taps[2]; + const int16_t c4 = hb_q15_taps[4]; + const int16_t c6 = hb_q15_taps[6]; + const int16_t c7 = hb_q15_taps[7]; /* center */ + auto get_iq = [&](int src_idx, int16_t &xi, int16_t &xq) { if (src_idx < hist_len) { xi = hist_i[src_idx]; xq = hist_q[src_idx]; @@ -572,10 +590,33 @@ static inline int hb_decim2_complex_interleaved(const int16_t *in, int in_len, i xq = lastQ; } } - int16_t c = hb_q15_taps[t]; - accI += (int32_t)c * (int32_t)xi; - accQ += (int32_t)c * (int32_t)xq; - } + }; + int16_t ci, cq; + int16_t im1, qm1, ip1, qp1; + int16_t im3, qm3, ip3, qp3; + int16_t im5, qm5, ip5, qp5; + int16_t im7, qm7, ip7, qp7; + get_iq(center_idx, ci, cq); + get_iq(center_idx - 1, im1, qm1); + get_iq(center_idx + 1, ip1, qp1); + get_iq(center_idx - 3, im3, qm3); + get_iq(center_idx + 3, ip3, qp3); + get_iq(center_idx - 5, im5, qm5); + get_iq(center_idx + 5, ip5, qp5); + get_iq(center_idx - 7, im7, qm7); + get_iq(center_idx + 7, ip7, qp7); + int64_t accI = 0; + int64_t accQ = 0; + accI += (int32_t)c7 * (int32_t)ci; + accQ += (int32_t)c7 * (int32_t)cq; + accI += (int32_t)c6 * (int32_t)(im1 + ip1); + accQ += (int32_t)c6 * (int32_t)(qm1 + qp1); + accI += (int32_t)c4 * (int32_t)(im3 + ip3); + accQ += (int32_t)c4 * (int32_t)(qm3 + qp3); + accI += (int32_t)c2 * (int32_t)(im5 + ip5); + accQ += (int32_t)c2 * (int32_t)(qm5 + qp5); + accI += (int32_t)c0 * (int32_t)(im7 + ip7); + accQ += (int32_t)c0 * (int32_t)(qm7 + qp7); accI += (1 << 14); accQ += (1 << 14); int32_t yI = (int32_t)(accI >> 15); From 48b032e47aae81d0613864781ce1145d757edc7c Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Thu, 4 Sep 2025 09:01:17 -0500 Subject: [PATCH 41/48] rtl_sdr_fm: fix Gardner timing interpolation cost and gating --- src/rtl_sdr_fm.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 2d1c6d7..1cf7564 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -1797,7 +1797,10 @@ static inline void fll_update_error(struct demod_state *d) static inline void gardner_timing_adjust(struct demod_state *d) { if (!d->ted_enabled || d->ted_sps <= 1) return; + /* Guard: run TED only when we're near symbol rate to keep CPU low. + Skip when samples-per-symbol is very high unless explicitly forced. */ int sps = d->ted_sps; + if (sps > 12 && !d->ted_force) return; int mu = d->ted_mu_q20; /* Q20 */ int gain = d->ted_gain_q20; /* Q20 */ int16_t *x = d->lowpassed; From 09bf0408100aaca403428fee2f43f6db83104dfc Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Thu, 4 Sep 2025 09:06:11 -0500 Subject: [PATCH 42/48] rtl_sdr_fm: fix `low_pass_simple` - normalize to avoid amplitude growth/clipping --- src/rtl_sdr_fm.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 1cf7564..c9a5912 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -1373,9 +1373,9 @@ int low_pass_simple(int16_t *signal2, int len, int step) for(i2=0; i2= 0) ? (sum + step/2) / step : -(((-sum) + step/2) / step); + signal2[i/step] = (int16_t)val; } signal2[i/step + 1] = signal2[i/step]; return len / step; From f052271a1696df320316d541bed0dce81f6f1927 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Thu, 4 Sep 2025 09:13:05 -0500 Subject: [PATCH 43/48] rtl_sdr_fm: fix `mean_power` - integer-only implementation --- src/rtl_sdr_fm.cpp | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index c9a5912..1d89eb5 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -1983,26 +1983,24 @@ static int resamp_process_block(struct demod_state *s, const int16_t *in, int in } long int mean_power(int16_t *samples, int len, int step) -/* DC-corrected mean power (sqrt-free). Returns squared RMS units. */ +/* DC-corrected mean power (sqrt-free). Integer-only implementation. */ { - int i; int64_t p = 0; int64_t t = 0; - int64_t s; - double dc, err; - - for (i = 0; i < len; i += step) { - s = (int64_t)samples[i]; + for (int i = 0; i < len; i += step) { + int64_t s = (int64_t)samples[i]; t += s; p += s * s; } - /* correct for dc offset in squares */ - dc = (double)(t * step) / (double)len; - err = (double)t * 2.0 * dc - dc * dc * (double)len; - - double power = ((double)p - err) / (double)len; - if (power < 0.0) power = 0.0; - return (long int)power; + /* DC-corrected energy ≈ p - (t^2)/len with rounded division */ + int64_t dc_corr = 0; + if (len > 0) { + int64_t tt = t * t; + dc_corr = (tt + (len / 2)) / len; + } + int64_t energy = p - dc_corr; + if (energy < 0) energy = 0; + return (long int)(energy / (len > 0 ? len : 1)); } void full_demod(struct demod_state *d) From e094f95b0311c6252a5c2b00580740bf8e05d19e Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Thu, 4 Sep 2025 09:20:39 -0500 Subject: [PATCH 44/48] rtl_sdr_fm: FLL rotator - improve sine/cos approximation --- src/rtl_sdr_fm.cpp | 129 ++++++++++++++++++++++++++++++++++++++------- 1 file changed, 110 insertions(+), 19 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 1d89eb5..eb36762 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -103,6 +103,10 @@ static int atan_lut_size = 131072; /* 512 KB */ static int atan_lut_coef = 8; static pthread_once_t atan_lut_once = PTHREAD_ONCE_INIT; static pthread_mutex_t atan_lut_mutex = PTHREAD_MUTEX_INITIALIZER; +/* Optional quarter-wave sine LUT for FLL rotator (Q15). */ +static int16_t fll_qsine_q15_lut[1025]; /* 0..pi/2 in 1024 steps, +1 guard for exact pi/2 */ +static pthread_once_t fll_lut_once = PTHREAD_ONCE_INIT; +static int fll_lut_enabled = 0; /* DSD_FME_FLL_LUT (0 default: use fast approx) */ /* Debug/compat toggles via env */ static int combine_rotate_enabled = 1; /* DSD_FME_COMBINE_ROT (1 default) */ static int upsample_fixedpoint_enabled = 1;/* DSD_FME_UPSAMPLE_FP (1 default) */ @@ -659,6 +663,71 @@ static void atan_lut_once_init(void) } } +/* Build quarter-wave sine LUT in Q15: sin(theta) where theta in [0, pi/2] */ +static void fll_lut_once_init(void) +{ + for (int i = 0; i <= 1024; i++) { + double theta = (kPi * 0.5) * ((double)i / 1024.0); + int v = (int)lrint(sin(theta) * 32767.0); + if (v > 32767) v = 32767; + if (v < -32767) v = -32767; + fll_qsine_q15_lut[i] = (int16_t)v; +} +} + +/* Return cos/sin for phase in Q15 using quarter-wave LUT with linear interpolation. */ +static inline void fll_sin_cos_q15_from_phase_lut(int phase_q15, int16_t *c_out, int16_t *s_out) +{ + /* phase_q15 wraps at 1<<15 mapping to 2*pi */ + int p = phase_q15 & 0x7FFF; /* 0..32767 */ + int quad = p >> 13; /* 0..3 */ + int r = p & 0x1FFF; /* position within quadrant: 0..8191 */ + + /* Helper to sample quarter-wave S(r) with r in [0..8192] using 1024-segment linear interp */ + auto sample_quarter = [](int r8192) -> int16_t { + if (r8192 < 0) r8192 = 0; + if (r8192 > 8192) r8192 = 8192; + int idx = r8192 >> 3; /* 0..1024 */ + int frac = r8192 & 7; /* 0..7 */ + int16_t s0 = fll_qsine_q15_lut[idx]; + int16_t s1 = fll_qsine_q15_lut[(idx < 1024) ? (idx + 1) : 1024]; + int diff = (int)s1 - (int)s0; + int interp = (int)s0 + ((diff * frac + 4) >> 3); /* rounded */ + if (interp > 32767) interp = 32767; + if (interp < -32767) interp = -32767; + return (int16_t)interp; + }; + + int16_t s_pos, c_pos; + /* Cosine within quadrant uses complementary angle in the quarter-wave */ + switch (quad) { + case 0: /* [0, pi/2) */ + s_pos = sample_quarter(r); + c_pos = sample_quarter(8192 - r); + *s_out = s_pos; + *c_out = c_pos; + break; + case 1: /* [pi/2, pi) */ + s_pos = sample_quarter(8192 - r); + c_pos = sample_quarter(r); + *s_out = s_pos; + *c_out = (int16_t)(-c_pos); + break; + case 2: /* [pi, 3pi/2) */ + s_pos = sample_quarter(r); + c_pos = sample_quarter(8192 - r); + *s_out = (int16_t)(-s_pos); + *c_out = (int16_t)(-c_pos); + break; + default: /* 3: [3pi/2, 2pi) */ + s_pos = sample_quarter(8192 - r); + c_pos = sample_quarter(r); + *s_out = (int16_t)(-s_pos); + *c_out = c_pos; + break; + } +} + //UDP -- keep for compatibility reasons #include #include @@ -1738,25 +1807,42 @@ static inline void fll_mix_and_update(struct demod_state *d) const int N = d->lp_len; int phase = d->fll_phase_q15; /* Q15 wraps at 1<<15 ~ 2*pi */ const int freq = d->fll_freq_q15; /* Q15 increment per sample */ - /* Use small LUT-free rotator: sin/cos approximation via angle wrapping into quadrants. */ - for (int i = 0; i + 1 < N; i += 2) { - int p = phase & 0x7FFF; /* 0..32767 */ - int q = p >> 13; /* quadrant 0..3 */ - int16_t c = 32767, s = 0; - int16_t r = (int16_t)(p & 0x1FFF); /* 0..8191 */ - switch (q) { - case 0: c = 32767; s = (int16_t)((r * 4)); break; - case 1: c = (int16_t)(32767 - (r * 4)); s = 32767; break; - case 2: c = -32767; s = (int16_t)(32767 - (r * 4)); break; - default: c = (int16_t)(-32767 + (r * 4)); s = -32767; break; - } - int xr = x[i]; - int xj = x[i+1]; - int32_t yr = ((int32_t)xr * c + (int32_t)xj * s) >> 15; - int32_t yj = ((int32_t)xj * c - (int32_t)xr * s) >> 15; - x[i] = (int16_t)yr; - x[i+1] = (int16_t)yj; - phase += freq; + /* Optional: higher-quality quarter-wave LUT rotator (linear interp), enabled via DSD_FME_FLL_LUT=1. */ + if (fll_lut_enabled) { + /* Ensure LUT is initialized once before use */ + pthread_once(&fll_lut_once, fll_lut_once_init); + for (int i = 0; i + 1 < N; i += 2) { + int16_t c, s; + fll_sin_cos_q15_from_phase_lut(phase, &c, &s); + int xr = x[i]; + int xj = x[i+1]; + int32_t yr = ((int32_t)xr * c + (int32_t)xj * s) >> 15; + int32_t yj = ((int32_t)xj * c - (int32_t)xr * s) >> 15; + x[i] = (int16_t)yr; + x[i+1] = (int16_t)yj; + phase += freq; + } + } else { + /* Fast LUT-free rotator: piecewise-linear sin/cos within quadrants. */ + for (int i = 0; i + 1 < N; i += 2) { + int p = phase & 0x7FFF; /* 0..32767 */ + int q = p >> 13; /* quadrant 0..3 */ + int16_t c = 32767, s = 0; + int16_t r = (int16_t)(p & 0x1FFF); /* 0..8191 */ + switch (q) { + case 0: c = 32767; s = (int16_t)((r * 4)); break; + case 1: c = (int16_t)(32767 - (r * 4)); s = 32767; break; + case 2: c = -32767; s = (int16_t)(32767 - (r * 4)); break; + default: c = (int16_t)(-32767 + (r * 4)); s = -32767; break; + } + int xr = x[i]; + int xj = x[i+1]; + int32_t yr = ((int32_t)xr * c + (int32_t)xj * s) >> 15; + int32_t yj = ((int32_t)xj * c - (int32_t)xr * s) >> 15; + x[i] = (int16_t)yr; + x[i+1] = (int16_t)yj; + phase += freq; + } } d->fll_phase_q15 = phase & 0x7FFF; } @@ -2953,6 +3039,11 @@ void open_rtlsdr_stream(dsd_opts *opts) /* Configure FLL/TED via envs. Defaults: FLL on with small gains; TED off by default. */ const char *fll = getenv("DSD_FME_FLL"); demod.fll_enabled = (!fll || fll[0] == '\0' || fll[0] == '1') ? 1 : 0; + /* Optional: enable LUT-based FLL rotator via DSD_FME_FLL_LUT=1 */ + { + const char *flut = getenv("DSD_FME_FLL_LUT"); + fll_lut_enabled = (flut && flut[0] == '1') ? 1 : 0; + } /* Gains in Q15; very conservative defaults */ const char *fa = getenv("DSD_FME_FLL_ALPHA"); const char *fb = getenv("DSD_FME_FLL_BETA"); From a866d98e755601992a2502a734bc938d7cc22ab0 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Thu, 4 Sep 2025 11:38:41 -0500 Subject: [PATCH 45/48] rtl_sdr_fm: comment cleanup --- src/rtl_sdr_fm.cpp | 910 ++++++++++++++++++++++++++++++++++++++------- 1 file changed, 781 insertions(+), 129 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index eb36762..5aec1e7 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -5,6 +5,7 @@ * Copyright (C) 2012 by Kyle Keen * Copyright (C) 2013 by Elias Oenal * Copyright (C) 2014 by Kyle Keen + * Copyright (C) 2025 by arancormonk <180709949+arancormonk@users.noreply.github.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 @@ -53,6 +54,79 @@ #endif #endif +/* + * Environment variables (runtime configuration) + * Set in your shell before launching dsd-fme, e.g.: + * $ export DSD_FME_RESAMP=48000 + * $ export DSD_FME_FLL=1 DSD_FME_FLL_ALPHA=150 DSD_FME_FLL_BETA=15 + * $ export DSD_FME_TED=1 DSD_FME_TED_SPS=10 DSD_FME_TED_GAIN=96 + * $ export DSD_FME_AUDIO_LPF=3000 + * $ ./build/dsd-fme ... + * + * Realtime scheduling and CPU affinity + * - DSD_FME_RT_SCHED + * Enable best-effort realtime scheduling (SCHED_FIFO). Requires CAP_SYS_NICE or root. + * Values: "1" to enable, unset/other to disable. Default: disabled. + * - DSD_FME_RT_PRIO_USB | DSD_FME_RT_PRIO_DONGLE | DSD_FME_RT_PRIO_DEMOD + * Optional per-thread priorities (1..99, clamped to system limits). Used only if RT_SCHED=1. + * Example: export DSD_FME_RT_PRIO_DEMOD=85 + * - DSD_FME_CPU_USB | DSD_FME_CPU_DONGLE | DSD_FME_CPU_DEMOD + * Optional CPU core pinning for each thread. Integer CPU id (>=0). Example: export DSD_FME_CPU_DEMOD=2 + * + * Frontend/decimation/upsampling + * - DSD_FME_HB_DECIM + * Use half-band FIR decimator cascade (fast, good response) instead of legacy CIC-like path. + * Values: 1 enable, 0 disable. Default: 1 (enabled). + * - DSD_FME_COMBINE_ROT + * Combine 90° IQ rotation with USB byte→int16 widening in one pass when offset tuning is off. + * Values: 1 enable, 0 disable. Default: 1 (enabled). + * - DSD_FME_UPSAMPLE_FP + * Use fixed-point arithmetic in legacy linear upsampler for lower CPU/divisions. + * Values: 1 enable, 0 disable. Default: 1 (enabled). + * + * Rational resampler (polyphase upfirdn L/M) + * - DSD_FME_RESAMP + * Target output sample rate in Hz. Enables L/M resampler when set. + * Values: "off" or "0" to disable; integer Hz (e.g., 48000) to enable. Default: 48000 (enabled). + * + * Residual CFO frequency-locked loop (FLL) + * - DSD_FME_FLL + * Enable residual carrier frequency correction. + * Values: "1" or unset to enable; other values disable. Default: enabled. + * - DSD_FME_FLL_LUT + * Use higher-quality quarter-wave sine LUT mixer for FLL rotation. + * Values: 1 enable, 0/empty disable. Default: 0 (disabled; fast piecewise approx). + * - DSD_FME_FLL_ALPHA, DSD_FME_FLL_BETA + * Proportional and integral gains (Q15 fixed-point, ~value/32768). Typical small values. + * Defaults: ALPHA=100 (~0.003), BETA=10 (~0.0003). May be adjusted for digital modes if not set. + * + * Gardner timing error detector (TED) + * - DSD_FME_TED + * Enable lightweight fractional-delay timing correction. Generally off for analog FM. + * Values: 1 enable, else disabled. Default: 0 (disabled). For certain digital modes, defaults are adjusted + * only if envs are not provided (still off unless forced via DSD_FME_TED=1). + * - DSD_FME_TED_SPS + * Nominal samples-per-symbol (integer). If unset and a digital mode is active, it is derived from output rate. + * Default: 10. + * - DSD_FME_TED_GAIN + * Small loop gain (Q20). Default: 64; for common digital modes may default to 96 when not provided. + * - DSD_FME_TED_FORCE + * Force TED to run for FM/C4FM paths where it is normally skipped. Values: 1 enable, else disabled. Default: 0. + * + * Audio processing + * - DSD_FME_DEEMPH + * Post-demod deemphasis time constant. Applies only when the active demod preset enables deemphasis. + * Values: "75" (75µs, default), "50" (50µs), "nfm" (~750µs), "off" (disable). + * - DSD_FME_AUDIO_LPF + * Optional one-pole low-pass filter after demod. Approximate cutoff in Hz. + * Values: "off" or "0" to disable; integer (e.g., 3000, 5000) to enable. Default: off. + * + * Intra-block multithreading + * - DSD_FME_MT + * Enable a minimal 2-thread worker pool for certain CPU-heavy inner loops. + * Values: 1 enable, else disabled. Default: 0 (disabled). + */ + #define DEFAULT_SAMPLE_RATE 48000 #define DEFAULT_BUF_LENGTH (1 * 16384) #define MAXIMUM_OVERSAMPLE 16 @@ -70,18 +144,36 @@ static int ACTUAL_BUF_LENGTH; static const double kPi = 3.14159265358979323846; -/* ===================== - Vectorization helpers and alignment - ===================== */ #if defined(__GNUC__) || defined(__clang__) #define DSD_FME_PRAGMA(x) _Pragma(#x) #define DSD_FME_IVDEP DSD_FME_PRAGMA(GCC ivdep) +/** + * Hint that a pointer is aligned to a compile-time boundary for vectorization. + * + * This is a lightweight wrapper over compiler intrinsics to improve + * auto-vectorization by promising the compiler that the pointer meets the + * specified alignment. Use with care and only when the alignment guarantee + * is actually met. + * + * @tparam T Element type of the pointer. + * @param p Pointer to memory that is at least `align_unused` aligned. + * @param align_unused Alignment in bytes (ignored at runtime; for readability). + * @return Pointer `p` with alignment assumption applied. + */ template static inline T* assume_aligned_ptr(T* p, size_t /*align_unused*/) { return (T*)__builtin_assume_aligned(p, 64); } #else #define DSD_FME_IVDEP +/** + * See aligned variant: noop fallback when compiler does not support alignment + * assumptions. + * @tparam T Element type of the pointer. + * @param p Pointer to return as-is. + * @param align_unused Unused parameter for signature compatibility. + * @return Pointer `p` unchanged. + */ template static inline T* assume_aligned_ptr(T* p, size_t /*align_unused*/) { return p; @@ -116,15 +208,19 @@ static void dsd_fme_init_runtime_dispatch(void); static void dsd_fme_init_runtime_dispatch_once(void); static pthread_once_t dsd_fme_dispatch_once_control = PTHREAD_ONCE_INIT; -/* ===================== - USB widening helper dispatch setup - ===================== */ typedef void (*dsd_fme_widen_fn)(const unsigned char*, int16_t*, uint32_t); typedef void (*dsd_fme_widen_rot_fn)(const unsigned char*, int16_t*, uint32_t); static dsd_fme_widen_fn g_widen_impl = NULL; static dsd_fme_widen_rot_fn g_widen_rot_impl = NULL; -/* Public wrappers that lazy-init the runtime dispatch and call the chosen impl */ +/** + * Public wrapper that lazy-initializes runtime dispatch and widens u8 to s16 + * centered at 127. + * + * @param src Source buffer of unsigned bytes (I/Q interleaved). + * @param dst Destination int16 buffer. + * @param len Number of bytes in src to process. + */ static inline void widen_u8_to_s16_bias127(const unsigned char * DSD_FME_RESTRICT src, int16_t * DSD_FME_RESTRICT dst, uint32_t len) { @@ -132,6 +228,14 @@ static inline void widen_u8_to_s16_bias127(const unsigned char * DSD_FME_RESTRIC g_widen_impl(src, dst, len); } +/** + * Public wrapper that lazy-initializes runtime dispatch and performs 90° IQ + * rotation combined with widen u8→s16 centered at 127. + * + * @param src Source buffer of unsigned bytes (I/Q interleaved). + * @param dst Destination int16 buffer. + * @param len Number of bytes in src to process. + */ static inline void widen_rotate90_u8_to_s16_bias127(const unsigned char * DSD_FME_RESTRICT src, int16_t * DSD_FME_RESTRICT dst, uint32_t len) { @@ -139,7 +243,13 @@ static inline void widen_rotate90_u8_to_s16_bias127(const unsigned char * DSD_FM g_widen_rot_impl(src, dst, len); } -/* Scalar fallbacks (always available) */ +/** + * Scalar fallback: widen u8 to s16 centered at 127. + * + * @param src Source buffer of unsigned bytes. + * @param dst Destination int16 buffer. + * @param len Number of bytes to process. + */ static inline void widen_u8_to_s16_bias127_scalar(const unsigned char * DSD_FME_RESTRICT src, int16_t * DSD_FME_RESTRICT dst, uint32_t len) { @@ -150,9 +260,15 @@ static inline void widen_u8_to_s16_bias127_scalar(const unsigned char * DSD_FME_ } } -/* Scalar widening that subtracts 128 instead of 127. - Used to pair with legacy byte-wise rotate_90(u8) which performs 255 - x negation, - so that the overall effect equals correct centered negation (127 - x). */ +/** + * Scalar widening that subtracts 128 instead of 127. + * Intended to pair with legacy byte-wise rotate_90(u8) which performs 255-x + * negation so that overall effect equals correct centered negation (127-x). + * + * @param src Source buffer of unsigned bytes. + * @param dst Destination int16 buffer. + * @param len Number of bytes to process. + */ static inline void widen_u8_to_s16_bias128_scalar(const unsigned char * DSD_FME_RESTRICT src, int16_t * DSD_FME_RESTRICT dst, uint32_t len) { @@ -162,10 +278,14 @@ static inline void widen_u8_to_s16_bias128_scalar(const unsigned char * DSD_FME_ } } -/* ===================== - Combined rotate_90 (1, j, -1, -j) + widen (u8->s16 centered at 127) - Processes 4 IQ samples per iteration to avoid branches. - ===================== */ +/** + * Combined 90° rotation (1, j, -1, -j) + widen (u8→s16 centered at 127). + * Processes 4 IQ samples per iteration to avoid branches. + * + * @param src Source buffer of unsigned bytes (I/Q interleaved). + * @param dst Destination int16 buffer. + * @param len Number of bytes in src to process. + */ static inline void widen_rotate90_u8_to_s16_bias127_scalar(const unsigned char * DSD_FME_RESTRICT src, int16_t * DSD_FME_RESTRICT dst, uint32_t len) { @@ -216,9 +336,6 @@ static inline void widen_rotate90_u8_to_s16_bias127_scalar(const unsigned char * } } -/* ===================== - Runtime CPU feature dispatch (AVX2/SSE2/NEON) - ===================== */ #if defined(__GNUC__) || defined(__clang__) #define DSD_FME_TARGET_ATTR(x) __attribute__((target(x))) #else @@ -227,6 +344,12 @@ static inline void widen_rotate90_u8_to_s16_bias127_scalar(const unsigned char * #if defined(__x86_64__) || defined(__i386__) /* AVX2 specializations */ +/** + * AVX2: widen unsigned bytes to signed 16-bit centered at 127. + * @param src Source u8 buffer. + * @param dst Destination s16 buffer. + * @param len Number of bytes to process. + */ static void DSD_FME_TARGET_ATTR("avx2") widen_u8_to_s16_bias127_avx2(const unsigned char *src, int16_t *dst, uint32_t len) { uint32_t i = 0; @@ -244,6 +367,13 @@ static void DSD_FME_TARGET_ATTR("avx2") widen_u8_to_s16_bias127_avx2(const unsig for (; i < len; i++) dst[i] = (int16_t)src[i] - 127; } +/** + * AVX2: rotate (1,j,-1,-j) interleaved IQ and widen u8→s16 centered at 127. + * Tail elements are handled by a scalar helper to preserve the rotation pattern. + * @param src Source u8 buffer (I/Q interleaved). + * @param dst Destination s16 buffer. + * @param len Number of bytes to process. + */ static void DSD_FME_TARGET_ATTR("avx2") widen_rotate90_u8_to_s16_bias127_avx2(const unsigned char *src, int16_t *dst, uint32_t len) { const __m256i shuffle = _mm256_setr_epi8( @@ -276,7 +406,10 @@ static void DSD_FME_TARGET_ATTR("avx2") widen_rotate90_u8_to_s16_bias127_avx2(co } } -/* SSE2 specializations (safe on x86_64; guarded by runtime dispatch) */ + +/** + * SSE2: widen unsigned bytes to signed 16-bit centered at 127. + */ static void DSD_FME_TARGET_ATTR("sse2") widen_u8_to_s16_bias127_sse2(const unsigned char *src, int16_t *dst, uint32_t len) { uint32_t i = 0; @@ -294,6 +427,9 @@ static void DSD_FME_TARGET_ATTR("sse2") widen_u8_to_s16_bias127_sse2(const unsig for (; i < len; i++) dst[i] = (int16_t)src[i] - 127; } +/** + * SSE2: fallback rotate+widen via scalar since SSE2 lacks byte-wise shuffle. + */ static void DSD_FME_TARGET_ATTR("sse2") widen_rotate90_u8_to_s16_bias127_sse2(const unsigned char *src, int16_t *dst, uint32_t len) { /* Keep scalar logic for correctness without SSSE3 pshufb (SSE2 lacks byte shuffle). */ @@ -302,7 +438,10 @@ static void DSD_FME_TARGET_ATTR("sse2") widen_rotate90_u8_to_s16_bias127_sse2(co #endif /* x86 */ #if defined(__x86_64__) || defined(__i386__) -/* SSSE3 specialization (rotate+widen) */ +/** + * SSSE3: rotate (1,j,-1,-j) interleaved IQ and widen u8→s16 centered at 127. + * Tail elements are handled by a scalar helper to preserve the rotation pattern. + */ static void DSD_FME_TARGET_ATTR("ssse3") widen_rotate90_u8_to_s16_bias127_ssse3(const unsigned char *src, int16_t *dst, uint32_t len) { uint32_t i = 0; @@ -332,8 +471,10 @@ static void DSD_FME_TARGET_ATTR("ssse3") widen_rotate90_u8_to_s16_bias127_ssse3( } #endif /* x86 */ -/* NEON specializations */ #if defined(__ARM_NEON) || defined(__ARM_NEON__) || defined(__aarch64__) +/** + * NEON: widen unsigned bytes to signed 16-bit centered at 127. + */ static void widen_u8_to_s16_bias127_neon(const unsigned char *src, int16_t *dst, uint32_t len) { uint32_t i = 0; @@ -350,6 +491,10 @@ static void widen_u8_to_s16_bias127_neon(const unsigned char *src, int16_t *dst, for (; i < len; i++) dst[i] = (int16_t)src[i] - 127; } +/** + * NEON: rotate (1,j,-1,-j) interleaved IQ and widen u8→s16 centered at 127. + * Uses table lookup on aarch64; on ARMv7 (no vqtbl1q_u8) falls back to scalar. + */ static void widen_rotate90_u8_to_s16_bias127_neon(const unsigned char *src, int16_t *dst, uint32_t len) { #if defined(__aarch64__) @@ -385,7 +530,13 @@ static void widen_rotate90_u8_to_s16_bias127_neon(const unsigned char *src, int1 } #endif -/* Non-privileged CPU feature checks (CPUID/XGETBV on x86, getauxval on Linux/ARM) */ +/** + * Runtime CPU feature detection and dispatch binding for widening/rotation. + * + * Detects available SIMD features (AVX2/SSSE3/SSE2/NEON) and binds the + * function pointers `g_widen_impl` and `g_widen_rot_impl` accordingly. + * Safe to call concurrently; guarded by pthread_once in the public wrapper. + */ static void dsd_fme_init_runtime_dispatch_once(void) { @@ -459,15 +610,21 @@ static void dsd_fme_init_runtime_dispatch_once(void) #endif } +/** + * Ensure SIMD dispatch is initialized (thread-safe, idempotent). + */ static void dsd_fme_init_runtime_dispatch(void) { /* Thread-safe, idempotent initialization */ pthread_once(&dsd_fme_dispatch_once_control, dsd_fme_init_runtime_dispatch_once); } -/* ===================== - Saturating helpers - ===================== */ +/** + * Saturate 32-bit integer to 16-bit range. + * + * @param x Input 32-bit value. + * @return Clamped 16-bit value in [-32768, 32767]. + */ static inline int16_t sat16(int32_t x) { if (x > 32767) return 32767; @@ -475,10 +632,6 @@ static inline int16_t sat16(int32_t x) return (int16_t)x; } -/* ===================== - Half-band FIR decimator (2:1) - Q15 taps - ===================== */ - /* Runtime flag (default enabled). Set DSD_FME_HB_DECIM=0 to use legacy decimator */ static int use_halfband_decimator = 1; @@ -492,12 +645,15 @@ static const int16_t hb_q15_taps[HB_TAPS] = { 7000, 0, -500, 0, 1800, 0, -108 }; -/* Decimate one real channel by 2 using half-band FIR with persistent left history. - - in: pointer to real samples - - in_len: number of real samples - - out: pointer to output buffer (size >= in_len/2) - - hist: persistent history of length HB_TAPS-1 (left wing) - Returns number of output samples written (in_len/2). */ +/** + * Decimate one real channel by 2 using a half-band FIR with persistent left history. + * + * @param in Pointer to real input samples. + * @param in_len Number of real input samples. + * @param out Pointer to output buffer (size >= in_len/2). + * @param hist Persistent history of length HB_TAPS-1 (left wing). + * @return Number of output samples written (in_len/2). + */ static inline int hb_decim2_real(const int16_t *in, int in_len, int16_t *out, int16_t *hist) { const int hist_len = HB_TAPS - 1; @@ -556,11 +712,18 @@ static inline int hb_decim2_real(const int16_t *in, int in_len, int16_t *out, in return out_len; } -/* One 2:1 decimation stage on interleaved I/Q, using per-stage histories. */ -/* Removed hb_decim2_complex_stage in favor of inlined staged processing in full_demod() */ - -/* Fused interleaved complex half-band decimator: decimate I and Q in one pass without deinterleave/reinterleave. - in_len is number of interleaved int16_t (I,Q,I,Q,...). Returns interleaved output length (in_len/2). */ +/** + * Fused interleaved complex half-band decimator. Decimates I and Q in one pass + * without deinterleaving. Exploits the half-band property (zero odd taps) and + * symmetry; Q15 taps with rounding preserve unity DC gain. + * + * @param in Pointer to interleaved input samples (I,Q,I,Q,...). + * @param in_len Number of interleaved int16_t values in input. + * @param out Pointer to interleaved output buffer (size >= in_len/2). + * @param hist_i Persistent I-channel history of length HB_TAPS-1. + * @param hist_q Persistent Q-channel history of length HB_TAPS-1. + * @return Interleaved output length (in_len/2). + */ static inline int hb_decim2_complex_interleaved(const int16_t *in, int in_len, int16_t *out, int16_t *hist_i, int16_t *hist_q) { @@ -650,7 +813,6 @@ static inline int hb_decim2_complex_interleaved(const int16_t *in, int in_len, i return out_ch_len << 1; /* interleaved length */ } - static void atan_lut_once_init(void) { int i; @@ -675,7 +837,15 @@ static void fll_lut_once_init(void) } } -/* Return cos/sin for phase in Q15 using quarter-wave LUT with linear interpolation. */ +/** + * Compute sin/cos in Q15 from phase using a quarter-wave sine LUT. + * The choice to use the LUT vs. a fast piecewise approximation is + * made by the caller (see fll_mix_and_update). + * + * @param phase_q15 Phase accumulator (Q15, wrap at 2*pi -> 1<<15 scale). + * @param c_out [out] Cosine Q15. + * @param s_out [out] Sine Q15. + */ static inline void fll_sin_cos_q15_from_phase_lut(int phase_q15, int16_t *c_out, int16_t *s_out) { /* phase_q15 wraps at 1<<15 mapping to 2*pi */ @@ -739,7 +909,6 @@ int bandwidth_divisor = 48000; //divide bandwidth by this to get multiplier for short int volume_multiplier; short int port; - struct dongle_state { int exit_flag; @@ -762,8 +931,8 @@ struct demod_state int exit_flag; pthread_t thread; int16_t *lowpassed; - /* SPSC input ring buffer for callback→demod handoff */ - /* Scratch buffer used by callback to widen+rotate before enqueue */ + /* Scratch buffer for demod thread to read blocks from the input ring */ + /* Not a ring; callback writes directly into the global input ring. */ alignas(DSD_FME_ALIGN) int16_t input_cb_buf[MAXIMUM_BUF_LENGTH]; int lp_len; int16_t lp_i_hist[10][6]; @@ -803,7 +972,7 @@ struct demod_state int16_t hb_workbuf[MAXIMUM_BUF_LENGTH]; int16_t hb_hist_i[10][HB_TAPS-1]; int16_t hb_hist_q[10][HB_TAPS-1]; - /* Preallocated deinterleave/output buffers for HB decimator */ + /* Reserved buffers for potential deinterleave path (currently unused) */ alignas(DSD_FME_ALIGN) int16_t hb_i_buf[MAXIMUM_BUF_LENGTH/2]; alignas(DSD_FME_ALIGN) int16_t hb_q_buf[MAXIMUM_BUF_LENGTH/2]; alignas(DSD_FME_ALIGN) int16_t hb_i_out[MAXIMUM_BUF_LENGTH/2]; @@ -855,7 +1024,7 @@ struct demod_state struct { struct demod_state *s; int id; } mt_args[2]; int (*discriminator)(int, int, int, int); void (*mode_demod)(struct demod_state*); - /* Input SPSC ring (embedded below) replaces ready/condvar handoff */ + /* Ready/condvar kept for cleanup compatibility; input ring is a global SPSC ring */ pthread_cond_t ready; /* kept for cleanup compatibility; unused now */ pthread_mutex_t ready_m; struct output_state *output_target; @@ -866,12 +1035,16 @@ static void demod_mt_init(struct demod_state *s); static void demod_mt_destroy(struct demod_state *s); static void demod_mt_run_two(struct demod_state *s, void (*f0)(void*), void *a0, void (*f1)(void*), void *a1); -/* ===================== - Minimal 2-thread worker pool for DEMOD intra-block tasks - ===================== */ struct demod_mt_worker_arg { struct demod_state *s; int id; }; +/** + * Worker thread procedure for the minimal 2-thread DEMOD pool. + * Waits for posted tasks, executes them, and signals completion. + * + * @param arg Pointer to `demod_mt_worker_arg` with owning state and worker id. + * @return NULL when the worker exits. + */ static void *demod_mt_worker(void *arg) { struct demod_mt_worker_arg *wa = (struct demod_mt_worker_arg*)arg; @@ -908,6 +1081,12 @@ static void *demod_mt_worker(void *arg) return NULL; } +/** + * Initialize the minimal 2-thread worker pool for intra-block tasks. + * Enabled when `DSD_FME_MT=1` in the environment. + * + * @param s Demodulator state to initialize with worker threads. + */ static void demod_mt_init(struct demod_state *s) { const char *mt = getenv("DSD_FME_MT"); @@ -931,6 +1110,11 @@ static void demod_mt_init(struct demod_state *s) fprintf(stderr, "Intra-block multithreading enabled (DSD_FME_MT=1), workers: 2.\n"); } +/** + * Tear down the minimal worker pool created by demod_mt_init. + * + * @param s Demodulator state whose worker pool will be destroyed. + */ static void demod_mt_destroy(struct demod_state *s) { if (!s->mt_enabled) return; @@ -946,6 +1130,16 @@ static void demod_mt_destroy(struct demod_state *s) pthread_mutex_destroy(&s->mt_lock); } +/** + * Post up to two tasks to the worker pool and wait for their completion. + * If the pool is disabled, runs tasks synchronously on the caller thread. + * + * @param s Demodulator state with worker pool. + * @param f0 Task 0 function pointer (may be NULL). + * @param a0 Task 0 argument. + * @param f1 Task 1 function pointer (may be NULL). + * @param a1 Task 1 argument. + */ static void demod_mt_run_two(struct demod_state *s, void (*f0)(void*), void *a0, void (*f1)(void*), void *a1) { if (!s->mt_enabled) { @@ -989,6 +1183,9 @@ struct input_ring_state pthread_mutex_t ready_m; }; +/** + * Number of samples currently in the input ring. + */ static inline size_t input_ring_used(const struct input_ring_state *r) { size_t h = r->head.load(); @@ -997,24 +1194,45 @@ static inline size_t input_ring_used(const struct input_ring_state *r) return r->capacity - (t - h); } +/** + * Number of free slots available for writing in the input ring. + */ static inline size_t input_ring_free(const struct input_ring_state *r) { return (r->capacity - 1) - input_ring_used(r); } +/** + * Check if the input ring is empty. + */ static inline int input_ring_is_empty(const struct input_ring_state *r) { return r->head.load() == r->tail.load(); } +/** + * Clear the input ring head/tail indices. + */ static inline void input_ring_clear(struct input_ring_state *r) { r->tail.store(0); r->head.store(0); } -/* Reserve up to two contiguous writable regions totaling at least min_needed (or less if near full). - Returns available regions via p1/n1 and p2/n2. May drop oldest half when full to avoid blocking. */ +/** + * Reserve up to two contiguous writable regions totaling at least min_needed + * (or less if near full). + * + * May drop oldest half when full to avoid blocking. + * + * @param r Input ring buffer state. + * @param min_needed Minimum samples requested for writing. + * @param p1 [out] First writable region pointer or NULL. + * @param n1 [out] First writable region length. + * @param p2 [out] Second writable region pointer or NULL. + * @param n2 [out] Second writable region length. + * @return Total writable samples granted across regions. + */ static int input_ring_reserve(struct input_ring_state *r, size_t min_needed, int16_t **p1, size_t *n1, int16_t **p2, size_t *n2) { @@ -1043,7 +1261,12 @@ static int input_ring_reserve(struct input_ring_state *r, size_t min_needed, return (int)(*n1 + *n2); } -/* Commit produced samples and signal consumer on empty->non-empty transition */ +/** + * Commit produced samples and signal consumer on empty→non-empty transition. + * + * @param r Input ring buffer state. + * @param produced Number of samples produced to commit. + */ static void input_ring_commit(struct input_ring_state *r, size_t produced) { if (produced == 0) return; @@ -1059,6 +1282,14 @@ static void input_ring_commit(struct input_ring_state *r, size_t produced) } } +/** + * Write up to count samples into the input ring buffer, dropping oldest half + * when necessary to avoid blocking. + * + * @param r Input ring buffer state. + * @param data Source samples to write. + * @param count Number of samples to write. + */ static void input_ring_write(struct input_ring_state *r, const int16_t *data, size_t count) { int need_signal = input_ring_is_empty(r); @@ -1095,6 +1326,15 @@ static void input_ring_write(struct input_ring_state *r, const int16_t *data, si } } +/** + * Read up to max_count samples from input ring buffer, blocking with timeout + * until data is available or exit is requested. + * + * @param r Input ring buffer state. + * @param out Destination buffer for samples. + * @param max_count Maximum number of samples to read. + * @return Number of samples read (>=1), 0 if max_count is 0, or -1 on exit. + */ static int input_ring_read_block(struct input_ring_state *r, int16_t *out, size_t max_count) { if (max_count == 0) return 0; @@ -1148,10 +1388,18 @@ static struct input_ring_state input_ring; #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) -/* ===================== - Thread Scheduling Helpers (optional realtime/affinity) - ===================== */ - +/** + * Optionally enable realtime scheduling and set CPU affinity for the current + * thread based on environment variables. + * + * When `DSD_FME_RT_SCHED=1`, attempts to switch the calling thread to + * SCHED_FIFO with a priority derived from `DSD_FME_RT_PRIO_` if present. + * If `DSD_FME_CPU_` is set to a valid CPU index, pins the thread to that + * CPU. + * + * @param role Optional role label (e.g. "DEMOD", "DONGLE") used to look up + * per-role environment variables. + */ static void maybe_set_thread_realtime_and_affinity(const char *role) { const char *enable = getenv("DSD_FME_RT_SCHED"); @@ -1206,10 +1454,12 @@ static void maybe_set_thread_realtime_and_affinity(const char *role) } } -/* ===================== - SPSC Ring Buffer (output path) - ===================== */ - +/** + * Number of samples currently in the output ring. + * + * @param o Output ring state. + * @return Number of queued samples. + */ static inline size_t ring_used(const struct output_state *o) { /* Atomics policy: head/tail are atomics. We use default sequential @@ -1221,17 +1471,34 @@ static inline size_t ring_used(const struct output_state *o) return o->capacity - (t - h); } +/** + * Number of free slots available in the output ring. + * + * @param o Output ring state. + * @return Number of writable samples before the ring becomes full. + */ static inline size_t ring_free(const struct output_state *o) { return (o->capacity - 1) - ring_used(o); } +/** + * Check if the output ring is empty. + * + * @param o Output ring state. + * @return Non-zero if empty, zero otherwise. + */ static inline int ring_is_empty(const struct output_state *o) { /* See atomics note in ring_used() for ordering considerations. */ return o->head.load() == o->tail.load(); } +/** + * Clear the output ring head/tail indices. + * + * @param o Output ring state to clear. + */ static inline void ring_clear(struct output_state *o) { /* Clearing indices; with relaxed ordering this would be a release store. */ @@ -1298,8 +1565,14 @@ static void ring_write_no_signal(struct output_state *o, const int16_t *data, si } } -/* Write and signal consumer only if the ring transitions from empty to - non-empty. This reduces unnecessary wakeups while ensuring timely reads. */ +/** + * Write and signal consumer only if the ring transitions from empty to + * non-empty. Reduces unnecessary wakeups while ensuring timely reads. + * + * @param o Output ring buffer state. + * @param data Source samples to write. + * @param count Number of samples to write. + */ static void ring_write_signal_on_empty_transition(struct output_state *o, const int16_t *data, size_t count) { int need_signal = ring_is_empty(o); @@ -1309,7 +1582,14 @@ static void ring_write_signal_on_empty_transition(struct output_state *o, const } } -/* Read one sample, returns 0 on success, -1 on exit */ +/** + * Read one sample from the output ring, blocking with timeout until available + * or exit requested. + * + * @param o Output ring buffer state. + * @param out Destination for one sample. + * @return 0 on success, -1 on exit. + */ static int ring_read_one(struct output_state *o, int16_t *out) { while (ring_is_empty(o)) { @@ -1335,8 +1615,15 @@ static int ring_read_one(struct output_state *o, int16_t *out) return 0; } -/* Read up to max_count samples into out. Blocks until at least one sample is available or exit. Returns - number of samples read (>=1) or -1 on exit. Signals producer space once after the batch. */ +/** + * Read up to max_count samples into out. Blocks until at least one sample is available or exit. + * Signals producer space once after the batch. + * + * @param o Output ring buffer state. + * @param out Destination buffer for samples. + * @param max_count Maximum number of samples to read. + * @return Number of samples read (>=1) or -1 on exit. + */ static int ring_read_batch(struct output_state *o, int16_t *out, size_t max_count) { if (max_count == 0) return 0; @@ -1389,9 +1676,18 @@ int cic_9_tables[][10] = { {9, -199, -362, 5303, -25505, 77489, -25505, 5303, -362, -199}, }; +/** + * Rotate interleaved IQ bytes by 90 degrees in-place. + * + * 90° rotation sequence: 1+0j, 0+1j, -1+0j, 0-1j. Operates on u8 + * interleaved I/Q in-place. Negation is performed as (255 - x) to + * approximate centered negation for subsequent widening by subtracting 128. + * Assumes `len` is a multiple of 8 (four I/Q pairs per loop). + * + * @param buf Interleaved IQ byte buffer. + * @param len Buffer length in bytes (processed in blocks of 8). + */ 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; @@ -1410,8 +1706,15 @@ void rotate_90(unsigned char *buf, uint32_t len) } } +/** + * Simple boxcar low-pass accumulator with decimation on interleaved I/Q. + * Accumulates I and Q independently over `downsample` input samples and + * writes a single output (I,Q) pair per window. Scaling/normalization is + * deferred; this function sums and decimates with saturation on writeback. + * + * @param d Demodulator state (uses lowpassed buffer and decimation state). + */ void low_pass(struct demod_state *d) -/* simple square window FIR */ { int i=0, i2=0; while (i < d->lp_len) { @@ -1433,8 +1736,16 @@ void low_pass(struct demod_state *d) d->lp_len = i2; } +/** + * Boxcar low-pass and decimate by step (no wraparound). + * Length must be a multiple of step. + * + * @param signal2 In/out buffer of samples. + * @param len Length of input buffer. + * @param step Decimation factor. + * @return New length after decimation. + */ 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) { @@ -1442,17 +1753,22 @@ int low_pass_simple(int16_t *signal2, int len, int step) for(i2=0; i2= 0) ? (sum + step/2) / step : -(((-sum) + step/2) / step); signal2[i/step] = (int16_t)val; } + /* Duplicate the final sample to provide one-sample lookahead for callers + that expect at least one extra element. */ signal2[i/step + 1] = signal2[i/step]; return len / step; } +/** + * Simple square window FIR on real samples with decimation to rate_out2. + * + * @param s Demodulator state (uses result buffer and decimation state). + */ void low_pass_real(struct demod_state *s) -/* simple square window FIR */ -// add support for upsampling? { int i=0, i2=0; int16_t *r = assume_aligned_ptr(s->result, DSD_FME_ALIGN); @@ -1481,8 +1797,16 @@ void low_pass_real(struct demod_state *s) s->result_len = i2; } +/** + * Fifth-order half-band-like decimator operating on a single real sequence. + * Caller applies this separately to I and Q streams. Uses 6-tap state in + * `hist` and writes decimated output in-place. + * + * @param data In/out real data buffer (single channel). + * @param length Input length (elements), processed in-place. + * @param hist Persistent history buffer of length >= 6. + */ 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; @@ -1512,8 +1836,15 @@ void fifth_order(int16_t *data, int length, int16_t *hist) hist[5] = f; } +/** + * FIR filter with symmetric 9-tap coefficients (phase-saving implementation). + * + * @param data In/out data buffer (interleaved step of 2 assumed). + * @param length Number of input samples. + * @param fir Coefficient array (expects layout for length 9). + * @param hist History buffer used across calls. + */ 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; ddiscriminator. + * + * @param fm Demodulator state (uses lowpassed as input, writes to result). + */ void fm_demod(struct demod_state *fm) { int i, pcm; @@ -1717,6 +2106,11 @@ void fm_demod(struct demod_state *fm) fm->result_len = fm->lp_len/2; } +/** + * Pass-through demodulator: copies low-passed samples to output unchanged. + * + * @param fm Demodulator state (copies lowpassed to result). + */ void raw_demod(struct demod_state *fm) { int i; @@ -1726,6 +2120,11 @@ void raw_demod(struct demod_state *fm) fm->result_len = fm->lp_len; } +/** + * Apply post-demod deemphasis IIR filter with Q15 coefficient. + * + * @param fm Demodulator state (reads/writes result, updates deemph_avg). + */ void deemph_filter(struct demod_state *fm) { int avg = fm->deemph_avg; /* per-instance state */ @@ -1753,6 +2152,11 @@ void deemph_filter(struct demod_state *fm) fm->deemph_avg = avg; /* write back state */ } +/** + * Apply a simple DC blocking (leaky integrator high-pass) filter to audio. + * + * @param fm Demodulator state (reads/writes result, updates dc_avg). + */ void dc_block_filter(struct demod_state *fm) { int i; @@ -1770,9 +2174,13 @@ void dc_block_filter(struct demod_state *fm) fm->dc_avg = dc; } -/* Optional light post-demod audio low-pass filter (one-pole IIR) - y[n] = y[n-1] + alpha * (x[n] - y[n-1]) - alpha is Q15 in fm->audio_lpf_alpha. */ +/** + * Optional light post-demod audio low-pass filter (one-pole IIR). + * Implements: y[n] = y[n-1] + alpha * (x[n] - y[n-1]), where alpha is Q15 in + * `fm->audio_lpf_alpha`. + * + * @param fm Demodulator state (reads/writes `result`, updates `audio_lpf_state`). + */ static inline void audio_lpf_filter(struct demod_state *fm) { if (!fm->audio_lpf_enable) return; @@ -1795,11 +2203,12 @@ static inline void audio_lpf_filter(struct demod_state *fm) fm->audio_lpf_state = y; } -/* ===================== - Residual CFO loop (FLL) and Gardner timing correction - ===================== */ - -/* Mix lowpassed I/Q by NCO e^{j*phi}, update phi by fll_freq_q15 (Q15 0..2pi maps to 0..1<<15). */ +/** + * Mix lowpassed I/Q by NCO e^{j*phi}, update phase by `fll_freq_q15` per sample. + * Phase and frequency are Q15 where a full turn (2*pi) maps to 1<<15. + * + * @param d Demodulator state (reads/writes `lowpassed`, updates `fll_phase_q15`). + */ static inline void fll_mix_and_update(struct demod_state *d) { if (!d->fll_enabled) return; @@ -1847,7 +2256,13 @@ static inline void fll_mix_and_update(struct demod_state *d) d->fll_phase_q15 = phase & 0x7FFF; } -/* Estimate frequency error using a simple phase-difference discriminator and update FLL integrator. */ +/** + * Estimate frequency error using a simple phase-difference discriminator and + * update the FLL control in Q15. The proportional term is applied directly + * and the integral action is realized by accumulating into `fll_freq_q15`. + * + * @param d Demodulator state (updates `fll_freq_q15` and `fll_phase_q15`). + */ static inline void fll_update_error(struct demod_state *d) { if (!d->fll_enabled) return; @@ -1879,7 +2294,14 @@ static inline void fll_update_error(struct demod_state *d) d->fll_freq_q15 += (int)df; /* Q15 */ } -/* Lightweight Gardner timing correction: nearest-neighbor fractional selection around nominal sps. */ +/** + * Lightweight Gardner timing correction. + * Uses linear interpolation between adjacent complex samples around the + * nominal samples-per-symbol to reduce timing error; intended for digital + * modes when enabled. + * + * @param d Demodulator state (may adjust `result` in-place). + */ static inline void gardner_timing_adjust(struct demod_state *d) { if (!d->ted_enabled || d->ted_sps <= 1) return; @@ -1934,10 +2356,9 @@ static inline void gardner_timing_adjust(struct demod_state *d) d->ted_mu_q20 = mu; } -/* ===================== - Polyphase Rational Resampler (L/M) - ===================== */ - +/** + * Greatest common divisor via Euclidean algorithm. + */ static inline int gcd_int(int a, int b) { if (a < 0) a = -a; @@ -1950,14 +2371,26 @@ static inline int gcd_int(int a, int b) return (a == 0) ? 1 : a; } +/** + * Normalized sinc function: sin(pi*x)/(pi*x), with sinc(0)=1. + */ static inline double dsd_fme_sinc(double x) { if (x == 0.0) return 1.0; return sin(kPi * x) / (kPi * x); } -/* Design windowed-sinc low-pass prototype for upfirdn (runs at L*Fs_in). - - cutoff normalized to 0..0.5 (relative to L*Fs_in), use conservative margin. */ +/** + * Design windowed-sinc low-pass prototype for polyphase upfirdn (runs at L*Fs_in). + * Uses a Hamming window and conservative cutoff to balance CPU vs. stopband. + * Taps are stored phase-major with stride L (k*L + phase) so each phase sees + * a contiguous sub-filter. Taps are normalized to give ~unity DC per phase, + * then scaled by L to compensate polyphase upsampling (maintains amplitude). + * + * @param s Demodulator state to receive resampler taps/history. + * @param L Upsampling factor. + * @param M Downsampling factor. + */ static void resamp_design(struct demod_state *s, int L, int M) { /* Per-phase taps K; total taps = K*L. Keep small for CPU, but adequate stopband. */ @@ -2018,8 +2451,15 @@ static void resamp_design(struct demod_state *s, int L, int M) s->resamp_taps_per_phase = taps_per_phase; } -/* Process one block using polyphase upfirdn with history. - Returns number of output samples written. */ +/** + * Process one block using polyphase upfirdn with history. + * + * @param s Demodulator state containing resampler state. + * @param in Pointer to input samples. + * @param in_len Number of input samples. + * @param out Pointer to output buffer (sized to hold produced samples). + * @return Number of output samples written. + */ static int resamp_process_block(struct demod_state *s, const int16_t *in, int in_len, int16_t *out) { if (!s->resamp_enabled || !s->resamp_taps || !s->resamp_hist) { @@ -2068,8 +2508,15 @@ static int resamp_process_block(struct demod_state *s, const int16_t *in, int in return out_len; } +/** + * DC-corrected mean power (sqrt-free). Integer-only implementation. + * + * @param samples Input sample buffer. + * @param len Number of samples to process. + * @param step Step between processed samples (subsampling). + * @return Mean power (squared RMS) with DC bias removed. + */ long int mean_power(int16_t *samples, int len, int step) -/* DC-corrected mean power (sqrt-free). Integer-only implementation. */ { int64_t p = 0; int64_t t = 0; @@ -2089,6 +2536,13 @@ long int mean_power(int16_t *samples, int len, int step) return (long int)(energy / (len > 0 ? len : 1)); } +/** + * Full demodulation pipeline for one block. + * Applies decimation (HB cascade or legacy), optional FLL and timing + * correction, followed by the configured discriminator and post-processing. + * + * @param d Demodulator state (consumes lowpassed, produces result). + */ void full_demod(struct demod_state *d) { int i, ds_p; @@ -2135,7 +2589,9 @@ void full_demod(struct demod_state *d) if (d->ted_enabled && (d->mode_demod != &fm_demod || d->ted_force)) { gardner_timing_adjust(d); } - /* power squelch (sqrt-free): compare mean power to squared threshold */ + /* Power squelch (sqrt-free): compare mean power estimate against squared threshold. + Samples are decimated by `squelch_decim_stride`; an EMA smooths block power. + The sampling phase advances by lp_len % stride per block to cover all offsets. */ if (d->squelch_level) { /* Decimated block power estimate (no DC correction; EMA smooths) */ int stride = (d->squelch_decim_stride > 0) ? d->squelch_decim_stride : 16; @@ -2158,7 +2614,7 @@ void full_demod(struct demod_state *d) /* Initialize on first measurement to avoid long ramp */ d->squelch_running_power = block_mean; } else { - /* EMA: running += (block_mean - running) / window */ + /* EMA: running += (block_mean - running) / window, window ~ 2^shift */ int w = (d->squelch_window > 0) ? d->squelch_window : 2048; int shift = 0; /* approximate log2(window), prefer power-of-two windows */ @@ -2197,6 +2653,18 @@ void full_demod(struct demod_state *d) } } +/** + * RTL-SDR asynchronous USB callback. + * Converts incoming u8 I/Q to s16 and enqueues into the input ring. If + * `offset_tuning` is off and `DSD_FME_COMBINE_ROT` is enabled (default), a + * combined rotate+widen implementation is used. Otherwise it falls back to + * legacy two-pass (rotate_90 u8, then widen subtracting 128) or a simple + * widen subtracting 127. On overflow, drops oldest ring data to avoid stalls. + * + * @param buf USB I/Q byte buffer. + * @param len Buffer length in bytes (I/Q interleaved). + * @param ctx Opaque pointer to `dongle_state`. + */ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) { int i; @@ -2258,6 +2726,13 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) } } +/** + * RTL-SDR USB thread entry: reads samples asynchronously into the input ring. + * Applies optional realtime scheduling/affinity if configured. + * + * @param arg Pointer to `dongle_state`. + * @return NULL on exit. + */ static void *dongle_thread_fn(void *arg) { struct dongle_state *s = static_cast(arg); @@ -2266,6 +2741,13 @@ static void *dongle_thread_fn(void *arg) return 0; } +/** + * Demodulation thread entry: reads from input ring, runs the demod pipeline, + * and writes audio samples to the output ring. + * + * @param arg Pointer to `demod_state`. + * @return NULL on exit. + */ static void *demod_thread_fn(void *arg) { struct demod_state *d = static_cast(arg); @@ -2356,6 +2838,13 @@ static void *demod_thread_fn(void *arg) return 0; } +/** + * Find the nearest supported tuner gain to the requested value. + * + * @param dev RTL-SDR device handle. + * @param target_gain Desired gain in tenths of dB. + * @return Nearest supported gain in tenths of dB, or a negative error code. + */ int nearest_gain(rtlsdr_dev_t *dev, int target_gain) { int i, r, err1, err2, count, nearest; @@ -2383,6 +2872,13 @@ int nearest_gain(rtlsdr_dev_t *dev, int target_gain) return nearest; } +/** + * Set RTL-SDR center frequency with a brief status message. + * + * @param dev RTL-SDR device handle. + * @param frequency Center frequency in Hz. + * @return 0 on success or a negative error code. + */ int verbose_set_frequency(rtlsdr_dev_t *dev, uint32_t frequency) { int r; @@ -2395,6 +2891,13 @@ int verbose_set_frequency(rtlsdr_dev_t *dev, uint32_t frequency) return r; } +/** + * Set RTL-SDR sampling rate with a brief status message. + * + * @param dev RTL-SDR device handle. + * @param samp_rate Sampling rate in Hz. + * @return 0 on success or a negative error code. + */ int verbose_set_sample_rate(rtlsdr_dev_t *dev, uint32_t samp_rate) { int r; @@ -2407,6 +2910,13 @@ int verbose_set_sample_rate(rtlsdr_dev_t *dev, uint32_t samp_rate) return r; } +/** + * Enable or disable direct sampling mode. + * + * @param dev RTL-SDR device handle. + * @param on Non-zero to enable, zero to disable. + * @return 0 on success or a negative error code. + */ int verbose_direct_sampling(rtlsdr_dev_t *dev, int on) { int r; @@ -2424,6 +2934,12 @@ int verbose_direct_sampling(rtlsdr_dev_t *dev, int on) return r; } +/** + * Enable offset tuning on the tuner if supported. + * + * @param dev RTL-SDR device handle. + * @return 0 on success or a negative error code. + */ int verbose_offset_tuning(rtlsdr_dev_t *dev) { int r; @@ -2436,6 +2952,12 @@ int verbose_offset_tuning(rtlsdr_dev_t *dev) return r; } +/** + * Enable tuner automatic gain control. + * + * @param dev RTL-SDR device handle. + * @return 0 on success or a negative error code. + */ int verbose_auto_gain(rtlsdr_dev_t *dev) { int r; @@ -2448,6 +2970,13 @@ int verbose_auto_gain(rtlsdr_dev_t *dev) return r; } +/** + * Set a fixed tuner gain with a message indicating the result. + * + * @param dev RTL-SDR device handle. + * @param gain Desired gain in tenths of dB. + * @return 0 on success or a negative error code. + */ int verbose_gain_set(rtlsdr_dev_t *dev, int gain) { int r; @@ -2465,6 +2994,13 @@ int verbose_gain_set(rtlsdr_dev_t *dev, int gain) return r; } +/** + * Set tuner PPM frequency error correction. + * + * @param dev RTL-SDR device handle. + * @param ppm_error Error in parts-per-million. + * @return 0 on success or a negative error code. + */ int verbose_ppm_set(rtlsdr_dev_t *dev, int ppm_error) { int r; @@ -2479,6 +3015,12 @@ int verbose_ppm_set(rtlsdr_dev_t *dev, int ppm_error) return r; } +/** + * Reset RTL-SDR USB buffers. + * + * @param dev RTL-SDR device handle. + * @return 0 on success or a negative error code. + */ int verbose_reset_buffer(rtlsdr_dev_t *dev) { int r; @@ -2488,6 +3030,14 @@ int verbose_reset_buffer(rtlsdr_dev_t *dev) return r; } +/** + * Compute and stage tuner/demodulator capture settings based on the + * requested center frequency and current demod configuration. The actual + * device programming occurs elsewhere after these fields are updated. + * + * @param freq Desired RF center frequency in Hz. + * @param rate Current input sample rate (unused). + */ static void optimal_settings(int freq, int rate) { UNUSED(rate); @@ -2518,6 +3068,12 @@ static void optimal_settings(int freq, int rate) // fprintf (stderr, "Capture Frequency: %i Rate: %i \n", capture_freq, capture_rate); } +/** + * Controller thread: handles basic scanning/hopping between channels. + * + * @param arg Pointer to `controller_state`. + * @return NULL on exit. + */ static void *controller_thread_fn(void *arg) { // thoughts for multiple dongles @@ -2561,6 +3117,11 @@ static void *controller_thread_fn(void *arg) return 0; } +/** + * Initialize dongle (RTL-SDR source) state with default parameters. + * + * @param s Dongle state to initialize. + */ void dongle_init(struct dongle_state *s) { s->rate = rtl_bandwidth; @@ -2571,6 +3132,11 @@ void dongle_init(struct dongle_state *s) s->demod_target = &demod; } +/** + * Initialize demodulator state for analog FM path. + * + * @param s Demodulator state to initialize. + */ void demod_init_analog(struct demod_state *s) { s->rate_in = rtl_bandwidth; @@ -2638,12 +3204,22 @@ void demod_init_analog(struct demod_state *s) s->output_target = &output; if (s->custom_atan == 2 && atan_lut == NULL) { atan_lut_init(); } /* set discriminator function pointer */ + /* custom_atan mapping: + 0 -> polar_discriminant (double atan2; slow, highest accuracy) + 1 -> polar_disc_fast (int64 fast_atan2 approximation) + 2 -> polar_disc_lut (LUT-based atan2 approximation) + */ s->discriminator = (s->custom_atan == 0) ? &polar_discriminant : (s->custom_atan == 1) ? &polar_disc_fast : &polar_disc_lut; /* Init minimal worker pool (env-gated) */ demod_mt_init(s); } +/** + * Initialize demodulator state for RO2 path (no CIC, LUT atan by default). + * + * @param s Demodulator state to initialize. + */ void demod_init_ro2(struct demod_state *s) { s->rate_in = rtl_bandwidth; @@ -2717,6 +3293,11 @@ void demod_init_ro2(struct demod_state *s) demod_mt_init(s); } +/** + * Initialize demodulator state for default digital path. + * + * @param s Demodulator state to initialize. + */ void demod_init(struct demod_state *s) { s->rate_in = rtl_bandwidth; @@ -2790,6 +3371,11 @@ void demod_init(struct demod_state *s) demod_mt_init(s); } +/** + * Release resources owned by the demodulator state. + * + * @param s Demodulator state to clean up. + */ void demod_cleanup(struct demod_state *s) { pthread_cond_destroy(&s->ready); @@ -2801,6 +3387,11 @@ void demod_cleanup(struct demod_state *s) if (s->resamp_hist) { free(s->resamp_hist); s->resamp_hist = NULL; } } +/** + * Initialize output ring buffer and synchronization primitives. + * + * @param s Output state to initialize. + */ void output_init(struct output_state *s) { s->rate = rtl_bandwidth; @@ -2825,6 +3416,11 @@ void output_init(struct output_state *s) s->tail.store(0); } +/** + * Destroy output ring buffer and synchronization primitives. + * + * @param s Output state to clean up. + */ void output_cleanup(struct output_state *s) { pthread_cond_destroy(&s->ready); @@ -2833,6 +3429,11 @@ void output_cleanup(struct output_state *s) if (s->buffer) { free(s->buffer); s->buffer = NULL; } } +/** + * Initialize controller state (frequency list and hop control). + * + * @param s Controller state to initialize. + */ void controller_init(struct controller_state *s) { s->freqs[0] = 446000000; @@ -2843,12 +3444,21 @@ void controller_init(struct controller_state *s) pthread_mutex_init(&s->hop_m, NULL); } +/** + * Destroy controller synchronization primitives. + * + * @param s Controller state to clean up. + */ void controller_cleanup(struct controller_state *s) { pthread_cond_destroy(&s->hop); pthread_mutex_destroy(&s->hop_m); } +/** + * Validate runtime options and controller state prior to starting streams. + * Exits the process with an error message if constraints are violated. + */ void sanity_checks(void) { if (controller.freq_len == 0) { @@ -2868,7 +3478,14 @@ void sanity_checks(void) } -//UDP remote stuff +/** + * Convert a 5-byte UDP control message into an integer. + * Expects the first byte to be a command and the next four bytes to represent + * a little-endian 32-bit value. + * + * @param buf Pointer to 5-byte buffer. + * @return Decoded 32-bit little-endian integer from bytes 1..4. + */ static unsigned int chars_to_int(unsigned char* buf) { int i; @@ -2881,6 +3498,15 @@ static unsigned int chars_to_int(unsigned char* buf) { return val; } +/** + * UDP control thread: listens for frequency tuning commands and applies them. + * + * Message format: 5 bytes, where buf[0]==0 indicates a set-frequency command + * and buf[1..4] is a little-endian 32-bit frequency in Hz. + * + * @param arg Unused. + * @return NULL on exit. + */ static void *socket_thread_fn(void *arg) { UNUSED(arg); @@ -2926,14 +3552,21 @@ static void *socket_thread_fn(void *arg) { close(sockfd); return 0; } -//UDP stuff end -void rtlsdr_sighandler() +/** + * Signal handler to request RTL-SDR async cancel and exit. + */ +void rtlsdr_sighandler(void) { fprintf (stderr, "Signal caught, exiting!\n"); rtlsdr_cancel_async(dongle.dev); } +/** + * Initialize and open the RTL-SDR streaming pipeline, threads, and buffers. + * + * @param opts Decoder options used to configure the pipeline. + */ void open_rtlsdr_stream(dsd_opts *opts) { int r; @@ -3136,7 +3769,8 @@ void open_rtlsdr_stream(dsd_opts *opts) } if (demod.deemph) { - /* Configure deemphasis time constant via env DSD_FME_DEEMPH: 75 (default), 50, nfm, off */ + /* Configure deemphasis via env DSD_FME_DEEMPH: 75 (default), 50, nfm, off. + Computes a one-pole IIR with alpha = 1 - exp(-1/(Fs*tau)) stored in Q15. */ double tau_s = 75e-6; /* default 75 microseconds */ const char *deemph_env = getenv("DSD_FME_DEEMPH"); if (deemph_env && deemph_env[0] != '\0') { @@ -3167,8 +3801,9 @@ void open_rtlsdr_stream(dsd_opts *opts) /* Configure optional post-demod audio LPF via env DSD_FME_AUDIO_LPF. Values: - off or 0: disabled (default) - - NNNN: cutoff in Hz (approximate), e.g., 3000 or 5000. Uses one-pole IIR. - */ + - NNNN: cutoff in Hz (approximate), e.g., 3000 or 5000. + One-pole: y[n] = y[n-1] + alpha * (x[n] - y[n-1]), + alpha ≈ 1 - exp(-2*pi*fc/Fs) in Q15. */ { const char *alpf = getenv("DSD_FME_AUDIO_LPF"); demod.audio_lpf_enable = 0; @@ -3227,7 +3862,10 @@ void open_rtlsdr_stream(dsd_opts *opts) } } -void cleanup_rtlsdr_stream() +/** + * Stop threads, cleanup buffers/objects, and close the RTL-SDR stream. + */ +void cleanup_rtlsdr_stream(void) { fprintf (stderr, "cleaning up...\n"); rtlsdr_cancel_async(dongle.dev); @@ -3252,8 +3890,16 @@ void cleanup_rtlsdr_stream() rtlsdr_close(dongle.dev); } -/* Batched consumer API: read up to count samples with fewer wakeups/locks. - Returns number of samples read (>=1) or -1 on exit. Applies volume scaling. */ +/** + * Batched consumer API: read up to count samples with fewer wakeups/locks. + * Applies volume scaling. + * + * @param out Destination buffer for audio samples. + * @param count Maximum number of samples to read. + * @param opts Decoder options (used for runtime PPM changes). + * @param state Decoder state (unused). + * @return Number of samples read (>=1) or -1 on exit. + */ int get_rtlsdr_samples(int16_t *out, size_t count, dsd_opts * opts, dsd_state * state) { UNUSED(state); @@ -3278,20 +3924,14 @@ int get_rtlsdr_samples(int16_t *out, size_t count, dsd_opts * opts, dsd_state * return got; } -//original for safe keeping -// void get_rtlsdr_sample(int16_t *sample, dsd_opts * opts, dsd_state * state) -// { -// if (output.queue.empty()) -// { -// safe_cond_wait(&output.ready, &output.ready_m); -// } -// pthread_rwlock_wrlock(&output.rw); -// *sample = output.queue.front() * volume_multiplier; -// output.queue.pop(); -// pthread_rwlock_unlock(&output.rw); -// } - -//find way to modify this function to allow hopping (tuning) while squelched and send 0 sample? +/** + * Convenience wrapper to read a single sample via the batched API. + * + * @param sample Destination for one sample. + * @param opts Decoder options. + * @param state Decoder state (unused). + * @return 0 on success, -1 on exit. + */ int get_rtlsdr_sample(int16_t *sample, dsd_opts * opts, dsd_state * state) { /* Delegate to batched API for a single sample */ @@ -3300,7 +3940,12 @@ int get_rtlsdr_sample(int16_t *sample, dsd_opts * opts, dsd_state * state) return 0; } -//function may lag since it isn't running as its own thread +/** + * Tune RTL-SDR to a new center frequency, updating optimal settings. + * + * @param opts Decoder options. + * @param frequency Target center frequency in Hz. + */ void rtl_dev_tune(dsd_opts * opts, long int frequency) { int r; @@ -3318,8 +3963,13 @@ void rtl_dev_tune(dsd_opts * opts, long int frequency) } -//return RMS value (root means square) power level -- used as soft squelch inside of framesync -long int rtl_return_rms() +/** + * Return mean power approximation for soft squelch decisions. + * Uses a small fixed sample window for efficiency. + * + * @return Mean power value. + */ +long int rtl_return_rms(void) { long int sr = 0; // #ifdef __arm__ @@ -3335,8 +3985,10 @@ long int rtl_return_rms() return (sr); } -//simple function to clear the rtl sample queue when tuning and during other events (ncurses menu open/close) -void rtl_clean_queue() +/** + * Clear the output ring buffer and wake any waiting producer. + */ +void rtl_clean_queue(void) { /* Clear the entire ring to prevent sample 'lag' */ ring_clear(&output); From da8b9b61163584ba5e5fbda9bfe200ef30a784c2 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Thu, 4 Sep 2025 11:53:54 -0500 Subject: [PATCH 46/48] rtl_sdr_fm: add additional bounds checks and cleanup extra line --- src/rtl_sdr_fm.cpp | 32 +++++++++++++++++++++----------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 5aec1e7..f5116e6 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -406,7 +406,6 @@ static void DSD_FME_TARGET_ATTR("avx2") widen_rotate90_u8_to_s16_bias127_avx2(co } } - /** * SSE2: widen unsigned bytes to signed 16-bit centered at 127. */ @@ -1691,7 +1690,9 @@ void rotate_90(unsigned char *buf, uint32_t len) { uint32_t i; unsigned char tmp; - for (i=0; i 0) { + signal2[out_len] = signal2[out_len - 1]; + } + return out_len; } /** @@ -2667,7 +2672,7 @@ void full_demod(struct demod_state *d) */ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) { - int i; + struct dongle_state *s = static_cast(ctx); /* One-time: ensure the USB callback thread gets RT scheduling/affinity if enabled */ { @@ -2683,9 +2688,11 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) if (!ctx) { return;} if (s->mute) { - for (i=0; imute; i++) { - buf[i] = 127;} - s->mute = 0; + /* Clamp mute length to buffer size to avoid overwrite; carry remainder */ + uint32_t m = (uint32_t)s->mute; + if (m > len) m = len; + memset(buf, 127, m); + s->mute -= (int)m; } /* Convert incoming u8 I/Q and write directly into input ring without extra copy */ size_t need = len; @@ -3538,7 +3545,7 @@ static void *socket_thread_fn(void *arg) { int new_freq; while((n = read(sockfd,buffer,5)) != 0) { - if(buffer[0] == 0) { + if (n == 5 && buffer[0] == 0) { new_freq = chars_to_int(buffer); dongle.freq = new_freq; optimal_settings(new_freq, demod.rate_in); @@ -3980,7 +3987,10 @@ long int rtl_return_rms(void) //I've found that just using a sample size of 160 will give us a good approximation without killing the CPU // Return mean power (squared RMS) for soft squelch decisions (sqrt-free) // sr = mean_power(demod.lowpassed, demod.lp_len, 1); - sr = mean_power(demod.lowpassed, 160, 1); + int n = demod.lp_len; + if (n > 160) n = 160; + if (n < 0) n = 0; + sr = mean_power(demod.lowpassed, n, 1); // #endif return (sr); } From 8ec385fdf28b96b4731d2ba95dfe4967eed29df4 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Thu, 4 Sep 2025 13:08:22 -0500 Subject: [PATCH 47/48] ensure all old references of rms have been updated to pwr, we avoid sqrt calculations and squelch comparisons are using the same units with updated default level --- include/dsd.h | 7 ++++--- src/dsd_frame_sync.c | 4 ++-- src/dsd_main.c | 10 +++++----- src/dsd_misc.c | 24 ++++++++++++++++++++++++ src/dsd_ncurses_menu.c | 4 ++-- src/dsd_ncurses_printer.c | 12 ++++++------ src/dsd_symbol.c | 14 +++++++------- src/edacs-fme.c | 36 ++++++++++++++++++------------------ src/m17.c | 22 +++++++++++----------- src/rtl_sdr_fm.cpp | 25 ++++++++----------------- 10 files changed, 87 insertions(+), 71 deletions(-) diff --git a/include/dsd.h b/include/dsd.h index 4514c09..b22f35e 100644 --- a/include/dsd.h +++ b/include/dsd.h @@ -371,7 +371,7 @@ typedef struct int rtl_udp_port; int rtl_bandwidth; int rtl_started; - long int rtl_rms; + long int rtl_pwr; int monitor_input_audio; int analog_only; int pulse_raw_rate_in; @@ -960,7 +960,7 @@ typedef struct uint8_t m17_can; //can value that was decoded from signal int m17_can_en; //can value supplied to the encoding side int m17_rate; //sampling rate for audio input - int m17_vox; //vox enabled via RMS value + int m17_vox; //vox enabled via PWR value char m17_dst_csd[20]; char m17_src_csd[20]; @@ -1580,6 +1580,7 @@ void init_rrc_filter_memory(); //misc audio filtering for analog long int raw_rms(short *samples, int len, int step); +long int raw_pwr(short *samples, int len, int step); void init_audio_filters(dsd_state * state); void lpf(dsd_state * state, short * input, int len); void hpf(dsd_state * state, short * input, int len); @@ -1660,7 +1661,7 @@ void cleanup_rtlsdr_stream(); int get_rtlsdr_sample(int16_t *sample, dsd_opts * opts, dsd_state * state); void rtlsdr_sighandler(); void rtl_dev_tune(dsd_opts * opts, long int frequency); -long int rtl_return_rms(); +long int rtl_return_pwr(); void rtl_clean_queue(); #endif diff --git a/src/dsd_frame_sync.c b/src/dsd_frame_sync.c index c13e285..8ed3533 100644 --- a/src/dsd_frame_sync.c +++ b/src/dsd_frame_sync.c @@ -419,8 +419,8 @@ getFrameSync (dsd_opts * opts, dsd_state * state) } - //if using an rtl input method, do not look for sync patterns if the rms value is lower than our 'soft squelch' level - if (opts->audio_in_type == 3 && opts->rtl_rms < opts->rtl_squelch_level) + //if using an rtl input method, do not look for sync patterns if the pwr value is lower than our 'soft squelch' level + if (opts->audio_in_type == 3 && opts->rtl_pwr < opts->rtl_squelch_level) { if (opts->frame_nxdn48 == 1 || opts->frame_nxdn96 == 1 || opts->frame_dpmr == 1 || opts->frame_m17 == 1) { diff --git a/src/dsd_main.c b/src/dsd_main.c index 860cf93..94fd5d4 100644 --- a/src/dsd_main.c +++ b/src/dsd_main.c @@ -689,14 +689,14 @@ //all RTL user options -- enabled AGC by default due to weak signal related issues opts->rtl_dev_index = 0; //choose which device we want by index number opts->rtl_gain_value = 0; //mid value, 0 - AGC - 0 to 49 acceptable values - opts->rtl_squelch_level = 100; //100 by default, but only affects NXDN and dPMR during framesync test, compared to RMS value + opts->rtl_squelch_level = 10000; //default scaled for rtl_pwr (RMS^2 proxy); ~100 RMS equivalent opts->rtl_volume_multiplier = 2; //sample multiplier; This multiplies the sample value to produce a higher 'inlvl' for the demodulator opts->rtl_udp_port = 0; //set UDP port for RTL remote -- 0 by default, will be making this optional for some external/legacy use cases (edacs-fm, etc) opts->rtl_bandwidth = 12; //default is 12, reverted back to normal on this (no inherent benefit) opts->rtlsdr_ppm_error = 0; //initialize ppm with 0 value; opts->rtlsdr_center_freq = 850000000; //set to an initial value (if user is using a channel map, then they won't need to specify anything other than -i rtl if desired) opts->rtl_started = 0; - opts->rtl_rms = 0; //root means square power level on rtl input signal + opts->rtl_pwr = 0; // mean power approximation level on rtl input signal //end RTL user options opts->pulse_raw_rate_in = 48000; opts->pulse_raw_rate_out = 48000;// @@ -1461,7 +1461,7 @@ printf (" gain RTL-SDR Device Gain (0-49)(default = 0; Hardware AGC recommended)\n"); printf (" ppm RTL-SDR PPM Error (default = 0)\n"); printf (" bw RTL-SDR Bandwidth kHz (default = 12)(4, 6, 8, 12, 16, 24) \n"); - printf (" sq RTL-SDR Squelch Level vs RMS Value (Optional)\n"); + printf (" sq RTL-SDR Squelch Level vs PWR Value (Optional)\n"); // printf (" udp RTL-SDR Legacy UDP Remote Port (Optional -- External Use Only)\n"); //NOTE: This is still available as an option in the ncurses menu printf (" vol RTL-SDR Sample 'Volume' Multiplier (default = 2)(1,2,3)\n"); printf (" Example: dsd-fme -fs -i rtl -C cap_plus_channel.csv -T\n"); @@ -1644,8 +1644,8 @@ open_rtlsdr_stream(opts); opts->rtl_started = 1; //set here so ncurses terminal doesn't attempt to open it again // #ifdef __arm__ - // fprintf (stderr, "WARNING: RMS Function is Disabled on ARM Devices (Raspberry Pi) due to High CPU use. \n"); - // fprintf (stderr, "RMS/Squelch Functionality for NXDN, dPMR, EDACS Analog, M17 and Raw Audio Monitor are unavailable and these modes will not function properly. \n"); + // fprintf (stderr, "WARNING: PWR Function is Disabled on ARM Devices (Raspberry Pi) due to High CPU use. \n"); + // fprintf (stderr, "PWR/Squelch Functionality for NXDN, dPMR, EDACS Analog, M17 and Raw Audio Monitor are unavailable and these modes will not function properly. \n"); // if (opts->monitor_input_audio == 1) opts->monitor_input_audio = 0; // opts->rtl_squelch_level = 0; // #endif diff --git a/src/dsd_misc.c b/src/dsd_misc.c index cafba49..9974fa9 100644 --- a/src/dsd_misc.c +++ b/src/dsd_misc.c @@ -591,4 +591,28 @@ long int raw_rms(int16_t *samples, int len, int step) //use samplespersymbol as rms = (long int)sqrt((p-err) / len); if (rms < 0) rms = 150; return rms; +} + +/* + * Mean power (RMS^2 proxy) without sqrt, modeled after mean_power() in rtl_sdr_fm.cpp. + * Computes a DC-corrected average of squares to avoid costly sqrt operations. + */ +long int raw_pwr(int16_t *samples, int len, int step) +{ + int64_t p = 0; + int64_t t = 0; + for (int i = 0; i < len; i += step) { + int64_t s = (int64_t)samples[i]; + t += s; + p += s * s; + } + /* DC-corrected energy ≈ p - (t^2)/len with rounded division */ + int64_t dc_corr = 0; + if (len > 0) { + int64_t tt = t * t; + dc_corr = (tt + (len / 2)) / len; + } + int64_t energy = p - dc_corr; + if (energy < 0) energy = 0; + return (long int)(energy / (len > 0 ? len : 1)); } \ No newline at end of file diff --git a/src/dsd_ncurses_menu.c b/src/dsd_ncurses_menu.c index 5feeb30..509224d 100644 --- a/src/dsd_ncurses_menu.c +++ b/src/dsd_ncurses_menu.c @@ -417,7 +417,7 @@ void ncursesMenu (dsd_opts * opts, dsd_state * state) entry_win = newwin(8, WIDTH+22, starty+10, startx+10); box (entry_win, 0, 0); - mvwprintw(entry_win, 2, 2, " RTL RMS Squelch Level (NXDN/dPMR/Analog/Raw only): "); + mvwprintw(entry_win, 2, 2, " RTL Power Squelch Level (NXDN/dPMR/Analog/Raw only): "); mvwprintw(entry_win, 3, 3, " "); echo(); refresh(); @@ -463,7 +463,7 @@ void ncursesMenu (dsd_opts * opts, dsd_state * state) mvwprintw(entry_win, 8, 2, " RTL Volume Multiplier: %d", opts->rtl_volume_multiplier); mvwprintw(entry_win, 9, 2, " RTL Device UDP Port: %d", opts->rtl_udp_port); mvwprintw(entry_win, 10, 2, " RTL Device PPM: %d", opts->rtlsdr_ppm_error); - mvwprintw(entry_win, 11, 2, " RTL RMS Squelch: %d", opts->rtl_squelch_level); + mvwprintw(entry_win, 11, 2, " RTL PWR Squelch: %d", opts->rtl_squelch_level); mvwprintw(entry_win, 13, 2, " Are You Sure?"); mvwprintw(entry_win, 14, 2, " 1 = Yes, 2 = No "); mvwprintw(entry_win, 15, 3, " "); diff --git a/src/dsd_ncurses_printer.c b/src/dsd_ncurses_printer.c index 93d06be..ff649ef 100644 --- a/src/dsd_ncurses_printer.c +++ b/src/dsd_ncurses_printer.c @@ -255,7 +255,7 @@ ncursesPrinter (dsd_opts * opts, dsd_state * state) printw (" V: %iX;", opts->rtl_volume_multiplier); printw (" PPM: %i;", opts->rtlsdr_ppm_error); //Adjust manually now with { and } printw (" SQ: %i;", opts->rtl_squelch_level); - printw (" RMS: %04li;", opts->rtl_rms); + printw (" PWR: %04li;", opts->rtl_pwr); printw (" BW: %i;", opts->rtl_bandwidth); printw (" FRQ: %i;", opts->rtlsdr_center_freq); if (opts->rtl_udp_port != 0) printw ("\n| External RTL Tuning on UDP Port: %i", opts->rtl_udp_port); @@ -282,7 +282,7 @@ ncursesPrinter (dsd_opts * opts, dsd_state * state) printw ("| Pulse Analog Output: %i kHz; %i Ch; G: %02.0f%% (/|*) ", opts->pulse_raw_rate_out/1000, opts->pulse_raw_out_channels, opts->audio_gainA); if (opts->audio_gainA == 0.0f) printw ("Auto "); else printw ("Manual "); - if (opts->audio_in_type != 3) printw ("RMS: %04ld; ", opts->rtl_rms); + if (opts->audio_in_type != 3) printw ("PWR: %04ld; ", opts->rtl_pwr); if (opts->use_lpf == 1) printw ("F: |LP|"); else printw ("F: | |"); if (opts->use_hpf == 1) printw ("HP|"); else printw (" |"); if (opts->use_pbf == 1) printw ("PB|"); else printw (" |"); @@ -302,7 +302,7 @@ ncursesPrinter (dsd_opts * opts, dsd_state * state) if ( (opts->audio_out_type == 5 && opts->pulse_digi_rate_out == 48000 && opts->pulse_digi_out_channels == 1) && (opts->frame_provoice == 1 || opts->monitor_input_audio == 1) ) { - printw ("\n| Analog Monitor RMS: %04ld; G: %02.0f%% (/|*) ", opts->rtl_rms, opts->audio_gainA); + printw ("\n| Analog Monitor PWR: %04ld; G: %02.0f%% (/|*) ", opts->rtl_pwr, opts->audio_gainA); if (opts->audio_gainA == 0.0f) printw ("Auto "); else printw ("Manual "); if (opts->use_lpf == 1) printw ("F: |LP|"); else printw ("F: | |"); @@ -321,14 +321,14 @@ ncursesPrinter (dsd_opts * opts, dsd_state * state) if (opts->use_hpf_d == 1) printw (" HPF"); if (opts->call_alert == 1) printw (" *CA!"); //Call Alert if ( (opts->audio_out_type == 5 && opts->pulse_digi_rate_out == 48000 && opts->pulse_digi_out_channels == 1) && (opts->frame_provoice == 1 || opts->monitor_input_audio == 1) ) - printw (" - Monitor RMS: %04ld ", opts->rtl_rms); + printw (" - Monitor PWR: %04ld ", opts->rtl_pwr); printw (" \n"); if (opts->udp_sockfdA != 0) //Analog Output on udp port +2 { printw ("| UDP Analog Output: %s:%d; 48 kHz 1 Ch; G: %02.0f%% (/|*) ", opts->udp_hostname, opts->udp_portno+2, opts->audio_gainA); if (opts->audio_gainA == 0.0f) printw ("A "); else printw ("M "); - if (opts->audio_in_type != 3) printw ("RMS: %04ld; ", opts->rtl_rms); + if (opts->audio_in_type != 3) printw ("PWR: %04ld; ", opts->rtl_pwr); if (opts->use_lpf == 1) printw ("F: |LP|"); else printw ("F: | |"); if (opts->use_hpf == 1) printw ("HP|"); else printw (" |"); if (opts->use_pbf == 1) printw ("PB|"); else printw (" |"); @@ -347,7 +347,7 @@ ncursesPrinter (dsd_opts * opts, dsd_state * state) if (opts->use_lpf == 1) printw ("F: |LP|"); else printw ("F: | |"); if (opts->use_hpf == 1) printw ("HP|"); else printw (" |"); if (opts->use_pbf == 1) printw ("PB|"); else printw (" |"); - if (opts->audio_in_type != 3 && state->m17_vox == 1) printw ( " SQL: %04ld : %04d;", opts->rtl_rms, opts->rtl_squelch_level); + if (opts->audio_in_type != 3 && state->m17_vox == 1) printw ( " SQL: %04ld : %04d;", opts->rtl_pwr, opts->rtl_squelch_level); printw ("\n"); } diff --git a/src/dsd_symbol.c b/src/dsd_symbol.c index 64744e4..f159482 100644 --- a/src/dsd_symbol.c +++ b/src/dsd_symbol.c @@ -132,7 +132,7 @@ getSymbol (dsd_opts * opts, dsd_state * state, int have_sync) if (get_rtlsdr_sample(&sample, opts, state) < 0) cleanupAndExit(opts, state); //update root means square power level - opts->rtl_rms = rtl_return_rms(); + opts->rtl_pwr = rtl_return_pwr(); sample *= opts->rtl_volume_multiplier; #endif @@ -268,9 +268,9 @@ getSymbol (dsd_opts * opts, dsd_state * state, int have_sync) if (state->analog_sample_counter == 960) { - //get an rms value if not using the rtl built in version + //get an pwr value if not using the rtl built in version if (opts->audio_in_type != 3 && opts->monitor_input_audio == 1) - opts->rtl_rms = raw_rms(state->analog_out, 960, 1); + opts->rtl_pwr = raw_pwr(state->analog_out, 960, 1); //raw wav file saving -- only write when not NXDN, dPMR, or M17 due to noise that can cause tons of false positives when no sync if (opts->wav_out_raw != NULL && opts->frame_nxdn48 == 0 && opts->frame_nxdn96 == 0 && opts->frame_dpmr == 0 && opts->frame_m17 == 0) @@ -299,13 +299,13 @@ getSymbol (dsd_opts * opts, dsd_state * state, int have_sync) else agsm(opts, state, state->analog_out, 960); - //Running RMS after filtering does remove the analog spike from the RMS value + //Running PWR after filtering does remove the analog spike from the PWR value //but noise floor noise will still produce higher values // if (opts->audio_in_type != 3 && opts->monitor_input_audio == 1) - // opts->rtl_rms = raw_rms(state->analog_out, 960, 1); + // opts->rtl_pwr = raw_pwr(state->analog_out, 960, 1); - //seems to be working now, but RMS values are lower on actual analog signal than on no signal but noise - if ( (opts->rtl_rms > opts->rtl_squelch_level) && opts->monitor_input_audio == 1 && state->carrier == 0 ) //added carrier check here in lieu of disabling it above + //seems to be working now, but PWR values are lower on actual analog signal than on no signal but noise + if ( (opts->rtl_pwr > opts->rtl_squelch_level) && opts->monitor_input_audio == 1 && state->carrier == 0 ) //added carrier check here in lieu of disabling it above { if (opts->audio_out_type == 0) pa_simple_write(opts->pulse_raw_dev_out, state->analog_out, 960*2, NULL); diff --git a/src/edacs-fme.c b/src/edacs-fme.c index db18c0f..4a55ee5 100644 --- a/src/edacs-fme.c +++ b/src/edacs-fme.c @@ -179,7 +179,7 @@ unsigned long long int edacsVoteFr(unsigned long long int fr_1_4, unsigned long void edacs_analog(dsd_opts * opts, dsd_state * state, int afs, unsigned char lcn) { int i, result; - int count = 5; //RMS has a 5 count (5 * 180ms) now before cutting off; + int count = 5; //PWR has a 5 count (5 * 180ms) now before cutting off; short analog1[960]; short analog2[960]; short analog3[960]; @@ -202,7 +202,7 @@ void edacs_analog(dsd_opts * opts, dsd_state * state, int afs, unsigned char lcn memset (d2, 0, sizeof(d2)); memset (d3, 0, sizeof(d3)); - long int rms = opts->rtl_squelch_level + 1; //one more for the initial loop phase + long int pwr = opts->rtl_squelch_level + 1; //one more for the initial loop phase long int sql = opts->rtl_squelch_level; fprintf (stderr, "\n"); @@ -229,8 +229,8 @@ void edacs_analog(dsd_opts * opts, dsd_state * state, int afs, unsigned char lcn pa_simple_read(opts->pulse_digi_dev_in, &sample, 2, NULL ); analog3[i] = sample; } - //this rms will only work properly (for now) with squelch enabled in SDR++ or other - rms = raw_rms(analog3, 960, 1); + //this pwr will only work properly (for now) with squelch enabled in SDR++ or other + pwr = raw_pwr(analog3, 960, 1); } //NOTE: The core dumps observed previously were due to SDR++ Remote Server connection dropping due to Internet/Other issues @@ -287,8 +287,8 @@ void edacs_analog(dsd_opts * opts, dsd_state * state, int afs, unsigned char lcn analog3[i] = sample; } - //this rms will only work properly (for now) with squelch enabled in SDR++ - rms = raw_rms(analog3, 960, 1); + //this pwr will only work properly (for now) with squelch enabled in SDR++ + pwr = raw_pwr(analog3, 960, 1); } //RTL Input @@ -315,8 +315,8 @@ void edacs_analog(dsd_opts * opts, dsd_state * state, int afs, unsigned char lcn sample *= opts->rtl_volume_multiplier; analog3[i] = sample; } - //the rtl rms value works properly without needing a 'hard' squelch value - rms = rtl_return_rms(); + //the rtl pwr value works properly without needing a 'hard' squelch value + pwr = rtl_return_pwr(); } #endif @@ -387,12 +387,12 @@ void edacs_analog(dsd_opts * opts, dsd_state * state, int afs, unsigned char lcn agsm (opts, state, analog3, 960); } - //NOTE: Ideally, we would run raw_rms for TCP/VS here, but the analog spike on EDACS (STM) + //NOTE: Ideally, we would run raw_pwr for TCP/VS here, but the analog spike on EDACS (STM) //system gets filtered out, and when they hold the radio open and don't talk, //it counts against the squelch hit as no audio, so we will just have to use //the squelch checkbox in SDR++ and similar when using those input methods // if (opts->audio_in_type != 3) - // rms = raw_rms(analog3, 960, 1); + // pwr = raw_pwr(analog3, 960, 1); //reconfigured to use seperate audio out stream that is always 48k short if (opts->audio_out_type == 0 && opts->slot1_on == 1) @@ -425,18 +425,18 @@ void edacs_analog(dsd_opts * opts, dsd_state * state, int afs, unsigned char lcn write (opts->audio_out_fd, analog3, 960*2); } - opts->rtl_rms = rms; + opts->rtl_pwr = pwr; printFrameSync (opts, state, " EDACS", 0, "A"); - if (rms < sql) count--; + if (pwr < sql) count--; else count = 5; - if (rms > sql) fprintf(stderr, "%s", KGRN); + if (pwr > sql) fprintf(stderr, "%s", KGRN); else fprintf(stderr, "%s", KRED); - fprintf (stderr, " Analog RMS: %04ld SQL: %ld", rms, sql); + fprintf (stderr, " Analog PWR: %04ld SQL: %ld", pwr, sql); if (state->ea_mode == 0) { int a = (afs >> state->edacs_a_shift) & state->edacs_a_mask; @@ -1488,7 +1488,7 @@ void edacs(dsd_opts * opts, dsd_state * state) //NOTE: Restructured below so that analog and digital are handled the same, just that when //its analog, it will now start edacs_analog which will while loop analog samples until - //signal level drops (RMS, or a dotting sequence is detected) + //signal level drops (PWR, or a dotting sequence is detected) //this is working now with the new import setup if (opts->trunk_tune_group_calls == 1 && opts->p25_trunk == 1 && (strcmp(mode, "DE") != 0) && (strcmp(mode, "B") != 0) ) //DE is digital encrypted, B is block @@ -1743,7 +1743,7 @@ void edacs(dsd_opts * opts, dsd_state * state) //NOTE: Restructured below so that analog and digital are handled the same, just that when //its analog, it will now start edacs_analog which will while loop analog samples until - //signal level drops (RMS, or a dotting sequence is detected) + //signal level drops (PWR, or a dotting sequence is detected) //this is working now with the new import setup if (((is_individual == 0 && opts->trunk_tune_group_calls == 1) || (is_individual == 1 && opts->trunk_tune_private_calls == 1)) && @@ -1857,7 +1857,7 @@ void edacs(dsd_opts * opts, dsd_state * state) //NOTE: Restructured below so that analog and digital are handled the same, just that when //its analog, it will now start edacs_analog which will while loop analog samples until - //signal level drops (RMS, or a dotting sequence is detected) + //signal level drops (PWR, or a dotting sequence is detected) //this is working now with the new import setup if ((opts->trunk_tune_private_calls == 1) && opts->p25_trunk == 1 && (strcmp(mode, "DE") != 0) && (strcmp(mode, "B") != 0) ) //DE is digital encrypted, B is block @@ -2153,7 +2153,7 @@ void edacs(dsd_opts * opts, dsd_state * state) //NOTE: Restructured below so that analog and digital are handled the same, just that when //its analog, it will now start edacs_analog which will while loop analog samples until - //signal level drops (RMS, or a dotting sequence is detected) + //signal level drops (PWR, or a dotting sequence is detected) //this is working now with the new import setup if ((opts->trunk_tune_group_calls == 1) && opts->p25_trunk == 1 && (strcmp(mode, "DE") != 0) && (strcmp(mode, "B") != 0) ) //DE is digital encrypted, B is block diff --git a/src/m17.c b/src/m17.c index 702c5ea..a540b9d 100644 --- a/src/m17.c +++ b/src/m17.c @@ -1962,13 +1962,13 @@ void encodeM17STR(dsd_opts * opts, dsd_state * state) voice2[i] = sample; } } - opts->rtl_rms = rtl_return_rms(); + opts->rtl_pwr = rtl_return_pwr(); #endif } - //read in RMS value for vox function; NOTE: will not work correctly SOCAT STDIO TCP due to blocking when no samples to read + //read in power value for vox function; use mean power (RMS^2 proxy) for consistency with rtl_pwr if (opts->audio_in_type != 3) - opts->rtl_rms = raw_rms(voice1, nsam, 1) / 2; //dividing by two so mic isn't so sensitive on vox + opts->rtl_pwr = raw_pwr(voice1, nsam, 1); //low pass filter if (opts->use_lpf == 1) @@ -2010,11 +2010,11 @@ void encodeM17STR(dsd_opts * opts, dsd_state * state) agsm (opts, state, voice2, 160); } - //NOTE: Similar to EDACS analog, if calculating raw rms here after filtering, + //NOTE: Similar to EDACS analog, if calculating raw pwr here after filtering, //anytime the walkie-talkie is held open but no voice, the center spike is removed, //and counts against the squelch hits making vox mode inconsistent // if (opts->audio_in_type != 3) - // opts->rtl_rms = raw_rms(voice1, 160, 1); + // opts->rtl_pwr = raw_pwr(voice1, 160, 1); //convert out audio input into CODEC2 (3200bps) 8 byte data stream uint8_t vc1_bytes[8]; memset (vc1_bytes, 0, sizeof(vc1_bytes)); @@ -2080,8 +2080,8 @@ void encodeM17STR(dsd_opts * opts, dsd_state * state) m17_v1[i+16+64] = v2_bits[i]; } - //tally consecutive squelch hits based on RMS value, or reset - if (opts->rtl_rms > opts->rtl_squelch_level) sql_hit = 0; + //tally consecutive squelch hits based on PWR value, or reset + if (opts->rtl_pwr > opts->rtl_squelch_level) sql_hit = 0; else sql_hit++; //may eventually roll over to 0 again //if vox enabled, toggle tx/eot with sql_hit comparison @@ -2215,10 +2215,10 @@ void encodeM17STR(dsd_opts * opts, dsd_state * state) if (use_ip == 1 && lich_cnt != 5) fprintf (stderr, " UDP: %s:%d", opts->m17_hostname, udpport); - //debug RMS Value + //debug power value if (state->m17_vox == 1) { - fprintf (stderr, " RMS: %04ld", opts->rtl_rms); + fprintf (stderr, " PWR: %04ld", opts->rtl_pwr); fprintf (stderr, " SQL HIT: %d;", sql_hit); } @@ -2463,10 +2463,10 @@ void encodeM17STR(dsd_opts * opts, dsd_state * state) if (use_ip == 1 && lich_cnt != 5) fprintf (stderr, " UDP: %s:%d", opts->m17_hostname, udpport); - //debug RMS Value + //debug power value if (state->m17_vox == 1) { - fprintf (stderr, " RMS: %04ld", opts->rtl_rms); + fprintf (stderr, " PWR: %04ld", opts->rtl_pwr); fprintf (stderr, " SQL HIT: %d;", sql_hit); } diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index f5116e6..40e802c 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -3744,10 +3744,10 @@ void open_rtlsdr_stream(dsd_opts *opts) } dongle.dev_index = opts->rtl_dev_index; - // demod.squelch_level = opts->rtl_squelch_level; //no longer used here, used in framesync vc rms value under select conditions + // demod.squelch_level = opts->rtl_squelch_level; //no longer used here, used in framesync vc pwr value under select conditions fprintf (stderr, "Setting RTL Bandwidth to %d Hz\n", rtl_bandwidth); // fprintf (stderr, "Setting RTL Sample Multiplier to %d\n", bandwidth_multiplier); - fprintf (stderr, "Setting RTL RMS Squelch Level to %d\n", opts->rtl_squelch_level); + fprintf (stderr, "Setting RTL Power Squelch Level to %d\n", opts->rtl_squelch_level); if (opts->rtl_udp_port != 0) port = opts->rtl_udp_port; //set this here, only open socket thread if set if (opts->rtl_gain_value > 0) { dongle.gain = opts->rtl_gain_value * 10; //multiple by ten to make it consitent with the way rtl_fm works @@ -3971,28 +3971,19 @@ void rtl_dev_tune(dsd_opts * opts, long int frequency) } /** - * Return mean power approximation for soft squelch decisions. + * Return mean power approximation (RMS^2 proxy) for soft squelch decisions. * Uses a small fixed sample window for efficiency. * - * @return Mean power value. + * @return Mean power value (approximate RMS squared). */ -long int rtl_return_rms(void) +long int rtl_return_pwr(void) { - long int sr = 0; - // #ifdef __arm__ - // sr = 100; - // #else - //debug -- on main machine, lp_len is around 6420, so this probably contributes to very high CPU usage - // fprintf (stderr, "LP_LEN: %d \n", demod.lp_len); - //I've found that just using a sample size of 160 will give us a good approximation without killing the CPU - // Return mean power (squared RMS) for soft squelch decisions (sqrt-free) - // sr = mean_power(demod.lowpassed, demod.lp_len, 1); + long int pwr = 0; int n = demod.lp_len; if (n > 160) n = 160; if (n < 0) n = 0; - sr = mean_power(demod.lowpassed, n, 1); - // #endif - return (sr); + pwr = mean_power(demod.lowpassed, n, 1); + return (pwr); } /** From 8b423a2f4290bda1ef65c8dda2966c368fd78017 Mon Sep 17 00:00:00 2001 From: arancormonk <180709949+arancormonk@users.noreply.github.com> Date: Thu, 4 Sep 2025 18:11:25 -0500 Subject: [PATCH 48/48] rtl_sdr_fm: fix potential race condition, zero int all demod init funcs, add timeouts to prevent indefinite blocking, remove producer-side tail advancement, harden udp socket read loop --- src/rtl_sdr_fm.cpp | 79 +++++++++++++++++++++++++++++++--------------- 1 file changed, 54 insertions(+), 25 deletions(-) diff --git a/src/rtl_sdr_fm.cpp b/src/rtl_sdr_fm.cpp index 40e802c..cf3b48d 100644 --- a/src/rtl_sdr_fm.cpp +++ b/src/rtl_sdr_fm.cpp @@ -921,7 +921,7 @@ struct dongle_state int ppm_error; int offset_tuning; int direct_sampling; - int mute; + std::atomic mute; struct demod_state *demod_target; }; @@ -1236,14 +1236,7 @@ static int input_ring_reserve(struct input_ring_state *r, size_t min_needed, int16_t **p1, size_t *n1, int16_t **p2, size_t *n2) { size_t free_sp = input_ring_free(r); - if (free_sp == 0) { - /* match write() behavior: drop oldest half to free space */ - size_t t = r->tail.load(); - size_t drop = r->capacity / 2; - t = (t + drop) % r->capacity; - r->tail.store(t); - free_sp = input_ring_free(r); - } + /* Producer must never advance consumer tail; if full, grant nothing */ /* Provide up to min(free_sp, min_needed) across at most two regions */ size_t grant = (min_needed < free_sp) ? min_needed : free_sp; size_t h = r->head.load(); @@ -1295,12 +1288,8 @@ static void input_ring_write(struct input_ring_state *r, const int16_t *data, si while (count > 0 && !exitflag) { size_t free_sp = input_ring_free(r); if (free_sp == 0) { - /* drop oldest half to avoid blocking USB callback */ - size_t t = r->tail.load(); - size_t drop = r->capacity / 2; - t = (t + drop) % r->capacity; - r->tail.store(t); - free_sp = input_ring_free(r); + /* Ring full: to avoid racing the consumer, drop remainder */ + break; } size_t write_now = (count < free_sp) ? count : free_sp; size_t h = r->head.load(); @@ -1512,8 +1501,18 @@ static void ring_write(struct output_state *o, const int16_t *data, size_t count while (count > 0 && !exitflag) { size_t free_sp = ring_free(o); if (free_sp == 0) { - /* Wait for space */ - safe_cond_wait(&o->space, &o->ready_m); + /* Wait for space with timeout to avoid indefinite blocking */ + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_nsec += 50L * 1000000L; /* 50ms */ + if (ts.tv_nsec >= 1000000000L) { + ts.tv_sec += ts.tv_nsec / 1000000000L; + ts.tv_nsec = ts.tv_nsec % 1000000000L; + } + pthread_mutex_lock(&o->ready_m); + pthread_cond_timedwait(&o->space, &o->ready_m, &ts); + pthread_mutex_unlock(&o->ready_m); + if (exitflag) break; continue; } size_t write_now = (count < free_sp) ? count : free_sp; @@ -1543,7 +1542,18 @@ static void ring_write_no_signal(struct output_state *o, const int16_t *data, si while (count > 0 && !exitflag) { size_t free_sp = ring_free(o); if (free_sp == 0) { - safe_cond_wait(&o->space, &o->ready_m); + /* Wait for space with timeout to avoid indefinite blocking */ + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_nsec += 50L * 1000000L; /* 50ms */ + if (ts.tv_nsec >= 1000000000L) { + ts.tv_sec += ts.tv_nsec / 1000000000L; + ts.tv_nsec = ts.tv_nsec % 1000000000L; + } + pthread_mutex_lock(&o->ready_m); + pthread_cond_timedwait(&o->space, &o->ready_m, &ts); + pthread_mutex_unlock(&o->ready_m); + if (exitflag) break; continue; } size_t write_now = (count < free_sp) ? count : free_sp; @@ -2689,10 +2699,13 @@ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) return;} if (s->mute) { /* Clamp mute length to buffer size to avoid overwrite; carry remainder */ - uint32_t m = (uint32_t)s->mute; - if (m > len) m = len; - memset(buf, 127, m); - s->mute -= (int)m; + int old = s->mute.load(std::memory_order_relaxed); + if (old > 0) { + uint32_t m = (uint32_t)old; + if (m > len) m = len; + memset(buf, 127, m); + s->mute.fetch_sub((int)m, std::memory_order_relaxed); + } } /* Convert incoming u8 I/Q and write directly into input ring without extra copy */ size_t need = len; @@ -3203,6 +3216,11 @@ void demod_init_analog(struct demod_state *s) memset(s->hb_hist_i[st], 0, sizeof(s->hb_hist_i[st])); memset(s->hb_hist_q[st], 0, sizeof(s->hb_hist_q[st])); } + /* Legacy CIC histories used by fifth_order path */ + for (int st = 0; st < 10; st++) { + memset(s->lp_i_hist[st], 0, sizeof(s->lp_i_hist[st])); + memset(s->lp_q_hist[st], 0, sizeof(s->lp_q_hist[st])); + } /* Input ring does not require double-buffer init */ s->lowpassed = s->input_cb_buf; s->lp_len = 0; @@ -3286,6 +3304,11 @@ void demod_init_ro2(struct demod_state *s) memset(s->hb_hist_i[st], 0, sizeof(s->hb_hist_i[st])); memset(s->hb_hist_q[st], 0, sizeof(s->hb_hist_q[st])); } + /* Legacy CIC histories used by fifth_order path */ + for (int st = 0; st < 10; st++) { + memset(s->lp_i_hist[st], 0, sizeof(s->lp_i_hist[st])); + memset(s->lp_q_hist[st], 0, sizeof(s->lp_q_hist[st])); + } /* Input ring does not require double-buffer init */ s->lowpassed = s->input_cb_buf; s->lp_len = 0; @@ -3364,6 +3387,11 @@ void demod_init(struct demod_state *s) memset(s->hb_hist_i[st], 0, sizeof(s->hb_hist_i[st])); memset(s->hb_hist_q[st], 0, sizeof(s->hb_hist_q[st])); } + /* Legacy CIC histories used by fifth_order path */ + for (int st = 0; st < 10; st++) { + memset(s->lp_i_hist[st], 0, sizeof(s->lp_i_hist[st])); + memset(s->lp_q_hist[st], 0, sizeof(s->lp_q_hist[st])); + } /* Input ring does not require double-buffer init */ s->lowpassed = s->input_cb_buf; s->lp_len = 0; @@ -3544,7 +3572,7 @@ static void *socket_thread_fn(void *arg) { int new_freq; - while((n = read(sockfd,buffer,5)) != 0) { + while ((n = read(sockfd, buffer, 5)) > 0) { if (n == 5 && buffer[0] == 0) { new_freq = chars_to_int(buffer); dongle.freq = new_freq; @@ -3552,8 +3580,9 @@ static void *socket_thread_fn(void *arg) { rtlsdr_set_center_freq(dongle.dev, dongle.freq); fprintf (stderr, "\nTuning to: %d [Hz] \n", new_freq); } - - + } + if (n < 0) { + perror("ERROR on read"); } close(sockfd);