1/* Copyright (c) 2013 The Chromium OS Authors. All rights reserved.
2 * Use of this source code is governed by a BSD-style license that can be
3 * found in the LICENSE file.
4 */
5
6/* Copyright (C) 2011 Google Inc. All rights reserved.
7 * Use of this source code is governed by a BSD-style license that can be
8 * found in the LICENSE.WEBKIT file.
9 */
10
11#include <assert.h>
12#include <stdlib.h>
13
14#include "drc.h"
15#include "drc_math.h"
16
17static void set_default_parameters(struct drc *drc);
18static void init_data_buffer(struct drc *drc);
19static void init_emphasis_eq(struct drc *drc);
20static void init_crossover(struct drc *drc);
21static void init_kernel(struct drc *drc);
22static void free_data_buffer(struct drc *drc);
23static void free_emphasis_eq(struct drc *drc);
24static void free_kernel(struct drc *drc);
25
26struct drc *drc_new(float sample_rate)
27{
28	struct drc *drc = (struct drc *)calloc(1, sizeof(struct drc));
29	drc->sample_rate = sample_rate;
30	set_default_parameters(drc);
31	return drc;
32}
33
34void drc_init(struct drc *drc)
35{
36	init_data_buffer(drc);
37	init_emphasis_eq(drc);
38	init_crossover(drc);
39	init_kernel(drc);
40}
41
42void drc_free(struct drc *drc)
43{
44	free_kernel(drc);
45	free_emphasis_eq(drc);
46	free_data_buffer(drc);
47	free(drc);
48}
49
50/* Allocates temporary buffers used during drc_process(). */
51static void init_data_buffer(struct drc *drc)
52{
53	int i;
54	size_t size = sizeof(float) * DRC_PROCESS_MAX_FRAMES;
55
56	for (i = 0; i < DRC_NUM_CHANNELS; i++) {
57		drc->data1[i] = (float *)calloc(1, size);
58		drc->data2[i] = (float *)calloc(1, size);
59	}
60}
61
62/* Frees temporary buffers */
63static void free_data_buffer(struct drc *drc)
64{
65	int i;
66
67	for (i = 0; i < DRC_NUM_CHANNELS; i++) {
68		free(drc->data1[i]);
69		free(drc->data2[i]);
70	}
71}
72
73void drc_set_param(struct drc *drc, int index, unsigned paramID, float value)
74{
75	assert(paramID < PARAM_LAST);
76	if (paramID < PARAM_LAST)
77		drc->parameters[index][paramID] = value;
78}
79
80static float drc_get_param(struct drc *drc, int index, unsigned paramID)
81{
82	assert(paramID < PARAM_LAST);
83	return drc->parameters[index][paramID];
84}
85
86/* Initializes parameters to default values. */
87static void set_default_parameters(struct drc *drc)
88{
89	float nyquist = drc->sample_rate / 2;
90	int i;
91
92	for (i = 0; i < DRC_NUM_KERNELS; i++) {
93		float *param = drc->parameters[i];
94		param[PARAM_THRESHOLD] = -24; /* dB */
95		param[PARAM_KNEE] = 30; /* dB */
96		param[PARAM_RATIO] = 12; /* unit-less */
97		param[PARAM_ATTACK] = 0.003f; /* seconds */
98		param[PARAM_RELEASE] = 0.250f; /* seconds */
99		param[PARAM_PRE_DELAY] = DRC_DEFAULT_PRE_DELAY; /* seconds */
100
101		/* Release zone values 0 -> 1. */
102		param[PARAM_RELEASE_ZONE1] = 0.09f;
103		param[PARAM_RELEASE_ZONE2] = 0.16f;
104		param[PARAM_RELEASE_ZONE3] = 0.42f;
105		param[PARAM_RELEASE_ZONE4] = 0.98f;
106
107		/* This is effectively a master volume on the compressed
108		 * signal */
109		param[PARAM_POST_GAIN] = 0; /* dB */
110		param[PARAM_ENABLED] = 0;
111	}
112
113	drc->parameters[0][PARAM_CROSSOVER_LOWER_FREQ] = 0;
114	drc->parameters[1][PARAM_CROSSOVER_LOWER_FREQ] = 200 / nyquist;
115	drc->parameters[2][PARAM_CROSSOVER_LOWER_FREQ] = 2000 / nyquist;
116
117	/* These parameters has only one copy */
118	drc->parameters[0][PARAM_FILTER_STAGE_GAIN] = 4.4f; /* dB */
119	drc->parameters[0][PARAM_FILTER_STAGE_RATIO] = 2;
120	drc->parameters[0][PARAM_FILTER_ANCHOR] = 15000 / nyquist;
121}
122
123/* Finds the zero and pole for one stage of the emphasis filter */
124static void emphasis_stage_roots(float gain, float normalized_frequency,
125				 float *zero, float *pole)
126{
127	float gk = 1 - gain / 20;
128	float f1 = normalized_frequency * gk;
129	float f2 = normalized_frequency / gk;
130	*zero = expf(-f1 * PI_FLOAT);
131	*pole = expf(-f2 * PI_FLOAT);
132}
133
134/* Calculates the biquad coefficients for two emphasis stages. */
135static void emphasis_stage_pair_biquads(float gain, float f1, float f2,
136					struct biquad *emphasis,
137					struct biquad *deemphasis)
138{
139	float z1, p1;
140	float z2, p2;
141
142	emphasis_stage_roots(gain, f1, &z1, &p1);
143	emphasis_stage_roots(gain, f2, &z2, &p2);
144
145	float b0 = 1;
146	float b1 = -(z1 + z2);
147	float b2 = z1 * z2;
148	float a0 = 1;
149	float a1 = -(p1 + p2);
150	float a2 = p1 * p2;
151
152	/* Gain compensation to make 0dB @ 0Hz */
153	float alpha = (a0 + a1 + a2) / (b0 + b1 + b2);
154
155	emphasis->b0 = b0 * alpha;
156	emphasis->b1 = b1 * alpha;
157	emphasis->b2 = b2 * alpha;
158	emphasis->a1 = a1;
159	emphasis->a2 = a2;
160
161	float beta = (b0 + b1 + b2) / (a0 + a1 + a2);
162
163	deemphasis->b0 = a0 * beta;
164	deemphasis->b1 = a1 * beta;
165	deemphasis->b2 = a2 * beta;
166	deemphasis->a1 = b1;
167	deemphasis->a2 = b2;
168}
169
170/* Initializes the emphasis and deemphasis filter */
171static void init_emphasis_eq(struct drc *drc)
172{
173	struct biquad e;
174	struct biquad d;
175	int i, j;
176
177	float stage_gain = drc_get_param(drc, 0, PARAM_FILTER_STAGE_GAIN);
178	float stage_ratio = drc_get_param(drc, 0, PARAM_FILTER_STAGE_RATIO);
179	float anchor_freq = drc_get_param(drc, 0,  PARAM_FILTER_ANCHOR);
180
181	drc->emphasis_eq = eq2_new();
182	drc->deemphasis_eq = eq2_new();
183
184	for (i = 0; i < 2; i++) {
185		emphasis_stage_pair_biquads(stage_gain, anchor_freq,
186					    anchor_freq / stage_ratio,
187					    &e, &d);
188		for (j = 0; j < 2; j++) {
189			eq2_append_biquad_direct(drc->emphasis_eq, j, &e);
190			eq2_append_biquad_direct(drc->deemphasis_eq, j, &d);
191		}
192		anchor_freq /= (stage_ratio * stage_ratio);
193	}
194}
195
196/* Frees the emphasis and deemphasis filter */
197static void free_emphasis_eq(struct drc *drc)
198{
199	eq2_free(drc->emphasis_eq);
200	eq2_free(drc->deemphasis_eq);
201}
202
203/* Initializes the crossover filter */
204static void init_crossover(struct drc *drc)
205{
206	float freq1 = drc->parameters[1][PARAM_CROSSOVER_LOWER_FREQ];
207	float freq2 = drc->parameters[2][PARAM_CROSSOVER_LOWER_FREQ];
208
209	crossover2_init(&drc->xo2, freq1, freq2);
210}
211
212/* Initializes the compressor kernels */
213static void init_kernel(struct drc *drc)
214{
215	int i;
216
217	for (i = 0; i < DRC_NUM_KERNELS; i++) {
218		dk_init(&drc->kernel[i], drc->sample_rate);
219
220		float db_threshold = drc_get_param(drc, i, PARAM_THRESHOLD);
221		float db_knee = drc_get_param(drc, i, PARAM_KNEE);
222		float ratio = drc_get_param(drc, i, PARAM_RATIO);
223		float attack_time = drc_get_param(drc, i, PARAM_ATTACK);
224		float release_time = drc_get_param(drc, i, PARAM_RELEASE);
225		float pre_delay_time = drc_get_param(drc, i, PARAM_PRE_DELAY);
226		float releaseZone1 = drc_get_param(drc, i, PARAM_RELEASE_ZONE1);
227		float releaseZone2 = drc_get_param(drc, i, PARAM_RELEASE_ZONE2);
228		float releaseZone3 = drc_get_param(drc, i, PARAM_RELEASE_ZONE3);
229		float releaseZone4 = drc_get_param(drc, i, PARAM_RELEASE_ZONE4);
230		float db_post_gain = drc_get_param(drc, i, PARAM_POST_GAIN);
231		int enabled = drc_get_param(drc, i, PARAM_ENABLED);
232
233		dk_set_parameters(&drc->kernel[i],
234				  db_threshold,
235				  db_knee,
236				  ratio,
237				  attack_time,
238				  release_time,
239				  pre_delay_time,
240				  db_post_gain,
241				  releaseZone1,
242				  releaseZone2,
243				  releaseZone3,
244				  releaseZone4
245			);
246
247		dk_set_enabled(&drc->kernel[i], enabled);
248	}
249}
250
251/* Frees the compressor kernels */
252static void free_kernel(struct drc *drc)
253{
254	int i;
255	for (i = 0; i < DRC_NUM_KERNELS; i++)
256		dk_free(&drc->kernel[i]);
257}
258
259#if defined(__ARM_NEON__)
260#include <arm_neon.h>
261static void sum3(float *data, float *data1, float *data2, int n)
262{
263	float32x4_t x, y, z;
264	int count = n / 4;
265	int i;
266
267	if (count) {
268		__asm__ __volatile(
269			"1:                                         \n"
270			"vld1.32 {%e[x],%f[x]}, [%[data1]]!         \n"
271			"vld1.32 {%e[y],%f[y]}, [%[data2]]!         \n"
272			"vld1.32 {%e[z],%f[z]}, [%[data]]           \n"
273			"vadd.f32 %q[y], %q[x]                      \n"
274			"vadd.f32 %q[z], %q[y]                      \n"
275			"vst1.32 {%e[z],%f[z]}, [%[data]]!          \n"
276			"subs %[count], #1                          \n"
277			"bne 1b                                     \n"
278			: /* output */
279			  "=r"(data),
280			  "=r"(data1),
281			  "=r"(data2),
282			  "=r"(count),
283			  [x]"=&w"(x),
284			  [y]"=&w"(y),
285			  [z]"=&w"(z)
286			: /* input */
287			  [data]"0"(data),
288			  [data1]"1"(data1),
289			  [data2]"2"(data2),
290			  [count]"3"(count)
291			: /* clobber */
292			  "memory", "cc"
293			);
294	}
295
296	n &= 3;
297	for (i = 0; i < n; i++)
298		data[i] += data1[i] + data2[i];
299}
300#elif defined(__SSE3__)
301#include <emmintrin.h>
302static void sum3(float *data, float *data1, float *data2, int n)
303{
304	__m128 x, y, z;
305	int count = n / 4;
306	int i;
307
308	if (count) {
309		__asm__ __volatile(
310			"1:                                         \n"
311			"lddqu (%[data1]), %[x]                     \n"
312			"lddqu (%[data2]), %[y]                     \n"
313			"lddqu (%[data]), %[z]                      \n"
314			"addps %[x], %[y]                           \n"
315			"addps %[y], %[z]                           \n"
316			"movdqu %[z], (%[data])                     \n"
317			"add $16, %[data1]                          \n"
318			"add $16, %[data2]                          \n"
319			"add $16, %[data]                           \n"
320			"sub $1, %[count]                           \n"
321			"jne 1b                                     \n"
322			: /* output */
323			  "=r"(data),
324			  "=r"(data1),
325			  "=r"(data2),
326			  "=r"(count),
327			  [x]"=&x"(x),
328			  [y]"=&x"(y),
329			  [z]"=&x"(z)
330			: /* input */
331			  [data]"0"(data),
332			  [data1]"1"(data1),
333			  [data2]"2"(data2),
334			  [count]"3"(count)
335			: /* clobber */
336			  "memory", "cc"
337			);
338	}
339
340	n &= 3;
341	for (i = 0; i < n; i++)
342		data[i] += data1[i] + data2[i];
343}
344#else
345static void sum3(float *data, float *data1, float *data2, int n)
346{
347	int i;
348	for (i = 0; i < n; i++)
349		data[i] += data1[i] + data2[i];
350}
351#endif
352
353void drc_process(struct drc *drc, float **data, int frames)
354{
355	int i;
356	float **data1 = drc->data1;
357	float **data2 = drc->data2;
358
359	/* Apply pre-emphasis filter if it is not disabled. */
360	if (!drc->emphasis_disabled)
361		eq2_process(drc->emphasis_eq, data[0], data[1], frames);
362
363	/* Crossover */
364	crossover2_process(&drc->xo2, frames, data[0], data[1],
365			   data1[0], data1[1], data2[0], data2[1]);
366
367	/* Apply compression to each band of the signal. The processing is
368	 * performed in place.
369	 */
370	dk_process(&drc->kernel[0], data, frames);
371	dk_process(&drc->kernel[1], data1, frames);
372	dk_process(&drc->kernel[2], data2, frames);
373
374	/* Sum the three bands of signal */
375	for (i = 0; i < DRC_NUM_CHANNELS; i++)
376		sum3(data[i], data1[i], data2[i], frames);
377
378	/* Apply de-emphasis filter if emphasis is not disabled. */
379	if (!drc->emphasis_disabled)
380		eq2_process(drc->deemphasis_eq, data[0], data[1], frames);
381}
382