mirror of
https://github.com/lwvmobile/dsd-fme.git
synced 2026-11-20 20:55:40 -05:00
rtl_sdr_fm: enable vectorization and alignment for DSP arrays
This commit is contained in:
@@ -45,6 +45,27 @@ static int ACTUAL_BUF_LENGTH;
|
|||||||
|
|
||||||
static const double kPi = 3.14159265358979323846;
|
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 <typename T>
|
||||||
|
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 <typename T>
|
||||||
|
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 = NULL;
|
||||||
static int atan_lut_size = 131072; /* 512 KB */
|
static int atan_lut_size = 131072; /* 512 KB */
|
||||||
static int atan_lut_coef = 8;
|
static int atan_lut_coef = 8;
|
||||||
@@ -98,14 +119,14 @@ struct demod_state
|
|||||||
pthread_t thread;
|
pthread_t thread;
|
||||||
int16_t *lowpassed;
|
int16_t *lowpassed;
|
||||||
/* Double-buffered input for callback→demod handoff */
|
/* 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<int> write_buf_index;
|
std::atomic<int> write_buf_index;
|
||||||
std::atomic<int> ready_buf_index;
|
std::atomic<int> ready_buf_index;
|
||||||
std::atomic<uint32_t> input_len[2];
|
std::atomic<uint32_t> input_len[2];
|
||||||
int lp_len;
|
int lp_len;
|
||||||
int16_t lp_i_hist[10][6];
|
int16_t lp_i_hist[10][6];
|
||||||
int16_t lp_q_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_i_hist[9];
|
||||||
int16_t droop_q_hist[9];
|
int16_t droop_q_hist[9];
|
||||||
int result_len;
|
int result_len;
|
||||||
@@ -456,6 +477,7 @@ void low_pass_real(struct demod_state *s)
|
|||||||
// add support for upsampling?
|
// add support for upsampling?
|
||||||
{
|
{
|
||||||
int i=0, i2=0;
|
int i=0, i2=0;
|
||||||
|
int16_t *r = assume_aligned_ptr(s->result, DSD_FME_ALIGN);
|
||||||
int fast = (int)s->rate_out;
|
int fast = (int)s->rate_out;
|
||||||
int slow = s->rate_out2;
|
int slow = s->rate_out2;
|
||||||
/* Precompute fixed-point reciprocal of decimation factor to avoid per-sample division */
|
/* 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;
|
if (decim < 1) decim = 1;
|
||||||
const int kShiftLPR = 15; /* Q15 reciprocal */
|
const int kShiftLPR = 15; /* Q15 reciprocal */
|
||||||
int recip_decim_q = (1 << kShiftLPR) / decim;
|
int recip_decim_q = (1 << kShiftLPR) / decim;
|
||||||
|
DSD_FME_IVDEP
|
||||||
while (i < s->result_len) {
|
while (i < s->result_len) {
|
||||||
s->now_lpr += s->result[i];
|
s->now_lpr += r[i];
|
||||||
i++;
|
i++;
|
||||||
s->prev_lpr_index += slow;
|
s->prev_lpr_index += slow;
|
||||||
if (s->prev_lpr_index < fast) {
|
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) */
|
/* Multiply by reciprocal and shift instead of dividing by (fast/slow) */
|
||||||
int64_t scaled = ((int64_t)s->now_lpr * recip_decim_q);
|
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->prev_lpr_index -= fast;
|
||||||
s->now_lpr = 0;
|
s->now_lpr = 0;
|
||||||
i2 += 1;
|
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)
|
void fm_demod(struct demod_state *fm)
|
||||||
{
|
{
|
||||||
int i, pcm;
|
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 */
|
/* Use selected discriminator from the very first sample */
|
||||||
pcm = fm->discriminator(lp[0], lp[1], fm->pre_r, fm->pre_j);
|
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) {
|
for (i = 2; i < (fm->lp_len-1); i += 2) {
|
||||||
pcm = fm->discriminator(lp[i], lp[i+1], lp[i-2], lp[i-1]);
|
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_r = lp[fm->lp_len - 2];
|
||||||
fm->pre_j = lp[fm->lp_len - 1];
|
fm->pre_j = lp[fm->lp_len - 1];
|
||||||
@@ -727,6 +752,7 @@ void deemph_filter(struct demod_state *fm)
|
|||||||
{
|
{
|
||||||
static int avg; // cheating...
|
static int avg; // cheating...
|
||||||
int i, d;
|
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 */
|
/* Precompute fixed-point reciprocal of deemphasis constant to avoid per-sample division */
|
||||||
const int kShiftDeemph = 15; /* Q15 */
|
const int kShiftDeemph = 15; /* Q15 */
|
||||||
int a = fm->deemph_a;
|
int a = fm->deemph_a;
|
||||||
@@ -734,8 +760,9 @@ void deemph_filter(struct demod_state *fm)
|
|||||||
int recip_q = (1 << kShiftDeemph) / a;
|
int recip_q = (1 << kShiftDeemph) / a;
|
||||||
// de-emph IIR
|
// de-emph IIR
|
||||||
// avg = avg * (1 - alpha) + sample * alpha;
|
// avg = avg * (1 - alpha) + sample * alpha;
|
||||||
|
DSD_FME_IVDEP
|
||||||
for (i = 0; i < fm->result_len; i++) {
|
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 */
|
/* Use multiply+shift with sign-aware rounding to mirror original behavior */
|
||||||
int64_t delta = (int64_t)d * recip_q;
|
int64_t delta = (int64_t)d * recip_q;
|
||||||
if (d > 0) {
|
if (d > 0) {
|
||||||
@@ -744,7 +771,7 @@ void deemph_filter(struct demod_state *fm)
|
|||||||
delta -= (1LL << (kShiftDeemph - 1));
|
delta -= (1LL << (kShiftDeemph - 1));
|
||||||
}
|
}
|
||||||
avg += (int)(delta >> kShiftDeemph);
|
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);
|
pthread_mutex_init(&s->ready_m, NULL);
|
||||||
/* Allocate SPSC ring buffer */
|
/* Allocate SPSC ring buffer */
|
||||||
s->capacity = (size_t)(MAXIMUM_BUF_LENGTH * 8);
|
s->capacity = (size_t)(MAXIMUM_BUF_LENGTH * 8);
|
||||||
s->buffer = static_cast<int16_t*>(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<int16_t*>(mem_ptr);
|
||||||
|
}
|
||||||
s->head.store(0);
|
s->head.store(0);
|
||||||
s->tail.store(0);
|
s->tail.store(0);
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user