1 files changed, 79 insertions, 38 deletions

diff --git a/lib/rbcodec/dsp/crossfeed.c b/lib/rbcodec/dsp/crossfeed.c
index 36a98f1f33..fc40c6b4d5 100644
--- a/lib/rbcodec/dsp/crossfeed.c
+++ b/lib/rbcodec/dsp/crossfeed.c
@@ -24,6 +24,7 @@
 #include "fixedpoint.h"
 #include "fracmul.h"
 #include "replaygain.h"
+#include "dsp_misc.h"
 #include "dsp_proc_entry.h"
 #include "dsp_filter.h"
 #include "crossfeed.h"
@@ -44,32 +45,40 @@ void crossfeed_meier_process(struct dsp_proc_entry *this,
  * to listen to on headphones with no crossfeed.
  */
 
+#define DELAY_LEN(fs)   ((300*(fs) / 1000000)*2) /* ~300 uS */
+
 /* Crossfeed */
 static struct crossfeed_state
 {
-    int32_t gain;           /* 00h: Direct path gain */
-    int32_t coefs[3];       /* 04h: Coefficients for the shelving filter */
     union
     {
-        struct              /* 10h: Data for meier crossfeed */
+        struct /* Data for meier crossfeed */
         {
-            int32_t vcl;
-            int32_t vcr;
-            int32_t vdiff;
-            int32_t coef1;
-            int32_t coef2;
+            int32_t reserved;   /* 00h: Reserved: overlaps gain */
+            int32_t vcl;        /* 04h: Left filter output */
+            int32_t vcr;        /* 08h: Right filter output */
+            int32_t vdiff;      /* 0ch: L-R difference signal */
+            int32_t coef1;      /* 10h: Left/right filter coef */
+            int32_t coef2;      /* 14h: Crossfeed filter coef */
         };
-        struct              /* 10h: Data for custom crossfeed */
+        struct /* Data for custom crossfeed */
         {
+            int32_t gain;       /* 00h: Direct path gain */
+            int32_t coefs[3];   /* 04h: Filter coefficients: b0, b1, a1 */
             int32_t history[4]; /* 10h: Format is x[n - 1], y[n - 1] (L + R) */
-            int32_t delay[13*2];/* 20h: Delay line buffer (L + R interleaved) */
+            int32_t *index;     /* 20h: Current pointer into the delay line */
+            int32_t *index_max; /* 24h: Current max pointer of delay line */
+                                /* 28h: Delay line buffer (L + R interleaved) */
+            int32_t delay[DELAY_LEN(DSP_OUT_MAX_HZ)]; /* Target-dependent size */
         };
     };
-    int32_t *index;         /* 88h: Current pointer into the delay line */
-                            /* 8ch */
 } crossfeed_state IBSS_ATTR;
 
 static int crossfeed_type = CROSSFEED_TYPE_NONE;
+/* Cached custom settings */
+static long crossfeed_lf_gain;
+static long crossfeed_hf_gain;
+static long crossfeed_cutoff;
 
 /* Discard the sample histories */
 static void crossfeed_flush(struct dsp_proc_entry *this)
@@ -82,12 +91,49 @@ static void crossfeed_flush(struct dsp_proc_entry *this)
     }
     else
     {
-        memset(state->history, 0,
-            sizeof (state->history) + sizeof (state->delay));
+        memset(state->history, 0, sizeof (state->history));
+        memset(state->delay, 0, sizeof (state->delay));
         state->index = state->delay;
     }
 }
 
+static void crossfeed_meier_update_filter(struct crossfeed_state *state,
+                                          unsigned int fout)
+{
+    /* 1 / (F.Rforward.C) */
+    state->coef1 = fp_div(2128, fout, 31);
+    /* 1 / (F.Rcross.C) */
+    state->coef2 = fp_div(1000, fout, 31);
+}
+
+static void crossfeed_custom_update_filter(struct crossfeed_state *state,
+                                           unsigned int fout)
+{
+    long lf_gain = crossfeed_lf_gain;
+    long hf_gain = crossfeed_hf_gain;
+    long cutoff  = crossfeed_cutoff;
+    int32_t *c = state->coefs;
+
+    long scaler = get_replaygain_int(lf_gain * 10) << 7;
+
+    cutoff = fp_div(cutoff, fout, 32);
+    hf_gain -= lf_gain;
+    /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB
+     * point instead of shelf midpoint. This is for compatibility with the old
+     * crossfeed shelf filter and should be removed if crossfeed settings are
+     * ever made incompatible for any other good reason.
+     */
+    cutoff = fp_div(cutoff, get_replaygain_int(hf_gain*5), 24);
+
+    filter_shelf_coefs(cutoff, hf_gain, false, c);
+    /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains
+     * over 1 and can do this safely
+     */
+    c[0] = FRACMUL_SHL(c[0], scaler, 4);
+    c[1] = FRACMUL_SHL(c[1], scaler, 4);
+    c[2] <<= 4;
+}
+
 
 /** DSP interface **/
 
@@ -114,24 +160,13 @@ void dsp_set_crossfeed_direct_gain(int gain)
 /* Both gains should be below 0 dB */
 void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff)
 {
-    int32_t *c = crossfeed_state.coefs;
-    long scaler = get_replaygain_int(lf_gain * 10) << 7;
+    crossfeed_lf_gain = lf_gain;
+    crossfeed_hf_gain = hf_gain;
+    crossfeed_cutoff  = cutoff;
 
-    cutoff = 0xffffffff / NATIVE_FREQUENCY * cutoff;
-    hf_gain -= lf_gain;
-    /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB
-     * point instead of shelf midpoint. This is for compatibility with the old
-     * crossfeed shelf filter and should be removed if crossfeed settings are
-     * ever made incompatible for any other good reason.
-     */
-    cutoff = fp_div(cutoff, get_replaygain_int(hf_gain*5), 24);
-    filter_shelf_coefs(cutoff, hf_gain, false, c);
-    /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains
-     * over 1 and can do this safely
-     */
-    c[0] = FRACMUL_SHL(c[0], scaler, 4);
-    c[1] = FRACMUL_SHL(c[1], scaler, 4);
-    c[2] <<= 4;
+    struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO);
+    crossfeed_custom_update_filter(&crossfeed_state,
+                                   dsp_get_output_frequency(dsp));
 }
 
 #if !defined(CPU_COLDFIRE) && !defined(CPU_ARM)
@@ -147,6 +182,7 @@ void crossfeed_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p)
     int32_t *coefs = &state->coefs[0];
     int32_t gain = state->gain;
     int32_t *di = state->index;
+    int32_t *di_max = state->index_max;
 
     int count = buf->remcount;
 
@@ -176,7 +212,7 @@ void crossfeed_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p)
         buf->p32[1][i] = FRACMUL(right, gain) + hist_l[1];
 
         /* Wrap delay line index if bigger than delay line size */
-        if (di >= delay + 13*2)
+        if (di >= di_max)
             di = delay;
     }
 
@@ -234,17 +270,21 @@ static void update_process_fn(struct dsp_proc_entry *this,
                               struct dsp_config *dsp)
 {
     struct crossfeed_state *state = (struct crossfeed_state *)this->data;
-    dsp_proc_fn_type fn = crossfeed_process;
+    dsp_proc_fn_type fn;
+
+    unsigned int fout = dsp_get_output_frequency(dsp);
 
     if (crossfeed_type != CROSSFEED_TYPE_CUSTOM)
     {
-        /* Set up for Meier */
-        /* 1 / (F.Rforward.C) */
-        state->coef1 = (0x7fffffff / NATIVE_FREQUENCY) * 2128;
-        /* 1 / (F.Rcross.C) */
-        state->coef2 = (0x7fffffff / NATIVE_FREQUENCY) * 1000;
+        crossfeed_meier_update_filter(state, fout);
         fn = crossfeed_meier_process;
     }
+    else
+    {
+        state->index_max = state->delay + DELAY_LEN(fout);
+        crossfeed_custom_update_filter(state, fout);
+        fn = crossfeed_process;
+    }
 
     if (this->process != fn)
     {
@@ -292,6 +332,7 @@ static intptr_t crossfeed_configure(struct dsp_proc_entry *this,
         if (value == 0)
             this->data = (intptr_t)&crossfeed_state;
 
+    case DSP_SET_OUT_FREQUENCY:
         update_process_fn(this, dsp);
         break;