diff options
Diffstat (limited to 'lib/rbcodec/dsp/eq.c')
-rw-r--r-- | lib/rbcodec/dsp/eq.c | 77 |
1 files changed, 55 insertions, 22 deletions
diff --git a/lib/rbcodec/dsp/eq.c b/lib/rbcodec/dsp/eq.c index 94cb61deec..e4d7bf5e02 100644 --- a/lib/rbcodec/dsp/eq.c +++ b/lib/rbcodec/dsp/eq.c | |||
@@ -25,6 +25,7 @@ | |||
25 | #include "dsp_filter.h" | 25 | #include "dsp_filter.h" |
26 | #include "dsp_proc_entry.h" | 26 | #include "dsp_proc_entry.h" |
27 | #include "dsp_core.h" | 27 | #include "dsp_core.h" |
28 | #include "dsp_misc.h" | ||
28 | #include "eq.h" | 29 | #include "eq.h" |
29 | #include "pga.h" | 30 | #include "pga.h" |
30 | #include "replaygain.h" | 31 | #include "replaygain.h" |
@@ -42,6 +43,9 @@ | |||
42 | #error Band count must be greater than or equal to 3 | 43 | #error Band count must be greater than or equal to 3 |
43 | #endif | 44 | #endif |
44 | 45 | ||
46 | /* Cached band settings */ | ||
47 | static struct eq_band_setting settings[EQ_NUM_BANDS]; | ||
48 | |||
45 | static struct eq_state | 49 | static struct eq_state |
46 | { | 50 | { |
47 | uint32_t enabled; /* Mask of enabled bands */ | 51 | uint32_t enabled; /* Mask of enabled bands */ |
@@ -49,16 +53,48 @@ static struct eq_state | |||
49 | struct dsp_filter filters[EQ_NUM_BANDS]; /* Data for each filter */ | 53 | struct dsp_filter filters[EQ_NUM_BANDS]; /* Data for each filter */ |
50 | } eq_data IBSS_ATTR; | 54 | } eq_data IBSS_ATTR; |
51 | 55 | ||
56 | #define FOR_EACH_ENB_BAND(b) \ | ||
57 | for (uint8_t *b = eq_data.bands; *b < EQ_NUM_BANDS; b++) | ||
58 | |||
52 | /* Clear histories of all enabled bands */ | 59 | /* Clear histories of all enabled bands */ |
53 | static void eq_flush(void) | 60 | static void eq_flush(void) |
54 | { | 61 | { |
55 | if (eq_data.enabled == 0) | 62 | if (eq_data.enabled == 0) |
56 | return; /* Not initialized yet/no bands on */ | 63 | return; /* Not initialized yet/no bands on */ |
57 | 64 | ||
58 | for (uint8_t *b = eq_data.bands; *b < EQ_NUM_BANDS; b++) | 65 | FOR_EACH_ENB_BAND(b) |
59 | filter_flush(&eq_data.filters[*b]); | 66 | filter_flush(&eq_data.filters[*b]); |
60 | } | 67 | } |
61 | 68 | ||
69 | static void update_band_filter(int band, unsigned int fout) | ||
70 | { | ||
71 | /* Convert user settings to format required by coef generator | ||
72 | functions */ | ||
73 | typeof (filter_pk_coefs) *coef_gen = filter_pk_coefs; | ||
74 | |||
75 | /* Only first and last bands are not peaking filters */ | ||
76 | if (band == 0) | ||
77 | coef_gen = filter_ls_coefs; | ||
78 | else if (band == EQ_NUM_BANDS-1) | ||
79 | coef_gen = filter_hs_coefs; | ||
80 | |||
81 | const struct eq_band_setting *setting = &settings[band]; | ||
82 | struct dsp_filter *filter = &eq_data.filters[band]; | ||
83 | |||
84 | coef_gen(fp_div(setting->cutoff, fout, 32), setting->q ?: 1, | ||
85 | setting->gain, filter); | ||
86 | } | ||
87 | |||
88 | /* Resync all bands to a new DSP output frequency */ | ||
89 | static void update_samplerate(unsigned int fout) | ||
90 | { | ||
91 | if (eq_data.enabled == 0) | ||
92 | return; /* Not initialized yet/no bands on */ | ||
93 | |||
94 | FOR_EACH_ENB_BAND(b) | ||
95 | update_band_filter(*b, fout); | ||
96 | } | ||
97 | |||
62 | /** DSP interface **/ | 98 | /** DSP interface **/ |
63 | 99 | ||
64 | /* Set the precut gain value */ | 100 | /* Set the precut gain value */ |
@@ -73,11 +109,14 @@ void dsp_set_eq_coefs(int band, const struct eq_band_setting *setting) | |||
73 | if (band < 0 || band >= EQ_NUM_BANDS) | 109 | if (band < 0 || band >= EQ_NUM_BANDS) |
74 | return; | 110 | return; |
75 | 111 | ||
112 | settings[band] = *setting; /* cache setting */ | ||
113 | |||
114 | struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO); | ||
115 | |||
76 | /* NOTE: The coef functions assume the EMAC unit is in fractional mode, | 116 | /* NOTE: The coef functions assume the EMAC unit is in fractional mode, |
77 | which it should be, since we're executed from the main thread. */ | 117 | which it should be, since we're executed from the main thread. */ |
78 | 118 | ||
79 | uint32_t mask = eq_data.enabled; | 119 | uint32_t mask = eq_data.enabled; |
80 | struct dsp_filter *filter = &eq_data.filters[band]; | ||
81 | 120 | ||
82 | /* Assume a band is disabled if the gain is zero */ | 121 | /* Assume a band is disabled if the gain is zero */ |
83 | mask &= ~BIT_N(band); | 122 | mask &= ~BIT_N(band); |
@@ -85,33 +124,19 @@ void dsp_set_eq_coefs(int band, const struct eq_band_setting *setting) | |||
85 | if (setting->gain != 0) | 124 | if (setting->gain != 0) |
86 | { | 125 | { |
87 | mask |= BIT_N(band); | 126 | mask |= BIT_N(band); |
88 | 127 | update_band_filter(band, dsp_get_output_frequency(dsp)); | |
89 | /* Convert user settings to format required by coef generator | ||
90 | functions */ | ||
91 | void (* coef_gen)(unsigned long cutoff, unsigned long Q, long db, | ||
92 | struct dsp_filter *f) = filter_pk_coefs; | ||
93 | |||
94 | /* Only first and last bands are not peaking filters */ | ||
95 | if (band == 0) | ||
96 | coef_gen = filter_ls_coefs; | ||
97 | else if (band == EQ_NUM_BANDS-1) | ||
98 | coef_gen = filter_hs_coefs; | ||
99 | |||
100 | coef_gen(0xffffffff / NATIVE_FREQUENCY * setting->cutoff, | ||
101 | setting->q ?: 1, setting->gain, filter); | ||
102 | } | 128 | } |
103 | 129 | ||
104 | if (mask == eq_data.enabled) | 130 | if (mask == eq_data.enabled) |
105 | return; /* No change in band-enable state */ | 131 | return; /* No change in band-enable state */ |
106 | 132 | ||
107 | if (mask & BIT_N(band)) | 133 | if (mask & BIT_N(band)) |
108 | filter_flush(filter); /* Coming online */ | 134 | filter_flush(&eq_data.filters[band]); /* Coming online */ |
109 | 135 | ||
110 | eq_data.enabled = mask; | 136 | eq_data.enabled = mask; |
111 | 137 | ||
112 | /* Only be active if there are bands to process - if EQ is off, then | 138 | /* Only be active if there are bands to process - if EQ is off, then |
113 | this call has no effect */ | 139 | this call has no effect */ |
114 | struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO); | ||
115 | dsp_proc_activate(dsp, DSP_PROC_EQUALIZER, mask != 0); | 140 | dsp_proc_activate(dsp, DSP_PROC_EQUALIZER, mask != 0); |
116 | 141 | ||
117 | /* Prepare list of enabled bands for efficient iteration */ | 142 | /* Prepare list of enabled bands for efficient iteration */ |
@@ -125,6 +150,11 @@ void dsp_set_eq_coefs(int band, const struct eq_band_setting *setting) | |||
125 | void dsp_eq_enable(bool enable) | 150 | void dsp_eq_enable(bool enable) |
126 | { | 151 | { |
127 | struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO); | 152 | struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO); |
153 | bool enabled = dsp_proc_enabled(dsp, DSP_PROC_EQUALIZER); | ||
154 | |||
155 | if (enable == enabled) | ||
156 | return; | ||
157 | |||
128 | dsp_proc_enable(dsp, DSP_PROC_EQUALIZER, enable); | 158 | dsp_proc_enable(dsp, DSP_PROC_EQUALIZER, enable); |
129 | 159 | ||
130 | if (enable && eq_data.enabled != 0) | 160 | if (enable && eq_data.enabled != 0) |
@@ -139,7 +169,7 @@ static void eq_process(struct dsp_proc_entry *this, | |||
139 | int count = buf->remcount; | 169 | int count = buf->remcount; |
140 | unsigned int channels = buf->format.num_channels; | 170 | unsigned int channels = buf->format.num_channels; |
141 | 171 | ||
142 | for (uint8_t *b = eq_data.bands; *b < EQ_NUM_BANDS; b++) | 172 | FOR_EACH_ENB_BAND(b) |
143 | filter_process(&eq_data.filters[*b], buf->p32, count, channels); | 173 | filter_process(&eq_data.filters[*b], buf->p32, count, channels); |
144 | 174 | ||
145 | (void)this; | 175 | (void)this; |
@@ -154,10 +184,9 @@ static intptr_t eq_configure(struct dsp_proc_entry *this, | |||
154 | switch (setting) | 184 | switch (setting) |
155 | { | 185 | { |
156 | case DSP_PROC_INIT: | 186 | case DSP_PROC_INIT: |
157 | if (value != 0) | ||
158 | break; /* Already enabled */ | ||
159 | |||
160 | this->process = eq_process; | 187 | this->process = eq_process; |
188 | /* Wouldn't have been getting frequency updates */ | ||
189 | update_samplerate(dsp_get_output_frequency(dsp)); | ||
161 | /* Fall-through */ | 190 | /* Fall-through */ |
162 | case DSP_PROC_CLOSE: | 191 | case DSP_PROC_CLOSE: |
163 | pga_enable_gain(PGA_EQ_PRECUT, setting == DSP_PROC_INIT); | 192 | pga_enable_gain(PGA_EQ_PRECUT, setting == DSP_PROC_INIT); |
@@ -166,6 +195,10 @@ static intptr_t eq_configure(struct dsp_proc_entry *this, | |||
166 | case DSP_FLUSH: | 195 | case DSP_FLUSH: |
167 | eq_flush(); | 196 | eq_flush(); |
168 | break; | 197 | break; |
198 | |||
199 | case DSP_SET_OUT_FREQUENCY: | ||
200 | update_samplerate(value); | ||
201 | break; | ||
169 | } | 202 | } |
170 | 203 | ||
171 | return 0; | 204 | return 0; |